From 6032284eb9cbdf7cf20bb465b2de3a3e5bfaab46 Mon Sep 17 00:00:00 2001 From: Vladimir Kondratyev Date: Sat, 12 Aug 2017 21:20:51 +0000 Subject: [PATCH] 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 Reviewed by: emax Approved by: bapt (mentor), gonzo (mentor) Differential Revision: https://reviews.freebsd.org/D3702 --- usr.sbin/bluetooth/bthidcontrol/sdp.c | 95 ++++++++++++++++---- usr.sbin/bluetooth/bthidd/bthid_config.h | 3 + usr.sbin/bluetooth/bthidd/bthidd.conf.sample | 6 ++ usr.sbin/bluetooth/bthidd/lexer.l | 16 ++++ usr.sbin/bluetooth/bthidd/parser.y | 31 ++++++- 5 files changed, 130 insertions(+), 21 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 int32_t -hid_sdp_query(bdaddr_t const *local, struct hid_device *hd, int32_t *error) -{ - 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); - +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, *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; 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 T_BDADDRSTRING %token 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 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",