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