Attachment #214350 for bug #246141

Lines 8-14 Link Here

(-)usr.sbin/bluetooth/hccontrol/Makefile (-1 / +1 lines)
8	MAN= hccontrol.8	8	MAN= hccontrol.8
9	SRCS= send_recv.c link_policy.c link_control.c le.c\	9	SRCS= send_recv.c link_policy.c link_control.c le.c\
10	host_controller_baseband.c info.c status.c node.c hccontrol.c \	10	host_controller_baseband.c info.c status.c node.c hccontrol.c \
11	util.c	11	util.c adv_data.c
12	WARNS?= 2	12	WARNS?= 2
13		13
14	LIBADD= bluetooth	14	LIBADD= bluetooth




/*-
 * 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;
	}
	fprintf(stdout, "\tRSSI: %d dBm\n", (int8_t)advdata[n]);
	fprintf(stdout, "\n");
}

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;
}

Lines 25-31 Link Here

(-)usr.sbin/bluetooth/hccontrol/hccontrol.8 (-1 / +2 lines)
25	.\" $Id: hccontrol.8,v 1.6 2003/08/06 21:26:38 max Exp $	25	.\" $Id: hccontrol.8,v 1.6 2003/08/06 21:26:38 max Exp $
26	.\" $FreeBSD$	26	.\" $FreeBSD$
27	.\"	27	.\"
28	.Dd April 27, 2020	28	.Dd May 3, 2020
29	.Dt HCCONTROL 8	29	.Dt HCCONTROL 8
30	.Os	30	.Os
31	.Sh NAME	31	.Sh NAME
Lines 156-161 Link Here
156	.It Cm LE_Set_Scan_Enable	156	.It Cm LE_Set_Scan_Enable
157	.It Cm LE_Read_Supported_States	157	.It Cm LE_Read_Supported_States
158	.It Cm LE_Read_Buffer_Size	158	.It Cm LE_Read_Buffer_Size
		159	.It Cm LE Scan
159	.El	160	.El
160	.Pp	161	.Pp
161	The currently supported node commands in	162	The currently supported node commands in

Lines 79-84 Link Here

(-)usr.sbin/bluetooth/hccontrol/hccontrol.h (+4 lines)
79	char const * hci_con_state2str (int);	79	char const * hci_con_state2str (int);
80	char const * hci_status2str (int);	80	char const * hci_status2str (int);
81	char const * hci_bdaddr2str (bdaddr_t const *);	81	char const * hci_bdaddr2str (bdaddr_t const *);
		82	char const * hci_addrtype2str (int type);
82		83
		84	void dump_adv_data(int len, uint8_t* advdata);
		85	void print_adv_data(int len, uint8_t* advdata);
		86
83	#endif /* _HCCONTROL_H_ */	87	#endif /* _HCCONTROL_H_ */
84		88

Lines 39-44 Link Here

(-)usr.sbin/bluetooth/hccontrol/le.c (+158 lines)
39	#include <errno.h>	39	#include <errno.h>
40	#include <netgraph/ng_message.h>	40	#include <netgraph/ng_message.h>
41	#include <errno.h>	41	#include <errno.h>
		42	#include <stdbool.h>
42	#include <stdio.h>	43	#include <stdio.h>
43	#include <stdlib.h>	44	#include <stdlib.h>
44	#include <string.h>	45	#include <string.h>
Lines 60-65 Link Here
60	static int le_set_advertising_enable(int s, int argc, char *argv[]);	61	static int le_set_advertising_enable(int s, int argc, char *argv[]);
61	static int le_set_advertising_param(int s, int argc, char *argv[]);	62	static int le_set_advertising_param(int s, int argc, char *argv[]);
62	static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]);	63	static int le_read_advertising_channel_tx_power(int s, int argc, char *argv[]);
		64	static int le_scan(int s, int argc, char *argv[]);
		65	static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose);
