1 /* -*- c -*- */ 2 /* 3 * Copyright 2007 - 2012 Mike Ryan, Dominic Spill, Michael Ossmann 4 * Copyright 2005, 2006 Free Software Foundation, Inc. 5 * 6 * This file is part of libbtbb 7 * 8 * This program is free software; you can redistribute it and/or modify 9 * it under the terms of the GNU General Public License as published by 10 * the Free Software Foundation; either version 2, or (at your option) 11 * any later version. 12 * 13 * This program is distributed in the hope that it will be useful, 14 * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 * GNU General Public License for more details. 17 * 18 * You should have received a copy of the GNU General Public License 19 * along with libbtbb; see the file COPYING. If not, write to 20 * the Free Software Foundation, Inc., 51 Franklin Street, 21 * Boston, MA 02110-1301, USA. 22 */ 23 24 #ifdef HAVE_CONFIG_H 25 #include "config.h" 26 #endif 27 28 #include <string.h> 29 #include "bluetooth_le_packet.h" 30 #include <ctype.h> 31 32 /* string representations of advertising packet type */ 33 static const char *ADV_TYPE_NAMES[] = { 34 "ADV_IND", "ADV_DIRECT_IND", "ADV_NONCONN_IND", "SCAN_REQ", 35 "SCAN_RSP", "CONNECT_REQ", "ADV_SCAN_IND", 36 }; 37 38 /* source clock accuracy in a connect packet */ 39 static const char *CONNECT_SCA[] = { 40 "251 ppm to 500 ppm", "151 ppm to 250 ppm", "101 ppm to 150 ppm", 41 "76 ppm to 100 ppm", "51 ppm to 75 ppm", "31 ppm to 50 ppm", 42 "21 ppm to 30 ppm", "0 ppm to 20 ppm", 43 }; 44 45 // count of objects in an array, shamelessly stolen from Chrome 46 #define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x]))))) 47 48 void decode_le(uint8_t *stream, uint16_t phys_channel, uint32_t clk100ns, le_packet_t *p) { 49 memcpy(p->symbols, stream, MAX_LE_SYMBOLS); 50 51 p->channel_idx = le_channel_index(phys_channel); 52 p->clk100ns = clk100ns; 53 54 p->access_address = 0; 55 p->access_address |= p->symbols[0]; 56 p->access_address |= p->symbols[1] << 8; 57 p->access_address |= p->symbols[2] << 16; 58 p->access_address |= p->symbols[3] << 24; 59 60 if (le_packet_is_data(p)) { 61 // data PDU 62 p->length = p->symbols[5] & 0x1f; 63 } else { 64 // advertising PDU 65 p->length = p->symbols[5] & 0x3f; 66 p->adv_type = p->symbols[4] & 0xf; 67 p->adv_tx_add = p->symbols[4] & 0x40 ? 1 : 0; 68 p->adv_rx_add = p->symbols[4] & 0x80 ? 1 : 0; 69 } 70 } 71 72 int le_packet_is_data(le_packet_t *p) { 73 return p->channel_idx < 37; 74 } 75 76 uint8_t le_channel_index(uint16_t phys_channel) { 77 uint8_t ret; 78 if (phys_channel == 2402) { 79 ret = 37; 80 } else if (phys_channel < 2426) { // 0 - 10 81 ret = (phys_channel - 2404) / 2; 82 } else if (phys_channel == 2426) { 83 ret = 38; 84 } else if (phys_channel < 2480) { // 11 - 36 85 ret = 11 + (phys_channel - 2428) / 2; 86 } else { 87 ret = 39; 88 } 89 return ret; 90 } 91 92 const char *le_adv_type(le_packet_t *p) { 93 if (le_packet_is_data(p)) 94 return NULL; 95 if (p->adv_type < COUNT_OF(ADV_TYPE_NAMES)) 96 return ADV_TYPE_NAMES[p->adv_type]; 97 return "UNKNOWN"; 98 } 99 100 static void _dump_addr(char *name, uint8_t *buf, int offset, int random) { 101 int i; 102 printf(" %s%02x", name, buf[offset+5]); 103 for (i = 4; i >= 0; --i) 104 printf(":%02x", buf[offset+i]); 105 printf(" (%s)\n", random ? "random" : "public"); 106 } 107 108 static void _dump_8(char *name, uint8_t *buf, int offset) { 109 printf(" %s%02x (%d)\n", name, buf[offset], buf[offset]); 110 } 111 112 static void _dump_16(char *name, uint8_t *buf, int offset) { 113 uint16_t val = buf[offset+1] << 8 | buf[offset]; 114 printf(" %s%04x (%d)\n", name, val, val); 115 } 116 117 static void _dump_24(char *name, uint8_t *buf, int offset) { 118 uint16_t val = buf[offset+2] << 16 | buf[offset+1] << 8 | buf[offset]; 119 printf(" %s%06x\n", name, val); 120 } 121 122 static void _dump_32(char *name, uint8_t *buf, int offset) { 123 uint32_t val = buf[offset+3] << 24 | 124 buf[offset+2] << 16 | 125 buf[offset+1] << 8 | 126 buf[offset+0]; 127 printf(" %s%08x\n", name, val); 128 } 129 130 static void _dump_uuid(uint8_t *uuid) { 131 int i; 132 for (i = 0; i < 4; ++i) 133 printf("%02x", uuid[i]); 134 printf("-"); 135 for (i = 4; i < 6; ++i) 136 printf("%02x", uuid[i]); 137 printf("-"); 138 for (i = 6; i < 8; ++i) 139 printf("%02x", uuid[i]); 140 printf("-"); 141 for (i = 8; i < 10; ++i) 142 printf("%02x", uuid[i]); 143 printf("-"); 144 for (i = 10; i < 16; ++i) 145 printf("%02x", uuid[i]); 146 } 147 148 // Refer to pg 1735 of Bluetooth Core Spec 4.0 149 static void _dump_scan_rsp_data(uint8_t *buf, int len) { 150 int pos = 0; 151 int sublen, i; 152 uint8_t type; 153 uint16_t val; 154 char *cval; 155 156 while (pos < len) { 157 sublen = buf[pos]; 158 ++pos; 159 if (pos + sublen > len) { 160 printf("Error: attempt to read past end of buffer (%d + %d > %d)\n", pos, sublen, len); 161 return; 162 } 163 if (sublen == 0) { 164 printf("Early return due to 0 length\n"); 165 return; 166 } 167 type = buf[pos]; 168 printf(" Type %02x", type); 169 switch (type) { 170 case 0x01: 171 printf(" (Flags)\n"); 172 printf(" "); 173 for (i = 0; i < 8; ++i) 174 printf("%d", buf[pos+1] & (1 << (7-i)) ? 1 : 0); 175 printf("\n"); 176 break; 177 case 0x06: 178 printf(" (128-bit Service UUIDs, more available)\n"); 179 goto print128; 180 case 0x07: 181 printf(" (128-bit Service UUIDs)\n"); 182 print128: 183 if ((sublen - 1) % 16 == 0) { 184 uint8_t uuid[16]; 185 for (i = 0; i < sublen - 1; ++i) { 186 uuid[15 - (i % 16)] = buf[pos+1+i]; 187 if ((i & 15) == 15) { 188 printf(" "); 189 _dump_uuid(uuid); 190 printf("\n"); 191 } 192 } 193 } 194 else { 195 printf("Wrong length (%d, must be divisible by 16)\n", sublen-1); 196 } 197 break; 198 case 0x09: 199 printf(" (Complete Local Name)\n"); 200 printf(" "); 201 for (i = 1; i < sublen; ++i) 202 printf("%c", isprint(buf[pos+i]) ? buf[pos+i] : '.'); 203 printf("\n"); 204 break; 205 case 0x0a: 206 printf(" (Tx Power Level)\n"); 207 printf(" "); 208 if (sublen-1 == 1) { 209 cval = (char *)&buf[pos+1]; 210 printf("%d dBm\n", *cval); 211 } else { 212 printf("Wrong length (%d, should be 1)\n", sublen-1); 213 } 214 break; 215 case 0x12: 216 printf(" (Slave Connection Interval Range)\n"); 217 printf(" "); 218 if (sublen-1 == 4) { 219 val = (buf[pos+2] << 8) | buf[pos+1]; 220 printf("(%0.2f, ", val * 1.25); 221 val = (buf[pos+4] << 8) | buf[pos+3]; 222 printf("%0.2f) ms\n", val * 1.25); 223 } 224 else { 225 printf("Wrong length (%d, should be 4)\n", sublen-1); 226 } 227 break; 228 case 0x16: 229 printf(" (Service Data)\n"); 230 printf(" "); 231 if (sublen-1 >= 2) { 232 val = (buf[pos+2] << 8) | buf[pos+1]; 233 printf("UUID: %02x", val); 234 if (sublen-1 > 2) { 235 printf(", Additional:"); 236 for (i = 3; i < sublen; ++i) 237 printf(" %02x", buf[pos+i]); 238 } 239 printf("\n"); 240 } 241 else { 242 printf("Wrong length (%d, should be >= 2)\n", sublen-1); 243 } 244 break; 245 default: 246 printf("\n"); 247 printf(" "); 248 for (i = 1; i < sublen; ++i) 249 printf(" %02x", buf[pos+i]); 250 printf("\n"); 251 } 252 pos += sublen; 253 } 254 } 255 256 void le_print(le_packet_t *p) { 257 int i; 258 if (le_packet_is_data(p)) { 259 int llid = p->symbols[4] & 0x3; 260 static const char *llid_str[] = { 261 "Reserved", 262 "LL Data PDU / empty or L2CAP continuation", 263 "LL Data PDU / L2CAP start", 264 "LL Control PDU", 265 }; 266 267 printf("Data / AA %08x / %2d bytes\n", p->access_address, p->length); 268 printf(" Channel Index: %d\n", p->channel_idx); 269 printf(" LLID: %d / %s\n", llid, llid_str[llid]); 270 printf(" NESN: %d SN: %d MD: %d\n", (p->symbols[4] >> 2) & 1, 271 (p->symbols[4] >> 3) & 1, 272 (p->symbols[4] >> 4) & 1); 273 } else { 274 printf("Advertising / AA %08x / %2d bytes\n", p->access_address, p->length); 275 printf(" Channel Index: %d\n", p->channel_idx); 276 printf(" Type: %s\n", le_adv_type(p)); 277 278 switch(p->adv_type) { 279 case ADV_IND: 280 _dump_addr("AdvA: ", p->symbols, 6, p->adv_tx_add); 281 if (p->length-6 > 0) { 282 printf(" AdvData:"); 283 for (i = 0; i < p->length - 6; ++i) 284 printf(" %02x", p->symbols[12+i]); 285 printf("\n"); 286 _dump_scan_rsp_data(&p->symbols[12], p->length-6); 287 } 288 break; 289 case SCAN_REQ: 290 _dump_addr("ScanA: ", p->symbols, 6, p->adv_tx_add); 291 _dump_addr("AdvA: ", p->symbols, 12, p->adv_rx_add); 292 break; 293 case SCAN_RSP: 294 _dump_addr("AdvA: ", p->symbols, 6, p->adv_tx_add); 295 printf(" ScanRspData:"); 296 for (i = 0; i < p->length - 6; ++i) 297 printf(" %02x", p->symbols[12+i]); 298 printf("\n"); 299 _dump_scan_rsp_data(&p->symbols[12], p->length-6); 300 break; 301 case CONNECT_REQ: 302 _dump_addr("InitA: ", p->symbols, 6, p->adv_tx_add); 303 _dump_addr("AdvA: ", p->symbols, 12, p->adv_rx_add); 304 _dump_32("AA: ", p->symbols, 18); 305 _dump_24("CRCInit: ", p->symbols, 22); 306 _dump_8("WinSize: ", p->symbols, 25); 307 _dump_16("WinOffset: ", p->symbols, 26); 308 _dump_16("Interval: ", p->symbols, 28); 309 _dump_16("Latency: ", p->symbols, 30); 310 _dump_16("Timeout: ", p->symbols, 32); 311 312 printf(" ChM:"); 313 for (i = 0; i < 5; ++i) 314 printf(" %02x", p->symbols[34+i]); 315 printf("\n"); 316 317 printf(" Hop: %d\n", p->symbols[37] & 0x1f); 318 printf(" SCA: %d, %s\n", 319 p->symbols[37] >> 5, 320 CONNECT_SCA[p->symbols[37] >> 5]); 321 break; 322 } 323 } 324 325 printf("\n"); 326 printf(" Data: "); 327 for (i = 6; i < 6 + p->length; ++i) 328 printf(" %02x", p->symbols[i]); 329 printf("\n"); 330 331 printf(" CRC: "); 332 for (i = 0; i < 3; ++i) 333 printf(" %02x", p->symbols[6 + p->length + i]); 334 printf("\n"); 335 } 336