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
42static char* const adv_data2str(int len, uint8_t* data, char* buffer,
43	int size);
44static char* const adv_name2str(int len, uint8_t* advdata, char* buffer,
45	int size);
46static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer,
47	int size);
48
49void 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
59void 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}
186
187static char* const adv_data2str(int datalen, uint8_t* data, char* buffer,
188	int size)
189{
190        int i = 0;
191	char tmpbuf[5];
192
193	if (buffer == NULL)
194		return NULL;
195
196	memset(buffer, 0, size);
197
198	while(i < datalen) {
199		(void)snprintf(tmpbuf, sizeof(tmpbuf), "%02x ", data[i]);
200		/* Check if buffer is full */
201		if (strlcat(buffer, tmpbuf, size) > size)
202			break;
203		i++;
204	}
205	return buffer;
206}
207
208static char* const adv_name2str(int datalen, uint8_t* data, char* buffer,
209	int size)
210{
211	if (buffer == NULL)
212		return NULL;
213
214	memset(buffer, 0, size);
215
216	(void)strlcpy(buffer, (char*)data, datalen+1);
217	return buffer;
218}
219
220static char* const adv_uuid2str(int datalen, uint8_t* data, char* buffer,
221	int size)
222{
223	int i;
224	uuid_t uuid;
225	uint32_t ustatus;
226	char* tmpstr;
227
228	if (buffer == NULL)
229		return NULL;
230
231	memset(buffer, 0, size);
232	if (datalen < 16)
233		return buffer;
234	uuid.time_low = le32dec(data+12);
235	uuid.time_mid = le16dec(data+10);
236	uuid.time_hi_and_version = le16dec(data+8);
237	uuid.clock_seq_hi_and_reserved = data[7];
238	uuid.clock_seq_low = data[6];
239	for(i = 0; i < _UUID_NODE_LEN; i++){
240		uuid.node[i] = data[5 - i];
241	}
242	uuid_to_string(&uuid, &tmpstr, &ustatus);
243	if(ustatus == uuid_s_ok) {
244		strlcpy(buffer, tmpstr, size);
245	}
246	free(tmpstr);
247
248	return buffer;
249}
250