Received-SPF: pass (google.com: domain of linux-bluetooth-owner@vger.kernel.org designates 2620:137:e000::1:20 as permitted sender) client-ip=2620:137:e000::1:20;
From:   Iulia Tanasescu <iulia.tanasescu@nxp.com>
To:     linux-bluetooth@vger.kernel.org
Cc:     claudia.rosu@nxp.com, silviu.barbulescu@nxp.com,
        andrei.istodorescu@nxp.com, mihai-octavian.urzica@nxp.com,
        vlad.pruteanu@nxp.com, Iulia Tanasescu <iulia.tanasescu@nxp.com>
Subject: [PATCH BlueZ 2/2] shared/bass: Implement CP opcode handlers
Date:   Tue, 13 Jun 2023 17:16:25 +0300
Message-Id: <20230613141625.9197-3-iulia.tanasescu@nxp.com>
In-Reply-To: <20230613141625.9197-1-iulia.tanasescu@nxp.com>
References: <20230613141625.9197-1-iulia.tanasescu@nxp.com>
Content-Transfer-Encoding: 8bit
Content-Type: text/plain
MIME-Version: 1.0
X-MS-Exchange-AntiSpam-MessageData-ChunkCount: 1
X-MS-Exchange-AntiSpam-MessageData-0: =?us-ascii?Q?SauqVI2bCKpyiyFvgJ9mFSxyU4HV3ihBMGZp8rdInxlKrhl81C61FXzFs99C?=
 =?us-ascii?Q?lzXTta7n0fNUK9Bvr+qf9IssNgj49u8Co8zpbq1HLYkBWjfAmATE9tAQ88Vi?=
 =?us-ascii?Q?NRFAe510iGm2U+yyG2kQD9+TLv/PPKNBM9NPm9+jNATNwGzqgPeMp8qUFkL2?=
 =?us-ascii?Q?pu9fdleXvPDxaDKJkhdgBLOedEaLgEKN9KEwYYwUKwP/7rr5Kjk94CoIgBre?=
 =?us-ascii?Q?Pm6g0Jh4GL3//7XpnnYJwR14jtyzhuJlD4A8V3PhsyYz0pPpSQuHxHrv9AJw?=
 =?us-ascii?Q?9AoZyEU1sPitCkB+G0/+/fSjRhoOmCSGEojtxE8X3dFBlMoQSnxaqTh4pWkR?=
 =?us-ascii?Q?o4seeFGAqOz75NZmkTSjGRf7vID4G9j2MzgZs03lo9jSno0E1sua59J5UkRO?=
 =?us-ascii?Q?zsq+QQTJ4yUE/v2AeEjkPuqYtHt/Kvc5Bap7hnRRBXg5MpQG1Q2bQAGy027s?=
 =?us-ascii?Q?L0ba5JsfDmg6riKIR7XaS3bDQ7obQaAK5LYWIUq/Ag9PrgcBvJyDmJV9zMAi?=
 =?us-ascii?Q?O2PbSkrzR4WX4qwJ7VWubr5NOJNsZ4HGSeq7o+Xmg8GDgjJBtWrtcM7KlT3Z?=
 =?us-ascii?Q?CSh1wg72JvcCSdZCUGPhiJRYsiaMWky/0Fw+0ZaGbVZPkCna7oGJmcBGAtR7?=
 =?us-ascii?Q?Y0p7IQScmwVlIIKGPNKj6ARlSXtVWTtlC7wg3rZmNo7QD4SvgyMuNFPlS7Ei?=
 =?us-ascii?Q?cfuEYekfZD/6s4+fegWLw0cC8uehSrLthAU0TEE7lY/yybryjo2APjqvBC3O?=
 =?us-ascii?Q?mtwuW1LdkaUQPytQjQYzlJDoK87VouZTOym7MgZTALUtpSqIyGnlIDUBHwOf?=
 =?us-ascii?Q?VwF4rQQc5vRCNKySzIFnSka7CjZMXia+X7AZkU80OyUuObTdqbXnBBtZjH4o?=
 =?us-ascii?Q?mWkfFwzXB9Ma/6mqlIzyWDo5G3WBFWmYzmaOMFoZsLbrL/fhyn1+YkxX6WP+?=
 =?us-ascii?Q?ZJXCA5fwC4Ee2Y8y691dVv6nSadBp1WBxa1cJqIEjLM+1GiLoN5oYxI+k50r?=
 =?us-ascii?Q?2VRqraBGGp0usVeiS/xw8sqLNVM7YMPgxj9pli/0PPoi09Sn9eRfYDWaOkOR?=
 =?us-ascii?Q?LCIxYNH/DdZFxu0kwzOsXGs2Sp2xc6LJbjacxtfYiyIsM0EHzX6JOmnlckGj?=
 =?us-ascii?Q?+qEob4rzynK4Lu/5uhqkK+afJRSu9BnhAL/P3s1w8vICaXZDDYAjwd3MV17t?=
 =?us-ascii?Q?R9td00uRELI1nnVA5uuru4jGuMKtc5ijvHLSrRWmaLS1sCJjMt5PLNZcJqiE?=
 =?us-ascii?Q?VEgbdP/VPVf5LkVPdGP7/Q+bFl5sYoD8lmDzjejyWyH5wKZvw9rWgVzZl6wa?=
 =?us-ascii?Q?4KHuWCkfk56dEXQ3knXfsOTWCPX+hUQrZWy+irTWKeWrTHsWdkay83D2Uyv1?=
 =?us-ascii?Q?lP+Ca5G/DAfEEj68N79gARXFF8yWxflArvAc4NmW+1Ip5u+5QJeJ/JdT5iA8?=
 =?us-ascii?Q?wenWh/2qlvBufqPo12PgCT5DoVi1JmbvFluVjwnuCsuRidRSle4DhJeMTVXk?=
 =?us-ascii?Q?YzaqLWbYxEjPSKfmQbVLithrUqv6PY4bg2CvAkQ0mvWX48OdZcHiVx7OMA8A?=
 =?us-ascii?Q?ey6cPfVJ4fDVATt146eGmu96f4MSfZMb8Luw/FFBHYZAc1IygRyEHlBztZhl?=
 =?us-ascii?Q?YA=3D=3D?=
