[PATCH v2 BlueZ 1/2] shared/gatt-db: Remove services with match function

@@ -408,6 +408,47 @@ bool gatt_db_remove_service(struct gatt_db *db,
 	return true;
 }
 
+struct remove_services_data {
+	gatt_db_attribute_match_cb_t func;
+	void *user_data;
+};
+
+static bool remove_services_match(const void *data, const void *match_data)
+{
+	const struct remove_services_data *remove_svc_data = match_data;
+	const struct gatt_db_service *service = data;
+	struct gatt_db_attribute *attrib = service->attributes[0];
+
+	return remove_svc_data->func(attrib, remove_svc_data->user_data);
+}
+
+bool gatt_db_remove_services(struct gatt_db *db,
+					gatt_db_attribute_match_cb_t func,
+					void *user_data)
+{
+	if (!db)
+		return false;
+
+	if (func) {
+		struct remove_services_data remove_services_data = {
+			.func = func,
+			.user_data = user_data,
+		};
+
+		queue_remove_all(db->services, remove_services_match,
+						&remove_services_data,
+						gatt_db_service_destroy);
+	} else {
+		queue_remove_all(db->services, NULL, NULL,
+						gatt_db_service_destroy);
+	}
+
+	if (gatt_db_isempty(db))
+		db->next_handle = 0;
+
+	return true;
+}
+
 bool gatt_db_clear(struct gatt_db *db)
 {
 	return gatt_db_clear_range(db, 1, UINT16_MAX);

@@ -38,6 +38,13 @@ struct gatt_db_attribute *gatt_db_add_service(struct gatt_db *db,
 
 bool gatt_db_remove_service(struct gatt_db *db,
 					struct gatt_db_attribute *attrib);
+
+typedef bool (*gatt_db_attribute_match_cb_t)(struct gatt_db_attribute *attrib,
+							void *user_data);
+
+bool gatt_db_remove_services(struct gatt_db *db,
+					gatt_db_attribute_match_cb_t func,
+					void *user_data);
 bool gatt_db_clear(struct gatt_db *db);
 bool gatt_db_clear_range(struct gatt_db *db, uint16_t start_handle,
 							uint16_t end_handle);

@@ -316,6 +316,7 @@ struct discovery_op {
 	struct queue *pending_svcs;
 	struct queue *pending_chrcs;
 	struct queue *svcs;
+	struct queue *found_svcs;
 	struct queue *ext_prop_desc;
 	struct gatt_db_attribute *cur_svc;
 	bool success;

@@ -332,13 +333,31 @@ static void discovery_op_free(struct discovery_op *op)
 	queue_destroy(op->pending_svcs, NULL);
 	queue_destroy(op->pending_chrcs, free);
 	queue_destroy(op->svcs, NULL);
+	queue_destroy(op->found_svcs, NULL);
 	queue_destroy(op->ext_prop_desc, NULL);
 	free(op);
 }
 
+static bool validate_svc_cb(struct gatt_db_attribute *attrib, void *user_data)
+{
+	struct discovery_op *op = user_data;
+	uint16_t start_h;
+
+	gatt_db_attribute_get_service_data(attrib, &start_h, NULL, NULL, NULL);
+
+	if (op->start > start_h || op->end < start_h)
+		return false;
+
+	return (queue_find(op->found_svcs, NULL, attrib) == NULL);
+}
+
 static void discovery_op_complete(struct discovery_op *op, bool success,
 								uint8_t err)
 {
+	struct bt_gatt_client *client = op->client;
+
+	gatt_db_remove_services(client->db, validate_svc_cb, op);
+
 	/* Reset remaining range */
 	if (op->last != UINT16_MAX)
 		gatt_db_clear_range(op->client->db, op->last + 1, UINT16_MAX);

@@ -358,6 +377,7 @@ static struct discovery_op *discovery_op_create(struct bt_gatt_client *client,
 	op->pending_svcs = queue_new();
 	op->pending_chrcs = queue_new();
 	op->svcs = queue_new();
+	op->found_svcs = queue_new();
 	op->ext_prop_desc = queue_new();
 	op->client = client;
 	op->complete_func = complete_func;

@@ -988,6 +1008,8 @@ static void discover_secondary_cb(bool success, uint8_t att_ecode,
 		if (!gatt_db_service_get_active(attr))
 			queue_push_tail(op->pending_svcs, attr);
 
+		queue_push_tail(op->found_svcs, attr);
+
 		/* Update last handle */
 		if (end > op->last)
 			op->last = end;

@@ -1101,6 +1123,8 @@ static void discover_primary_cb(bool success, uint8_t att_ecode,
 		if (!gatt_db_service_get_active(attr))
 			queue_push_tail(op->pending_svcs, attr);
 
+		queue_push_tail(op->found_svcs, attr);
+
 		/* Update last handle */
 		if (end > op->last)
 			op->last = end;