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 "btbb.h" 29 #include "bluetooth_le_packet.h" 30 #include <ctype.h> 31 #include <string.h> 32 33 /* company identifier lookup */ 34 const char *bt_compidtostr(uint16_t compid); 35 36 /* string representations of advertising packet type */ 37 static const char *ADV_TYPE_NAMES[] = { 38 "ADV_IND", "ADV_DIRECT_IND", "ADV_NONCONN_IND", "SCAN_REQ", 39 "SCAN_RSP", "CONNECT_REQ", "ADV_SCAN_IND", 40 }; 41 42 /* source clock accuracy in a connect packet */ 43 static const char *CONNECT_SCA[] = { 44 "251 ppm to 500 ppm", "151 ppm to 250 ppm", "101 ppm to 150 ppm", 45 "76 ppm to 100 ppm", "51 ppm to 75 ppm", "31 ppm to 50 ppm", 46 "21 ppm to 30 ppm", "0 ppm to 20 ppm", 47 }; 48 49 /* flags */ 50 static const char *FLAGS[] = { 51 "LE Limited Discoverable Mode", "LE General Discoverable Mode", 52 "BR/EDR Not Supported", 53 "Simultaneous LE and BR/EDR to Same Device Capable (Controller)", 54 "Simultaneous LE and BR/EDR to Same Device Capable (Host)", 55 "Reserved", "Reserved", "Reserved", 56 }; 57 58 // count of objects in an array, shamelessly stolen from Chrome 59 #define COUNT_OF(x) ((sizeof(x)/sizeof(0[x])) / ((size_t)(!(sizeof(x) % sizeof(0[x]))))) 60 61 static uint8_t count_bits(uint32_t n) 62 { 63 uint8_t i = 0; 64 for (i = 0; n != 0; i++) 65 n &= n - 1; 66 return i; 67 } 68 69 static int aa_access_channel_off_by_one(const uint32_t aa) { 70 int retval = 0; 71 if(count_bits(aa ^ LE_ADV_AA) == 1) { 72 retval = 1; 73 } 74 return retval; 75 } 76 77 /* 78 * A helper function for filtering bogus packets on data channels. 79 * 80 * If a candidate capture packet is random noise we would expect its 81 * Access Address to be a randomly distributed 32-bit number. An 82 * exhaustive software analysis reveals that of 4294967296 possible 83 * 32-bit Access Address values, 2900629660 (67.5%) are acceptable and 84 * 1394337636 (32.5%) are invalid. This function will identify which 85 * category a candidate Access Address falls into by returning the 86 * number of offenses contained. 87 * 88 * Refer to BT 4.x, Vol 6, Par B, Section 2.1.2. 89 * 90 * The Access Address in data channel packets meet the 91 * following requirements: 92 * - It shall have no more than six consecutive zeros or ones. 93 * - It shall not be the advertising channel packets’ Access Address. 94 * - It shall not be a sequence that differs from the advertising channel packets’ 95 * Access Address by only one bit. 96 * - It shall not have all four octets equal. 97 * - It shall have no more than 24 transitions. 98 * - It shall have a minimum of two transitions in the most significant six bits. 99 */ 100 static int aa_data_channel_offenses(const uint32_t aa) { 101 int retval = 0, transitions = 0; 102 unsigned shift, odd = (unsigned) (aa & 1); 103 uint8_t aab3, aab2, aab1, aab0 = (uint8_t) (aa & 0xff); 104 105 const uint8_t EIGHT_BIT_TRANSITIONS_EVEN[256] = { 106 0, 2, 2, 2, 2, 4, 2, 2, 2, 4, 4, 4, 2, 4, 2, 2, 107 2, 4, 4, 4, 4, 6, 4, 4, 2, 4, 4, 4, 2, 4, 2, 2, 108 2, 4, 4, 4, 4, 6, 4, 4, 4, 6, 6, 6, 4, 6, 4, 4, 109 2, 4, 4, 4, 4, 6, 4, 4, 2, 4, 4, 4, 2, 4, 2, 2, 110 2, 4, 4, 4, 4, 6, 4, 4, 4, 6, 6, 6, 4, 6, 4, 4, 111 4, 6, 6, 6, 6, 8, 6, 6, 4, 6, 6, 6, 4, 6, 4, 4, 112 2, 4, 4, 4, 4, 6, 4, 4, 4, 6, 6, 6, 4, 6, 4, 4, 113 2, 4, 4, 4, 4, 6, 4, 4, 2, 4, 4, 4, 2, 4, 2, 2, 114 1, 3, 3, 3, 3, 5, 3, 3, 3, 5, 5, 5, 3, 5, 3, 3, 115 3, 5, 5, 5, 5, 7, 5, 5, 3, 5, 5, 5, 3, 5, 3, 3, 116 3, 5, 5, 5, 5, 7, 5, 5, 5, 7, 7, 7, 5, 7, 5, 5, 117 3, 5, 5, 5, 5, 7, 5, 5, 3, 5, 5, 5, 3, 5, 3, 3, 118 1, 3, 3, 3, 3, 5, 3, 3, 3, 5, 5, 5, 3, 5, 3, 3, 119 3, 5, 5, 5, 5, 7, 5, 5, 3, 5, 5, 5, 3, 5, 3, 3, 120 1, 3, 3, 3, 3, 5, 3, 3, 3, 5, 5, 5, 3, 5, 3, 3, 121 1, 3, 3, 3, 3, 5, 3, 3, 1, 3, 3, 3, 1, 3, 1, 1 122 }; 123 124 const uint8_t EIGHT_BIT_TRANSITIONS_ODD[256] = { 125 1, 1, 3, 1, 3, 3, 3, 1, 3, 3, 5, 3, 3, 3, 3, 1, 126 3, 3, 5, 3, 5, 5, 5, 3, 3, 3, 5, 3, 3, 3, 3, 1, 127 3, 3, 5, 3, 5, 5, 5, 3, 5, 5, 7, 5, 5, 5, 5, 3, 128 3, 3, 5, 3, 5, 5, 5, 3, 3, 3, 5, 3, 3, 3, 3, 1, 129 3, 3, 5, 3, 5, 5, 5, 3, 5, 5, 7, 5, 5, 5, 5, 3, 130 5, 5, 7, 5, 7, 7, 7, 5, 5, 5, 7, 5, 5, 5, 5, 3, 131 3, 3, 5, 3, 5, 5, 5, 3, 5, 5, 7, 5, 5, 5, 5, 3, 132 3, 3, 5, 3, 5, 5, 5, 3, 3, 3, 5, 3, 3, 3, 3, 1, 133 2, 2, 4, 2, 4, 4, 4, 2, 4, 4, 6, 4, 4, 4, 4, 2, 134 4, 4, 6, 4, 6, 6, 6, 4, 4, 4, 6, 4, 4, 4, 4, 2, 135 4, 4, 6, 4, 6, 6, 6, 4, 6, 6, 8, 6, 6, 6, 6, 4, 136 4, 4, 6, 4, 6, 6, 6, 4, 4, 4, 6, 4, 4, 4, 4, 2, 137 2, 2, 4, 2, 4, 4, 4, 2, 4, 4, 6, 4, 4, 4, 4, 2, 138 4, 4, 6, 4, 6, 6, 6, 4, 4, 4, 6, 4, 4, 4, 4, 2, 139 2, 2, 4, 2, 4, 4, 4, 2, 4, 4, 6, 4, 4, 4, 4, 2, 140 2, 2, 4, 2, 4, 4, 4, 2, 2, 2, 4, 2, 2, 2, 2, 0 141 }; 142 143 transitions += (odd ? EIGHT_BIT_TRANSITIONS_ODD[aab0] : EIGHT_BIT_TRANSITIONS_EVEN[aab0] ); 144 odd = (unsigned) (aab0 & 0x80); 145 aab1 = (uint8_t) (aa >> 8); 146 transitions += (odd ? EIGHT_BIT_TRANSITIONS_ODD[aab1] : EIGHT_BIT_TRANSITIONS_EVEN[aab1] ); 147 odd = (unsigned) (aab1 & 0x80); 148 aab2 = (uint8_t) (aa >> 16); 149 transitions += (odd ? EIGHT_BIT_TRANSITIONS_ODD[aab2] : EIGHT_BIT_TRANSITIONS_EVEN[aab2] ); 150 odd = (unsigned) (aab2 & 0x80); 151 aab3 = (uint8_t) (aa >> 24); 152 transitions += (odd ? EIGHT_BIT_TRANSITIONS_ODD[aab3] : EIGHT_BIT_TRANSITIONS_EVEN[aab3] ); 153 154 /* consider excessive transitions as offenses */ 155 if (transitions > 24) { 156 retval += (transitions - 24); 157 } 158 159 const uint8_t AA_MSB6_ALLOWED[64] = { 160 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 0, 161 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 162 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 163 0, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 0, 1, 0, 0 164 }; 165 166 /* consider excessive transitions in the 6 MSBs as an offense */ 167 retval += (1 - AA_MSB6_ALLOWED[aab3>>2]); 168 169 /* consider all bytes as being equal an offense */ 170 retval += (((aab0 == aab1) && (aab0 == aab2) && (aab0 == aab3)) ? 1 : 0); 171 172 /* access-channel address and off-by-ones are illegal */ 173 retval += ((aa == LE_ADV_AA) ? 1 : 0); 174 retval += aa_access_channel_off_by_one(aa); 175 176 /* inspect nibble triples for insufficient bit transitions */ 177 for(shift=0; shift<=20; shift+=4) { 178 uint16_t twelvebits = (uint16_t) ((aa >> shift) & 0xfff); 179 switch( twelvebits ) { 180 /* seven consecutive zeroes */ 181 case 0x080: case 0x180: case 0x280: case 0x380: case 0x480: 182 case 0x580: case 0x680: case 0x780: case 0x880: case 0x980: 183 case 0xa80: case 0xb80: case 0xc80: case 0xd80: case 0xe80: 184 case 0xf80: case 0x101: case 0x301: case 0x501: case 0x701: 185 case 0x901: case 0xb01: case 0xd01: case 0xf01: case 0x202: 186 case 0x602: case 0xa02: case 0xe02: case 0x203: case 0x603: 187 case 0xa03: case 0xe03: case 0x404: case 0xc04: case 0x405: 188 case 0xc05: case 0x406: case 0xc06: case 0x407: case 0xc07: 189 case 0x808: case 0x809: case 0x80a: case 0x80b: case 0x80c: 190 case 0x80d: case 0x80e: case 0x80f: case 0x010: case 0x011: 191 case 0x012: case 0x013: case 0x014: case 0x015: case 0x016: 192 case 0x017: case 0x018: case 0x019: case 0x01a: case 0x01b: 193 case 0x01c: case 0x01d: case 0x01e: case 0x01f: 194 /* eight consecutive zeroes */ 195 case 0x100: case 0x300: case 0x500: case 0x700: case 0x900: 196 case 0xb00: case 0xd00: case 0xf00: case 0x201: case 0x601: 197 case 0xa01: case 0xe01: case 0x402: case 0xc02: case 0x403: 198 case 0xc03: case 0x804: case 0x805: case 0x806: case 0x807: 199 case 0x008: case 0x009: case 0x00a: case 0x00b: case 0x00c: 200 case 0x00d: case 0x00e: case 0x00f: 201 /* nine consecutive zeroes */ 202 case 0xe00: case 0xc01: case 0x802: case 0x803: case 0x004: 203 case 0x005: case 0x006: case 0x007: 204 /* ten consecutive zeroes */ 205 case 0x400: case 0xc00: case 0x801: case 0x002: case 0x003: 206 /* eleven consecutive zeroes */ 207 case 0x800: case 0x001: 208 /* twelve consecutive zeroes */ 209 case 0x000: 210 /* seven consecutive ones */ 211 case 0x07f: case 0x0fe: case 0x2fe: case 0x4fe: case 0x6fe: 212 case 0x8fe: case 0xafe: case 0xcfe: case 0xefe: case 0x1fc: 213 case 0x5fc: case 0x9fc: case 0xdfc: case 0x1fd: case 0x5fd: 214 case 0x9fd: case 0xdfd: case 0x3f8: case 0xbf8: case 0x3f9: 215 case 0xbf9: case 0x3fa: case 0xbfa: case 0x3fb: case 0xbfb: 216 case 0x7f4: case 0x7f5: case 0x7f6: case 0x7f7: case 0xfe0: 217 /* eight consecutive ones */ 218 case 0x0ff: case 0x2ff: case 0x4ff: case 0x6ff: case 0x8ff: 219 case 0xaff: case 0xcff: case 0xeff: case 0x1fe: case 0x5fe: 220 case 0x9fe: case 0xdfe: case 0x3fc: case 0xbfc: case 0x3fd: 221 case 0xbfd: case 0x7f8: case 0x7f9: case 0x7fa: case 0x7fb: 222 case 0xff0: case 0xff1: case 0xff2: case 0xff3: case 0xff4: 223 case 0xff5: case 0xff6: case 0xff7: 224 /* nine consecutive ones */ 225 case 0x1ff: case 0x5ff: case 0x9ff: case 0xdff: case 0x3fe: 226 case 0xbfe: case 0x7fc: case 0x7fd: case 0xff8: case 0xff9: 227 case 0xffa: case 0xffb: 228 /* ten consecutive ones */ 229 case 0x3ff: case 0xbff: case 0x7fe: case 0xffc: case 0xffd: 230 /* eleven consecutive ones */ 231 case 0x7ff: case 0xffe: 232 /* all ones */ 233 case 0xfff: 234 retval++; 235 break; 236 default: 237 break; 238 } 239 } 240 241 return retval; 242 } 243 244 lell_packet * 245 lell_packet_new(void) 246 { 247 lell_packet *pkt = (lell_packet *)calloc(1, sizeof(lell_packet)); 248 pkt->refcount = 1; 249 return pkt; 250 } 251 252 void 253 lell_packet_ref(lell_packet *pkt) 254 { 255 pkt->refcount++; 256 } 257 258 void 259 lell_packet_unref(lell_packet *pkt) 260 { 261 pkt->refcount--; 262 if (pkt->refcount == 0) 263 free(pkt); 264 } 265 266 static uint8_t le_channel_index(uint16_t phys_channel) { 267 uint8_t ret; 268 if (phys_channel == 2402) { 269 ret = 37; 270 } else if (phys_channel < 2426) { // 0 - 10 271 ret = (phys_channel - 2404) / 2; 272 } else if (phys_channel == 2426) { 273 ret = 38; 274 } else if (phys_channel < 2480) { // 11 - 36 275 ret = 11 + (phys_channel - 2428) / 2; 276 } else { 277 ret = 39; 278 } 279 return ret; 280 } 281 282 void lell_allocate_and_decode(const uint8_t *stream, uint16_t phys_channel, uint32_t clk100ns, lell_packet **pkt) 283 { 284 *pkt = lell_packet_new( ); 285 memcpy((*pkt)->symbols, stream, MAX_LE_SYMBOLS); 286 287 (*pkt)->channel_idx = le_channel_index(phys_channel); 288 (*pkt)->channel_k = (phys_channel-2402)/2; 289 (*pkt)->clk100ns = clk100ns; 290 291 (*pkt)->access_address = 0; 292 (*pkt)->access_address |= (*pkt)->symbols[0]; 293 (*pkt)->access_address |= (*pkt)->symbols[1] << 8; 294 (*pkt)->access_address |= (*pkt)->symbols[2] << 16; 295 (*pkt)->access_address |= (*pkt)->symbols[3] << 24; 296 297 if (lell_packet_is_data(*pkt)) { 298 // data PDU 299 (*pkt)->length = (*pkt)->symbols[5] & 0x1f; 300 (*pkt)->access_address_offenses = aa_data_channel_offenses((*pkt)->access_address); 301 (*pkt)->flags.as_bits.access_address_ok = (*pkt)->access_address_offenses ? 0 : 1; 302 } else { 303 // advertising PDU 304 (*pkt)->length = (*pkt)->symbols[5] & 0x3f; 305 (*pkt)->adv_type = (*pkt)->symbols[4] & 0xf; 306 (*pkt)->adv_tx_add = (*pkt)->symbols[4] & 0x40 ? 1 : 0; 307 (*pkt)->adv_rx_add = (*pkt)->symbols[4] & 0x80 ? 1 : 0; 308 (*pkt)->flags.as_bits.access_address_ok = ((*pkt)->access_address == 0x8e89bed6); 309 (*pkt)->access_address_offenses = (*pkt)->flags.as_bits.access_address_ok ? 0 : 310 (aa_access_channel_off_by_one((*pkt)->access_address) ? 1 : 32); 311 } 312 } 313 314 unsigned lell_packet_is_data(const lell_packet *pkt) 315 { 316 return (unsigned) (pkt->channel_idx < 37); 317 } 318 319 uint32_t lell_get_access_address(const lell_packet *pkt) 320 { 321 return pkt->access_address; 322 } 323 324 unsigned lell_get_access_address_offenses(const lell_packet *pkt) 325 { 326 return pkt->access_address_offenses; 327 } 328 329 unsigned lell_get_channel_index(const lell_packet *pkt) 330 { 331 return pkt->channel_idx; 332 } 333 334 unsigned lell_get_channel_k(const lell_packet *pkt) 335 { 336 return pkt->channel_k; 337 } 338 339 const char * lell_get_adv_type_str(const lell_packet *pkt) 340 { 341 if (lell_packet_is_data(pkt)) 342 return NULL; 343 if (pkt->adv_type < COUNT_OF(ADV_TYPE_NAMES)) 344 return ADV_TYPE_NAMES[pkt->adv_type]; 345 return "UNKNOWN"; 346 } 347 348 static void _dump_addr(const char *name, const uint8_t *buf, int offset, int random) { 349 int i; 350 printf(" %s%02x", name, buf[offset+5]); 351 for (i = 4; i >= 0; --i) 352 printf(":%02x", buf[offset+i]); 353 printf(" (%s)\n", random ? "random" : "public"); 354 } 355 356 static void _dump_8(const char *name, const uint8_t *buf, int offset) { 357 printf(" %s%02x (%d)\n", name, buf[offset], buf[offset]); 358 } 359 360 static void _dump_16(const char *name, const uint8_t *buf, int offset) { 361 uint16_t val = buf[offset+1] << 8 | buf[offset]; 362 printf(" %s%04x (%d)\n", name, val, val); 363 } 364 365 static void _dump_24(char *name, const uint8_t *buf, int offset) { 366 uint32_t val = buf[offset+2] << 16 | buf[offset+1] << 8 | buf[offset]; 367 printf(" %s%06x\n", name, val); 368 } 369 370 static void _dump_32(const char *name, const uint8_t *buf, int offset) { 371 uint32_t val = buf[offset+3] << 24 | 372 buf[offset+2] << 16 | 373 buf[offset+1] << 8 | 374 buf[offset+0]; 375 printf(" %s%08x\n", name, val); 376 } 377 378 static void _dump_uuid(const uint8_t *uuid) { 379 int i; 380 for (i = 0; i < 4; ++i) 381 printf("%02x", uuid[i]); 382 printf("-"); 383 for (i = 4; i < 6; ++i) 384 printf("%02x", uuid[i]); 385 printf("-"); 386 for (i = 6; i < 8; ++i) 387 printf("%02x", uuid[i]); 388 printf("-"); 389 for (i = 8; i < 10; ++i) 390 printf("%02x", uuid[i]); 391 printf("-"); 392 for (i = 10; i < 16; ++i) 393 printf("%02x", uuid[i]); 394 } 395 396 // Refer to pg 1735 of Bluetooth Core Spec 4.0 397 static void _dump_scan_rsp_data(const uint8_t *buf, int len) { 398 int pos = 0; 399 int sublen, i; 400 uint8_t type; 401 uint16_t val; 402 char *cval; 403 404 while (pos < len) { 405 sublen = buf[pos]; 406 ++pos; 407 if (pos + sublen > len) { 408 printf("Error: attempt to read past end of buffer (%d + %d > %d)\n", pos, sublen, len); 409 return; 410 } 411 if (sublen == 0) { 412 printf("Early return due to 0 length\n"); 413 return; 414 } 415 type = buf[pos]; 416 printf(" Type %02x", type); 417 switch (type) { 418 case 0x01: 419 printf(" (Flags)\n"); 420 printf(" "); 421 for (i = 0; i < 8; ++i) 422 printf("%d", buf[pos+1] & (1 << (7-i)) ? 1 : 0); 423 printf("\n"); 424 for (i = 0; i < 8; ++i) { 425 if (buf[pos+1] & (1 << i)) { 426 printf(" "); 427 printf("%s\n", FLAGS[i]); 428 } 429 } 430 printf("\n"); 431 break; 432 case 0x02: 433 printf(" (16-bit Service UUIDs, more available)\n"); 434 goto print16; 435 case 0x03: 436 printf(" (16-bit Service UUIDs) \n"); 437 print16: 438 if ((sublen - 1) % 2 == 0) { 439 for (i = 0; i < sublen - 1; i += 2) { 440 uint16_t *uuid = (uint16_t *)&buf[pos+1+i]; 441 printf(" %04x\n", *uuid); 442 } 443 } 444 break; 445 case 0x06: 446 printf(" (128-bit Service UUIDs, more available)\n"); 447 goto print128; 448 case 0x07: 449 printf(" (128-bit Service UUIDs)\n"); 450 print128: 451 if ((sublen - 1) % 16 == 0) { 452 uint8_t uuid[16]; 453 for (i = 0; i < sublen - 1; ++i) { 454 uuid[15 - (i % 16)] = buf[pos+1+i]; 455 if ((i & 15) == 15) { 456 printf(" "); 457 _dump_uuid(uuid); 458 printf("\n"); 459 } 460 } 461 } 462 else { 463 printf("Wrong length (%d, must be divisible by 16)\n", sublen-1); 464 } 465 break; 466 case 0x09: 467 printf(" (Complete Local Name)\n"); 468 printf(" "); 469 for (i = 1; i < sublen; ++i) 470 printf("%c", isprint(buf[pos+i]) ? buf[pos+i] : '.'); 471 printf("\n"); 472 break; 473 case 0x0a: 474 printf(" (Tx Power Level)\n"); 475 printf(" "); 476 if (sublen-1 == 1) { 477 cval = (char *)&buf[pos+1]; 478 printf("%d dBm\n", *cval); 479 } else { 480 printf("Wrong length (%d, should be 1)\n", sublen-1); 481 } 482 break; 483 case 0x12: 484 printf(" (Slave Connection Interval Range)\n"); 485 printf(" "); 486 if (sublen-1 == 4) { 487 val = (buf[pos+2] << 8) | buf[pos+1]; 488 printf("(%0.2f, ", val * 1.25); 489 val = (buf[pos+4] << 8) | buf[pos+3]; 490 printf("%0.2f) ms\n", val * 1.25); 491 } 492 else { 493 printf("Wrong length (%d, should be 4)\n", sublen-1); 494 } 495 break; 496 case 0x16: 497 printf(" (Service Data)\n"); 498 printf(" "); 499 if (sublen-1 >= 2) { 500 val = (buf[pos+2] << 8) | buf[pos+1]; 501 printf("UUID: %02x", val); 502 if (sublen-1 > 2) { 503 printf(", Additional:"); 504 for (i = 3; i < sublen; ++i) 505 printf(" %02x", buf[pos+i]); 506 } 507 printf("\n"); 508 } 509 else { 510 printf("Wrong length (%d, should be >= 2)\n", sublen-1); 511 } 512 break; 513 case 0xff: 514 printf(" (Manufacturer Specific Data)\n"); 515 printf(" "); 516 if (sublen - 1 >= 2) { 517 uint16_t company = (buf[pos+2] << 8) | buf[pos+1]; 518 printf("Company: %s\n", bt_compidtostr(company)); 519 printf(" "); 520 printf("Data:"); 521 for (i = 3; i < sublen; ++i) 522 printf(" %02x", buf[pos+i]); 523 printf("\n"); 524 } 525 else { 526 printf("Wrong length (%d, should be >= 2)\n", sublen-1); 527 } 528 break; 529 default: 530 printf("\n"); 531 printf(" "); 532 for (i = 1; i < sublen; ++i) 533 printf(" %02x", buf[pos+i]); 534 printf("\n"); 535 } 536 pos += sublen; 537 } 538 } 539 540 void lell_print(const lell_packet *pkt) 541 { 542 int i, opcode; 543 if (lell_packet_is_data(pkt)) { 544 int llid = pkt->symbols[4] & 0x3; 545 static const char *llid_str[] = { 546 "Reserved", 547 "LL Data PDU / empty or L2CAP continuation", 548 "LL Data PDU / L2CAP start", 549 "LL Control PDU", 550 }; 551 552 printf("Data / AA %08x (%s) / %2d bytes\n", pkt->access_address, 553 pkt->flags.as_bits.access_address_ok ? "valid" : "invalid", 554 pkt->length); 555 printf(" Channel Index: %d\n", pkt->channel_idx); 556 printf(" LLID: %d / %s\n", llid, llid_str[llid]); 557 printf(" NESN: %d SN: %d MD: %d\n", (pkt->symbols[4] >> 2) & 1, 558 (pkt->symbols[4] >> 3) & 1, 559 (pkt->symbols[4] >> 4) & 1); 560 switch (llid) { 561 case 3: // LL Control PDU 562 opcode = pkt->symbols[6]; 563 static const char *opcode_str[] = { 564 "LL_CONNECTION_UPDATE_REQ", 565 "LL_CHANNEL_MAP_REQ", 566 "LL_TERMINATE_IND", 567 "LL_ENC_REQ", 568 "LL_ENC_RSP", 569 "LL_START_ENC_REQ", 570 "LL_START_ENC_RSP", 571 "LL_UNKNOWN_RSP", 572 "LL_FEATURE_REQ", 573 "LL_FEATURE_RSP", 574 "LL_PAUSE_ENC_REQ", 575 "LL_PAUSE_ENC_RSP", 576 "LL_VERSION_IND", 577 "LL_REJECT_IND", 578 "LL_SLAVE_FEATURE_REQ", 579 "LL_CONNECTION_PARAM_REQ", 580 "LL_CONNECTION_PARAM_RSP", 581 "LL_REJECT_IND_EXT", 582 "LL_PING_REQ", 583 "LL_PING_RSP", 584 "Reserved for Future Use", 585 }; 586 printf(" Opcode: %d / %s\n", opcode, opcode_str[(opcode<0x14)?opcode:0x14]); 587 break; 588 default: 589 break; 590 } 591 } else { 592 printf("Advertising / AA %08x (%s)/ %2d bytes\n", pkt->access_address, 593 pkt->flags.as_bits.access_address_ok ? "valid" : "invalid", 594 pkt->length); 595 printf(" Channel Index: %d\n", pkt->channel_idx); 596 printf(" Type: %s\n", lell_get_adv_type_str(pkt)); 597 598 switch(pkt->adv_type) { 599 case ADV_IND: 600 case ADV_NONCONN_IND: 601 case ADV_SCAN_IND: 602 _dump_addr("AdvA: ", pkt->symbols, 6, pkt->adv_tx_add); 603 if (pkt->length-6 > 0) { 604 printf(" AdvData:"); 605 for (i = 0; i < pkt->length - 6; ++i) 606 printf(" %02x", pkt->symbols[12+i]); 607 printf("\n"); 608 _dump_scan_rsp_data(&pkt->symbols[12], pkt->length-6); 609 } 610 break; 611 case ADV_DIRECT_IND: 612 _dump_addr("AdvA: ", pkt->symbols, 6, pkt->adv_tx_add); 613 _dump_addr("InitA: ", pkt->symbols, 12, pkt->adv_rx_add); 614 break; 615 case SCAN_REQ: 616 _dump_addr("ScanA: ", pkt->symbols, 6, pkt->adv_tx_add); 617 _dump_addr("AdvA: ", pkt->symbols, 12, pkt->adv_rx_add); 618 break; 619 case SCAN_RSP: 620 _dump_addr("AdvA: ", pkt->symbols, 6, pkt->adv_tx_add); 621 printf(" ScanRspData:"); 622 for (i = 0; i < pkt->length - 6; ++i) 623 printf(" %02x", pkt->symbols[12+i]); 624 printf("\n"); 625 _dump_scan_rsp_data(&pkt->symbols[12], pkt->length-6); 626 break; 627 case CONNECT_REQ: 628 _dump_addr("InitA: ", pkt->symbols, 6, pkt->adv_tx_add); 629 _dump_addr("AdvA: ", pkt->symbols, 12, pkt->adv_rx_add); 630 _dump_32("AA: ", pkt->symbols, 18); 631 _dump_24("CRCInit: ", pkt->symbols, 22); 632 _dump_8("WinSize: ", pkt->symbols, 25); 633 _dump_16("WinOffset: ", pkt->symbols, 26); 634 _dump_16("Interval: ", pkt->symbols, 28); 635 _dump_16("Latency: ", pkt->symbols, 30); 636 _dump_16("Timeout: ", pkt->symbols, 32); 637 638 printf(" ChM:"); 639 for (i = 0; i < 5; ++i) 640 printf(" %02x", pkt->symbols[34+i]); 641 printf("\n"); 642 643 printf(" Hop: %d\n", pkt->symbols[39] & 0x1f); 644 printf(" SCA: %d, %s\n", 645 pkt->symbols[39] >> 5, 646 CONNECT_SCA[pkt->symbols[39] >> 5]); 647 break; 648 } 649 } 650 651 printf("\n"); 652 printf(" Data: "); 653 for (i = 6; i < 6 + pkt->length; ++i) 654 printf(" %02x", pkt->symbols[i]); 655 printf("\n"); 656 657 printf(" CRC: "); 658 for (i = 0; i < 3; ++i) 659 printf(" %02x", pkt->symbols[6 + pkt->length + i]); 660 printf("\n"); 661 } 662