X-OriginatorOrg: nxp.com
X-MS-Exchange-CrossTenant-Network-Message-Id: c8babfc0-bb0a-4585-4d96-08db6c18e6b3
X-MS-Exchange-CrossTenant-AuthSource: AS8PR04MB8898.eurprd04.prod.outlook.com
X-MS-Exchange-CrossTenant-AuthAs: Internal
X-MS-Exchange-CrossTenant-OriginalArrivalTime: 13 Jun 2023 14:17:20.9887
 (UTC)
X-MS-Exchange-CrossTenant-FromEntityHeader: Hosted
X-MS-Exchange-CrossTenant-Id: 686ea1d3-bc2b-4c6f-a92c-d99c5c301635
X-MS-Exchange-CrossTenant-MailboxType: HOSTED
X-MS-Exchange-CrossTenant-UserPrincipalName: lohAxsGrHIDyT9Y+iI90yzOD1C24G8JZbRRzxuaK4C0xFBYqRI89blKJsve0RAN0TSGYvxPI8Q7zQnUFQ2jsvQ==
X-MS-Exchange-Transport-CrossTenantHeadersStamped: VE1PR04MB7213
X-Spam-Status: No, score=-2.1 required=5.0 tests=BAYES_00,DKIM_SIGNED,
        DKIM_VALID,DKIM_VALID_AU,DKIM_VALID_EF,RCVD_IN_DNSWL_NONE,
        RCVD_IN_MSPIKE_H2,SPF_HELO_PASS,SPF_PASS,T_SCC_BODY_TEXT_LINE,
        URIBL_BLOCKED autolearn=ham autolearn_force=no version=3.4.6
X-Spam-Checker-Version: SpamAssassin 3.4.6 (2021-04-09) on
        lindbergh.monkeyblade.net
Precedence: bulk
List-ID: <linux-bluetooth.vger.kernel.org>
X-Mailing-List: linux-bluetooth@vger.kernel.org

This adds handlers for the following BASS Broadcast Audio Scan
Control Point opcodes:
   Remote Scan Stopped
   Remote Scan Started
   Remove Source

---
 src/shared/bass.c | 155 ++++++++++++++++++++++++++++++++++++++++++----
 1 file changed, 142 insertions(+), 13 deletions(-)

diff --git a/src/shared/bass.c b/src/shared/bass.c
index 8906ca1ef..423ab5bf7 100644
--- a/src/shared/bass.c
+++ b/src/shared/bass.c
@@ -82,6 +82,8 @@ static struct queue *bass_db;
 static struct queue *bass_cbs;
 static struct queue *sessions;
 
