1 files changed, 299 insertions, 10 deletions

diff --git a/android/gatt.c b/android/gatt.c
index 28635edf4..6b36985d3 100644
--- a/android/gatt.c
+++ b/android/gatt.c
@@ -46,6 +46,7 @@
 #include "src/shared/queue.h"
 #include "src/shared/att.h"
 #include "src/shared/gatt-db.h"
+#include "src/shared/ad.h"
 #include "attrib/gattrib.h"
 #include "attrib/att.h"
 #include "attrib/gatt.h"
@@ -110,6 +111,8 @@ struct gatt_app {
 	struct queue *notifications;
 
 	gatt_conn_cb_t func;
+
+	struct adv_instance *adv;
 };
 
 struct element_id {
@@ -192,6 +195,7 @@ static struct ipc *hal_ipc = NULL;
 static bdaddr_t adapter_addr;
 static bool scanning = false;
 static unsigned int advertising_cnt = 0;
+static uint32_t adv_inst_bits = 0;
 
 static struct queue *gatt_apps = NULL;
 static struct queue *gatt_devices = NULL;
@@ -650,6 +654,19 @@ static void connection_cleanup(struct gatt_device *device)
 		bt_auto_connect_remove(&device->bdaddr);
 }
 
+static void free_adv_instance(struct adv_instance *adv)
+{
+	if (!adv)
+		return;
+
+	if (adv->instance)
+		adv_inst_bits &= ~(1 << (adv->instance - 1));
+
+	bt_ad_unref(adv->ad);
+	bt_ad_unref(adv->sr);
+	free(adv);
+}
+
 static void destroy_gatt_app(void *data)
 {
 	struct gatt_app *app = data;
@@ -674,6 +691,8 @@ static void destroy_gatt_app(void *data)
 
 	queue_destroy(app->notifications, free);
 
+	free_adv_instance(app->adv);
+
 	free(app);
 }
 
@@ -5586,19 +5605,162 @@ static void handle_client_set_scan_param(const void *buf, uint16_t len)
 					HAL_STATUS_UNSUPPORTED);
 }
 
