tools/parser/lmp.c

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