+static void bass_bcast_src_free(void *data);
+
 static void bass_debug(struct bt_bass *bass, const char *format, ...)
 {
 	va_list ap;
@@ -385,7 +387,7 @@ static bool bass_check_cp_command_subgroup_data_len(uint8_t num_subgroups,
 	return true;
 }
 
-static bool bass_check_cp_command_len(struct iovec *iov)
+static bool bass_check_cp_command_len(const uint8_t *value, size_t len)
 {
 	struct bt_bass_bcast_audio_scan_cp_hdr *hdr;
 	union {
@@ -395,8 +397,13 @@ static bool bass_check_cp_command_len(struct iovec *iov)
 		struct bt_bass_remove_src_params *remove_src_params;
 	} params;
 
+	struct iovec iov = {
+		.iov_base = (void *)value,
+		.iov_len = len,
+	};
+
 	/* Get command header */
-	hdr = util_iov_pull_mem(iov, sizeof(*hdr));
+	hdr = util_iov_pull_mem(&iov, sizeof(*hdr));
 
 	if (!hdr)
 		return false;
@@ -404,38 +411,38 @@ static bool bass_check_cp_command_len(struct iovec *iov)
 	/* Check command parameters */
 	switch (hdr->op) {
 	case BT_BASS_ADD_SRC:
-		params.add_src_params = util_iov_pull_mem(iov,
+		params.add_src_params = util_iov_pull_mem(&iov,
 						sizeof(*params.add_src_params));
 		if (!params.add_src_params)
 			return false;
 
 		if (!bass_check_cp_command_subgroup_data_len(
 					params.add_src_params->num_subgroups,
-					iov))
+					&iov))
 			return false;
 
 		break;
 	case BT_BASS_MOD_SRC:
-		params.mod_src_params = util_iov_pull_mem(iov,
+		params.mod_src_params = util_iov_pull_mem(&iov,
 						sizeof(*params.mod_src_params));
 		if (!params.mod_src_params)
 			return false;
 
 		if (!bass_check_cp_command_subgroup_data_len(
 					params.mod_src_params->num_subgroups,
-					iov))
+					&iov))
 			return false;
 
 		break;
 	case BT_BASS_SET_BCAST_CODE:
-		params.set_bcast_code_params = util_iov_pull_mem(iov,
+		params.set_bcast_code_params = util_iov_pull_mem(&iov,
 					sizeof(*params.set_bcast_code_params));
 		if (!params.set_bcast_code_params)
 			return false;
 
 		break;
 	case BT_BASS_REMOVE_SRC:
-		params.remove_src_params = util_iov_pull_mem(iov,
+		params.remove_src_params = util_iov_pull_mem(&iov,
 					sizeof(*params.remove_src_params));
 		if (!params.remove_src_params)
 			return false;
@@ -448,25 +455,134 @@ static bool bass_check_cp_command_len(struct iovec *iov)
 		return true;
 	}
 
-	if (iov->iov_len > 0)
+	if (iov.iov_len > 0)
 		return false;
 
 	return true;
 }
 
+static void bass_handle_remote_scan_stopped_op(struct bt_bass_db *bdb,
+					struct gatt_db_attribute *attrib,
+					uint8_t opcode,
+					unsigned int id,
+					struct iovec *iov,
+					struct bt_att *att)
+{
+	if (opcode == BT_ATT_OP_WRITE_REQ)
+		gatt_db_attribute_write_result(attrib, id, 0x00);
+}
+
+static void bass_handle_remote_scan_started_op(struct bt_bass_db *bdb,
+					struct gatt_db_attribute *attrib,
+					uint8_t opcode,
+					unsigned int id,
+					struct iovec *iov,
+					struct bt_att *att)
+{
+	if (opcode == BT_ATT_OP_WRITE_REQ)
+		gatt_db_attribute_write_result(attrib, id, 0x00);
+}
+
+static bool bass_src_id_match(const void *data, const void *match_data)
+{
+	const struct bt_bcast_src *bcast_src = data;
+	const uint8_t *id = match_data;
+
+	return (bcast_src->id == *id);
+}
+
+static void bass_handle_remove_src_op(struct bt_bass_db *bdb,
+					struct gatt_db_attribute *attrib,
+					uint8_t opcode,
+					unsigned int id,
+					struct iovec *iov,
+					struct bt_att *att)
+{
+	struct bt_bass_remove_src_params *params;
+	struct bt_bcast_src *bcast_src;
+
+	/* Get Remove Source command parameters */
+	params = util_iov_pull_mem(iov, sizeof(*params));
+
+	bcast_src = queue_find(bdb->bcast_srcs,
+						bass_src_id_match,
+						&params->id);
+
+	if (!bcast_src) {
+		/* No source matches the written source id */
+		if (opcode == BT_ATT_OP_WRITE_REQ)
+			gatt_db_attribute_write_result(attrib, id,
+					BT_BASS_ERROR_INVALID_SOURCE_ID);
+
+		return;
+	}
+
+	/* Ignore if server is synchronized to the PA
+	 * of the source
+	 */
+	if (bcast_src->sync_state == BT_BASS_SYNCHRONIZED_TO_PA)
+		return;
+
+	/* Ignore if server is synchronized to any BIS
+	 * of the source
+	 */
+	for (int i = 0; i < bcast_src->num_subgroups; i++)
+		if (bcast_src->subgroup_data[i].bis_sync)
+			return;
+
+	/* Accept the operation and remove source */
+	queue_remove(bdb->bcast_srcs, bcast_src);
+	gatt_db_attribute_notify(bcast_src->attr, NULL, 0, att);
+	bass_bcast_src_free(bcast_src);
+
+	if (opcode == BT_ATT_OP_WRITE_REQ)
+		gatt_db_attribute_write_result(attrib, id, 0x00);
+}
+
+#define BASS_OP(_str, _op, _size, _func) \
+	{ \
+		.str = _str, \
+		.op = _op, \
+		.size = _size, \
+		.func = _func, \
+	}
+
+struct bass_op_handler {
+	const char	*str;
+	uint8_t		op;
+	size_t		size;
+	void		(*func)(struct bt_bass_db *bdb,
+				struct gatt_db_attribute *attrib,
+				uint8_t opcode,
+				unsigned int id,
+				struct iovec *iov,
+				struct bt_att *att);
+} bass_handlers[] = {
+	BASS_OP("Remote Scan Stopped", BT_BASS_REMOTE_SCAN_STOPPED,
+		0, bass_handle_remote_scan_stopped_op),
+	BASS_OP("Remote Scan Started", BT_BASS_REMOTE_SCAN_STARTED,
+		0, bass_handle_remote_scan_started_op),
+	BASS_OP("Remove Source", BT_BASS_REMOVE_SRC,
+		0, bass_handle_remove_src_op),
+	{}
+};
+
 static void bass_bcast_audio_scan_cp_write(struct gatt_db_attribute *attrib,
 				unsigned int id, uint16_t offset,
 				const uint8_t *value, size_t len,
 				uint8_t opcode, struct bt_att *att,
 				void *user_data)
 {
+	struct bt_bass_db *bdb = user_data;
+	struct bt_bass_bcast_audio_scan_cp_hdr *hdr;
+	struct bass_op_handler *handler;
 	struct iovec iov = {
 		.iov_base = (void *)value,
 		.iov_len = len,
 	};
 
 	/* Validate written command length */
-	if (!bass_check_cp_command_len(&iov)) {
+	if (!bass_check_cp_command_len(value, len)) {
 		if (opcode == BT_ATT_OP_WRITE_REQ) {
 			gatt_db_attribute_write_result(attrib, id,
 					BT_ERROR_WRITE_REQUEST_REJECTED);
@@ -474,9 +590,22 @@ static void bass_bcast_audio_scan_cp_write(struct gatt_db_attribute *attrib,
 		return;
 	}
 
-	/* TODO: Implement handlers for the written opcodes */
-	gatt_db_attribute_write_result(attrib, id,
-			BT_BASS_ERROR_OPCODE_NOT_SUPPORTED);
+	/* Get command header */
+	hdr = util_iov_pull_mem(&iov, sizeof(*hdr));
+
+	/* Call the appropriate opcode handler */
+	for (handler = bass_handlers; handler && handler->str; handler++) {
+		if (handler->op == hdr->op) {
+			handler->func(bdb, attrib, opcode, id, &iov, att);
+			return;
+		}
+	}
+
+	/* Send error response if unsupported opcode was written */
+	if (opcode == BT_ATT_OP_WRITE_REQ) {
+		gatt_db_attribute_write_result(attrib, id,
+				BT_BASS_ERROR_OPCODE_NOT_SUPPORTED);
+	}
 }
 
 static bool bass_src_match_attrib(const void *data, const void *match_data)
-- 
2.34.1