63		66
64	static int	67	static int
65	le_set_scan_param(int s, int argc, char *argv[])	68	le_set_scan_param(int s, int argc, char *argv[])
Lines 613-618 Link Here
613	return (OK);	616	return (OK);
614	}	617	}
615		618
		619	static int
		620	le_scan(int s, int argc, char *argv[])
		621	{
		622	int n, bufsize, scancount, numscans;
		623	bool verbose;
		624	uint8_t active = 0;
		625	char ch;
		626
		627	char b[512];
		628	ng_hci_event_pkt_t e = (ng_hci_event_pkt_t ) b;
		629
		630	ng_hci_le_set_scan_parameters_cp scan_param_cp;
		631	ng_hci_le_set_scan_parameters_rp scan_param_rp;
		632
		633	ng_hci_le_set_scan_enable_cp scan_enable_cp;
		634	ng_hci_le_set_scan_enable_rp scan_enable_rp;
		635
		636	optreset = 1;
		637	optind = 0;
		638	verbose = false;
		639	numscans = 1;
		640
		641	while ((ch = getopt(argc, argv , "an:v")) != -1) {
		642	switch(ch) {
		643	case 'a':
		644	active = 1;
		645	break;
		646	case 'n':
		647	numscans = (uint8_t)strtol(optarg, NULL, 10);
		648	break;
		649	case 'v':
		650	verbose = true;
		651	break;
		652	}
		653	}
		654
		655	scan_param_cp.le_scan_type = active;
		656	scan_param_cp.le_scan_interval = (uint16_t)(100/0.625);
		657	scan_param_cp.le_scan_window = (uint16_t)(50/0.625);
		658	/* Address type public */
		659	scan_param_cp.own_address_type = 0;
		660	/* 'All' filter policy */
		661	scan_param_cp.scanning_filter_policy = 0;
		662	n = sizeof(scan_param_rp);
		663	if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
		664	NG_HCI_OCF_LE_SET_SCAN_PARAMETERS),
		665	(void *)&scan_param_cp, sizeof(scan_param_cp),
		666	(void *)&scan_param_rp, &n) == ERROR)
		667	return (ERROR);
		668
		669	if (scan_param_rp.status != 0x00) {
		670	fprintf(stdout, "LE_Set_Scan_Parameters failed. Status: %s [%#02x]\n",
		671	hci_status2str(scan_param_rp.status),
		672	scan_param_rp.status);
		673	return (FAILED);
		674	}
		675
		676	/* Enable scanning */
		677	n = sizeof(scan_enable_rp);
		678	scan_enable_cp.le_scan_enable = 1;
		679	scan_enable_cp.filter_duplicates = 1;
		680	if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
		681	NG_HCI_OCF_LE_SET_SCAN_ENABLE),
		682	(void *)&scan_enable_cp, sizeof(scan_enable_cp),
683	(void *)&scan_enable_rp, &n) == ERROR)
684	return (ERROR);
685
686	if (scan_enable_rp.status != 0x00) {
687	fprintf(stdout, "LE_Scan_Enable enable failed. Status: %s [%#02x]\n",
688	hci_status2str(scan_enable_rp.status),
689	scan_enable_rp.status);
690	return (FAILED);
691	}
692	/* Set event mask */
693	set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT \|
694	NG_HCI_EVENT_MASK_LE);
695	set_le_event_mask(s, NG_HCI_LE_EVENT_MASK_ALL);
696
697
698	scancount = 0;
699	while (scancount < numscans) {
700	/* wait for scan events */
701	bufsize = sizeof(b);
702	if (hci_recv(s, b, &bufsize) == ERROR) {
703	return (ERROR);
704	}
705
706	if (bufsize < sizeof(*e)) {
707	errno = EIO;
708	return (ERROR);
709	}
710	scancount++;
711	if (e->event == NG_HCI_EVENT_LE) {
712	fprintf(stdout, "Scan %d\n", scancount);
713	handle_le_event(e, verbose);
714	}
715	}
716
717	fprintf(stdout, "Scan complete\n");
718	/* Clear event mask */
719	set_event_mask(s, NG_HCI_EVENT_MASK_DEFAULT);
720
721
722	/* Disable scanning */
723	n = sizeof(scan_enable_rp);
724	scan_enable_cp.le_scan_enable = 0;
725	if (hci_request(s, NG_HCI_OPCODE(NG_HCI_OGF_LE,
726	NG_HCI_OCF_LE_SET_SCAN_ENABLE),
727	(void *)&scan_enable_cp, sizeof(scan_enable_cp),
728	(void *)&scan_enable_rp, &n) == ERROR)
729	return (ERROR);
730
731	if (scan_enable_rp.status != 0x00) {
732	fprintf(stdout, "LE_Scan_Enable disable failed. Status: %s [%#02x]\n",
733	hci_status2str(scan_enable_rp.status),
734	scan_enable_rp.status);
735	return (FAILED);
736	}
737
738	return (OK);
739	}
740
741	static void handle_le_event(ng_hci_event_pkt_t* e, bool verbose)
742	{
743	int rc;
744	ng_hci_le_ep *leer =
745	(ng_hci_le_ep *)(e + 1);
746	ng_hci_le_advertising_report_ep *advrep =
747	(ng_hci_le_advertising_report_ep *)(leer + 1);
748	ng_hci_le_advreport *reports =
749	(ng_hci_le_advreport *)(advrep + 1);
750
751	if (leer->subevent_code == NG_HCI_LEEV_ADVREP) {
752	fprintf(stdout, "Scan result, num_reports: %d\n",
753	advrep->num_reports);
754	for(rc = 0; rc < advrep->num_reports; rc++) {
755	uint8_t length = (uint8_t)reports[rc].length_data;
756	fprintf(stdout, "\tBD_ADDR %s \n",
757	hci_bdaddr2str(&reports[rc].bdaddr));
758	fprintf(stdout, "\tAddress type: %s\n",
759	hci_addrtype2str(reports[rc].addr_type));
760	if (length > 0 && verbose) {
761	dump_adv_data(length, reports[rc].data);
762	print_adv_data(length, reports[rc].data);
763	}
764	}
765	}
766	}
767
616	struct hci_command le_commands[] = {	768	struct hci_command le_commands[] = {
617	{	769	{
618	"le_enable",	770	"le_enable",
Lines 685-688 Link Here
685	"Read the maximum size of ACL and ISO data packets",	837	"Read the maximum size of ACL and ISO data packets",
686	&le_read_buffer_size	838	&le_read_buffer_size
687	},	839	},
		840	{
		841	"le_scan",
		842	"le_scan [-a] [-v] [-n number_of_scans]\n"
		843	"Do an LE scan",
		844	&le_scan
		845	},
688	};	846	};




	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)

			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);
		print_adv_data(r.entries[n].extinq_size,
			r.entries[n].extinq_data);
		fprintf(stdout,"\n");
	}
