monitor/lmp.c

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