Bluetooth: CSIP: Set member: Fix issue with re-registration

subsys/bluetooth/audio/csip_set_member.c

@@ -2,7 +2,7 @@
 
 /*
  * Copyright (c) 2019 Bose Corporation
- * Copyright (c) 2020-2022 Nordic Semiconductor ASA
+ * Copyright (c) 2020-2025 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -909,15 +909,9 @@ int bt_csip_set_member_register(const struct bt_csip_set_member_register_param *
 				struct bt_csip_set_member_svc_inst **svc_inst)
 {
 	static bool first_register;
-	static uint8_t instance_cnt;
 	struct bt_csip_set_member_svc_inst *inst;
 	int err;
 
-	if (instance_cnt == ARRAY_SIZE(svc_insts)) {
-		LOG_DBG("Too many set member registrations");
-		return -ENOMEM;
-	}
-
 	CHECKIF(param == NULL) {
 		LOG_DBG("NULL param");
 		return -EINVAL;
@@ -937,15 +931,26 @@ int bt_csip_set_member_register(const struct bt_csip_set_member_register_param *
 		first_register = true;
 	}
 
-	inst = &svc_insts[instance_cnt];
+	inst = NULL;
+	ARRAY_FOR_EACH(svc_insts, i) {
+		if (svc_insts[i].service_p == NULL) {
+			inst = &svc_insts[i];
 
-	err = k_mutex_lock(&inst->mutex, K_NO_WAIT);
-	if (err != 0) {
-		LOG_DBG("Failed to lock mutex: %d", err);
-		return -EBUSY;
+			err = k_mutex_lock(&inst->mutex, K_NO_WAIT);
+			if (err != 0) {
+				/* Try the next */
+				continue;
+			}
+
+			inst->service_p = &csip_set_member_service_list[i];
+			break;
+		}
 	}
 
-	inst->service_p = &csip_set_member_service_list[instance_cnt];
+	if (inst == NULL) {
+		LOG_DBG("Too many set member registrations");
+		return -ENOMEM;
+	}
 
 	/* The removal of the optional characteristics should be done in reverse order of the order
 	 * in BT_CSIP_SERVICE_DEFINITION, as that improves the performance of remove_csis_char,
@@ -973,7 +978,6 @@ int bt_csip_set_member_register(const struct bt_csip_set_member_register_param *
 		return err;
 	}
 
-	instance_cnt++;
 	k_work_init_delayable(&inst->set_lock_timer,
 			      set_lock_timer_handler);
 	inst->rank = param->rank;
@@ -1030,6 +1034,16 @@ int bt_csip_set_member_unregister(struct bt_csip_set_member_svc_inst *svc_inst)
 		return err;
 	}
 
+	/* Restore original declaration */
+
+	/* attrs_0 is an array of the original attributes, and while the actual number of attributes
+	 * may change, the size of the array stays the same, so we can use that to restore the
+	 * original attribute count
+	 */
+	(void)memcpy(svc_inst->service_p->attrs,
+		     (struct bt_gatt_attr[])BT_CSIP_SERVICE_DEFINITION(svc_inst), sizeof(attrs_0));
+	svc_inst->service_p->attr_count = ARRAY_SIZE(attrs_0);
+
 	(void)k_work_cancel_delayable(&svc_inst->set_lock_timer);
 
 	memset(svc_inst, 0, offsetof(struct bt_csip_set_member_svc_inst, mutex));

tests/bsim/bluetooth/audio/src/csip_set_member_test.c

@@ -1,6 +1,6 @@
 /*
  * Copyright (c) 2019 Bose Corporation
- * Copyright (c) 2020-2024 Nordic Semiconductor ASA
+ * Copyright (c) 2020-2025 Nordic Semiconductor ASA
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -16,6 +16,7 @@
 #include <zephyr/sys/printk.h>
 #include <zephyr/sys/util.h>
 
+#include "bs_tracing.h"
 #include "bstests.h"
 #include "common.h"
 #ifdef CONFIG_BT_CSIP_SET_MEMBER
@@ -288,6 +289,46 @@ static void test_new_set_size_and_rank(void)
 	PASS("CSIP Set member passed: Client successfully disconnected\n");
 }
 
+static void test_register(void)
+{
+	for (size_t iteration = 1; iteration <= 5; iteration++) {
+		struct bt_csip_set_member_svc_inst
+			*svc_insts[CONFIG_BT_CSIP_SET_MEMBER_MAX_INSTANCE_COUNT];
+		int err;
+
+		printk("Running iteration %zu\n", iteration);
+
+		ARRAY_FOR_EACH(svc_insts, i) {
+			err = bt_csip_set_member_register(&param, &svc_insts[i]);
+			if (err != 0) {
+				FAIL("[%zu]: Could not register CSIS (err %d)\n", i, err);
+				return;
+			}
+		}
+
+		err = bt_csip_set_member_register(&param, &svc_inst);
+		if (err == 0) {
+			FAIL("Registered more CSIS than expected\n");
+			return;
+		}
+
+		ARRAY_FOR_EACH(svc_insts, i) {
+			err = bt_csip_set_member_unregister(svc_insts[i]);
+			if (err != 0) {
+				FAIL("[%zu]: Could not unregister CSIS (err %d)\n", i, err);
+				return;
+			}
+			svc_insts[i] = NULL;
+		}
+	}
+
+	PASS("CSIP Set member register passed\n");
+	/* We can terminate the test immediately here as there is no peer devices we keep
+	 * communicating with.
+	 */
+	bs_trace_silent_exit(0);
+}
+
 static void test_args(int argc, char *argv[])
 {
 	for (size_t argn = 0; argn < argc; argn++) {
@@ -352,6 +393,13 @@ static const struct bst_test_instance test_connect[] = {
 		.test_main_f = test_new_set_size_and_rank,
 		.test_args_f = test_args,
 	},
+	{
+		.test_id = "csip_set_member_register",
+		.test_pre_init_f = test_init,
+		.test_tick_f = test_tick,
+		.test_main_f = test_register,
+		.test_args_f = test_args,
+	},
 	BSTEST_END_MARKER,
 };
 

tests/bsim/bluetooth/audio/test_scripts/csip_register.sh

@@ -0,0 +1,24 @@
+#!/usr/bin/env bash
+#
+# Copyright (c) 2025 Nordic Semiconductor ASA
+#
+# SPDX-License-Identifier: Apache-2.0
+
+source ${ZEPHYR_BASE}/tests/bsim/sh_common.source
+
+VERBOSITY_LEVEL=2
+EXECUTE_TIMEOUT=30
+
+cd ${BSIM_OUT_PATH}/bin
+
+SIMULATION_ID="csip_register"
+
+Execute ./bs_${BOARD_TS}_tests_bsim_bluetooth_audio_prj_conf \
+  -v=${VERBOSITY_LEVEL} -s=${SIMULATION_ID} -d=0 -testid=csip_set_member_register \
+  -RealEncryption=1 -rs=20 -D=1 -argstest rank 1 size 2
+
+# Simulation time should be larger than the WAIT_TIME in common.h
+Execute ./bs_2G4_phy_v1 -v=${VERBOSITY_LEVEL} -s=${SIMULATION_ID} \
+  -D=1 -sim_length=1e6 $@
+
+wait_for_background_jobs