out:




	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 */


Return to bug 246141

Lines 211-293 Link Here

(-)usr.sbin/bluetooth/hccontrol/node.c (-79 / +2 lines)
211	return (OK);	211	return (OK);
212	} /* hci_flush_neighbor_cache */	212	} /* hci_flush_neighbor_cache */
213		213
214	#define MIN(a,b) (((a)>(b)) ? (b) :(a) )
215
216	static int hci_dump_adv(uint8_t *data, int length)
217	{
218	int elemlen;
219	int type;
220	int i;
221
222	while(length>0){
223	elemlen = *data;
224	data++;
225	length --;
226	if(length<=0)
227	break;
228	type = *data;
229	data++;
230	length --;
231	elemlen--;
232	if(length <= 0)
233	break;
234	switch(type){
235	case 0x1:
236	printf("NDflag:%x\n", *data);
237	break;
238	case 0x8:
239	case 0x9:
240	printf("LocalName:");
241	for(i = 0; i < MIN(length,elemlen); i++){
242	putchar(data[i]);
243	}
244	printf("\n");
245	break;
246	case 0x6:
247	case 0x7:
248	{
249	uuid_t uuid;
250	char *uuidstr;
251	uint32_t ustatus;
252	if (elemlen < 16)
253	break;
254	uuid.time_low = le32dec(data+12);
255	uuid.time_mid = le16dec(data+10);
256	uuid.time_hi_and_version = le16dec(data+8);
257	uuid.clock_seq_hi_and_reserved = data[7];
258	uuid.clock_seq_low = data[6];
259	for(i = 0; i < _UUID_NODE_LEN; i++){
260	uuid.node[i] = data[5 - i];
261	}
262	uuid_to_string(&uuid, &uuidstr, &ustatus);
263
264	printf("ServiceUUID: %s\n", uuidstr);
265	break;
266	}
267	case 0xff:
268	if (elemlen < 2)
269	break;
270	printf("Vendor:%s:",
271	hci_manufacturer2str(data[0]\|data[1]<<8));
272	for (i = 2; i < MIN(length,elemlen); i++) {
273	printf("%02x ",data[i]);
274	}
275	printf("\n");
276	break;
277	default:
278	printf("Type%d:", type);
279	for(i=0; i < MIN(length,elemlen); i++){
280	printf("%02x ",data[i]);
281	}
282	printf("\n");
283	break;
284	}
285	data += elemlen;
286	length -= elemlen;
287	}
288	return 0;
289	}
290	#undef MIN
291	/* Send Read_Neighbor_Cache command to the node */	214	/* Send Read_Neighbor_Cache command to the node */
292	static int	215	static int
293	hci_read_neighbor_cache(int s, int argc, char **argv)	216	hci_read_neighbor_cache(int s, int argc, char **argv)
Lines 337-344 Link Here
337	r.entries[n].features[6], r.entries[n].features[7],	260	r.entries[n].features[6], r.entries[n].features[7],
338	r.entries[n].clock_offset, r.entries[n].page_scan_mode,	261	r.entries[n].clock_offset, r.entries[n].page_scan_mode,
339	r.entries[n].page_scan_rep_mode);	262	r.entries[n].page_scan_rep_mode);
340	hci_dump_adv(r.entries[n].extinq_data,	263	print_adv_data(r.entries[n].extinq_size,
341	r.entries[n].extinq_size);	264	r.entries[n].extinq_data);
342	fprintf(stdout,"\n");	265	fprintf(stdout,"\n");
343	}	266	}
344	out:	267	out:

