Add le_scan subcommand to hccontrol.
PR: 246141 Submitted by: Marc Veldman
This commit is contained in:
parent
87c5e64c64
commit
9287f06d08
Notes:
svn2git
2020-12-20 02:59:44 +00:00
svn path=/head/; revision=360899
@ -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
|
||||
|
249
usr.sbin/bluetooth/hccontrol/adv_data.c
Normal file
249
usr.sbin/bluetooth/hccontrol/adv_data.c
Normal file
@ -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;
|
||||
}
|
@ -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
|
||||
|
@ -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_ */
|
||||
|
||||
|
@ -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
|
||||
},
|
||||
};
|
||||
|
@ -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:
|
||||
|
@ -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 */
|
||||
|
||||
|
Loading…
Reference in New Issue
Block a user