monitor/lmp.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2014  Intel Corporation
   7  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <stdio.h>
  17 #include <string.h>
  18
  19 #include "src/shared/util.h"
  20 #include "display.h"
  21 #include "packet.h"
  22 #include "bt.h"
  23 #include "lmp.h"
  24
  25 #define COLOR_OPCODE            COLOR_MAGENTA
  26 #define COLOR_OPCODE_UNKNOWN    COLOR_WHITE_BG
  27
  28 static const char *get_opcode_str(uint16_t opcode);
  29
  30 static void print_opcode(uint16_t opcode)
  31 {
  32         const char *str;
  33
  34         str = get_opcode_str(opcode);
  35         if (!str)
  36                 str = "Unknown";
  37
  38         if (opcode & 0xff00)
  39                 print_field("Operation: %s (%u/%u)", str,
  40                                                 opcode >> 8, opcode & 0xff);
  41         else
  42                 print_field("Operation: %s (%u)", str, opcode);
  43 }
  44
  45 static void name_req(const void *data, uint8_t size)
  46 {
  47         const struct bt_lmp_name_req *pdu = data;
  48
  49         print_field("Offset: %u", pdu->offset);
  50 }
  51
  52 static void name_rsp(const void *data, uint8_t size)
  53 {
  54         const struct bt_lmp_name_rsp *pdu = data;
  55         char str[15];
  56
  57         memcpy(str, pdu->fragment, 14);
  58         str[14] = '\0';
  59
  60         print_field("Offset: %u", pdu->offset);
  61         print_field("Length: %u", pdu->length);
  62         print_field("Fragment: %s", str);
  63 }
  64
  65 static void accepted(const void *data, uint8_t size)
  66 {
  67         const struct bt_lmp_accepted *pdu = data;
  68
  69         print_opcode(pdu->opcode);
  70 }
  71
  72 static void not_accepted(const void *data, uint8_t size)
  73 {
  74         const struct bt_lmp_not_accepted *pdu = data;
  75
  76         print_opcode(pdu->opcode);
  77         packet_print_error("Error code", pdu->error);
  78 }
  79
  80 static void clkoffset_req(const void *data, uint8_t size)
  81 {
  82 }
  83
  84 static void clkoffset_rsp(const void *data, uint8_t size)
  85 {
  86         const struct bt_lmp_clkoffset_rsp *pdu = data;
  87
  88         print_field("Clock offset: 0x%4.4x", le16_to_cpu(pdu->offset));
  89 }
  90
  91 static void detach(const void *data, uint8_t size)
  92 {
  93         const struct bt_lmp_detach *pdu = data;
  94
  95         packet_print_error("Error code", pdu->error);
  96 }
  97
  98 static void au_rand(const void *data, uint8_t size)
  99 {
 100         const struct bt_lmp_au_rand *pdu = data;
 101
 102         packet_hexdump(pdu->number, 16);
 103 }
 104
 105 static void sres(const void *data, uint8_t size)
 106 {
 107         const struct bt_lmp_sres *pdu = data;
 108
 109         packet_hexdump(pdu->response, 4);
 110 }
 111
 112 static void encryption_mode_req(const void *data, uint8_t size)
 113 {
 114         const struct bt_lmp_encryption_mode_req *pdu = data;
 115         const char *str;
 116
 117         switch (pdu->mode) {
 118         case 0x00:
 119                 str = "No encryption";
 120                 break;
 121         case 0x01:
 122                 str = "Encryption";
 123                 break;
 124         case 0x02:
 125                 str = "Encryption";
 126                 break;
 127         default:
 128                 str = "Reserved";
 129                 break;
 130         }
 131
 132         print_field("Mode: %s (%u)", str, pdu->mode);
 133 }
 134
 135 static void encryption_key_size_req(const void *data, uint8_t size)
 136 {
 137         const struct bt_lmp_encryption_key_size_req *pdu = data;
 138
 139         print_field("Key size: %u", pdu->key_size);
 140 }
 141
 142 static void start_encryption_req(const void *data, uint8_t size)
 143 {
 144         const struct bt_lmp_start_encryption_req *pdu = data;
 145
 146         packet_hexdump(pdu->number, 16);
 147 }
 148
 149 static void stop_encryption_req(const void *data, uint8_t size)
 150 {
 151 }
 152
 153 static void switch_req(const void *data, uint8_t size)
 154 {
 155         const struct bt_lmp_switch_req *pdu = data;
 156
 157         print_field("Instant: 0x%8.8x", le32_to_cpu(pdu->instant));
 158 }
 159
 160 static void unsniff_req(const void *data, uint8_t size)
 161 {
 162 }
 163
 164 static void max_power(const void *data, uint8_t size)
 165 {
 166 }
 167
 168 static void min_power(const void *data, uint8_t size)
 169 {
 170 }
 171
 172 static void auto_rate(const void *data, uint8_t size)
 173 {
 174 }
 175
 176 static void preferred_rate(const void *data, uint8_t size)
 177 {
 178         const struct bt_lmp_preferred_rate *pdu = data;
 179         const char *str;
 180
 181         str = (pdu->rate & 0x01) ? "do not use FEC" : "use FEC";
 182
 183         print_field("Basic data rate: %s (0x%02x)", str, pdu->rate & 0x01);
 184
 185         switch ((pdu->rate & 0x06) >> 1) {
 186         case 0:
 187                 str = "No packet-size preference available";
 188                 break;
 189         case 1:
 190                 str = "use 1-slot packets";
 191                 break;
 192         case 2:
 193                 str = "use 3-slot packets";
 194                 break;
 195         case 3:
 196                 str = "use 5-slot packets";
 197                 break;
 198         }
 199
 200         print_field("Basic data rate: %s (0x%02x)", str, pdu->rate & 0x06);
 201
 202         switch ((pdu->rate & 0x11) >> 3) {
 203         case 0:
 204                 str = "use DM1 packets";
 205                 break;
 206         case 1:
 207                 str = "use 2 Mb/s packets";
 208                 break;
 209         case 2:
 210                 str = "use 3 MB/s packets";
 211                 break;
 212         case 3:
 213                 str = "reserved";
 214                 break;
 215         }
 216
 217         print_field("Enhanced data rate: %s (0x%2.2x)", str, pdu->rate & 0x11);
 218
 219         switch ((pdu->rate & 0x60) >> 5) {
 220         case 0:
 221                 str = "No packet-size preference available";
 222                 break;
 223         case 1:
 224                 str = "use 1-slot packets";
 225                 break;
 226         case 2:
 227                 str = "use 3-slot packets";
 228                 break;
 229         case 3:
 230                 str = "use 5-slot packets";
 231                 break;
 232         }
 233
 234         print_field("Enhanced data rate: %s (0x%2.2x)", str, pdu->rate & 0x60);
 235 }
 236
 237 static void version_req(const void *data, uint8_t size)
 238 {
 239         const struct bt_lmp_version_req *pdu = data;
 240
 241         packet_print_version("Version", pdu->version,
 242                                 "Subversion", le16_to_cpu(pdu->subversion));
 243         packet_print_company("Company", le16_to_cpu(pdu->company));
 244 }
 245
 246 static void version_res(const void *data, uint8_t size)
 247 {
 248         const struct bt_lmp_version_res *pdu = data;
 249
 250         packet_print_version("Version", pdu->version,
 251                                 "Subversion", le16_to_cpu(pdu->subversion));
 252         packet_print_company("Company", le16_to_cpu(pdu->company));
 253 }
 254
 255 static void features_req(const void *data, uint8_t size)
 256 {
 257         const struct bt_lmp_features_req *pdu = data;
 258
 259         packet_print_features_lmp(pdu->features, 0x00);
 260 }
 261
 262 static void features_res(const void *data, uint8_t size)
 263 {
 264         const struct bt_lmp_features_res *pdu = data;
 265
 266         packet_print_features_lmp(pdu->features, 0x00);
 267 }
 268
 269 static void max_slot(const void *data, uint8_t size)
 270 {
 271         const struct bt_lmp_max_slot *pdu = data;
 272
 273         print_field("Slots: 0x%4.4x", pdu->slots);
 274 }
 275
 276 static void max_slot_req(const void *data, uint8_t size)
 277 {
 278         const struct bt_lmp_max_slot_req *pdu = data;
 279
 280         print_field("Slots: 0x%4.4x", pdu->slots);
 281 }
 282
 283 static void timing_accuracy_req(const void *data, uint8_t size)
 284 {
 285 }
 286
 287 static void timing_accuracy_res(const void *data, uint8_t size)
 288 {
 289         const struct bt_lmp_timing_accuracy_res *pdu = data;
 290
 291         print_field("Drift: %u ppm", pdu->drift);
 292         print_field("Jitter: %u usec", pdu->jitter);
 293 }
 294
 295 static void setup_complete(const void *data, uint8_t size)
 296 {
 297 }
 298
 299 static void use_semi_permanent_key(const void *data, uint8_t size)
 300 {
 301 }
 302
 303 static void host_connection_req(const void *data, uint8_t size)
 304 {
 305 }
 306
 307 static void slot_offset(const void *data, uint8_t size)
 308 {
 309         const struct bt_lmp_slot_offset *pdu = data;
 310
 311         print_field("Offset: %u usec", le16_to_cpu(pdu->offset));
 312         packet_print_addr("Address", pdu->bdaddr, 0x00);
 313 }
 314
 315 static void page_scan_mode_req(const void *data, uint8_t size)
 316 {
 317         const struct bt_lmp_page_scan_mode_req *pdu = data;
 318         const char *str;
 319
 320         switch (pdu->scheme) {
 321         case 0x00:
 322                 str = "Mandatory";
 323                 break;
 324         default:
 325                 str = "Reserved";
 326                 break;
 327         }
 328
 329         print_field("Paging scheme: %s (%u)", str, pdu->scheme);
 330
 331         if (pdu->scheme == 0x00) {
 332                 switch (pdu->settings) {
 333                 case 0x00:
 334                         str = "R0";
 335                         break;
 336                 case 0x01:
 337                         str = "R1";
 338                         break;
 339                 case 0x02:
 340                         str = "R2";
 341                         break;
 342                 default:
 343                         str = "Reserved";
 344                         break;
 345                 }
 346         } else
 347                 str = "Reserved";
 348
 349         print_field("Paging scheme settings: %s (%u)", str, pdu->settings);
 350 }
 351
 352 static void test_activate(const void *data, uint8_t size)
 353 {
 354 }
 355
 356 static void encryption_key_size_mask_req(const void *data, uint8_t size)
 357 {
 358 }
 359
 360 static void set_afh(const void *data, uint8_t size)
 361 {
 362         const struct bt_lmp_set_afh *pdu = data;
 363         const char *str;
 364
 365         print_field("Instant: %u", le32_to_cpu(pdu->instant));
 366
 367         switch (pdu->mode) {
 368         case 0x00:
 369                 str = "Disabled";
 370                 break;
 371         case 0x01:
 372                 str = "Enabled";
 373                 break;
 374         default:
 375                 str = "Reserved";
 376                 break;
 377         }
 378
 379         print_field("Mode: %s (0x%2.2x)", str, pdu->mode);
 380         packet_print_channel_map_lmp(pdu->map);
 381 }
 382
 383 static void encapsulated_header(const void *data, uint8_t size)
 384 {
 385         const struct bt_lmp_encapsulated_header *pdu = data;
 386         const char *str;
 387
 388         print_field("Major type: %u", pdu->major);
 389         print_field("Minor type: %u", pdu->minor);
 390
 391         if (pdu->major == 0x01) {
 392                 switch (pdu->minor) {
 393                 case 0x01:
 394                         str = "P-192 Public Key";
 395                         break;
 396                 case 0x02:
 397                         str = "P-256 Public Key";
 398                         break;
 399                 default:
 400                         str = "Reserved";
 401                         break;
 402                 }
 403
 404                 print_field("  %s", str);
 405         }
 406
 407         print_field("Length: %u", pdu->length);
 408 }
 409
 410 static void encapsulated_payload(const void *data, uint8_t size)
 411 {
 412         const struct bt_lmp_encapsulated_payload *pdu = data;
 413
 414         packet_hexdump(pdu->data, 16);
 415 }
 416
 417 static void simple_pairing_confirm(const void *data, uint8_t size)
 418 {
 419         const struct bt_lmp_simple_pairing_confirm *pdu = data;
 420
 421         packet_hexdump(pdu->value, 16);
 422 }
 423
 424 static void simple_pairing_number(const void *data, uint8_t size)
 425 {
 426         const struct bt_lmp_simple_pairing_number *pdu = data;
 427
 428         packet_hexdump(pdu->value, 16);
 429 }
 430
 431 static void dhkey_check(const void *data, uint8_t size)
 432 {
 433         const struct bt_lmp_dhkey_check *pdu = data;
 434
 435         packet_hexdump(pdu->value, 16);
 436 }
 437
 438 static void accepted_ext(const void *data, uint8_t size)
 439 {
 440         const struct bt_lmp_accepted_ext *pdu = data;
 441         uint16_t opcode;
 442
 443         switch (pdu->escape) {
 444         case 127:
 445                 opcode = LMP_ESC4(pdu->opcode);
 446                 break;
 447         default:
 448                 return;
 449         }
 450
 451         print_opcode(opcode);
 452 }
 453
 454 static void not_accepted_ext(const void *data, uint8_t size)
 455 {
 456         const struct bt_lmp_not_accepted_ext *pdu = data;
 457         uint16_t opcode;
 458
 459         switch (pdu->escape) {
 460         case 127:
 461                 opcode = LMP_ESC4(pdu->opcode);
 462                 break;
 463         default:
 464                 return;
 465         }
 466
 467         print_opcode(opcode);
 468         print_field("Error code: %u", pdu->error);
 469 }
 470
 471 static void features_req_ext(const void *data, uint8_t size)
 472 {
 473         const struct bt_lmp_features_req_ext *pdu = data;
 474
 475         print_field("Features page: %u", pdu->page);
 476         print_field("Max supported page: %u", pdu->max_page);
 477         packet_print_features_lmp(pdu->features, pdu->page);
 478 }
 479
 480 static void features_res_ext(const void *data, uint8_t size)
 481 {
 482         const struct bt_lmp_features_res_ext *pdu = data;
 483
 484         print_field("Features page: %u", pdu->page);
 485         print_field("Max supported page: %u", pdu->max_page);
 486         packet_print_features_lmp(pdu->features, pdu->page);
 487 }
 488
 489 static void packet_type_table_req(const void *data, uint8_t size)
 490 {
 491         const struct bt_lmp_packet_type_table_req *pdu = data;
 492         const char *str;
 493
 494         switch (pdu->table) {
 495         case 0x00:
 496                 str = "1 Mbps only";
 497                 break;
 498         case 0x01:
 499                 str = "2/3 Mbps";
 500                 break;
 501         default:
 502                 str = "Reserved";
 503                 break;
 504         }
 505
 506         print_field("Table: %s (0x%2.2x)", str, pdu->table);
 507 }
 508
 509 static void channel_classification_req(const void *data, uint8_t size)
 510 {
 511         const struct bt_lmp_channel_classification_req *pdu = data;
 512         const char *str;
 513
 514         switch (pdu->mode) {
 515         case 0x00:
 516                 str = "Disabled";
 517                 break;
 518         case 0x01:
 519                 str = "Enabled";
 520                 break;
 521         default:
 522                 str = "Reserved";
 523                 break;
 524         }
 525
 526         print_field("Reporting mode: %s (0x%2.2x)", str, pdu->mode);
 527         print_field("Min interval: 0x%2.2x", pdu->min_interval);
 528         print_field("Max interval: 0x%2.2x", pdu->max_interval);
 529 }
 530
 531 static void channel_classification(const void *data, uint8_t size)
 532 {
 533         const struct bt_lmp_channel_classification *pdu = data;
 534         char str[21];
 535         int i;
 536
 537         for (i = 0; i < 10; i++)
 538                 sprintf(str + (i * 2), "%2.2x", pdu->classification[i]);
 539
 540         print_field("Classification: 0x%s", str);
 541 }
 542
 543 static void pause_encryption_req(const void *data, uint8_t size)
 544 {
 545 }
 546
 547 static void resume_encryption_req(const void *data, uint8_t size)
 548 {
 549 }
 550
 551 static void io_capability_req(const void *data, uint8_t size)
 552 {
 553         const struct bt_lmp_io_capability_req *pdu = data;
 554         const char *str;
 555
 556         packet_print_io_capability(pdu->capability);
 557
 558         switch (pdu->oob_data) {
 559         case 0x00:
 560                 str = "No authentication data received";
 561                 break;
 562         case 0x01:
 563                 str = "Authentication data received";
 564                 break;
 565         default:
 566                 str = "Reserved";
 567                 break;
 568         }
 569
 570         print_field("OOB data: %s (0x%2.2x)", str, pdu->oob_data);
 571
 572         packet_print_io_authentication(pdu->authentication);
 573 }
 574
 575 static void io_capability_res(const void *data, uint8_t size)
 576 {
 577         const struct bt_lmp_io_capability_res *pdu = data;
 578         const char *str;
 579
 580         packet_print_io_capability(pdu->capability);
 581
 582         switch (pdu->oob_data) {
 583         case 0x00:
 584                 str = "No authentication data received";
 585                 break;
 586         case 0x01:
 587                 str = "Authentication data received";
 588                 break;
 589         default:
 590                 str = "Reserved";
 591                 break;
 592         }
 593
 594         print_field("OOB data: %s (0x%2.2x)", str, pdu->oob_data);
 595
 596         packet_print_io_authentication(pdu->authentication);
 597 }
 598
 599 static void numeric_comparison_failed(const void *data, uint8_t size)
 600 {
 601 }
 602
 603 static void passkey_failed(const void *data, uint8_t size)
 604 {
 605 }
 606
 607 static void oob_failed(const void *data, uint8_t size)
 608 {
 609 }
 610
 611 static void power_control_req(const void *data, uint8_t size)
 612 {
 613         const struct bt_lmp_power_control_req *pdu = data;
 614         const char *str;
 615
 616         switch (pdu->request) {
 617         case 0x00:
 618                 str = "Decrement power one step";
 619                 break;
 620         case 0x01:
 621                 str = "Increment power one step";
 622                 break;
 623         case 0x02:
 624                 str = "Increase to maximum power";
 625                 break;
 626         default:
 627                 str = "Reserved";
 628                 break;
 629         }
 630
 631         print_field("Request: %s (0x%2.2x)", str, pdu->request);
 632 }
 633
 634 static void power_control_res(const void *data, uint8_t size)
 635 {
 636         const struct bt_lmp_power_control_res *pdu = data;
 637         const char *str;
 638
 639         print_field("Response: 0x%2.2x", pdu->response);
 640
 641         switch (pdu->response & 0x03) {
 642         case 0x00:
 643                 str = "Not supported";
 644                 break;
 645         case 0x01:
 646                 str = "Changed one step";
 647                 break;
 648         case 0x02:
 649                 str = "Max power";
 650                 break;
 651         case 0x03:
 652                 str = "Min power";
 653                 break;
 654         default:
 655                 str = "Reserved";
 656                 break;
 657         }
 658
 659         print_field("  GFSK: %s", str);
 660
 661         switch ((pdu->response & 0x0c) >> 2) {
 662         case 0x00:
 663                 str = "Not supported";
 664                 break;
 665         case 0x01:
 666                 str = "Changed one step";
 667                 break;
 668         case 0x02:
 669                 str = "Max power";
 670                 break;
 671         case 0x03:
 672                 str = "Min power";
 673                 break;
 674         default:
 675                 str = "Reserved";
 676                 break;
 677         }
 678
 679         print_field("  DQPSK: %s", str);
 680
 681         switch ((pdu->response & 0x30) >> 4) {
 682         case 0x00:
 683                 str = "Not supported";
 684                 break;
 685         case 0x01:
 686                 str = "Changed one step";
 687                 break;
 688         case 0x02:
 689                 str = "Max power";
 690                 break;
 691         case 0x03:
 692                 str = "Min power";
 693                 break;
 694         default:
 695                 str = "Reserved";
 696                 break;
 697         }
 698
 699         print_field("  8DPSK: %s", str);
 700 }
 701
 702 static void ping_req(const void *data, uint8_t size)
 703 {
 704 }
 705
 706 static void ping_res(const void *data, uint8_t size)
 707 {
 708 }
 709
 710 struct lmp_data {
 711         uint16_t opcode;
 712         const char *str;
 713         void (*func) (const void *data, uint8_t size);
 714         uint8_t size;
 715         bool fixed;
 716 };
 717
 718 static const struct lmp_data lmp_table[] = {
 719         {  1, "LMP_name_req", name_req, 1, true },
 720         {  2, "LMP_name_res", name_rsp, 16, true },
 721         {  3, "LMP_accepted", accepted, 1, true },
 722         {  4, "LMP_not_accepted", not_accepted, 2, true },
 723         {  5, "LMP_clkoffset_req", clkoffset_req, 0, true },
 724         {  6, "LMP_clkoffset_res", clkoffset_rsp, 2, true },
 725         {  7, "LMP_detach", detach, 1, true },
 726         {  8, "LMP_in_rand" },
 727         {  9, "LMP_comb_key" },
 728         { 10, "LMP_unit_key" },
 729         { 11, "LMP_au_rand", au_rand, 16, true },
 730         { 12, "LMP_sres", sres, 4, true },
 731         { 13, "LMP_temp_rand" },
 732         { 14, "LMP_temp_key" },
 733         { 15, "LMP_encryption_mode_req", encryption_mode_req, 1, true },
 734         { 16, "LMP_encryption_key_size_req", encryption_key_size_req, 1, true },
 735         { 17, "LMP_start_encryption_req", start_encryption_req, 16, true },
 736         { 18, "LMP_stop_encryption_req", stop_encryption_req, 0, true },
 737         { 19, "LMP_switch_req", switch_req, 4, true },
 738         { 20, "LMP_hold" },
 739         { 21, "LMP_hold_req" },
 740         { 22, "LMP_sniff" },
 741         { 23, "LMP_sniff_req" },
 742         { 24, "LMP_unsniff_req", unsniff_req, 0, true },
 743         { 25, "LMP_park_req" },
 744         { 26, "LMP_park" },
 745         { 27, "LMP_set_broadcast_scan_window" },
 746         { 28, "LMP_modify_beacon" },
 747         { 29, "LMP_unpark_BD_ADDR_req" },
 748         { 30, "LMP_unpark_PM_ADDR_req" },
 749         { 31, "LMP_incr_power_req" },
 750         { 32, "LMP_decr_power_req" },
 751         { 33, "LMP_max_power", max_power, 0, true },
 752         { 34, "LMP_min_power", min_power, 0, true },
 753         { 35, "LMP_auto_rate", auto_rate, 0, true },
 754         { 36, "LMP_preferred_rate", preferred_rate, 1, true },
 755         { 37, "LMP_version_req", version_req, 5, true },
 756         { 38, "LMP_version_res", version_res, 5, true },
 757         { 39, "LMP_features_req", features_req, 8, true },
 758         { 40, "LMP_features_res", features_res, 8, true },
 759         { 41, "LMP_quality_of_service" },
 760         { 42, "LMP_quality_of_service_req" },
 761         { 43, "LMP_SCO_link_req" },
 762         { 44, "LMP_remove_SCO_link_req" },
 763         { 45, "LMP_max_slot", max_slot, 1, true },
 764         { 46, "LMP_max_slot_req", max_slot_req, 1, true },
 765         { 47, "LMP_timing_accuracy_req", timing_accuracy_req, 0, true },
 766         { 48, "LMP_timing_accuracy_res", timing_accuracy_res, 2, true },
 767         { 49, "LMP_setup_complete", setup_complete, 0, true },
 768         { 50, "LMP_use_semi_permanent_key", use_semi_permanent_key, 0, true },
 769         { 51, "LMP_host_connection_req", host_connection_req, 0, true },
 770         { 52, "LMP_slot_offset", slot_offset, 8, true },
 771         { 53, "LMP_page_mode_req" },
 772         { 54, "LMP_page_scan_mode_req", page_scan_mode_req, 2, true },
 773         { 55, "LMP_supervision_timeout" },
 774         { 56, "LMP_test_activate", test_activate, 0, true },
 775         { 57, "LMP_test_control" },
 776         { 58, "LMP_encryption_key_size_mask_req", encryption_key_size_mask_req, 0, true },
 777         { 59, "LMP_encryption_key_size_mask_res" },
 778         { 60, "LMP_set_AFH", set_afh, 15, true },
 779         { 61, "LMP_encapsulated_header", encapsulated_header, 3, true },
 780         { 62, "LMP_encapsulated_payload", encapsulated_payload, 16, true },
 781         { 63, "LMP_simple_pairing_confirm", simple_pairing_confirm, 16, true },
 782         { 64, "LMP_simple_pairing_number", simple_pairing_number, 16, true },
 783         { 65, "LMP_DHkey_check", dhkey_check, 16, true },
 784         { 66, "LMP_pause_encryption_aes_req" },
 785         { LMP_ESC4(1),  "LMP_accepted_ext", accepted_ext, 2, true },
 786         { LMP_ESC4(2),  "LMP_not_accepted_ext", not_accepted_ext, 3, true },
 787         { LMP_ESC4(3),  "LMP_features_req_ext", features_req_ext, 10, true },
 788         { LMP_ESC4(4),  "LMP_features_res_ext", features_res_ext, 10, true },
 789         { LMP_ESC4(5),  "LMP_clk_adj" },
 790         { LMP_ESC4(6),  "LMP_clk_adj_ack" },
 791         { LMP_ESC4(7),  "LMP_clk_adj_req" },
 792         { LMP_ESC4(11), "LMP_packet_type_table_req", packet_type_table_req, 1, true },
 793         { LMP_ESC4(12), "LMP_eSCO_link_req" },
 794         { LMP_ESC4(13), "LMP_remove_eSCO_link_req" },
 795         { LMP_ESC4(16), "LMP_channel_classification_req", channel_classification_req, 5, true },
 796         { LMP_ESC4(17), "LMP_channel_classification", channel_classification, 10, true },
 797         { LMP_ESC4(21), "LMP_sniff_subrating_req" },
 798         { LMP_ESC4(22), "LMP_sniff_subrating_res" },
 799         { LMP_ESC4(23), "LMP_pause_encryption_req", pause_encryption_req, 0, true },
 800         { LMP_ESC4(24), "LMP_resume_encryption_req", resume_encryption_req, 0, true },
 801         { LMP_ESC4(25), "LMP_IO_capability_req", io_capability_req, 3, true },
 802         { LMP_ESC4(26), "LMP_IO_capability_res", io_capability_res, 3, true },
 803         { LMP_ESC4(27), "LMP_numeric_comparison_failed", numeric_comparison_failed, 0, true },
 804         { LMP_ESC4(28), "LMP_passkey_failed", passkey_failed, 0, true },
 805         { LMP_ESC4(29), "LMP_oob_failed", oob_failed, 0, true },
 806         { LMP_ESC4(30), "LMP_keypress_notification" },
 807         { LMP_ESC4(31), "LMP_power_control_req", power_control_req, 1, true },
 808         { LMP_ESC4(32), "LMP_power_control_res", power_control_res, 1, true },
 809         { LMP_ESC4(33), "LMP_ping_req", ping_req, 0, true },
 810         { LMP_ESC4(34), "LMP_ping_res", ping_res, 0, true },
 811         { LMP_ESC4(35), "LMP_SAM_set_type0" },
 812         { LMP_ESC4(36), "LMP_SAM_define_map" },
 813         { LMP_ESC4(37), "LMP_SAM_switch" },
 814         { }
 815 };
 816
 817 static const char *get_opcode_str(uint16_t opcode)
 818 {
 819         int i;
 820
 821         for (i = 0; lmp_table[i].str; i++) {
 822                 if (lmp_table[i].opcode == opcode)
 823                         return lmp_table[i].str;
 824         }
 825
 826         return NULL;
 827 }
 828
 829 void lmp_packet(const void *data, uint8_t size, bool padded)
 830 {
 831         const struct lmp_data *lmp_data = NULL;
 832         const char *opcode_color, *opcode_str;
 833         uint16_t opcode;
 834         uint8_t tid, off;
 835         const char *tid_str;
 836         int i;
 837
 838         tid = ((const uint8_t *) data)[0] & 0x01;
 839         opcode = (((const uint8_t *) data)[0] & 0xfe) >> 1;
 840
 841         tid_str = tid == 0x00 ? "Central" : "Peripheral";
 842
 843         switch (opcode) {
 844         case 127:
 845                 if (size < 2) {
 846                         print_text(COLOR_ERROR, "extended opcode too short");
 847                         packet_hexdump(data, size);
 848                         return;
 849                 }
 850                 opcode = LMP_ESC4(((const uint8_t *) data)[1]);
 851                 off = 2;
 852                 break;
 853         case 126:
 854         case 125:
 855         case 124:
 856                 return;
 857         default:
 858                 off = 1;
 859                 break;
 860         }
 861
 862         for (i = 0; lmp_table[i].str; i++) {
 863                 if (lmp_table[i].opcode == opcode) {
 864                         lmp_data = &lmp_table[i];
 865                         break;
 866                 }
 867         }
 868
 869         if (lmp_data) {
 870                 if (lmp_data->func)
 871                         opcode_color = COLOR_OPCODE;
 872                 else
 873                         opcode_color = COLOR_OPCODE_UNKNOWN;
 874                 opcode_str = lmp_data->str;
 875         } else {
 876                 opcode_color = COLOR_OPCODE_UNKNOWN;
 877                 opcode_str = "Unknown";
 878         }
 879
 880         if (opcode & 0xff00)
 881                 print_indent(6, opcode_color, "", opcode_str, COLOR_OFF,
 882                                 " (%u/%u) %s transaction (%u)",
 883                                 opcode >> 8, opcode & 0xff, tid_str, tid);
 884         else
 885                 print_indent(6, opcode_color, "", opcode_str, COLOR_OFF,
 886                                 " (%u) %s transaction (%d)",
 887                                 opcode, tid_str, tid);
 888
 889         if (!lmp_data || !lmp_data->func) {
 890                 packet_hexdump(data + off, size - off);
 891                 return;
 892         }
 893
 894         if (lmp_data->fixed && !padded) {
 895                 if (size - off != lmp_data->size) {
 896                         print_text(COLOR_ERROR, "invalid packet size");
 897                         packet_hexdump(data + off, size - off);
 898                         return;
 899                 }
 900         } else {
 901                 if (size - off < lmp_data->size) {
 902                         print_text(COLOR_ERROR, "too short packet");
 903                         packet_hexdump(data + off, size - off);
 904                         return;
 905                 }
 906         }
 907
 908         lmp_data->func(data + off, size - off);
 909 }
 910
 911 void lmp_todo(void)
 912 {
 913         int i;
 914
 915         printf("LMP operations with missing decodings:\n");
 916
 917         for (i = 0; lmp_table[i].str; i++) {
 918                 if (lmp_table[i].func)
 919                         continue;
 920
 921                 printf("\t%s\n", lmp_table[i].str);
 922         }
 923 }