Lines 3281-3283 Link Here

(-)usr.sbin/bluetooth/hccontrol/util.c (+14 lines)
3281	return (buffer);	3281	return (buffer);
3282	} /* hci_bdaddr2str */	3282	} /* hci_bdaddr2str */
3283		3283
		3284
		3285	char const *
		3286	hci_addrtype2str(int type)
		3287	{
		3288	static char const * const t[] = {
		3289	/* 0x00 */ "Public Device Address",
		3290	/* 0x01 */ "Random Device Address",
		3291	/* 0x02 */ "Public Identity Address",
		3292	/* 0x03 */ "Random (static) Identity Address"
		3293	};
		3294
		3295	return (type >= SIZE(t)? "?" : t[type]);
		3296	} /* hci_addrtype2str */
		3297

Line 0 Link Here

(-)usr.sbin/bluetooth/hccontrol/adv_data.c (+251 lines)
		1	/*-
		2	* adv_data.c
		3	*
		4	* SPDX-License-Identifier: BSD-2-Clause-FreeBSD
		5
		6	* Copyright (c) 2020 Marc Veldman <marc@bumblingdork.com>
		7	* All rights reserved.
		8	*
		9	* Redistribution and use in source and binary forms, with or without
		10	* modification, are permitted provided that the following conditions
		11	* are met:
		12	* 1. Redistributions of source code must retain the above copyright
		13	* notice, this list of conditions and the following disclaimer.
		14	* 2. Redistributions in binary form must reproduce the above copyright
		15	* notice, this list of conditions and the following disclaimer in the
		16	* documentation and/or other materials provided with the distribution.
		17	*
		18	* THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
		19	* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
		20	* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
		21	* ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
		22	* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
		23	* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
		24	* OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
		25	* HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
		26	* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
		27	* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
		28	* SUCH DAMAGE.
		29	*
		30	* $Id$
		31	* $FreeBSD$
		32	*/
		33
		34	#include <sys/types.h>
		35	#include <stdio.h>
		36	#include <string.h>
		37	#include <uuid.h>
		38	#define L2CAP_SOCKET_CHECKED
		39	#include <bluetooth.h>
		40	#include "hccontrol.h"
		41
		42	static char* const adv_data2str(int len, uint8_t* data, char* buffer,
		43	int size);
		44	static char* const adv_name2str(int len, uint8_t* advdata, char* buffer,
		45	int size);
		46	static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer,
		47	int size);
		48
		49	void dump_adv_data(int len, uint8_t* advdata)
		50	{
		51	int n=0;
		52	fprintf(stdout, "\tADV Data: ");
		53	for (n = 0; n < len+1; n++) {
		54	fprintf(stdout, "%02x ", advdata[n]);
		55	}
		56	fprintf(stdout, "\n");
		57	}
		58
		59	void print_adv_data(int len, uint8_t* advdata)
		60	{
		61	int n=0;
		62	while(n < len)
		63	{
		64	char buffer[2048];
65	uint8_t datalen = advdata[n];
66	uint8_t datatype = advdata[++n];
67	/* Skip type */
68	++n;
69	datalen--;
70	switch (datatype) {
71	case 0x01:
72	fprintf(stdout,
73	"\tFlags: %s\n",
74	adv_data2str(
75	datalen,
76	&advdata[n],
77	buffer,
78	sizeof(buffer)));
79	break;
80	case 0x02:
81	fprintf(stdout,
82	"\tIncomplete list of service"
83	" class UUIDs (16-bit): %s\n",
84	adv_data2str(
85	datalen,
86	&advdata[n],
87	buffer,
88	sizeof(buffer)));
89	break;
90	case 0x03:
91	fprintf(stdout,
92	"\tComplete list of service "
93	"class UUIDs (16-bit): %s\n",
94	adv_data2str(
95	datalen,
96	&advdata[n],
97	buffer,
98	sizeof(buffer)));
99	break;
100	case 0x07:
101	fprintf(stdout,
102	"\tComplete list of service "
103	"class UUIDs (128 bit): %s\n",
104	adv_uuid2str(
105	datalen,
106	&advdata[n],
107	buffer,
108	sizeof(buffer)));
109	break;
110	case 0x08:
111	fprintf(stdout,
112	"\tShortened local name: %s\n",
113	adv_name2str(
114	datalen,
115	&advdata[n],
116	buffer,
117	sizeof(buffer)));
118	break;
119	case 0x09:
120	fprintf(stdout,
121	"\tComplete local name: %s\n",
122	adv_name2str(
123	datalen,
124	&advdata[n],
125	buffer,
126	sizeof(buffer)));
127	break;
128	case 0x0a:
129	fprintf(stdout,
130	"\tTx Power level: %d dBm\n",
131	(int8_t)advdata[n]);
132	break;
133	case 0x0d:
134	fprintf(stdout,
135	"\tClass of device: %s\n",
136	adv_data2str(
137	datalen,
138	&advdata[n],
139	buffer,
140	sizeof(buffer)));
141	break;
142	case 0x16:
143	fprintf(stdout,
144	"\tService data: %s\n",
145	adv_data2str(
146	datalen,
147	&advdata[n],
148	buffer,
149	sizeof(buffer)));
150	break;
151	case 0x19:
152	fprintf(stdout,
153	"\tAppearance: %s\n",
154	adv_data2str(
155	datalen,
156	&advdata[n],
157	buffer,
158	sizeof(buffer)));
159	break;
160	case 0xff:
161	fprintf(stdout,
162	"\tManufacturer: %s\n",
163	hci_manufacturer2str(
164	advdata[n]\|advdata[n+1]<<8));
165	fprintf(stdout,
166	"\tManufacturer specific data: %s\n",
167	adv_data2str(
168	datalen-2,
169	&advdata[n+2],
170	buffer,
171	sizeof(buffer)));
172	break;
173	default:
174	fprintf(stdout,
175	"\tUNKNOWN datatype: %02x data %s\n",
176	datatype,
177	adv_data2str(
178	datalen,
179	&advdata[n],
180	buffer,
181	sizeof(buffer)));
182	}
183	n += datalen;
184	}
185	fprintf(stdout, "\tRSSI: %d dBm\n", (int8_t)advdata[n]);
186	fprintf(stdout, "\n");
187	}
188
189	static char* const adv_data2str(int datalen, uint8_t* data, char* buffer,
190	int size)
191	{
192	int i = 0;
193	char tmpbuf[5];
194
195	if (buffer == NULL)
196	return NULL;
197
198	memset(buffer, 0, size);
199
200	while(i < datalen) {
201	(void)snprintf(tmpbuf, sizeof(tmpbuf), "%02x ", data[i]);
202	/* Check if buffer is full */
203	if (strlcat(buffer, tmpbuf, size) > size)
204	break;
205	i++;
206	}
207	return buffer;
208	}
209
210	static char* const adv_name2str(int datalen, uint8_t* data, char* buffer,
211	int size)
212	{
213	if (buffer == NULL)
214	return NULL;
215
216	memset(buffer, 0, size);
217
218	(void)strlcpy(buffer, (char*)data, datalen+1);
219	return buffer;
220	}
221
222	static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer,
223	int size)
224	{
225	int i;
226	uuid_t uuid;
227	uint32_t ustatus;
228	char* tmpstr;
229
230	if (buffer == NULL)
231	return NULL;
232
233	memset(buffer, 0, size);
234	if (datalen < 16)
235	return buffer;
236	uuid.time_low = le32dec(data+12);
237	uuid.time_mid = le16dec(data+10);
238	uuid.time_hi_and_version = le16dec(data+8);
239	uuid.clock_seq_hi_and_reserved = data[7];
240	uuid.clock_seq_low = data[6];
241	for(i = 0; i < _UUID_NODE_LEN; i++){
242	uuid.node[i] = data[5 - i];
243	}
244	uuid_to_string(&uuid, &tmpstr, &ustatus);
245	if(ustatus == uuid_s_ok) {
246	strlcpy(buffer, tmpstr, size);
247	}
248	free(tmpstr);
249
250	return buffer;
251	}