5 files changed, 123 insertions, 14 deletions

diff --git a/usr.sbin/bluetooth/bthidcontrol/sdp.c b/usr.sbin/bluetooth/bthidcontrol/sdp.c
index 3beabc14be77..8b68058a3340 100644
--- a/usr.sbin/bluetooth/bthidcontrol/sdp.c
+++ b/usr.sbin/bluetooth/bthidcontrol/sdp.c
@@ -47,7 +47,20 @@ static int32_t hid_sdp_parse_protocol_descriptor_list	(sdp_attr_p a);
 static int32_t hid_sdp_parse_hid_descriptor		(sdp_attr_p a);
 static int32_t hid_sdp_parse_boolean			(sdp_attr_p a);
 
+/*
+ * Hard coded attibute IDs taken from the
+ * DEVICE IDENTIFICATION PROFILE SPECIFICATION V13 p.12
+ */
+
+#define SDP_ATTR_DEVICE_ID_SERVICE_VENDORID  0x0201
+#define SDP_ATTR_DEVICE_ID_SERVICE_PRODUCTID 0x0202
+#define SDP_ATTR_DEVICE_ID_SERVICE_VERSION   0x0203
+#define SDP_ATTR_DEVICE_ID_RANGE SDP_ATTR_RANGE( \
+ SDP_ATTR_DEVICE_ID_SERVICE_VENDORID, SDP_ATTR_DEVICE_ID_SERVICE_VERSION )
+
 static uint16_t		service = SDP_SERVICE_CLASS_HUMAN_INTERFACE_DEVICE;
