1 /*-
2 * adv_data.c
3 *
4 * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
5
6 * Copyright (c) 2020 Marc Veldman <[email protected]>
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
dump_adv_data(int len,uint8_t * advdata)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
print_adv_data(int len,uint8_t * advdata)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 }
186
adv_data2str(int datalen,uint8_t * data,char * buffer,int size)187 static 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
adv_name2str(int datalen,uint8_t * data,char * buffer,int size)208 static 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
adv_uuid2str(int datalen,uint8_t * data,char * buffer,int size)220 static 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