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