+static uint16_t		service_devid = SDP_SERVICE_CLASS_PNP_INFORMATION;
+static uint32_t 	attrs_devid   = SDP_ATTR_DEVICE_ID_RANGE;
 
 static uint32_t		attrs[] = {
 SDP_ATTR_RANGE(	SDP_ATTR_PROTOCOL_DESCRIPTOR_LIST,
@@ -85,27 +98,34 @@ static uint8_t		buffer[nvalues][512];
 	return (((e) == 0)? 0 : -1);	\
 }
 
+static void
+hid_init_return_values() {
+	int i;
+	for (i = 0; i < nvalues; i ++) {
+		values[i].flags = SDP_ATTR_INVALID;
+		values[i].attr = 0;
+		values[i].vlen = sizeof(buffer[i]);
+		values[i].value = buffer[i];
+	}
+}
+
 static int32_t
 hid_sdp_query(bdaddr_t const *local, struct hid_device *hd, int32_t *error)
 {
 	void	*ss = NULL;
-	uint8_t	*hid_descriptor = NULL;
+	uint8_t	*hid_descriptor = NULL, *v;
 	int32_t	 i, control_psm = -1, interrupt_psm = -1,
 		 reconnect_initiate = -1,
 		 normally_connectable = 0, battery_power = 0,
-		 hid_descriptor_length = -1;
+		 hid_descriptor_length = -1, type;
+	int16_t  vendor_id = 0, product_id = 0, version = 0;
 
 	if (local == NULL)
 		local = NG_HCI_BDADDR_ANY;
 	if (hd == NULL)
 		hid_sdp_query_exit(EINVAL);
 
-	for (i = 0; i < nvalues; i ++) {
-		values[i].flags = SDP_ATTR_INVALID;
-		values[i].attr = 0;
-		values[i].vlen = sizeof(buffer[i]);
-		values[i].value = buffer[i];
-	}
+	hid_init_return_values();
 
 	if ((ss = sdp_open(local, &hd->bdaddr)) == NULL)
 		hid_sdp_query_exit(ENOMEM);
@@ -114,9 +134,6 @@ hid_sdp_query(bdaddr_t const *local, struct hid_device *hd, int32_t *error)
 	if (sdp_search(ss, 1, &service, nattrs, attrs, nvalues, values) != 0)
                 hid_sdp_query_exit(sdp_error(ss));
 
-        sdp_close(ss);
-        ss = NULL;
-
 	for (i = 0; i < nvalues; i ++) {
 		if (values[i].flags != SDP_ATTR_OK)
 			continue;
@@ -151,11 +168,51 @@ hid_sdp_query(bdaddr_t const *local, struct hid_device *hd, int32_t *error)
 		}
 	}
 
+	hid_init_return_values();
+
+	if (sdp_search(ss, 1, &service_devid, 1, &attrs_devid, nvalues, values) != 0)
+                hid_sdp_query_exit(sdp_error(ss));
+
+        sdp_close(ss);
+        ss = NULL;
+
+	/* If search is successful, scan through return vals */
+	for (i = 0; i < 3; i ++ ) {
+		if (values[i].flags == SDP_ATTR_INVALID )
+			continue;
+
+		/* Expecting tag + uint16_t on all 3 attributes */
+		if (values[i].vlen != 3)
+			continue;
+
+		/* Make sure, we're reading a uint16_t */
+		v = values[i].value;
+		SDP_GET8(type, v);
+		if (type != SDP_DATA_UINT16 )
+			continue;
+
+		switch (values[i].attr) {
+			case SDP_ATTR_DEVICE_ID_SERVICE_VENDORID:
+				SDP_GET16(vendor_id, v);
+				break;
+			case SDP_ATTR_DEVICE_ID_SERVICE_PRODUCTID:
+				SDP_GET16(product_id, v);
+				break;
+			case SDP_ATTR_DEVICE_ID_SERVICE_VERSION:
+				SDP_GET16(version, v);
+				break;
+			default:
+				break;
+		}
+	}
+
 	if (control_psm == -1 || interrupt_psm == -1 ||
 	    reconnect_initiate == -1 ||
 	    hid_descriptor == NULL || hid_descriptor_length == -1)
 		hid_sdp_query_exit(ENOATTR);
-
+	hd->vendor_id = vendor_id;
+	hd->product_id = product_id;
+	hd->version = version;
 	hd->control_psm = control_psm;
 	hd->interrupt_psm = interrupt_psm;
 	hd->reconnect_initiate = reconnect_initiate? 1 : 0;
diff --git a/usr.sbin/bluetooth/bthidd/bthid_config.h b/usr.sbin/bluetooth/bthidd/bthid_config.h
index 71cb42562ef2..1b86d8cd40db 100644
--- a/usr.sbin/bluetooth/bthidd/bthid_config.h
+++ b/usr.sbin/bluetooth/bthidd/bthid_config.h
@@ -42,6 +42,9 @@ struct hid_device
 	bdaddr_t		bdaddr;		/* HID device BDADDR */
 	uint16_t		control_psm;	/* control PSM */
 	uint16_t		interrupt_psm;	/* interrupt PSM */
+	uint16_t		vendor_id;	/* primary vendor id */
+	uint16_t		product_id;
+	uint16_t		version;
 	unsigned		new_device           : 1;
 	unsigned		reconnect_initiate   : 1;
 	unsigned		battery_power        : 1;
diff --git a/usr.sbin/bluetooth/bthidd/bthidd.conf.sample b/usr.sbin/bluetooth/bthidd/bthidd.conf.sample
index e23dc1252f95..1750a4c26dce 100644
--- a/usr.sbin/bluetooth/bthidd/bthidd.conf.sample
+++ b/usr.sbin/bluetooth/bthidd/bthidd.conf.sample
@@ -2,6 +2,9 @@
 
 device {
 	bdaddr			00:50:f2:e5:68:84;
+	vendor_id		0x0000;
+	product_id		0x0000;
+	version			0x0000;
 	control_psm		0x11;
 	interrupt_psm		0x13;
 	reconnect_initiate	true;
@@ -24,6 +27,9 @@ device {
 
 device {
 	bdaddr			00:50:f2:e3:fb:e1;
+	vendor_id		0x0000;
+	product_id		0x0000;
+	version			0x0000;
 	control_psm		0x11;
 	interrupt_psm		0x13;
 	reconnect_initiate	true;
diff --git a/usr.sbin/bluetooth/bthidd/lexer.l b/usr.sbin/bluetooth/bthidd/lexer.l
index 6d913eede778..829530dc4e3c 100644
--- a/usr.sbin/bluetooth/bthidd/lexer.l
+++ b/usr.sbin/bluetooth/bthidd/lexer.l
@@ -50,9 +50,13 @@ comment				\#.*
 
 hexdigit			[0-9a-fA-F]
 hexbyte				{hexdigit}{hexdigit}?
+hexword				{hexdigit}{hexdigit}?{hexdigit}?{hexdigit}?
 
 device_word			device
 bdaddr_word			bdaddr
+vendor_id_word			vendor_id
+product_id_word			product_id
+version_word			version
 control_psm_word		control_psm
 interrupt_psm_word		interrupt_psm
 reconnect_initiate_word		reconnect_initiate
@@ -64,6 +68,7 @@ false_word			false
 
 bdaddrstring			{hexbyte}:{hexbyte}:{hexbyte}:{hexbyte}:{hexbyte}:{hexbyte}
 hexbytestring			0x{hexbyte}
+hexwordstring			0x{hexword}
 
 %%
 
@@ -78,6 +83,9 @@ hexbytestring			0x{hexbyte}
 
 {device_word}			return (T_DEVICE);
 {bdaddr_word}			return (T_BDADDR);
+{vendor_id_word}		return (T_VENDOR_ID);
+{product_id_word}		return (T_PRODUCT_ID);
+{version_word}			return (T_VERSION);
 {control_psm_word}		return (T_CONTROL_PSM);
 {interrupt_psm_word}		return (T_INTERRUPT_PSM);
 {reconnect_initiate_word}	return (T_RECONNECT_INITIATE);
@@ -100,6 +108,14 @@ hexbytestring			0x{hexbyte}
 				return (*ep == '\0'? T_HEXBYTE : T_ERROR);
 				}
 
+{hexwordstring}			{
+				char	*ep;
+
+				yylval.num = strtoul(yytext, &ep, 16);
+
+				return (*ep == '\0'? T_HEXWORD : T_ERROR);
+				}
+
 .				return (T_ERROR);
 
 %%
diff --git a/usr.sbin/bluetooth/bthidd/parser.y b/usr.sbin/bluetooth/bthidd/parser.y
index dbb27632680c..c042f85cfe4f 100644
--- a/usr.sbin/bluetooth/bthidd/parser.y
+++ b/usr.sbin/bluetooth/bthidd/parser.y
@@ -87,8 +87,10 @@ static	LIST_HEAD(, hid_device)	 hid_devices;
 
 %token <bdaddr> T_BDADDRSTRING
 %token <num>	T_HEXBYTE
-%token T_DEVICE T_BDADDR T_CONTROL_PSM T_INTERRUPT_PSM T_RECONNECT_INITIATE
-%token T_BATTERY_POWER T_NORMALLY_CONNECTABLE T_HID_DESCRIPTOR
+%token <num>	T_HEXWORD
+%token T_DEVICE T_BDADDR T_VENDOR_ID T_PRODUCT_ID T_VERSION T_CONTROL_PSM
+%token T_INTERRUPT_PSM T_RECONNECT_INITIATE T_BATTERY_POWER
+%token T_NORMALLY_CONNECTABLE T_HID_DESCRIPTOR
 %token T_TRUE T_FALSE T_ERROR
 
 %%
@@ -124,6 +126,9 @@ options:	option ';'
 		;
 
 option:		bdaddr
+		| vendor_id
+		| product_id
+		| version
 		| control_psm
 		| interrupt_psm
 		| reconnect_initiate
@@ -139,6 +144,24 @@ bdaddr:		T_BDADDR T_BDADDRSTRING
 			}
 		;
 
+vendor_id:	T_VENDOR_ID T_HEXWORD
+			{
+			hid_device->vendor_id = $2;
+			}
+		;
+
+product_id:	T_PRODUCT_ID T_HEXWORD
+			{
+			hid_device->product_id = $2;
+			}
+		;
+
+version:	T_VERSION T_HEXWORD
+			{
+			hid_device->version = $2;
+			}
+		;
+
 control_psm:	T_CONTROL_PSM T_HEXBYTE
 			{
 			hid_device->control_psm = $2;
@@ -307,6 +330,9 @@ print_hid_device(hid_device_p d, FILE *f)
 	fprintf(f,
 "device {\n"					\
 "	bdaddr			%s;\n"		\
+"	vendor_id		0x%04x;\n"	\
+"	product_id		0x%04x;\n"	\
+"	version			0x%04x;\n"	\
 "	control_psm		0x%x;\n"	\
 "	interrupt_psm		0x%x;\n"	\
 "	reconnect_initiate	%s;\n"		\
@@ -314,6 +340,7 @@ print_hid_device(hid_device_p d, FILE *f)
 "	normally_connectable	%s;\n"		\
 "	hid_descriptor		{",
 		bt_ntoa(&d->bdaddr, NULL),
+		d->vendor_id, d->product_id, d->version,
 		d->control_psm, d->interrupt_psm,
                 d->reconnect_initiate? "true" : "false",
                 d->battery_power? "true" : "false",