From 9287f06d0890877708d421a604c461ab6625e4f5 Mon Sep 17 00:00:00 2001 From: Takanori Watanabe Date: Mon, 11 May 2020 15:32:32 +0000 Subject: [PATCH] Add le_scan subcommand to hccontrol. PR: 246141 Submitted by: Marc Veldman --- usr.sbin/bluetooth/hccontrol/Makefile | 2 +- usr.sbin/bluetooth/hccontrol/adv_data.c | 249 +++++++++++++++++++++++ usr.sbin/bluetooth/hccontrol/hccontrol.8 | 3 +- usr.sbin/bluetooth/hccontrol/hccontrol.h | 4 + usr.sbin/bluetooth/hccontrol/le.c | 155 ++++++++++++++ usr.sbin/bluetooth/hccontrol/node.c | 81 +------- usr.sbin/bluetooth/hccontrol/util.c | 14 ++ 7 files changed, 427 insertions(+), 81 deletions(-) create mode 100644 usr.sbin/bluetooth/hccontrol/adv_data.c diff --git a/usr.sbin/bluetooth/hccontrol/Makefile b/usr.sbin/bluetooth/hccontrol/Makefile index cfe3d92842a2..9191b656ff02 100644 --- a/usr.sbin/bluetooth/hccontrol/Makefile +++ b/usr.sbin/bluetooth/hccontrol/Makefile @@ -8,7 +8,7 @@ PROG= hccontrol MAN= hccontrol.8 SRCS= send_recv.c link_policy.c link_control.c le.c\ host_controller_baseband.c info.c status.c node.c hccontrol.c \ - util.c + util.c adv_data.c WARNS?= 2 LIBADD= bluetooth diff --git a/usr.sbin/bluetooth/hccontrol/adv_data.c b/usr.sbin/bluetooth/hccontrol/adv_data.c new file mode 100644 index 000000000000..8f11d99a397f --- /dev/null +++ b/usr.sbin/bluetooth/hccontrol/adv_data.c @@ -0,0 +1,249 @@ +/*- + * adv_data.c + * + * SPDX-License-Identifier: BSD-2-Clause-FreeBSD + + * Copyright (c) 2020 Marc Veldman + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * $Id$ + * $FreeBSD$ + */ + +#include +#include +#include +#include +#define L2CAP_SOCKET_CHECKED +#include +#include "hccontrol.h" + +static char* const adv_data2str(int len, uint8_t* data, char* buffer, + int size); +static char* const adv_name2str(int len, uint8_t* advdata, char* buffer, + int size); +static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer, + int size); + +void dump_adv_data(int len, uint8_t* advdata) +{ + int n=0; + fprintf(stdout, "\tADV Data: "); + for (n = 0; n < len+1; n++) { + fprintf(stdout, "%02x ", advdata[n]); + } + fprintf(stdout, "\n"); +} + +void print_adv_data(int len, uint8_t* advdata) +{ + int n=0; + while(n < len) + { + char buffer[2048]; + uint8_t datalen = advdata[n]; + uint8_t datatype = advdata[++n]; + /* Skip type */ + ++n; + datalen--; + switch (datatype) { + case 0x01: + fprintf(stdout, + "\tFlags: %s\n", + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x02: + fprintf(stdout, + "\tIncomplete list of service" + " class UUIDs (16-bit): %s\n", + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x03: + fprintf(stdout, + "\tComplete list of service " + "class UUIDs (16-bit): %s\n", + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x07: + fprintf(stdout, + "\tComplete list of service " + "class UUIDs (128 bit): %s\n", + adv_uuid2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x08: + fprintf(stdout, + "\tShortened local name: %s\n", + adv_name2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x09: + fprintf(stdout, + "\tComplete local name: %s\n", + adv_name2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x0a: + fprintf(stdout, + "\tTx Power level: %d dBm\n", + (int8_t)advdata[n]); + break; + case 0x0d: + fprintf(stdout, + "\tClass of device: %s\n", + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x16: + fprintf(stdout, + "\tService data: %s\n", + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0x19: + fprintf(stdout, + "\tAppearance: %s\n", + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + break; + case 0xff: + fprintf(stdout, + "\tManufacturer: %s\n", + hci_manufacturer2str( + advdata[n]|advdata[n+1]<<8)); + fprintf(stdout, + "\tManufacturer specific data: %s\n", + adv_data2str( + datalen-2, + &advdata[n+2], + buffer, + sizeof(buffer))); + break; + default: + fprintf(stdout, + "\tUNKNOWN datatype: %02x data %s\n", + datatype, + adv_data2str( + datalen, + &advdata[n], + buffer, + sizeof(buffer))); + } + n += datalen; + } +} + +static char* const adv_data2str(int datalen, uint8_t* data, char* buffer, + int size) +{ + int i = 0; + char tmpbuf[5]; + + if (buffer == NULL) + return NULL; + + memset(buffer, 0, size); + + while(i < datalen) { + (void)snprintf(tmpbuf, sizeof(tmpbuf), "%02x ", data[i]); + /* Check if buffer is full */ + if (strlcat(buffer, tmpbuf, size) > size) + break; + i++; + } + return buffer; +} + +static char* const adv_name2str(int datalen, uint8_t* data, char* buffer, + int size) +{ + if (buffer == NULL) + return NULL; + + memset(buffer, 0, size); + + (void)strlcpy(buffer, (char*)data, datalen+1); + return buffer; +} + +static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer, + int size) +{ + int i; + uuid_t uuid; + uint32_t ustatus; + char* tmpstr; + + if (buffer == NULL) + return NULL; + + memset(buffer, 0, size); + if (datalen < 16) + return buffer; + uuid.time_low = le32dec(data+12); + uuid.time_mid = le16dec(data+10); + uuid.time_hi_and_version = le16dec(data+8); + uuid.clock_seq_hi_and_reserved = data[7]; + uuid.clock_seq_low = data[6]; + for(i = 0; i < _UUID_NODE_LEN; i++){ + uuid.node[i] = data[5 - i]; + } + uuid_to_string(&uuid, &tmpstr, &ustatus); + if(ustatus == uuid_s_ok) { + strlcpy(buffer, tmpstr, size); + } + free(tmpstr); + + return buffer; +} diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.8 b/usr.sbin/bluetooth/hccontrol/hccontrol.8 index 8e89f6d81823..ee35a3d0f365 100644 --- a/usr.sbin/bluetooth/hccontrol/hccontrol.8 +++ b/usr.sbin/bluetooth/hccontrol/hccontrol.8 @@ -25,7 +25,7 @@ .\" $Id: hccontrol.8,v 1.6 2003/08/06 21:26:38 max Exp $ .\" $FreeBSD$ .\" -.Dd April 27, 2020 +.Dd May 3, 2020 .Dt HCCONTROL 8 .Os .Sh NAME @@ -156,6 +156,7 @@ are: .It Cm LE_Set_Scan_Enable .It Cm LE_Read_Supported_States .It Cm LE_Read_Buffer_Size +.It Cm LE Scan .El .Pp The currently supported node commands in diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.h b/usr.sbin/bluetooth/hccontrol/hccontrol.h index 3758faa5c8f2..9c69146652b5 100644 --- a/usr.sbin/bluetooth/hccontrol/hccontrol.h +++ b/usr.sbin/bluetooth/hccontrol/hccontrol.h @@ -79,6 +79,10 @@ char const * hci_cc2str (int); char const * hci_con_state2str (int); char const * hci_status2str (int); char const * hci_bdaddr2str (bdaddr_t const *); +char const * hci_addrtype2str (int type); + +void dump_adv_data(int len, uint8_t* advdata); +void print_adv_data(int len, uint8_t* advdata); #endif /* _HCCONTROL_H_ */ diff --git a/usr.sbin/bluetooth/hccontrol/le.c b/usr.sbin/bluetooth/hccontrol/le.c index 1b465585664a..1a74e40c7191 100644 --- a/usr.sbin/bluetooth/hccontrol/le.c +++ b/usr.sbin/bluetooth/hccontrol/le.c @@ -39,6 +39,7 @@ #include #include #include +#include #include #include #include @@ -60,6 +61,8 @@ static int le_enable(int s, int argc, char *argv[]); static int le_set_advertising_enable(int s, int argc, char *argv[]); static int le_set_advertising_param(int s, int argc, char *argv[]); static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]); +static int le_scan(int s, int argc, char *argv[]); +static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose); static int le_set_scan_param(int s, int argc, char *argv[]) @@ -613,6 +616,152 @@ le_read_buffer_size(int s, int argc, char *argv[]) return (OK); } +static int +le_scan(int s, int argc, char *argv[]) +{ + int n, bufsize, scancount, numscans; + bool verbose; + uint8_t active = 0; + char ch; + + char b[512]; + ng_hci_event_pkt_t *e = (ng_hci_event_pkt_t *) b; + + ng_hci_le_set_scan_parameters_cp scan_param_cp; + ng_hci_le_set_scan_parameters_rp scan_param_rp; + + ng_hci_le_set_scan_enable_cp scan_enable_cp; + ng_hci_le_set_scan_enable_rp scan_enable_rp; + + optreset = 1; + optind = 0; + verbose = false; + numscans = 1; + + while ((ch = getopt(argc, argv , "an:v")) != -1) { + switch(ch) { + case 'a': + active = 1; + break; + case 'n': + numscans = (uint8_t)strtol(optarg, NULL, 10); + break; + case 'v': + verbose = true; + break; + } + } + + scan_param_cp.le_scan_type = active; + scan_param_cp.le_scan_interval = (uint16_t)(100/0.625); + scan_param_cp.le_scan_window = (uint16_t)(50/0.625); + /* Address type public */ + scan_param_cp.own_address_type = 0; + /* 'All' filter policy */ + scan_param_cp.scanning_filter_policy = 0; + n = sizeof(scan_param_rp); + + if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_SET_SCAN_PARAMETERS), + (void *)&scan_param_cp, sizeof(scan_param_cp), + (void *)&scan_param_rp, &n) == ERROR) + return (ERROR); + + if (scan_param_rp.status != 0x00) { + fprintf(stdout, "LE_Set_Scan_Parameters failed. Status: %s [%#02x]\n", + hci_status2str(scan_param_rp.status), + scan_param_rp.status); + return (FAILED); + } + + /* Enable scanning */ + n = sizeof(scan_enable_rp); + scan_enable_cp.le_scan_enable = 1; + scan_enable_cp.filter_duplicates = 1; + if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_SET_SCAN_ENABLE), + (void *)&scan_enable_cp, sizeof(scan_enable_cp), + (void *)&scan_enable_rp, &n) == ERROR) + return (ERROR); + + if (scan_enable_rp.status != 0x00) { + fprintf(stdout, "LE_Scan_Enable enable failed. Status: %s [%#02x]\n", + hci_status2str(scan_enable_rp.status), + scan_enable_rp.status); + return (FAILED); + } + + scancount = 0; + while (scancount < numscans) { + /* wait for scan events */ + bufsize = sizeof(b); + if (hci_recv(s, b, &bufsize) == ERROR) { + return (ERROR); + } + + if (bufsize < sizeof(*e)) { + errno = EIO; + return (ERROR); + } + scancount++; + if (e->event == NG_HCI_EVENT_LE) { + fprintf(stdout, "Scan %d\n", scancount); + handle_le_event(e, verbose); + } + } + + fprintf(stdout, "Scan complete\n"); + + /* Disable scanning */ + n = sizeof(scan_enable_rp); + scan_enable_cp.le_scan_enable = 0; + if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE, + NG_HCI_OCF_LE_SET_SCAN_ENABLE), + (void *)&scan_enable_cp, sizeof(scan_enable_cp), + (void *)&scan_enable_rp, &n) == ERROR) + return (ERROR); + + if (scan_enable_rp.status != 0x00) { + fprintf(stdout, "LE_Scan_Enable disable failed. Status: %s [%#02x]\n", + hci_status2str(scan_enable_rp.status), + scan_enable_rp.status); + return (FAILED); + } + + return (OK); +} + +static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose) +{ + int rc; + ng_hci_le_ep *leer = + (ng_hci_le_ep *)(e + 1); + ng_hci_le_advertising_report_ep *advrep = + (ng_hci_le_advertising_report_ep *)(leer + 1); + ng_hci_le_advreport *reports = + (ng_hci_le_advreport *)(advrep + 1); + + if (leer->subevent_code == NG_HCI_LEEV_ADVREP) { + fprintf(stdout, "Scan result, num_reports: %d\n", + advrep->num_reports); + for(rc = 0; rc < advrep->num_reports; rc++) { + uint8_t length = (uint8_t)reports[rc].length_data; + fprintf(stdout, "\tBD_ADDR %s \n", + hci_bdaddr2str(&reports[rc].bdaddr)); + fprintf(stdout, "\tAddress type: %s\n", + hci_addrtype2str(reports[rc].addr_type)); + if (length > 0 && verbose) { + dump_adv_data(length, reports[rc].data); + print_adv_data(length, reports[rc].data); + fprintf(stdout, + "\tRSSI: %d dBm\n", + (int8_t)reports[rc].data[length]); + fprintf(stdout, "\n"); + } + } + } +} + struct hci_command le_commands[] = { { "le_enable", @@ -685,4 +834,10 @@ struct hci_command le_commands[] = { "Read the maximum size of ACL and ISO data packets", &le_read_buffer_size }, + { + "le_scan", + "le_scan [-a] [-v] [-n number_of_scans]\n" + "Do an LE scan", + &le_scan + }, }; diff --git a/usr.sbin/bluetooth/hccontrol/node.c b/usr.sbin/bluetooth/hccontrol/node.c index b4c2a4cfa78e..2b41171bbd16 100644 --- a/usr.sbin/bluetooth/hccontrol/node.c +++ b/usr.sbin/bluetooth/hccontrol/node.c @@ -211,83 +211,6 @@ hci_flush_neighbor_cache(int s, int argc, char **argv) return (OK); } /* hci_flush_neighbor_cache */ -#define MIN(a,b) (((a)>(b)) ? (b) :(a) ) - -static int hci_dump_adv(uint8_t *data, int length) -{ - int elemlen; - int type; - int i; - - while(length>0){ - elemlen = *data; - data++; - length --; - if(length<=0) - break; - type = *data; - data++; - length --; - elemlen--; - if(length <= 0) - break; - switch(type){ - case 0x1: - printf("NDflag:%x\n", *data); - break; - case 0x8: - case 0x9: - printf("LocalName:"); - for(i = 0; i < MIN(length,elemlen); i++){ - putchar(data[i]); - } - printf("\n"); - break; - case 0x6: - case 0x7: - { - uuid_t uuid; - char *uuidstr; - uint32_t ustatus; - if (elemlen < 16) - break; - uuid.time_low = le32dec(data+12); - uuid.time_mid = le16dec(data+10); - uuid.time_hi_and_version = le16dec(data+8); - uuid.clock_seq_hi_and_reserved = data[7]; - uuid.clock_seq_low = data[6]; - for(i = 0; i < _UUID_NODE_LEN; i++){ - uuid.node[i] = data[5 - i]; - } - uuid_to_string(&uuid, &uuidstr, &ustatus); - - printf("ServiceUUID: %s\n", uuidstr); - break; - } - case 0xff: - if (elemlen < 2) - break; - printf("Vendor:%s:", - hci_manufacturer2str(data[0]|data[1]<<8)); - for (i = 2; i < MIN(length,elemlen); i++) { - printf("%02x ",data[i]); - } - printf("\n"); - break; - default: - printf("Type%d:", type); - for(i=0; i < MIN(length,elemlen); i++){ - printf("%02x ",data[i]); - } - printf("\n"); - break; - } - data += elemlen; - length -= elemlen; - } - return 0; -} -#undef MIN /* Send Read_Neighbor_Cache command to the node */ static int hci_read_neighbor_cache(int s, int argc, char **argv) @@ -337,8 +260,8 @@ hci_read_neighbor_cache(int s, int argc, char **argv) r.entries[n].features[6], r.entries[n].features[7], r.entries[n].clock_offset, r.entries[n].page_scan_mode, r.entries[n].page_scan_rep_mode); - hci_dump_adv(r.entries[n].extinq_data, - r.entries[n].extinq_size); + print_adv_data(r.entries[n].extinq_size, + r.entries[n].extinq_data); fprintf(stdout,"\n"); } out: diff --git a/usr.sbin/bluetooth/hccontrol/util.c b/usr.sbin/bluetooth/hccontrol/util.c index 22beedaff51f..2e6ecc78a5ac 100644 --- a/usr.sbin/bluetooth/hccontrol/util.c +++ b/usr.sbin/bluetooth/hccontrol/util.c @@ -3281,3 +3281,17 @@ hci_bdaddr2str(bdaddr_t const *ba) return (buffer); } /* hci_bdaddr2str */ + +char const * +hci_addrtype2str(int type) +{ + static char const * const t[] = { + /* 0x00 */ "Public Device Address", + /* 0x01 */ "Random Device Address", + /* 0x02 */ "Public Identity Address", + /* 0x03 */ "Random (static) Identity Address" + }; + + return (type >= SIZE(t)? "?" : t[type]); +} /* hci_addrtype2str */ +