1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2004-2007 Marcel Holtmann <marcel (at) holtmann.org> 6 * 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 of the License, or 11 * (at your option) 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 this program; if not, write to the Free Software 20 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 21 * 22 */ 23 24 #ifdef HAVE_CONFIG_H 25 #include <config.h> 26 #endif 27 28 #include <stdio.h> 29 #include <errno.h> 30 #include <ctype.h> 31 #include <sys/socket.h> 32 33 #include <bluetooth/bluetooth.h> 34 #include <bluetooth/hci.h> 35 #include <bluetooth/hci_lib.h> 36 37 #include "parser.h" 38 39 #define LMP_U8(frm) (get_u8(frm)) 40 #define LMP_U16(frm) (btohs(htons(get_u16(frm)))) 41 #define LMP_U32(frm) (btohl(htonl(get_u32(frm)))) 42 43 static enum { 44 IN_RAND, 45 COMB_KEY_M, 46 COMB_KEY_S, 47 AU_RAND_M, 48 AU_RAND_S, 49 SRES_M, 50 SRES_S, 51 } pairing_state = IN_RAND; 52 53 static struct { 54 uint8_t in_rand[16]; 55 uint8_t comb_key_m[16]; 56 uint8_t comb_key_s[16]; 57 uint8_t au_rand_m[16]; 58 uint8_t au_rand_s[16]; 59 uint8_t sres_m[4]; 60 uint8_t sres_s[4]; 61 } pairing_data; 62 63 static inline void pairing_data_dump(void) 64 { 65 int i; 66 67 p_indent(6, NULL); 68 printf("IN_RAND "); 69 for (i = 0; i < 16; i++) 70 printf("%2.2x", pairing_data.in_rand[i]); 71 printf("\n"); 72 73 p_indent(6, NULL); 74 printf("COMB_KEY "); 75 for (i = 0; i < 16; i++) 76 printf("%2.2x", pairing_data.comb_key_m[i]); 77 printf(" (M)\n"); 78 79 p_indent(6, NULL); 80 printf("COMB_KEY "); 81 for (i = 0; i < 16; i++) 82 printf("%2.2x", pairing_data.comb_key_s[i]); 83 printf(" (S)\n"); 84 85 p_indent(6, NULL); 86 printf("AU_RAND "); 87 for (i = 0; i < 16; i++) 88 printf("%2.2x", pairing_data.au_rand_m[i]); 89 printf(" SRES "); 90 for (i = 0; i < 4; i++) 91 printf("%2.2x", pairing_data.sres_m[i]); 92 printf(" (M)\n"); 93 94 p_indent(6, NULL); 95 printf("AU_RAND "); 96 for (i = 0; i < 16; i++) 97 printf("%2.2x", pairing_data.au_rand_s[i]); 98 printf(" SRES "); 99 for (i = 0; i < 4; i++) 100 printf("%2.2x", pairing_data.sres_s[i]); 101 printf(" (S)\n"); 102 } 103 104 static inline void in_rand(struct frame *frm) 105 { 106 uint8_t *val = frm->ptr; 107 108 memcpy(pairing_data.in_rand, val, 16); 109 pairing_state = COMB_KEY_M; 110 } 111 112 static inline void comb_key(struct frame *frm) 113 { 114 uint8_t *val = frm->ptr; 115 116 switch (pairing_state) { 117 case COMB_KEY_M: 118 memcpy(pairing_data.comb_key_m, val, 16); 119 pairing_state = COMB_KEY_S; 120 break; 121 case COMB_KEY_S: 122 memcpy(pairing_data.comb_key_s, val, 16); 123 pairing_state = AU_RAND_M; 124 break; 125 default: 126 pairing_state = IN_RAND; 127 break; 128 } 129 } 130 131 static inline void au_rand(struct frame *frm) 132 { 133 uint8_t *val = frm->ptr; 134 135 switch (pairing_state) { 136 case AU_RAND_M: 137 memcpy(pairing_data.au_rand_m, val, 16); 138 pairing_state = SRES_M; 139 break; 140 case AU_RAND_S: 141 memcpy(pairing_data.au_rand_s, val, 16); 142 pairing_state = SRES_S; 143 break; 144 default: 145 pairing_state = IN_RAND; 146 break; 147 } 148 } 149 150 static inline void sres(struct frame *frm) 151 { 152 uint8_t *val = frm->ptr; 153 154 switch (pairing_state) { 155 case SRES_M: 156 memcpy(pairing_data.sres_m, val, 4); 157 pairing_state = AU_RAND_S; 158 break; 159 case SRES_S: 160 memcpy(pairing_data.sres_s, val, 4); 161 pairing_state = IN_RAND; 162 pairing_data_dump(); 163 break; 164 default: 165 pairing_state = IN_RAND; 166 break; 167 } 168 } 169 170 static char *opcode2str(uint16_t opcode) 171 { 172 switch (opcode) { 173 case 1: 174 return "name_req"; 175 case 2: 176 return "name_res"; 177 case 3: 178 return "accepted"; 179 case 4: 180 return "not_accepted"; 181 case 5: 182 return "clkoffset_req"; 183 case 6: 184 return "clkoffset_res"; 185 case 7: 186 return "detach"; 187 case 8: 188 return "in_rand"; 189 case 9: 190 return "comb_key"; 191 case 10: 192 return "unit_key"; 193 case 11: 194 return "au_rand"; 195 case 12: 196 return "sres"; 197 case 13: 198 return "temp_rand"; 199 case 14: 200 return "temp_key"; 201 case 15: 202 return "encryption_mode_req"; 203 case 16: 204 return "encryption_key_size_req"; 205 case 17: 206 return "start_encryption_req"; 207 case 18: 208 return "stop_encryption_req"; 209 case 19: 210 return "switch_req"; 211 case 20: 212 return "hold"; 213 case 21: 214 return "hold_req"; 215 case 22: 216 return "sniff"; 217 case 23: 218 return "sniff_req"; 219 case 24: 220 return "unsniff_req"; 221 case 25: 222 return "park_req"; 223 case 26: 224 return "park"; 225 case 27: 226 return "set_broadcast_scan_window"; 227 case 28: 228 return "modify_beacon"; 229 case 29: 230 return "unpark_BD_ADDR_req"; 231 case 30: 232 return "unpark_PM_ADDR_req"; 233 case 31: 234 return "incr_power_req"; 235 case 32: 236 return "decr_power_req"; 237 case 33: 238 return "max_power"; 239 case 34: 240 return "min_power"; 241 case 35: 242 return "auto_rate"; 243 case 36: 244 return "preferred_rate"; 245 case 37: 246 return "version_req"; 247 case 38: 248 return "version_res"; 249 case 39: 250 return "feature_req"; 251 case 40: 252 return "feature_res"; 253 case 41: 254 return "quality_of_service"; 255 case 42: 256 return "quality_of_service_req"; 257 case 43: 258 return "SCO_link_req"; 259 case 44: 260 return "remove_SCO_link_req"; 261 case 45: 262 return "max_slot"; 263 case 46: 264 return "max_slot_req"; 265 case 47: 266 return "timing_accuracy_req"; 267 case 48: 268 return "timing_accuracy_res"; 269 case 49: 270 return "setup_complete"; 271 case 50: 272 return "use_semi_permanent_key"; 273 case 51: 274 return "host_connection_req"; 275 case 52: 276 return "slot_offset"; 277 case 53: 278 return "page_mode_req"; 279 case 54: 280 return "page_scan_mode_req"; 281 case 55: 282 return "supervision_timeout"; 283 case 56: 284 return "test_activate"; 285 case 57: 286 return "test_control"; 287 case 58: 288 return "encryption_key_size_mask_req"; 289 case 59: 290 return "encryption_key_size_mask_res"; 291 case 60: 292 return "set_AFH"; 293 case 61: 294 return "encapsulated_header"; 295 case 62: 296 return "encapsulated_payload"; 297 case 63: 298 return "simple_pairing_confirm"; 299 case 64: 300 return "simple_pairing_number"; 301 case 65: 302 return "DHkey_check"; 303 case 127 + (1 << 7): 304 return "accepted_ext"; 305 case 127 + (2 << 7): 306 return "not_accepted_ext"; 307 case 127 + (3 << 7): 308 return "features_req_ext"; 309 case 127 + (4 << 7): 310 return "features_res_ext"; 311 case 127 + (11 << 7): 312 return "packet_type_table_req"; 313 case 127 + (12 << 7): 314 return "eSCO_link_req"; 315 case 127 + (13 << 7): 316 return "remove_eSCO_link_req"; 317 case 127 + (16 << 7): 318 return "channel_classification_req"; 319 case 127 + (17 << 7): 320 return "channel_classification"; 321 case 127 + (21 << 7): 322 return "sniff_subrating_req"; 323 case 127 + (22 << 7): 324 return "sniff_subrating_res"; 325 case 127 + (23 << 7): 326 return "pause_encryption_req"; 327 case 127 + (24 << 7): 328 return "resume_encryption_req"; 329 case 127 + (25 << 7): 330 return "IO_capability_req"; 331 case 127 + (26 << 7): 332 return "IO_capability_res"; 333 case 127 + (27 << 7): 334 return "numeric_comparison_failed"; 335 case 127 + (28 << 7): 336 return "passkey_failed"; 337 case 127 + (29 << 7): 338 return "oob_failed"; 339 case 127 + (30 << 7): 340 return "keypress_notification"; 341 default: 342 return "unknown"; 343 } 344 } 345 346 static inline void name_req_dump(int level, struct frame *frm) 347 { 348 uint8_t offset = LMP_U8(frm); 349 350 p_indent(level, frm); 351 printf("name offset %d\n", offset); 352 } 353 354 static inline void name_res_dump(int level, struct frame *frm) 355 { 356 uint8_t offset = LMP_U8(frm); 357 uint8_t length = LMP_U8(frm); 358 uint8_t *name = frm->ptr; 359 int i, size; 360 361 frm->ptr += 14; 362 frm->len -= 14; 363 364 p_indent(level, frm); 365 printf("name offset %d\n", offset); 366 367 p_indent(level, frm); 368 printf("name length %d\n", length); 369 370 size = length - offset; 371 if (size > 14) 372 size = 14; 373 374 p_indent(level, frm); 375 printf("name fragment '"); 376 for (i = 0; i < size; i++) 377 if (isprint(name[i])) 378 printf("%c", name[i]); 379 else 380 printf("."); 381 printf("'\n"); 382 } 383 384 static inline void accepted_dump(int level, struct frame *frm) 385 { 386 uint8_t opcode = LMP_U8(frm); 387 388 p_indent(level, frm); 389 printf("op code %d (%s)\n", opcode, opcode2str(opcode)); 390 } 391 392 static inline void not_accepted_dump(int level, struct frame *frm) 393 { 394 uint8_t opcode = LMP_U8(frm); 395 uint8_t error = LMP_U8(frm); 396 397 p_indent(level, frm); 398 printf("op code %d (%s)\n", opcode, opcode2str(opcode)); 399 400 p_indent(level, frm); 401 printf("error code 0x%2.2x\n", error); 402 } 403 404 static inline void clkoffset_dump(int level, struct frame *frm) 405 { 406 uint16_t clkoffset = LMP_U16(frm); 407 408 p_indent(level, frm); 409 printf("clock offset 0x%4.4x\n", clkoffset); 410 } 411 412 static inline void detach_dump(int level, struct frame *frm) 413 { 414 uint8_t error = LMP_U8(frm); 415 416 p_indent(level, frm); 417 printf("error code 0x%2.2x\n", error); 418 } 419 420 static inline void random_number_dump(int level, struct frame *frm) 421 { 422 uint8_t *number = frm->ptr; 423 int i; 424 425 frm->ptr += 16; 426 frm->len -= 16; 427 428 p_indent(level, frm); 429 printf("random number "); 430 for (i = 0; i < 16; i++) 431 printf("%2.2x", number[i]); 432 printf("\n"); 433 } 434 435 static inline void key_dump(int level, struct frame *frm) 436 { 437 uint8_t *key = frm->ptr; 438 int i; 439 440 frm->ptr += 16; 441 frm->len -= 16; 442 443 p_indent(level, frm); 444 printf("key "); 445 for (i = 0; i < 16; i++) 446 printf("%2.2x", key[i]); 447 printf("\n"); 448 } 449 450 static inline void auth_resp_dump(int level, struct frame *frm) 451 { 452 uint8_t *resp = frm->ptr; 453 int i; 454 455 frm->ptr += 4; 456 frm->ptr -= 4; 457 458 p_indent(level, frm); 459 printf("authentication response "); 460 for (i = 0; i < 4; i++) 461 printf("%2.2x", resp[i]); 462 printf("\n"); 463 } 464 465 static inline void encryption_mode_req_dump(int level, struct frame *frm) 466 { 467 uint8_t mode = LMP_U8(frm); 468 469 p_indent(level, frm); 470 printf("encryption mode %d\n", mode); 471 } 472 473 static inline void encryption_key_size_req_dump(int level, struct frame *frm) 474 { 475 uint8_t keysize = LMP_U8(frm); 476 477 p_indent(level, frm); 478 printf("key size %d\n", keysize); 479 } 480 481 static inline void switch_req_dump(int level, struct frame *frm) 482 { 483 uint32_t instant = LMP_U32(frm); 484 485 p_indent(level, frm); 486 printf("switch instant 0x%4.4x\n", instant); 487 } 488 489 static inline void hold_dump(int level, struct frame *frm) 490 { 491 uint16_t time = LMP_U16(frm); 492 uint32_t instant = LMP_U32(frm); 493 494 p_indent(level, frm); 495 printf("hold time 0x%4.4x\n", time); 496 497 p_indent(level, frm); 498 printf("hold instant 0x%4.4x\n", instant); 499 } 500 501 static inline void sniff_req_dump(int level, struct frame *frm) 502 { 503 uint8_t timing = LMP_U8(frm); 504 uint16_t dsniff = LMP_U16(frm); 505 uint16_t tsniff = LMP_U16(frm); 506 uint16_t attempt = LMP_U16(frm); 507 uint16_t timeout = LMP_U16(frm); 508 509 p_indent(level, frm); 510 printf("timing control flags 0x%2.2x\n", timing); 511 512 p_indent(level, frm); 513 printf("D_sniff %d T_sniff %d\n", dsniff, tsniff); 514 515 p_indent(level, frm); 516 printf("sniff attempt %d\n", attempt); 517 518 p_indent(level, frm); 519 printf("sniff timeout %d\n", timeout); 520 } 521 522 static inline void park_req_dump(int level, struct frame *frm) 523 { 524 uint8_t timing = LMP_U8(frm); 525 uint16_t db = LMP_U16(frm); 526 uint16_t tb = LMP_U16(frm); 527 uint8_t nb = LMP_U8(frm); 528 uint8_t xb = LMP_U8(frm); 529 uint8_t pmaddr = LMP_U8(frm); 530 uint8_t araddr = LMP_U8(frm); 531 uint8_t nbsleep = LMP_U8(frm); 532 uint8_t dbsleep = LMP_U8(frm); 533 uint8_t daccess = LMP_U8(frm); 534 uint8_t taccess = LMP_U8(frm); 535 uint8_t nslots = LMP_U8(frm); 536 uint8_t npoll = LMP_U8(frm); 537 uint8_t access = LMP_U8(frm); 538 539 p_indent(level, frm); 540 printf("timing control flags 0x%2.2x\n", timing); 541 542 p_indent(level, frm); 543 printf("D_B %d T_B %d N_B %d X_B %d\n", db, tb, nb, xb); 544 545 p_indent(level, frm); 546 printf("PM_ADDR %d AR_ADDR %d\n", pmaddr, araddr); 547 548 p_indent(level, frm); 549 printf("N_Bsleep %d D_Bsleep %d\n", nbsleep, dbsleep); 550 551 p_indent(level, frm); 552 printf("D_access %d T_access %d\n", daccess, taccess); 553 554 p_indent(level, frm); 555 printf("N_acc-slots %d N_poll %d\n", nslots, npoll); 556 557 p_indent(level, frm); 558 printf("M_access %d\n", access & 0x0f); 559 560 p_indent(level, frm); 561 printf("access scheme 0x%2.2x\n", access >> 4); 562 } 563 564 static inline void modify_beacon_dump(int level, struct frame *frm) 565 { 566 uint8_t timing = LMP_U8(frm); 567 uint16_t db = LMP_U16(frm); 568 uint16_t tb = LMP_U16(frm); 569 uint8_t nb = LMP_U8(frm); 570 uint8_t xb = LMP_U8(frm); 571 uint8_t daccess = LMP_U8(frm); 572 uint8_t taccess = LMP_U8(frm); 573 uint8_t nslots = LMP_U8(frm); 574 uint8_t npoll = LMP_U8(frm); 575 uint8_t access = LMP_U8(frm); 576 577 p_indent(level, frm); 578 printf("timing control flags 0x%2.2x\n", timing); 579 580 p_indent(level, frm); 581 printf("D_B %d T_B %d N_B %d X_B %d\n", db, tb, nb, xb); 582 583 p_indent(level, frm); 584 printf("D_access %d T_access %d\n", daccess, taccess); 585 586 p_indent(level, frm); 587 printf("N_acc-slots %d N_poll %d\n", nslots, npoll); 588 589 p_indent(level, frm); 590 printf("M_access %d\n", access & 0x0f); 591 592 p_indent(level, frm); 593 printf("access scheme 0x%2.2x\n", access >> 4); 594 } 595 596 static inline void power_req_dump(int level, struct frame *frm) 597 { 598 uint8_t val = LMP_U8(frm); 599 600 p_indent(level, frm); 601 printf("future use 0x%2.2x\n", val); 602 } 603 604 static inline void preferred_rate_dump(int level, struct frame *frm) 605 { 606 uint8_t rate = LMP_U8(frm); 607 608 p_indent(level, frm); 609 printf("data rate 0x%2.2x\n", rate); 610 611 p_indent(level, frm); 612 printf("Basic: "); 613 614 printf("%suse FEC, ", rate & 0x01 ? "do not " : ""); 615 616 switch ((rate >> 1) & 0x03) { 617 case 0x00: 618 printf("no packet-size preference\n"); 619 break; 620 case 0x01: 621 printf("use 1-slot packets\n"); 622 break; 623 case 0x02: 624 printf("use 3-slot packets\n"); 625 break; 626 case 0x03: 627 printf("use 5-slot packets\n"); 628 break; 629 } 630 631 p_indent(level, frm); 632 printf("EDR: "); 633 634 switch ((rate >> 3) & 0x03) { 635 case 0x00: 636 printf("use DM1 packets, "); 637 break; 638 case 0x01: 639 printf("use 2 Mbps packets, "); 640 break; 641 case 0x02: 642 printf("use 3 Mbps packets, "); 643 break; 644 case 0x03: 645 printf("reserved, \n"); 646 break; 647 } 648 649 switch ((rate >> 5) & 0x03) { 650 case 0x00: 651 printf("no packet-size preference\n"); 652 break; 653 case 0x01: 654 printf("use 1-slot packets\n"); 655 break; 656 case 0x02: 657 printf("use 3-slot packets\n"); 658 break; 659 case 0x03: 660 printf("use 5-slot packets\n"); 661 break; 662 } 663 } 664 665 static inline void version_dump(int level, struct frame *frm) 666 { 667 uint8_t ver = LMP_U8(frm); 668 uint16_t compid = LMP_U16(frm); 669 uint16_t subver = LMP_U16(frm); 670 char *tmp; 671 672 p_indent(level, frm); 673 tmp = lmp_vertostr(ver); 674 printf("VersNr %d (%s)\n", ver, tmp); 675 bt_free(tmp); 676 677 p_indent(level, frm); 678 printf("CompId %d (%s)\n", compid, bt_compidtostr(compid)); 679 680 p_indent(level, frm); 681 printf("SubVersNr %d\n", subver); 682 } 683 684 static inline void features_dump(int level, struct frame *frm) 685 { 686 uint8_t *features = frm->ptr; 687 int i; 688 689 frm->ptr += 8; 690 frm->len -= 8; 691 692 p_indent(level, frm); 693 printf("features"); 694 for (i = 0; i < 8; i++) 695 printf(" 0x%2.2x", features[i]); 696 printf("\n"); 697 } 698 699 static inline void set_afh_dump(int level, struct frame *frm) 700 { 701 uint32_t instant = LMP_U32(frm); 702 uint8_t mode = LMP_U8(frm); 703 uint8_t *map = frm->ptr; 704 int i; 705 706 frm->ptr += 10; 707 frm->len -= 10; 708 709 p_indent(level, frm); 710 printf("AFH_instant 0x%04x\n", instant); 711 712 p_indent(level, frm); 713 printf("AFH_mode %d\n", mode); 714 715 p_indent(level, frm); 716 printf("AFH_channel_map 0x"); 717 for (i = 0; i < 10; i++) 718 printf("%2.2x", map[i]); 719 printf("\n"); 720 } 721 722 static inline void encapsulated_header_dump(int level, struct frame *frm) 723 { 724 uint8_t major = LMP_U8(frm); 725 uint8_t minor = LMP_U8(frm); 726 uint8_t length = LMP_U8(frm); 727 728 p_indent(level, frm); 729 printf("major type %d minor type %d payload length %d\n", 730 major, minor, length); 731 732 if (major == 1 && minor == 1) { 733 p_indent(level, frm); 734 printf("P-192 Public Key\n"); 735 } 736 } 737 738 static inline void encapsulated_payload_dump(int level, struct frame *frm) 739 { 740 uint8_t *value = frm->ptr; 741 int i; 742 743 frm->ptr += 16; 744 frm->len -= 16; 745 746 p_indent(level, frm); 747 printf("data "); 748 for (i = 0; i < 16; i++) 749 printf("%2.2x", value[i]); 750 printf("\n"); 751 } 752 753 static inline void simple_pairing_confirm_dump(int level, struct frame *frm) 754 { 755 uint8_t *value = frm->ptr; 756 int i; 757 758 frm->ptr += 16; 759 frm->len -= 16; 760 761 p_indent(level, frm); 762 printf("commitment value "); 763 for (i = 0; i < 16; i++) 764 printf("%2.2x", value[i]); 765 printf("\n"); 766 } 767 768 static inline void simple_pairing_number_dump(int level, struct frame *frm) 769 { 770 uint8_t *value = frm->ptr; 771 int i; 772 773 frm->ptr += 16; 774 frm->len -= 16; 775 776 p_indent(level, frm); 777 printf("nounce value "); 778 for (i = 0; i < 16; i++) 779 printf("%2.2x", value[i]); 780 printf("\n"); 781 } 782 783 static inline void dhkey_check_dump(int level, struct frame *frm) 784 { 785 uint8_t *value = frm->ptr; 786 int i; 787 788 frm->ptr += 16; 789 frm->len -= 16; 790 791 p_indent(level, frm); 792 printf("confirmation value "); 793 for (i = 0; i < 16; i++) 794 printf("%2.2x", value[i]); 795 printf("\n"); 796 } 797 798 static inline void accepted_ext_dump(int level, struct frame *frm) 799 { 800 uint16_t opcode = LMP_U8(frm) + (LMP_U8(frm) << 7); 801 802 p_indent(level, frm); 803 printf("op code %d/%d (%s)\n", opcode & 0x7f, opcode >> 7, opcode2str(opcode)); 804 } 805 806 static inline void not_accepted_ext_dump(int level, struct frame *frm) 807 { 808 uint16_t opcode = LMP_U8(frm) + (LMP_U8(frm) << 7); 809 uint8_t error = LMP_U8(frm); 810 811 p_indent(level, frm); 812 printf("op code %d/%d (%s)\n", opcode & 0x7f, opcode >> 7, opcode2str(opcode)); 813 814 p_indent(level, frm); 815 printf("error code 0x%2.2x\n", error); 816 } 817 818 static inline void features_ext_dump(int level, struct frame *frm) 819 { 820 uint8_t page = LMP_U8(frm); 821 uint8_t max = LMP_U8(frm); 822 uint8_t *features = frm->ptr; 823 int i; 824 825 frm->ptr += 8; 826 frm->len -= 8; 827 828 p_indent(level, frm); 829 printf("features page %d\n", page); 830 831 p_indent(level, frm); 832 printf("max supported page %d\n", max); 833 834 p_indent(level, frm); 835 printf("extended features"); 836 for (i = 0; i < 8; i++) 837 printf(" 0x%2.2x", features[i]); 838 printf("\n"); 839 } 840 841 static inline void quality_of_service_dump(int level, struct frame *frm) 842 { 843 uint16_t interval = LMP_U16(frm); 844 uint8_t nbc = LMP_U8(frm); 845 846 p_indent(level, frm); 847 printf("poll interval %d\n", interval); 848 849 p_indent(level, frm); 850 printf("N_BC %d\n", nbc); 851 } 852 853 static inline void sco_link_req_dump(int level, struct frame *frm) 854 { 855 uint8_t handle = LMP_U8(frm); 856 uint8_t timing = LMP_U8(frm); 857 uint8_t dsco = LMP_U8(frm); 858 uint8_t tsco = LMP_U8(frm); 859 uint8_t packet = LMP_U8(frm); 860 uint8_t airmode = LMP_U8(frm); 861 862 p_indent(level, frm); 863 printf("SCO handle %d\n", handle); 864 865 p_indent(level, frm); 866 printf("timing control flags 0x%2.2x\n", timing); 867 868 p_indent(level, frm); 869 printf("D_SCO %d T_SCO %d\n", dsco, tsco); 870 871 p_indent(level, frm); 872 printf("SCO packet 0x%2.2x\n", packet); 873 874 p_indent(level, frm); 875 printf("air mode 0x%2.2x\n", airmode); 876 } 877 878 static inline void remove_sco_link_req_dump(int level, struct frame *frm) 879 { 880 uint8_t handle = LMP_U8(frm); 881 uint8_t error = LMP_U8(frm); 882 883 p_indent(level, frm); 884 printf("SCO handle %d\n", handle); 885 886 p_indent(level, frm); 887 printf("error code 0x%2.2x\n", error); 888 } 889 890 static inline void max_slots_dump(int level, struct frame *frm) 891 { 892 uint8_t slots = LMP_U8(frm); 893 894 p_indent(level, frm); 895 printf("max slots %d\n", slots); 896 } 897 898 static inline void timing_accuracy_dump(int level, struct frame *frm) 899 { 900 uint8_t drift = LMP_U8(frm); 901 uint8_t jitter = LMP_U8(frm); 902 903 p_indent(level, frm); 904 printf("drift %d\n", drift); 905 906 p_indent(level, frm); 907 printf("jitter %d\n", jitter); 908 } 909 910 static inline void slot_offset_dump(int level, struct frame *frm) 911 { 912 uint16_t offset = LMP_U16(frm); 913 char addr[18]; 914 915 p_ba2str((bdaddr_t *) frm->ptr, addr); 916 917 p_indent(level, frm); 918 printf("slot offset %d\n", offset); 919 920 p_indent(level, frm); 921 printf("BD_ADDR %s\n", addr); 922 } 923 924 static inline void page_mode_dump(int level, struct frame *frm) 925 { 926 uint8_t scheme = LMP_U8(frm); 927 uint8_t settings = LMP_U8(frm); 928 929 p_indent(level, frm); 930 printf("page scheme %d\n", scheme); 931 932 p_indent(level, frm); 933 printf("page scheme settings %d\n", settings); 934 } 935 936 static inline void supervision_timeout_dump(int level, struct frame *frm) 937 { 938 uint16_t timeout = LMP_U16(frm); 939 940 p_indent(level, frm); 941 printf("supervision timeout %d\n", timeout); 942 } 943 944 static inline void test_control_dump(int level, struct frame *frm) 945 { 946 uint8_t scenario = LMP_U8(frm); 947 uint8_t hopping = LMP_U8(frm); 948 uint8_t txfreq = LMP_U8(frm); 949 uint8_t rxfreq = LMP_U8(frm); 950 uint8_t power = LMP_U8(frm); 951 uint8_t poll = LMP_U8(frm); 952 uint8_t packet = LMP_U8(frm); 953 uint16_t length = LMP_U16(frm); 954 955 p_indent(level, frm); 956 printf("test scenario %d\n", scenario); 957 958 p_indent(level, frm); 959 printf("hopping mode %d\n", hopping); 960 961 p_indent(level, frm); 962 printf("TX frequency %d\n", txfreq); 963 964 p_indent(level, frm); 965 printf("RX frequency %d\n", rxfreq); 966 967 p_indent(level, frm); 968 printf("power control mode %d\n", power); 969 970 p_indent(level, frm); 971 printf("poll period %d\n", poll); 972 973 p_indent(level, frm); 974 printf("poll period %d\n", poll); 975 976 p_indent(level, frm); 977 printf("packet type 0x%2.2x\n", packet); 978 979 p_indent(level, frm); 980 printf("length of test data %d\n", length); 981 } 982 983 static inline void encryption_key_size_mask_res_dump(int level, struct frame *frm) 984 { 985 uint16_t mask = LMP_U16(frm); 986 987 p_indent(level, frm); 988 printf("key size mask 0x%4.4x\n", mask); 989 } 990 991 static inline void packet_type_table_dump(int level, struct frame *frm) 992 { 993 uint8_t type = LMP_U8(frm); 994 995 p_indent(level, frm); 996 printf("packet type table %d ", type); 997 switch (type) { 998 case 0: 999 printf("(1Mbps only)\n"); 1000 break; 1001 case 1: 1002 printf("(2/3Mbps)\n"); 1003 break; 1004 default: 1005 printf("(Reserved)\n"); 1006 break; 1007 } 1008 } 1009 1010 static inline void esco_link_req_dump(int level, struct frame *frm) 1011 { 1012 uint8_t handle = LMP_U8(frm); 1013 uint8_t ltaddr = LMP_U8(frm); 1014 uint8_t timing = LMP_U8(frm); 1015 uint8_t desco = LMP_U8(frm); 1016 uint8_t tesco = LMP_U8(frm); 1017 uint8_t wesco = LMP_U8(frm); 1018 uint8_t mspkt = LMP_U8(frm); 1019 uint8_t smpkt = LMP_U8(frm); 1020 uint16_t mslen = LMP_U16(frm); 1021 uint16_t smlen = LMP_U16(frm); 1022 uint8_t airmode = LMP_U8(frm); 1023 uint8_t negstate = LMP_U8(frm); 1024 1025 p_indent(level, frm); 1026 printf("eSCO handle %d\n", handle); 1027 1028 p_indent(level, frm); 1029 printf("eSCO LT_ADDR %d\n", ltaddr); 1030 1031 p_indent(level, frm); 1032 printf("timing control flags 0x%2.2x\n", timing); 1033 1034 p_indent(level, frm); 1035 printf("D_eSCO %d T_eSCO %d W_eSCO %d\n", desco, tesco, wesco); 1036 1037 p_indent(level, frm); 1038 printf("eSCO M->S packet type 0x%2.2x length %d\n", mspkt, mslen); 1039 1040 p_indent(level, frm); 1041 printf("eSCO S->M packet type 0x%2.2x length %d\n", smpkt, smlen); 1042 1043 p_indent(level, frm); 1044 printf("air mode 0x%2.2x\n", airmode); 1045 1046 p_indent(level, frm); 1047 printf("negotiation state 0x%2.2x\n", negstate); 1048 } 1049 1050 static inline void remove_esco_link_req_dump(int level, struct frame *frm) 1051 { 1052 uint8_t handle = LMP_U8(frm); 1053 uint8_t error = LMP_U8(frm); 1054 1055 p_indent(level, frm); 1056 printf("eSCO handle %d\n", handle); 1057 1058 p_indent(level, frm); 1059 printf("error code 0x%2.2x\n", error); 1060 } 1061 1062 static inline void channel_classification_req_dump(int level, struct frame *frm) 1063 { 1064 uint8_t mode = LMP_U8(frm); 1065 uint16_t min = LMP_U16(frm); 1066 uint16_t max = LMP_U16(frm); 1067 1068 p_indent(level, frm); 1069 printf("AFH reporting mode %d\n", mode); 1070 1071 p_indent(level, frm); 1072 printf("AFH min interval 0x%4.4x\n", min); 1073 1074 p_indent(level, frm); 1075 printf("AFH max interval 0x%4.4x\n", max); 1076 } 1077 1078 static inline void channel_classification_dump(int level, struct frame *frm) 1079 { 1080 uint8_t *map = frm->ptr; 1081 int i; 1082 1083 frm->ptr += 10; 1084 frm->len -= 10; 1085 1086 p_indent(level, frm); 1087 printf("AFH channel classification 0x"); 1088 for (i = 0; i < 10; i++) 1089 printf("%2.2x", map[i]); 1090 printf("\n"); 1091 } 1092 1093 static inline void sniff_subrating_dump(int level, struct frame *frm) 1094 { 1095 uint8_t subrate = LMP_U8(frm); 1096 uint16_t timeout = LMP_U16(frm); 1097 uint32_t instant = LMP_U32(frm); 1098 1099 p_indent(level, frm); 1100 printf("max subrate %d\n", subrate); 1101 1102 p_indent(level, frm); 1103 printf("min sniff timeout %d\n", timeout); 1104 1105 p_indent(level, frm); 1106 printf("subrate instant 0x%4.4x\n", instant); 1107 } 1108 1109 static inline void io_capability_dump(int level, struct frame *frm) 1110 { 1111 uint8_t capability = LMP_U8(frm); 1112 uint8_t oob_data = LMP_U8(frm); 1113 uint8_t authentication = LMP_U8(frm); 1114 1115 p_indent(level, frm); 1116 printf("capability 0x%2.2x oob 0x%2.2x auth 0x%2.2x\n", 1117 capability, oob_data, authentication); 1118 } 1119 1120 static inline void keypress_notification_dump(int level, struct frame *frm) 1121 { 1122 uint8_t value = LMP_U8(frm); 1123 1124 p_indent(level, frm); 1125 printf("notification value %d\n", value); 1126 } 1127 1128 void lmp_dump(int level, struct frame *frm) 1129 { 1130 uint8_t tmp, tid; 1131 uint16_t opcode; 1132 1133 p_indent(level, frm); 1134 1135 tmp = LMP_U8(frm); 1136 tid = tmp & 0x01; 1137 opcode = (tmp & 0xfe) >> 1; 1138 if (opcode > 123) { 1139 tmp = LMP_U8(frm); 1140 opcode += tmp << 7; 1141 } 1142 1143 printf("LMP(%c): %s(%c): ", frm->master ? 's' : 'r', 1144 opcode2str(opcode), tid ? 's' : 'm'); 1145 1146 if (opcode > 123) 1147 printf("op code %d/%d", opcode & 0x7f, opcode >> 7); 1148 else 1149 printf("op code %d", opcode); 1150 1151 if (frm->handle > 17) 1152 printf(" handle %d\n", frm->handle); 1153 else 1154 printf("\n"); 1155 1156 if (!(parser.flags & DUMP_VERBOSE)) { 1157 raw_dump(level, frm); 1158 return; 1159 } 1160 1161 switch (opcode) { 1162 case 1: 1163 name_req_dump(level + 1, frm); 1164 return; 1165 case 2: 1166 name_res_dump(level + 1, frm); 1167 return; 1168 case 3: 1169 accepted_dump(level + 1, frm); 1170 return; 1171 case 4: 1172 not_accepted_dump(level + 1, frm); 1173 return; 1174 case 6: 1175 clkoffset_dump(level + 1, frm); 1176 return; 1177 case 7: 1178 detach_dump(level + 1, frm); 1179 return; 1180 case 8: 1181 in_rand(frm); 1182 random_number_dump(level + 1, frm); 1183 return; 1184 case 9: 1185 comb_key(frm); 1186 random_number_dump(level + 1, frm); 1187 return; 1188 case 11: 1189 au_rand(frm); 1190 random_number_dump(level + 1, frm); 1191 return; 1192 case 12: 1193 sres(frm); 1194 auth_resp_dump(level + 1, frm); 1195 return; 1196 case 13: 1197 case 17: 1198 random_number_dump(level + 1, frm); 1199 return; 1200 case 10: 1201 case 14: 1202 key_dump(level + 1, frm); 1203 return; 1204 case 15: 1205 encryption_mode_req_dump(level + 1, frm); 1206 return; 1207 case 16: 1208 encryption_key_size_req_dump(level + 1, frm); 1209 return; 1210 case 19: 1211 switch_req_dump(level + 1, frm); 1212 return; 1213 case 20: 1214 case 21: 1215 hold_dump(level + 1, frm); 1216 return; 1217 case 23: 1218 sniff_req_dump(level + 1, frm); 1219 return; 1220 case 25: 1221 park_req_dump(level + 1, frm); 1222 return; 1223 case 28: 1224 modify_beacon_dump(level + 1, frm); 1225 return; 1226 case 31: 1227 case 32: 1228 power_req_dump(level + 1, frm); 1229 return; 1230 case 36: 1231 preferred_rate_dump(level + 1, frm); 1232 return; 1233 case 37: 1234 case 38: 1235 version_dump(level + 1, frm); 1236 return; 1237 case 39: 1238 case 40: 1239 features_dump(level + 1, frm); 1240 return; 1241 case 41: 1242 case 42: 1243 quality_of_service_dump(level + 1, frm); 1244 return; 1245 case 43: 1246 sco_link_req_dump(level + 1, frm); 1247 return; 1248 case 44: 1249 remove_sco_link_req_dump(level + 1, frm); 1250 return; 1251 case 45: 1252 case 46: 1253 max_slots_dump(level + 1, frm); 1254 return; 1255 case 48: 1256 timing_accuracy_dump(level + 1, frm); 1257 return; 1258 case 52: 1259 slot_offset_dump(level + 1, frm); 1260 return; 1261 case 53: 1262 case 54: 1263 page_mode_dump(level + 1, frm); 1264 return; 1265 case 55: 1266 supervision_timeout_dump(level + 1, frm); 1267 return; 1268 case 57: 1269 test_control_dump(level + 1, frm); 1270 return; 1271 case 59: 1272 encryption_key_size_mask_res_dump(level + 1, frm); 1273 return; 1274 case 60: 1275 set_afh_dump(level + 1, frm); 1276 return; 1277 case 61: 1278 encapsulated_header_dump(level + 1, frm); 1279 return; 1280 case 62: 1281 encapsulated_payload_dump(level + 1, frm); 1282 return; 1283 case 63: 1284 simple_pairing_confirm_dump(level + 1, frm); 1285 return; 1286 case 64: 1287 simple_pairing_number_dump(level + 1, frm); 1288 return; 1289 case 65: 1290 dhkey_check_dump(level + 1, frm); 1291 return; 1292 case 5: 1293 case 18: 1294 case 24: 1295 case 33: 1296 case 34: 1297 case 35: 1298 case 47: 1299 case 49: 1300 case 50: 1301 case 51: 1302 case 56: 1303 case 58: 1304 case 127 + (23 << 7): 1305 case 127 + (24 << 7): 1306 case 127 + (27 << 7): 1307 case 127 + (28 << 7): 1308 case 127 + (29 << 7): 1309 return; 1310 case 127 + (1 << 7): 1311 accepted_ext_dump(level + 1, frm); 1312 return; 1313 case 127 + (2 << 7): 1314 not_accepted_ext_dump(level + 1, frm); 1315 return; 1316 case 127 + (3 << 7): 1317 case 127 + (4 << 7): 1318 features_ext_dump(level + 1, frm); 1319 return; 1320 case 127 + (11 << 7): 1321 packet_type_table_dump(level + 1, frm); 1322 return; 1323 case 127 + (12 << 7): 1324 esco_link_req_dump(level + 1, frm); 1325 return; 1326 case 127 + (13 << 7): 1327 remove_esco_link_req_dump(level + 1, frm); 1328 return; 1329 case 127 + (16 << 7): 1330 channel_classification_req_dump(level + 1, frm); 1331 return; 1332 case 127 + (17 << 7): 1333 channel_classification_dump(level + 1, frm); 1334 return; 1335 case 127 + (21 << 7): 1336 case 127 + (22 << 7): 1337 sniff_subrating_dump(level + 1, frm); 1338 return; 1339 case 127 + (25 << 7): 1340 case 127 + (26 << 7): 1341 io_capability_dump(level + 1, frm); 1342 return; 1343 case 127 + (30 << 7): 1344 keypress_notification_dump(level + 1, frm); 1345 return; 1346 } 1347 1348 raw_dump(level, frm); 1349 } 1350
