[BlueZ,1/2] csip: Fix not registering CSIS service

diff --git a/profiles/audio/csip.c b/profiles/audio/csip.c
index ec042afd54eb..05bf588d8d6f 100644
--- a/profiles/audio/csip.c
+++ b/profiles/audio/csip.c
@@ -54,7 +54,13 @@ 
 
 #define CSIS_UUID_STR "00001846-0000-1000-8000-00805f9b34fb"
 
+struct csis_data {
+	struct btd_adapter *adapter;
+	struct bt_csip *csip;
+};
+
 struct csip_data {
+	struct btd_adapter *adapter;
 	struct btd_device *device;
 	struct btd_service *service;
 	struct bt_csip *csip;
@@ -62,6 +68,7 @@  struct csip_data {
 };
 
 static struct queue *sessions;
+static struct queue *servers;
 
 static void csip_debug(const char *str, void *user_data)
 {
@@ -78,12 +85,6 @@  static struct csip_data *csip_data_new(struct btd_device *device)
 	return data;
 }
 
-static bool csip_ltk_read(struct bt_csip *csip, uint8_t k[16], void *user_data)
-{
-	/* TODO: Retrieve LTK using device object */
-	return false;
-}
-
 static void csip_data_add(struct csip_data *data)
 {
 	DBG("data %p", data);
@@ -95,10 +96,6 @@  static void csip_data_add(struct csip_data *data)
 
 	bt_csip_set_debug(data->csip, csip_debug, NULL, NULL);
 
-	bt_csip_set_sirk(data->csip, btd_opts.csis.encrypt, btd_opts.csis.sirk,
-				btd_opts.csis.size, btd_opts.csis.rank,
-				csip_ltk_read, data);
-
 	if (!sessions)
 		sessions = queue_new();
 
@@ -201,25 +198,6 @@  static void csip_attached(struct bt_csip *csip, void *user_data)
 	data->csip = csip;
 
 	csip_data_add(data);
-
-}
-
-static int csip_server_probe(struct btd_profile *p,
-				struct btd_adapter *adapter)
-{
-	struct btd_gatt_database *database = btd_adapter_get_database(adapter);
-
-	DBG("CSIP path %s", adapter_get_path(adapter));
-
-	bt_csip_add_db(btd_gatt_database_get_db(database));
-
-	return 0;
-}
-
-static void csip_server_remove(struct btd_profile *p,
-					struct btd_adapter *adapter)
-{
-	DBG("CSIP remove Adapter");
 }
 
 static int csip_accept(struct btd_service *service)
@@ -332,9 +310,114 @@  static struct btd_profile csip_profile = {
 	.accept		= csip_accept,
 	.disconnect	= csip_disconnect,
 
-	.adapter_probe	= csip_server_probe,
-	.adapter_remove	= csip_server_remove,
+	.experimental	= true,
+};
 
+static bool csis_ltk_read(struct bt_csip *csip, uint8_t k[16], void *user_data)
+{
+	/* TODO: Retrieve LTK using device object */
+	return false;
+}
+
+static void csis_data_add(struct csis_data *data)
+{
+	DBG("data %p", data);
+
+	if (queue_find(servers, NULL, data)) {
+		error("data %p already added", data);
+		return;
+	}
+
+	bt_csip_set_debug(data->csip, csip_debug, NULL, NULL);
+
+	bt_csip_set_sirk(data->csip, btd_opts.csis.encrypt, btd_opts.csis.sirk,
+				btd_opts.csis.size, btd_opts.csis.rank,
+				csis_ltk_read, data);
+
+	if (!servers)
+		servers = queue_new();
+
+	queue_push_tail(servers, data);
+}
+
+static struct csis_data *csis_data_new(struct btd_adapter *adapter)
+{
+	struct csis_data *data;
+
+	data = new0(struct csis_data, 1);
+	data->adapter = adapter;
+
+	return data;
+}
+
+static int csis_server_probe(struct btd_profile *p, struct btd_adapter *adapter)
+{
+	struct btd_gatt_database *database = btd_adapter_get_database(adapter);
+	struct csis_data *data;
+
+	DBG("path %s", adapter_get_path(adapter));
+
+	data = csis_data_new(adapter);
+
+	data->csip = bt_csip_new(btd_gatt_database_get_db(database), NULL);
+	if (!data->csip) {
+		error("Unable to create CSIP instance");
+		free(data);
+		return -EINVAL;
+	}
+
+	csis_data_add(data);
+
+	return 0;
+}
+
+static bool match_csis(const void *data, const void *match_data)
+{
+	const struct csis_data *csis = data;
+	const struct btd_adapter *adapter = match_data;
+
+	return csis->adapter == adapter;
+}
+
+static void csis_data_free(struct csis_data *data)
+{
+	bt_csip_unref(data->csip);
+	free(data);
+}
+
+static void csis_data_remove(struct csis_data *data)
+{
+	DBG("data %p", data);
+
+	csis_data_free(data);
+
+	if (queue_isempty(servers)) {
+		queue_destroy(servers, NULL);
+		servers = NULL;
+	}
+}
+
+static void csis_server_remove(struct btd_profile *p,
+					struct btd_adapter *adapter)
+{
+	struct csis_data *data;
+
+	DBG("path %s", adapter_get_path(adapter));
+
+	data = queue_remove_if(servers, match_csis, adapter);
+	if (!data)
+		return;
+
+	csis_data_remove(data);
+}
+
+static struct btd_profile csis_profile = {
+	.name		= "csis",
+	.priority	= BTD_PROFILE_PRIORITY_MEDIUM,
+	.local_uuid	= CSIS_UUID_STR,
+
+	.adapter_probe	= csis_server_probe,
+	.adapter_remove	= csis_server_remove,
 	.experimental	= true,
 };
 
@@ -344,6 +427,10 @@  static int csip_init(void)
 {
 	int err;
 
+	err = btd_profile_register(&csis_profile);
+	if (err)
+		return err;
+
 	err = btd_profile_register(&csip_profile);
 	if (err)
 		return err;
@@ -355,6 +442,7 @@  static int csip_init(void)
 
 static void csip_exit(void)
 {
+	btd_profile_unregister(&csis_profile);
 	btd_profile_unregister(&csip_profile);
 	bt_csip_unregister(csip_id);
 }