bthidd(8): Add support for vendor_id, product_id and version.

Extend bthidd.conf format to store vendor and product IDs of remote
Bluetooth HID devices to make possible implementation of device
specific quirks inside bthidd(8).
Add support for querying of this information from device's SDP records
with bthidcontrol(8) "Query" command.

Submitted by:		Dirk Engling <erdgeist@erdgeist.org>
Reviewed by:		emax
Approved by:		bapt (mentor), gonzo (mentor)
Differential Revision:	https://reviews.freebsd.org/D3702

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 int32_t
+static void
-hid_sdp_query(bdaddr_t const *local, struct hid_device *hd, int32_t *error)
+hid_init_return_values() {
-{
+	int i;
-	void	*ss = NULL;
-	uint8_t	*hid_descriptor = NULL;
-	int32_t	 i, control_psm = -1, interrupt_psm = -1,
-		 reconnect_initiate = -1,
-		 normally_connectable = 0, battery_power = 0,
-		 hid_descriptor_length = -1;
-
-	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];
 	}
+}
+
+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, *v;
+	int32_t	 i, control_psm = -1, interrupt_psm = -1,
+		 reconnect_initiate = -1,
+		 normally_connectable = 0, battery_power = 0,
+		 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);
+
+	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;

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;

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;

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);
 
 %%

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 <num>	T_HEXWORD
-%token T_BATTERY_POWER T_NORMALLY_CONNECTABLE T_HID_DESCRIPTOR
+%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",