Add le_scan subcommand to hccontrol.

PR: 246141
Submitted by:	Marc Veldman

7 files changed, 427 insertions, 81 deletions

diff --git a/usr.sbin/bluetooth/hccontrol/Makefile b/usr.sbin/bluetooth/hccontrol/Makefile
index cfe3d92842a2f..9191b656ff02a 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 0000000000000..8f11d99a397fb
--- /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 <marc@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;
+}
diff --git a/usr.sbin/bluetooth/hccontrol/hccontrol.8 b/usr.sbin/bluetooth/hccontrol/hccontrol.8
index 8e89f6d818235..ee35a3d0f3654 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 3758faa5c8f2b..9c69146652b5f 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 1b465585664ad..1a74e40c7191a 100644
--- a/usr.sbin/bluetooth/hccontrol/le.c
+++ b/usr.sbin/bluetooth/hccontrol/le.c
@@ -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",
@@ -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 b4c2a4cfa78ee..2b41171bbd163 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 22beedaff51f1..2e6ecc78a5aca 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 */
+