+static struct adv_instance *find_adv_instance(uint32_t client_if)
+{
+	struct gatt_app *app;
+	struct adv_instance *adv;
+	uint8_t inst = 0;
+	unsigned int i;
+
+	app = find_app_by_id(client_if);
+	if (!app)
+		return NULL;
+
+	if (app->adv)
+		return app->adv;
+
+	/* Assume that kernel supports <= 32 advertising instances (5 today)
+	 * We have already indicated the number to the android framework layers
+	 * via the LE features so we don't check again here.
+	 * The kernel will detect the error if needed
+	 */
+	for (i = 0; i < sizeof(adv_inst_bits) * 8; i++) {
+		uint32_t mask = 1 << i;
+
+		if (!(adv_inst_bits & mask)) {
+			inst = i + 1;
+			adv_inst_bits |= mask;
+			break;
+		}
+	}
+	if (!inst)
+		return NULL;
+
+	adv = new0(typeof(*adv), 1);
+	adv->instance = inst;
+	app->adv = adv;
+
+	DBG("Assigned advertising instance %d for client %d", inst, client_if);
+
+	return adv;
+};
+
+/* Build advertising data object from a data buffer containing
+ * manufacturer_data, service_data, service uuids (in that order)
+ * The input data is raw with no TLV structure and the service uuids are 128 bit
+ */
+static struct bt_ad *build_adv_data(int32_t manufacturer_data_len,
+					int32_t service_data_len,
+					int32_t service_uuid_len,
+					const uint8_t *data_in)
+{
+	const int one_svc_uuid_len = 128 / 8;  /* Android uses 128bit UUIDs */
+	uint8_t *src = (uint8_t *)data_in;
+	struct bt_ad *ad;
+	unsigned num_svc_uuids, i;
+
+	ad = bt_ad_new();
+
+	if (manufacturer_data_len >= 2) { /* Includes manufacturer id */
+		uint16_t manufacturer_id;
+
+		manufacturer_id = bt_get_le16(src);
+		src += 2;
+
+		if (!bt_ad_add_manufacturer_data(ad,
+						 manufacturer_id,
+						 src,
+						 manufacturer_data_len - 2))
+			goto err;
+
+		src +=  manufacturer_data_len - 2;
+	}
+
+	if (service_data_len >= 2) { /* Includes service uuid (always 16 bit) */
+		bt_uuid_t bt_uuid;
+		uint16_t uuid16;
+
+		uuid16 = bt_get_le16(src);
+		src += 2;
+		bt_uuid16_create(&bt_uuid, uuid16);
+
+		if (!bt_ad_add_service_data(ad,
+					    &bt_uuid,
+					    src,
+					    service_data_len - 2))
+			goto err;
+
+		src += service_data_len - 2;
+	}
+
+	if (service_uuid_len % one_svc_uuid_len) {
+		error("Service UUIDs not multiple of %d bytes (%d)",
+			one_svc_uuid_len, service_uuid_len);
+		num_svc_uuids = 0;
+	} else {
+		num_svc_uuids = service_uuid_len / one_svc_uuid_len;
+	}
+
+	for (i = 0; i  < num_svc_uuids; i++) {
+		bt_uuid_t bt_uuid;
+
+		android2uuid(src, &bt_uuid);
+		src += one_svc_uuid_len;
+
+		if (!bt_ad_add_service_uuid(ad, &bt_uuid))
+			goto err;
+	}
+
+	return ad;
+
+err:
+	bt_ad_unref(ad);
+	return NULL;
+}
+
+
 static void handle_client_setup_multi_adv(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_gatt_client_setup_multi_adv *cmd = buf;
+	struct hal_ev_gatt_client_multi_adv_enable ev;
+	struct adv_instance *adv;
+	uint8_t status;
 
-	DBG("client_if %d", cmd->client_if);
+	DBG("client_if %d min_interval=%d max_interval=%d type=%d channel_map=0x%x tx_power=%d timeout=%d",
+		cmd->client_if,
+		cmd->min_interval,
+		cmd->max_interval,
+		cmd->type,
+		cmd->channel_map,
+		cmd->tx_power,
+		cmd->timeout);
+
+	adv = find_adv_instance(cmd->client_if);
+	if (!adv) {
+		status = HAL_STATUS_FAILED;
+		goto out;
+	}
 
-	/* TODO */
+	status = HAL_STATUS_SUCCESS;
+	adv->timeout = cmd->timeout;
+	adv->type = cmd->type;
+	if (adv->type != ANDROID_ADVERTISING_EVENT_TYPE_SCANNABLE) {
+		bt_ad_unref(adv->sr);
+		adv->sr = NULL;
+	}
 
+out:
 	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
 					HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV,
-					HAL_STATUS_UNSUPPORTED);
+					status);
+
+	ev.client_if = cmd->client_if;
+	ev.status = status;
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+			HAL_EV_GATT_CLIENT_MULTI_ADV_ENABLE, sizeof(ev), &ev);
 }
 
+/* This is not currently called by Android 5.1 */
 static void handle_client_update_multi_adv(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_gatt_client_update_multi_adv *cmd = buf;
@@ -5612,30 +5774,157 @@ static void handle_client_update_multi_adv(const void *buf, uint16_t len)
 					HAL_STATUS_UNSUPPORTED);
 }
 
