From: Amit Purwar <amit.purwar@samsung.com>
Date: Mon, 24 Aug 2020 04:40:45 +0000 (+0530)
Subject: FRWK: Allows multiple service data in LE advertising
X-Git-Tag: accepted/tizen/unified/20200826.133059~2^2
X-Git-Url: http://review.tizen.org/git/?p=platform%2Fcore%2Fconnectivity%2Fbluetooth-frwk.git;a=commitdiff_plain;h=f13956d59f5d399115bbfaa031f560c496cd8ba6

FRWK: Allows multiple service data in LE advertising

Change-Id: I138df1b79a53ca63edcf0fb987ea126c454769de
Signed-off-by: Amit Purwar <amit.purwar@samsung.com>
---

diff --git a/bt-oal/bluez_hal/src/bt-hal-adapter-le.c b/bt-oal/bluez_hal/src/bt-hal-adapter-le.c
index e0128fa..041ace8 100644
--- a/bt-oal/bluez_hal/src/bt-hal-adapter-le.c
+++ b/bt-oal/bluez_hal/src/bt-hal-adapter-le.c
@@ -726,6 +726,35 @@ int _bt_hal_set_advertising_params(int server_if, int min_interval,
 	return BT_STATUS_SUCCESS;
 }
 
+static int __bt_hal_parse_service_data(int len, char *src, uint8_t *dest)
+{
+	int total_service_len = len;
+	int service_data_len = 0;
+	char *ptr = NULL;
+	int idx = 0;
+
+	if (src == NULL || dest == NULL)
+		return 0;
+
+	ptr = src;
+
+	while (total_service_len) {
+		/* parse length1 + service_data1 + length2 + service data2*/
+		service_data_len = ptr[0];
+
+		dest[idx] = 1 + service_data_len;
+		dest[idx + 1] =   0x16; /* Fixed */
+		memcpy(dest + idx + 2, ptr +1, service_data_len);
+		idx = idx + 2 + service_data_len;
+
+		ptr = ptr + service_data_len + 1;
+		total_service_len = total_service_len - service_data_len - 1;
+	}
+
+	return idx;
+}
+
+
 /* Takes care of both Scan Response and Advertising data */
 int _bt_hal_set_advertising_data(btgatt_adv_param_setup_t adv_param_setup)
 {
@@ -775,12 +804,12 @@ int _bt_hal_set_advertising_data(btgatt_adv_param_setup_t adv_param_setup)
 
 	/* Service Data  UUID*/
 	if (adv_param_setup.service_data_len > 0) {
-		adv_data[index] = 1 + adv_param_setup.service_data_len;
-		adv_data[index+1] = 0x16; /* Fixed */
-		memcpy(&adv_data[index+2], adv_param_setup.service_data, adv_param_setup.service_data_len);
-		index += (2 + adv_param_setup.service_data_len);
-		length += (2 + adv_param_setup.service_data_len);
-		DBG("After Service data: Index [%d]", index);
+		int l = 0;
+		l = __bt_hal_parse_service_data(adv_param_setup.service_data_len,
+				adv_param_setup.service_data, &adv_data[index]);
+
+		index = index + l;
+		length = length + l;
 	}
 
 	/* Set Apperance */
diff --git a/bt-service/services/adapter/bt-service-core-adapter-le.c b/bt-service/services/adapter/bt-service-core-adapter-le.c
index c7e5d1b..a930b4d 100644
--- a/bt-service/services/adapter/bt-service-core-adapter-le.c
+++ b/bt-service/services/adapter/bt-service-core-adapter-le.c
@@ -1410,6 +1410,17 @@ int _bt_get_scan_response_data(char *sender, int adv_handle, bluetooth_scan_resp
 	return BLUETOOTH_ERROR_NONE;
 }
 
+void print_adv_data(unsigned char * adv_data, int len)
+{
+	char adv_data_str[(31 * 2) + 1];
+
+	for (int i = 0; i < len ; i++)
+		snprintf(&adv_data_str[i * 2], 3, "%02X", adv_data[i]);
+
+	BT_INFO("data: [%s]", adv_data_str);
+}
+
+
 static int __bt_set_multi_adv_param(oal_ble_multi_adv_param_setup_t *adv_setup,
 		bluetooth_advertising_data_t *adv_data, int length)
 {
@@ -1476,9 +1487,30 @@ static int __bt_set_multi_adv_param(oal_ble_multi_adv_param_setup_t *adv_setup,
 			break;
 		}
 		case 0x16: { /* Service Data */
-			adv_setup->service_data = g_malloc0(sizeof(char) * (len - 1));
-			memcpy(adv_setup->service_data, (ptr + 2), (len - 1));
-			adv_setup->service_data_len = len - 1;
+			if (adv_setup->service_data == NULL) {
+				/* first service data  */
+				adv_setup->service_data = g_malloc0(sizeof(char) * (len));
+				adv_setup->service_data[0] = len -1;  /*length1 + service_data1*/
+				memcpy(adv_setup->service_data + 1, (ptr + 2), (len - 1));
+				adv_setup->service_data_len = len;
+			} else {
+				/* Next service data */
+				char *prev_service_data = adv_setup->service_data;
+				int prev_service_len = adv_setup->service_data_len;
+
+				/* create new memory */
+				adv_setup->service_data = g_malloc0(sizeof(char) * (len + prev_service_len));
+				memcpy(adv_setup->service_data, prev_service_data, prev_service_len);
+				adv_setup->service_data[prev_service_len] = len -1;  /*length2 + service_data2*/
+				memcpy(adv_setup->service_data + prev_service_len + 1, (ptr + 2), (len - 1));
+
+				adv_setup->service_data_len = prev_service_len + len;
+
+				/* remove the existing memory */
+				g_free(prev_service_data);
+			}
+			BT_INFO("service data is packed:");
+			print_adv_data((unsigned char *)adv_setup->service_data, adv_setup->service_data_len);
 			break;
 		}
 		case 0x21: {