Include all UUID-16 values in the extended inquiry response

1 files changed, 47 insertions, 4 deletions

diff --git a/sdpd/service.c b/sdpd/service.c
index bbc7fcc8..e51f4aed 100644
--- a/sdpd/service.c
+++ b/sdpd/service.c
@@ -141,18 +141,61 @@ void set_service_classes_callback(service_classes_callback_t callback)
 
 void create_ext_inquiry_response(const char *name, uint8_t *data)
 {
+	sdp_list_t *list = sdp_get_record_list();
+	uint8_t *ptr = data;
+	uint16_t uuid[24];
+	int i, index = 0;
+
 	if (name) {
 		int len = strlen(name);
 
 		if (len > 48) {
 			len = 48;
-			data[1] = 0x08;
+			ptr[1] = 0x08;
 		} else
-			data[1] = 0x09;
+			ptr[1] = 0x09;
+
+		ptr[0] = len + 1;
+
+		memcpy(ptr + 2, name, len);
+
+		ptr += len + 2;
+	}
+
+	ptr[1] = 0x03;
+
+	for (; list; list = list->next) {
+		sdp_record_t *rec = (sdp_record_t *) list->data;
+
+		if (rec->svclass.type != SDP_UUID16)
+			continue;
+
+		if (rec->svclass.value.uuid16 < 0x1100)
+			continue;
+
+		if (index > 23) {
+			ptr[1] = 0x02;
+			break;
+		}
 
-		data[0] = len + 1;
+		for (i = 0; i < index; i++)
+			if (uuid[i] == rec->svclass.value.uuid16)
+				break;
 
-		memcpy(data + 2, name, len);
+		if (i == index - 1)
+			continue;
+
+		uuid[index++] = rec->svclass.value.uuid16;
+	}
+
+	if (index > 0) {
+		ptr[0] = (index * 2) + 1;
+		ptr += 2;
+
+		for (i = 0; i < index; i++) {
+			*ptr++ = (uuid[i] & 0x00ff);
+			*ptr++ = (uuid[i] & 0xff00) >> 8;
+		}
 	}
 }