+struct addrm_adv_cb_data {
+	int32_t client_if;
+	struct adv_instance *adv;
+};
+
+static void add_advertising_cb(uint8_t status, void *user_data)
+{
+	struct addrm_adv_cb_data *cb_data = user_data;
+	struct hal_ev_gatt_client_multi_adv_data ev = {
+		.status = status,
+		.client_if = cb_data->client_if,
+	};
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+				HAL_EV_GATT_CLIENT_MULTI_ADV_DATA,
+				sizeof(ev), &ev);
+
+	free(cb_data);
+}
+
 static void handle_client_setup_multi_adv_inst(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_gatt_client_setup_multi_adv_inst *cmd = buf;
+	struct adv_instance *adv;
+	struct bt_ad *adv_data;
+	struct addrm_adv_cb_data *cb_data = NULL;
+	uint8_t status = HAL_STATUS_FAILED;
+
+	DBG("client_if %d set_scan_rsp=%d include_name=%d include_tx_power=%d appearance=%d manuf_data_len=%d svc_data_len=%d svc_uuid_len=%d",
+		cmd->client_if,
+		cmd->set_scan_rsp,
+		cmd->include_name,
+		cmd->include_tx_power,
+		cmd->appearance,
+		cmd->manufacturer_data_len,
+		cmd->service_data_len,
+		cmd->service_uuid_len
+	);
+
+	adv = find_adv_instance(cmd->client_if);
+	if (!adv)
+		goto out;
+
+	adv->include_tx_power = cmd->include_tx_power ? 1 : 0;
+
+	adv_data = build_adv_data(cmd->manufacturer_data_len,
+				  cmd->service_data_len,
+				  cmd->service_uuid_len,
+				  cmd->data_service_uuid);
+	if (!adv_data)
+		goto out;
+
+	if (cmd->set_scan_rsp) {
+		bt_ad_unref(adv->sr);
+		adv->sr = adv_data;
+	} else {
+		bt_ad_unref(adv->ad);
+		adv->ad = adv_data;
+	}
 
-	DBG("client_if %d", cmd->client_if);
+	cb_data = new0(typeof(*cb_data), 1);
+	cb_data->client_if = cmd->client_if;
+	cb_data->adv = adv;
 
-	/* TODO */
+	if (!bt_le_add_advertising(adv, add_advertising_cb, cb_data)) {
+		error("gatt: Could not add advertising");
+		free(cb_data);
+		goto out;
+	}
 
+	status = HAL_STATUS_SUCCESS;
+
+out:
 	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
-					HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV_INST,
-					HAL_STATUS_UNSUPPORTED);
+				HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV_INST,
+				status);
+
+	if (status != HAL_STATUS_SUCCESS) {
+		struct hal_ev_gatt_client_multi_adv_data ev = {
+			.status = status,
+			.client_if = cmd->client_if,
+		};
+
+		ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+				HAL_EV_GATT_CLIENT_MULTI_ADV_DATA,
+				sizeof(ev), &ev);
+	}
+}
+
+static void remove_advertising_cb(uint8_t status, void *user_data)
+{
+	struct addrm_adv_cb_data *cb_data = user_data;
+	struct hal_ev_gatt_client_multi_adv_data ev = {
+		.status = status,
+		.client_if = cb_data->client_if,
+	};
+
+	free_adv_instance(cb_data->adv);
+
+	ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+				HAL_EV_GATT_CLIENT_MULTI_ADV_DISABLE,
+				sizeof(ev), &ev);
+
+	free(cb_data);
 }
 
 static void handle_client_disable_multi_adv_inst(const void *buf, uint16_t len)
 {
 	const struct hal_cmd_gatt_client_disable_multi_adv_inst *cmd = buf;
+	struct adv_instance *adv;
+	struct gatt_app *app;
+	struct addrm_adv_cb_data *cb_data = NULL;
+	uint8_t status = HAL_STATUS_FAILED;
 
 	DBG("client_if %d", cmd->client_if);
 
-	/* TODO */
+	adv = find_adv_instance(cmd->client_if);
+	if (!adv)
+		goto out;
+
+	cb_data = new0(typeof(*cb_data), 1);
+	cb_data->client_if = cmd->client_if;
+	cb_data->adv = adv;
+
+	if (!bt_le_remove_advertising(adv, remove_advertising_cb, cb_data)) {
+		error("gatt: Could not remove advertising");
+		free(cb_data);
+		goto out;
+	}
+
+	app = find_app_by_id(cmd->client_if);
+	if (app)
+		app->adv = NULL;
 
+	status = HAL_STATUS_SUCCESS;
+
+out:
 	ipc_send_rsp(hal_ipc, HAL_SERVICE_ID_GATT,
 				HAL_OP_GATT_CLIENT_DISABLE_MULTI_ADV_INST,
-				HAL_STATUS_UNSUPPORTED);
+				status);
+
+	if (status != HAL_STATUS_SUCCESS) {
+		struct hal_ev_gatt_client_multi_adv_data ev = {
+			.status = status,
+			.client_if = cmd->client_if,
+		};
+
+		ipc_send_notif(hal_ipc, HAL_SERVICE_ID_GATT,
+				HAL_EV_GATT_CLIENT_MULTI_ADV_DISABLE,
+				sizeof(ev), &ev);
+	}
 }
 
 static void handle_client_configure_batchscan(const void *buf, uint16_t len)
@@ -5824,7 +6113,7 @@ static const struct ipc_handler cmd_handlers[] = {
 	{ handle_client_update_multi_adv, false,
 		sizeof(struct hal_cmd_gatt_client_update_multi_adv) },
 	/* HAL_OP_GATT_CLIENT_SETUP_MULTI_ADV_INST */
-	{ handle_client_setup_multi_adv_inst, false,
+	{ handle_client_setup_multi_adv_inst, true,
 		sizeof(struct hal_cmd_gatt_client_setup_multi_adv_inst) },
 	/* HAL_OP_GATT_CLIENT_DISABLE_MULTI_ADV_INST */
 	{ handle_client_disable_multi_adv_inst, false,