svn commit: r360899 - head/usr.sbin/bluetooth/hccontrol
Takanori Watanabe
takawata at FreeBSD.org
Mon May 11 15:32:34 UTC 2020
Author: takawata
Date: Mon May 11 15:32:32 2020
New Revision: 360899
URL: https://svnweb.freebsd.org/changeset/base/360899
Log:
Add le_scan subcommand to hccontrol.
PR: 246141
Submitted by: Marc Veldman
Added:
head/usr.sbin/bluetooth/hccontrol/adv_data.c (contents, props changed)
Modified:
head/usr.sbin/bluetooth/hccontrol/Makefile
head/usr.sbin/bluetooth/hccontrol/hccontrol.8
head/usr.sbin/bluetooth/hccontrol/hccontrol.h
head/usr.sbin/bluetooth/hccontrol/le.c
head/usr.sbin/bluetooth/hccontrol/node.c
head/usr.sbin/bluetooth/hccontrol/util.c
Modified: head/usr.sbin/bluetooth/hccontrol/Makefile
==============================================================================
--- head/usr.sbin/bluetooth/hccontrol/Makefile Mon May 11 15:21:03 2020 (r360898)
+++ head/usr.sbin/bluetooth/hccontrol/Makefile Mon May 11 15:32:32 2020 (r360899)
@@ -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
Added: head/usr.sbin/bluetooth/hccontrol/adv_data.c
==============================================================================
--- /dev/null 00:00:00 1970 (empty, because file is newly added)
+++ head/usr.sbin/bluetooth/hccontrol/adv_data.c Mon May 11 15:32:32 2020 (r360899)
@@ -0,0 +1,249 @@
+/*-
+ * adv_data.c
+ *
+ * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
+
+ * Copyright (c) 2020 Marc Veldman <marc at bumblingdork.com>
+ * 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 <sys/types.h>
+#include <stdio.h>
+#include <string.h>
+#include <uuid.h>
+#define L2CAP_SOCKET_CHECKED
+#include <bluetooth.h>
+#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;
+}
Modified: head/usr.sbin/bluetooth/hccontrol/hccontrol.8
==============================================================================
--- head/usr.sbin/bluetooth/hccontrol/hccontrol.8 Mon May 11 15:21:03 2020 (r360898)
+++ head/usr.sbin/bluetooth/hccontrol/hccontrol.8 Mon May 11 15:32:32 2020 (r360899)
@@ -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
Modified: head/usr.sbin/bluetooth/hccontrol/hccontrol.h
==============================================================================
--- head/usr.sbin/bluetooth/hccontrol/hccontrol.h Mon May 11 15:21:03 2020 (r360898)
+++ head/usr.sbin/bluetooth/hccontrol/hccontrol.h Mon May 11 15:32:32 2020 (r360899)
@@ -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_ */
Modified: head/usr.sbin/bluetooth/hccontrol/le.c
==============================================================================
--- head/usr.sbin/bluetooth/hccontrol/le.c Mon May 11 15:21:03 2020 (r360898)
+++ head/usr.sbin/bluetooth/hccontrol/le.c Mon May 11 15:32:32 2020 (r360899)
@@ -39,6 +39,7 @@
#include <errno.h>
#include <netgraph/ng_message.h>
#include <errno.h>
+#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@@ -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",
@@ -684,5 +833,11 @@ struct hci_command le_commands[] = {
"le_read_buffer_size [-v 1|2]\n"
"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
},
};
Modified: head/usr.sbin/bluetooth/hccontrol/node.c
==============================================================================
--- head/usr.sbin/bluetooth/hccontrol/node.c Mon May 11 15:21:03 2020 (r360898)
+++ head/usr.sbin/bluetooth/hccontrol/node.c Mon May 11 15:32:32 2020 (r360899)
@@ -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:
Modified: head/usr.sbin/bluetooth/hccontrol/util.c
==============================================================================
--- head/usr.sbin/bluetooth/hccontrol/util.c Mon May 11 15:21:03 2020 (r360898)
+++ head/usr.sbin/bluetooth/hccontrol/util.c Mon May 11 15:32:32 2020 (r360899)
@@ -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 */
+
More information about the svn-src-head
mailing list