Enhanced support for extended inquiry response

index cdbb4f4..1bb79a7 100644
@@ -6,6 +6,7 @@
  *  Copyright (C) 2002-2003  Maxim Krasnyansky <[email protected]>
  *  Copyright (C) 2002-2010  Marcel Holtmann <[email protected]>
  *  Copyright (C) 2002-2003  Stephen Crane <[email protected]>
+ *  Copyright (C) 2010, Code Aurora Forum. All rights reserved.
  *
  *
  *  This program is free software; you can redistribute it and/or modify

@@ -49,6 +50,8 @@
 #include "manager.h"
 #include "adapter.h"
 
+#define SIZEOF_UUID128 16
+
 static sdp_record_t *server = NULL;
 
 static uint16_t did_vendor = 0x0000;

@@ -174,41 +177,102 @@ static void update_svclass_list(const bdaddr_t *src)
 
 }
 
+static void eir_generate_uuid128(sdp_list_t *list, uint8_t *ptr, uint16_t *eir_len)
+{
+	int i, k, index = 0;
+	uint16_t len = *eir_len;
+	uint8_t *uuid128;
+	gboolean truncated = FALSE;
+
+	/* Store UUID128 in place, skip 2 bytes to insert type and length later */
+	uuid128 = ptr + 2;
+
+	for (; list; list = list->next) {
+		sdp_record_t *rec = (sdp_record_t *) list->data;
+
+		if (rec->svclass.type != SDP_UUID128)
+			continue;
+
+		/* Stop if not enough space to put next UUID128 */
+		if ((len + 2 + SIZEOF_UUID128) > EIR_DATA_LENGTH) {
+			truncated = TRUE;
+			break;
+		}
+
+		/* Check for duplicates, EIR data is Little Endian */
+		for (i = 0; i < index; i++) {
+			for (k = 0; k < SIZEOF_UUID128; k++) {
+				if (uuid128[i*SIZEOF_UUID128 + k] !=
+					rec->svclass.value.uuid128.data[SIZEOF_UUID128 - k])
+					break;
+			}
+			if (k == SIZEOF_UUID128)
+				break;
+		}
+
+		if (i < index)
+			continue;
+
+		/* EIR data is Little Endian */
+		for (k = 0; k < SIZEOF_UUID128; k++)
+			uuid128[index*SIZEOF_UUID128 + k] =
+				rec->svclass.value.uuid128.data[SIZEOF_UUID128 - 1 - k];
+
+		len += SIZEOF_UUID128;
+		index++;
+	}
+
+	if (index > 0 || truncated) {
+		/* EIR Data length */
+		ptr[0] = (index * SIZEOF_UUID128) + 1;
+		/* EIR Data type */
+		ptr[1] = (truncated) ? EIR_UUID128_SOME : EIR_UUID128_ALL;
+		len += 2;
+		*eir_len = len;
+	}
+}
+
 void create_ext_inquiry_response(const char *name,
 					int8_t tx_power, sdp_list_t *services,
 					uint8_t *data)
 {
 	sdp_list_t *list = services;
 	uint8_t *ptr = data;
-	uint16_t uuid[24];
+	uint16_t eir_len = 0;
+	uint16_t uuid16[EIR_DATA_LENGTH/2];
 	int i, index = 0;
+	gboolean truncated = FALSE;
 
 	if (name) {
 		int len = strlen(name);
 
+		/* EIR Data type */
 		if (len > 48) {
 			len = 48;
-			ptr[1] = 0x08;
+			ptr[1] = EIR_NAME_SHORT;
 		} else
-			ptr[1] = 0x09;
+			ptr[1] = EIR_NAME_COMPLETE;
 
+		/* EIR Data length */
 		ptr[0] = len + 1;
 
 		memcpy(ptr + 2, name, len);
 
-		ptr += len + 2;
+		eir_len += (len + 2);
+		ptr += (len + 2);
 	}
 
 	if (tx_power != 0) {
 		*ptr++ = 2;
-		*ptr++ = 0x0a;
+		*ptr++ = EIR_TX_POWER;
 		*ptr++ = (uint8_t) tx_power;
+		eir_len += 3;
 	}
 
 	if (did_vendor != 0x0000) {
 		uint16_t source = 0x0002;
 		*ptr++ = 9;
-		*ptr++ = 0x10;
+		*ptr++ = EIR_DEVICE_ID;
 		*ptr++ = (source & 0x00ff);
 		*ptr++ = (source & 0xff00) >> 8;
 		*ptr++ = (did_vendor & 0x00ff);

@@ -217,10 +281,10 @@ void create_ext_inquiry_response(const char *name,
 		*ptr++ = (did_product & 0xff00) >> 8;
 		*ptr++ = (did_version & 0x00ff);
 		*ptr++ = (did_version & 0xff00) >> 8;
+		eir_len += 10;
 	}
 
-	ptr[1] = 0x03;
-
+	/* Group all UUID16 types */
 	for (; list; list = list->next) {
 		sdp_record_t *rec = (sdp_record_t *) list->data;
 

@@ -233,30 +297,42 @@ void create_ext_inquiry_response(const char *name,
 		if (rec->svclass.value.uuid16 == PNP_INFO_SVCLASS_ID)
 			continue;
 
-		if (index > 23) {
-			ptr[1] = 0x02;
+		/* Stop if not enough space to put next UUID16 */
+		if ((eir_len + 2 + sizeof(uint16_t)) > EIR_DATA_LENGTH) {
+			truncated = TRUE;
 			break;
 		}
 
+		/* Check for duplicates */
 		for (i = 0; i < index; i++)
-			if (uuid[i] == rec->svclass.value.uuid16)
+			if (uuid16[i] == rec->svclass.value.uuid16)
 				break;
 
-		if (i == index - 1)
+		if (i < index)
 			continue;
 
-		uuid[index++] = rec->svclass.value.uuid16;
+		uuid16[index++] = rec->svclass.value.uuid16;
+		eir_len += sizeof(uint16_t);
 	}
 
 	if (index > 0) {
-		ptr[0] = (index * 2) + 1;
+		/* EIR Data length */
+		ptr[0] = (index * sizeof(uint16_t)) + 1;
+		/* EIR Data type */
+		ptr[1] = (truncated) ? EIR_UUID16_SOME : EIR_UUID16_ALL;
+
 		ptr += 2;
+		eir_len += 2;
 
 		for (i = 0; i < index; i++) {
-			*ptr++ = (uuid[i] & 0x00ff);
-			*ptr++ = (uuid[i] & 0xff00) >> 8;
+			*ptr++ = (uuid16[i] & 0x00ff);
+			*ptr++ = (uuid16[i] & 0xff00) >> 8;
 		}
 	}
+
+	/* Group all UUID128 types */
+	if (eir_len <= EIR_DATA_LENGTH - 2)
+		eir_generate_uuid128(services, ptr, &eir_len);
 }
 
 void register_public_browse_group(void)

index e93b0b6..8fd8bbe 100644
@@ -6,6 +6,7 @@
  *  Copyright (C) 2002-2003  Maxim Krasnyansky <[email protected]>
  *  Copyright (C) 2002-2010  Marcel Holtmann <[email protected]>
  *  Copyright (C) 2002-2003  Stephen Crane <[email protected]>
+ *  Copyright (C) 2010, Code Aurora Forum. All rights reserved.
  *
  *
  *  This program is free software; you can redistribute it and/or modify

@@ -34,6 +35,17 @@
 #define SDPDBG(fmt...)
 #endif
 
+#define EIR_DATA_LENGTH  240
+
+#define EIR_UUID16_SOME             0x02  /* 16-bit UUID, more available */
+#define EIR_UUID16_ALL              0x03  /* 16-bit UUID, all listed */
+#define EIR_UUID128_SOME            0x06  /* 128-bit UUID, more available */
+#define EIR_UUID128_ALL             0x07  /* 128-bit UUID, all listed */
+#define EIR_NAME_SHORT              0x08  /* shortened local name */
+#define EIR_NAME_COMPLETE           0x09  /* complete local name */
+#define EIR_DEVICE_ID               0x10  /* device ID */
+#define EIR_TX_POWER                0x0A  /* Transmit power level */
+
 typedef struct request {
 	bdaddr_t device;
 	bdaddr_t bdaddr;

index 96280bd..8ec4df4 100644
@@ -4,6 +4,7 @@
  *
  *  Copyright (C) 2006-2010  Nokia Corporation
  *  Copyright (C) 2004-2010  Marcel Holtmann <[email protected]>
+ *  Copyright (C) 2010, Code Aurora Forum. All rights reserved.
  *
  *
  *  This program is free software; you can redistribute it and/or modify

@@ -445,6 +446,8 @@ static DBusMessage *update_record(DBusConnection *conn, DBusMessage *msg,
 				strerror(EIO));
 	}
 
+	adapter_update_ext_inquiry_response(&src);
+
 	return dbus_message_new_method_return(msg);
 }
 

@@ -516,6 +519,7 @@ static DBusMessage *add_service_record(DBusConnection *conn,
 	const char *sender, *record;
 	dbus_uint32_t handle;
 	int err;
+	bdaddr_t addr;
 
 	if (dbus_message_get_args(msg, NULL,
 			DBUS_TYPE_STRING, &record, DBUS_TYPE_INVALID) == FALSE)

@@ -526,6 +530,13 @@ static DBusMessage *add_service_record(DBusConnection *conn,
 	if (err < 0)
 		return failed_strerror(msg, err);
 
+	if (serv_adapter->adapter)
+		adapter_get_address(serv_adapter->adapter, &addr);
+	else
+		bacpy(&addr, BDADDR_ANY);
+
+	adapter_update_ext_inquiry_response(&addr);
+
 	reply = dbus_message_new_method_return(msg);
 	if (!reply)
 		return NULL;

@@ -550,6 +561,7 @@ static DBusMessage *remove_service_record(DBusConnection *conn,
 	struct service_adapter *serv_adapter = data;
 	dbus_uint32_t handle;
 	const char *sender;
+	bdaddr_t addr;
 
 	if (dbus_message_get_args(msg, NULL, DBUS_TYPE_UINT32, &handle,
 						DBUS_TYPE_INVALID) == FALSE)

@@ -560,6 +572,13 @@ static DBusMessage *remove_service_record(DBusConnection *conn,
 	if (remove_record(conn, sender, serv_adapter, handle) < 0)
 		return not_available(msg);
 
+	if (serv_adapter->adapter)
+		adapter_get_address(serv_adapter->adapter, &addr);
+	else
+		bacpy(&addr, BDADDR_ANY);
+
+	adapter_update_ext_inquiry_response(&addr);
+
 	return dbus_message_new_method_return(msg);
 }
 

index 2a6ac18..8a05356 100644
@@ -4,6 +4,7 @@
  *
  *  Copyright (C) 2006-2010  Nokia Corporation
  *  Copyright (C) 2004-2010  Marcel Holtmann <[email protected]>
+ *  Copyright (C) 2010, Code Aurora Forum. All rights reserved.
  *
  *
  *  This program is free software; you can redistribute it and/or modify

@@ -820,7 +821,7 @@ static DBusMessage *set_pairable_timeout(DBusConnection *conn,
 
 static void update_ext_inquiry_response(struct btd_adapter *adapter)
 {
-	uint8_t fec = 0, data[240];
+	uint8_t fec = 0, data[EIR_DATA_LENGTH];
 	struct hci_dev *dev = &adapter->dev;
 	int dd;
 

@@ -846,6 +847,27 @@ static void update_ext_inquiry_response(struct btd_adapter *adapter)
 	hci_close_dev(dd);
 }
 
+void adapter_update_ext_inquiry_response(const bdaddr_t *src)
+{
+	struct btd_adapter *adapter;
+	GSList *adapters;
+
+	if (bacmp(src, BDADDR_ANY) != 0) {
+		adapter = manager_find_adapter(src);
+		if (adapter)
+			update_ext_inquiry_response(adapter);
+		else
+			error("Updating EIR failed: device not found");
+	} else {
+
+		/* Update extended inquiry reponse for ANY adapter */
+		for (adapters = manager_get_adapters(); adapters; adapters = adapters->next) {
+			adapter = adapters->data;
+			update_ext_inquiry_response(adapter);
+		}
+	}
+}
+
 void adapter_set_class_complete(bdaddr_t *bdaddr, uint8_t status)
 {
 	uint8_t class[3];

index 71d3387..4c3bc04 100644
@@ -4,6 +4,7 @@
  *
  *  Copyright (C) 2006-2010  Nokia Corporation
  *  Copyright (C) 2004-2010  Marcel Holtmann <[email protected]>
+ *  Copyright (C) 2010, Code Aurora Forum. All rights reserved.
  *
  *
  *  This program is free software; you can redistribute it and/or modify

@@ -126,6 +127,7 @@ void adapter_service_insert(const bdaddr_t *bdaddr, void *rec);
 void adapter_service_remove(const bdaddr_t *bdaddr, void *rec);
 sdp_list_t *adapter_get_services(struct btd_adapter *adapter);
 void adapter_set_class_complete(bdaddr_t *bdaddr, uint8_t status);
+void adapter_update_ext_inquiry_response(const bdaddr_t *src);
 
 struct agent *adapter_get_agent(struct btd_adapter *adapter);
 void adapter_add_connection(struct btd_adapter *adapter,

index 8a05356..9be12e7 100644
@@ -2716,6 +2716,7 @@ static void append_dict_valist(DBusMessageIter *iter,
 	DBusMessageIter dict;
 	const char *key;
 	int type;
+	int n_elements;
 	void *val;
 
 	dbus_message_iter_open_container(iter, DBUS_TYPE_ARRAY,

@@ -2727,7 +2728,12 @@ static void append_dict_valist(DBusMessageIter *iter,
 	while (key) {
 		type = va_arg(var_args, int);
 		val = va_arg(var_args, void *);
-		dict_append_entry(&dict, key, type, val);
+		if (type == DBUS_TYPE_ARRAY) {
+			n_elements = va_arg(var_args, int);
+			if (n_elements > 0)
+				dict_append_array(&dict, key, DBUS_TYPE_STRING, val, n_elements);
+		} else
+			dict_append_entry(&dict, key, type, val);
 		key = va_arg(var_args, char *);
 	}
 

@@ -2758,8 +2764,84 @@ static void emit_device_found(const char *path, const char *address,
 	g_dbus_send_message(connection, signal);
 }
 
+static int get_uuid_count_eir (uint8_t *eir_data)
+{
+	uint8_t len = 0;
+	int count = 0;
+
+	while (len < EIR_DATA_LENGTH) {
+		uint8_t type = eir_data[1];
+		uint8_t field_len = eir_data[0];
+		if ((type == EIR_UUID16_SOME) || (type == EIR_UUID16_ALL))
+			count += field_len/2;
+		else if ((type == EIR_UUID32_SOME) || (type == EIR_UUID32_ALL))
+			count += field_len/4;
+		else if ((type == EIR_UUID128_SOME) || (type == EIR_UUID128_ALL))
+			count += field_len/16;
+		len += field_len + 1;
+		eir_data += field_len + 1;
+	}
+
+	return count;
+}
+
+static void get_uuids_eir(char **uuids, uint8_t *eir_data)
+{
+	uint8_t len = 0;
+
+	/* Count UUID16, UUID32 and UUID128 */
+	while (len < EIR_DATA_LENGTH) {
+		uint8_t field_len = eir_data[0];
+		uint8_t type = eir_data[1];
+		int count;
+		uuid_t service;
+		int size;
+		uint8_t *data = &eir_data[2];
+		int i, k;
+
+		/* Generate uuids in SDP format (EIR data is Little Endian) */
+		if ((type == EIR_UUID16_SOME) || (type == EIR_UUID16_ALL)) {
+			size = 2;
+			count = field_len/size;
+			service.type = SDP_UUID16;
+			for (i = 0; i < count; i++) {
+				uint16_t val16 = data[1];
+				val16 = (val16<<8) + data[0];
+				service.value.uuid16 = val16;
+				*uuids++ = bt_uuid2string(&service);
+				data += size;
+			}
+		} else if ((type == EIR_UUID32_SOME) || (type == EIR_UUID32_ALL)) {
+			size = 4;
+			count = field_len/size;
+			service.type = SDP_UUID32;
+			for (i = 0; i < count; i++) {
+				uint32_t val32 = data[3];
+				for (k = size-2; k >= 0; k--)
+					val32 = (val32<<8) + data[k];
+				service.value.uuid32 = val32;
+				*uuids++ = bt_uuid2string(&service);
+				data += size;
+			}
+		} else if ((type == EIR_UUID128_SOME) || (type == EIR_UUID128_ALL)) {
+			size = 16;
+			count = field_len/size;
+			service.type = SDP_UUID128;
+			for (i = 0; i < count; i++) {
+				for (k = 0; k < size; k++)
+					service.value.uuid128.data[k] = data[size-k-1];
+				*uuids++ = bt_uuid2string(&service);
+				data += size;
+			}
+		}
+
+		len += field_len + 1;
+		eir_data += field_len + 1;
+	}
+}
+
 void adapter_emit_device_found(struct btd_adapter *adapter,
-				struct remote_dev_info *dev)
+				struct remote_dev_info *dev, uint8_t *eir_data)
 {
 	struct btd_device *device;
 	char peer_addr[18], local_addr[18];

@@ -2767,6 +2849,8 @@ void adapter_emit_device_found(struct btd_adapter *adapter,
 	dbus_bool_t paired = FALSE;
 	dbus_int16_t rssi = dev->rssi;
 	char *alias;
+	char **uuids = NULL;
+	int uuid_count = 0;
 
 	ba2str(&dev->bdaddr, peer_addr);
 	ba2str(&adapter->bdaddr, local_addr);

@@ -2786,6 +2870,15 @@ void adapter_emit_device_found(struct btd_adapter *adapter,
 	} else
 		alias = g_strdup(dev->alias);
 
+	/* Extract UUIDs from extended inquiry response if any*/
+	if (eir_data != NULL)
+		uuid_count = get_uuid_count_eir(eir_data);
+
+	if (uuid_count > 0) {
+		uuids = g_new0(char *, uuid_count + 1);
+		get_uuids_eir(uuids, eir_data);
+	}
+
 	emit_device_found(adapter->path, paddr,
 			"Address", DBUS_TYPE_STRING, &paddr,
 			"Class", DBUS_TYPE_UINT32, &dev->class,

@@ -2795,15 +2888,17 @@ void adapter_emit_device_found(struct btd_adapter *adapter,
 			"Alias", DBUS_TYPE_STRING, &alias,
 			"LegacyPairing", DBUS_TYPE_BOOLEAN, &dev->legacy,
 			"Paired", DBUS_TYPE_BOOLEAN, &paired,
+			"UUIDs", DBUS_TYPE_ARRAY, &uuids, uuid_count,
 			NULL);
 
 	g_free(alias);
+	g_strfreev(uuids);
 }
 
 void adapter_update_found_devices(struct btd_adapter *adapter, bdaddr_t *bdaddr,
 				int8_t rssi, uint32_t class, const char *name,
 				const char *alias, gboolean legacy,
-				name_status_t name_status)
+				name_status_t name_status, uint8_t *eir_data)
 {
 	struct remote_dev_info *dev, match;
 

@@ -2842,7 +2937,7 @@ done:
 	adapter->found_devices = g_slist_sort(adapter->found_devices,
 						(GCompareFunc) dev_rssi_cmp);
 
-	adapter_emit_device_found(adapter, dev);
+	adapter_emit_device_found(adapter, dev, eir_data);
 }
 
 int adapter_remove_found_device(struct btd_adapter *adapter, bdaddr_t *bdaddr)

index 4c3bc04..7726eca 100644
@@ -114,10 +114,10 @@ struct remote_dev_info *adapter_search_found_devices(struct btd_adapter *adapter
 void adapter_update_found_devices(struct btd_adapter *adapter, bdaddr_t *bdaddr,
 				int8_t rssi, uint32_t class, const char *name,
 				const char *alias, gboolean legacy,
-				name_status_t name_status);
+								  name_status_t name_status, uint8_t *eir_data);
 int adapter_remove_found_device(struct btd_adapter *adapter, bdaddr_t *bdaddr);
 void adapter_emit_device_found(struct btd_adapter *adapter,
-				struct remote_dev_info *dev);
+				struct remote_dev_info *dev, uint8_t *eir_data);
 void adapter_update_oor_devices(struct btd_adapter *adapter);
 void adapter_mode_changed(struct btd_adapter *adapter, uint8_t scan_mode);
 void adapter_setname_complete(bdaddr_t *local, uint8_t status);

index b83506f..912af02 100644
@@ -4,6 +4,7 @@
  *
  *  Copyright (C) 2006-2010  Nokia Corporation
  *  Copyright (C) 2004-2010  Marcel Holtmann <[email protected]>
+ *  Copyright (C) 2010, Code Aurora Forum. All rights reserved.
  *
  *
  *  This program is free software; you can redistribute it and/or modify

@@ -515,7 +516,7 @@ void hcid_dbus_inquiry_result(bdaddr_t *local, bdaddr_t *peer, uint32_t class,
 	if (dev) {
 		adapter_update_found_devices(adapter, peer, rssi, class,
 						NULL, NULL, dev->legacy,
-						NAME_NOT_REQUIRED);
+						NAME_NOT_REQUIRED, data);
 		return;
 	}
 

@@ -566,7 +567,7 @@ void hcid_dbus_inquiry_result(bdaddr_t *local, bdaddr_t *peer, uint32_t class,
 
 	/* add in the list to track name sent/pending */
 	adapter_update_found_devices(adapter, peer, rssi, class, name, alias,
-					legacy, name_status);
+					legacy, name_status, data);
 
 	g_free(name);
 	g_free(alias);

@@ -642,7 +643,7 @@ void hcid_dbus_remote_name(bdaddr_t *local, bdaddr_t *peer, uint8_t status,
 	if (dev_info) {
 		g_free(dev_info->name);
 		dev_info->name = g_strdup(name);
-		adapter_emit_device_found(adapter, dev_info);
+		adapter_emit_device_found(adapter, dev_info, NULL);
 	}
 
 	if (device)

index 8fd8bbe..38677d3 100644
@@ -39,6 +39,8 @@
 
 #define EIR_UUID16_SOME             0x02  /* 16-bit UUID, more available */
 #define EIR_UUID16_ALL              0x03  /* 16-bit UUID, all listed */
+#define EIR_UUID32_SOME             0x04  /* 32-bit UUID, more available */
+#define EIR_UUID32_ALL              0x05  /* 32-bit UUID, all listed */
 #define EIR_UUID128_SOME            0x06  /* 128-bit UUID, more available */
 #define EIR_UUID128_ALL             0x07  /* 128-bit UUID, all listed */
 #define EIR_NAME_SHORT              0x08  /* shortened local name */