monitor/l2cap.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 <stdlib.h>
  18 #include <string.h>
  19 #include <inttypes.h>
  20
  21 #include "lib/bluetooth.h"
  22 #include "lib/uuid.h"
  23
  24 #include "src/shared/util.h"
  25 #include "src/shared/queue.h"
  26 #include "bt.h"
  27 #include "packet.h"
  28 #include "display.h"
  29 #include "l2cap.h"
  30 #include "keys.h"
  31 #include "sdp.h"
  32 #include "avctp.h"
  33 #include "avdtp.h"
  34 #include "rfcomm.h"
  35 #include "bnep.h"
  36 #include "att.h"
  37
  38 #define L2CAP_MODE_BASIC                0x00
  39 #define L2CAP_MODE_RETRANS              0x01
  40 #define L2CAP_MODE_FLOWCTL              0x02
  41 #define L2CAP_MODE_ERTM                 0x03
  42 #define L2CAP_MODE_STREAMING            0x04
  43 #define L2CAP_MODE_LE_FLOWCTL           0x80
  44 #define L2CAP_MODE_ECRED                0x81
  45
  46 /* L2CAP Control Field bit masks */
  47 #define L2CAP_CTRL_SAR_MASK             0xC000
  48 #define L2CAP_CTRL_REQSEQ_MASK          0x3F00
  49 #define L2CAP_CTRL_TXSEQ_MASK           0x007E
  50 #define L2CAP_CTRL_SUPERVISE_MASK       0x000C
  51
  52 #define L2CAP_CTRL_RETRANS              0x0080
  53 #define L2CAP_CTRL_FINAL                0x0080
  54 #define L2CAP_CTRL_POLL                 0x0010
  55 #define L2CAP_CTRL_FRAME_TYPE           0x0001 /* I- or S-Frame */
  56
  57 #define L2CAP_CTRL_TXSEQ_SHIFT          1
  58 #define L2CAP_CTRL_SUPER_SHIFT          2
  59 #define L2CAP_CTRL_REQSEQ_SHIFT         8
  60 #define L2CAP_CTRL_SAR_SHIFT            14
  61
  62 #define L2CAP_EXT_CTRL_TXSEQ_MASK       0xFFFC0000
  63 #define L2CAP_EXT_CTRL_SAR_MASK         0x00030000
  64 #define L2CAP_EXT_CTRL_SUPERVISE_MASK   0x00030000
  65 #define L2CAP_EXT_CTRL_REQSEQ_MASK      0x0000FFFC
  66
  67 #define L2CAP_EXT_CTRL_POLL             0x00040000
  68 #define L2CAP_EXT_CTRL_FINAL            0x00000002
  69 #define L2CAP_EXT_CTRL_FRAME_TYPE       0x00000001 /* I- or S-Frame */
  70
  71 #define L2CAP_EXT_CTRL_REQSEQ_SHIFT     2
  72 #define L2CAP_EXT_CTRL_SAR_SHIFT        16
  73 #define L2CAP_EXT_CTRL_SUPER_SHIFT      16
  74 #define L2CAP_EXT_CTRL_TXSEQ_SHIFT      18
  75
  76 /* L2CAP Supervisory Function */
  77 #define L2CAP_SUPER_RR          0x00
  78 #define L2CAP_SUPER_REJ         0x01
  79 #define L2CAP_SUPER_RNR         0x02
  80 #define L2CAP_SUPER_SREJ        0x03
  81
  82 /* L2CAP Segmentation and Reassembly */
  83 #define L2CAP_SAR_UNSEGMENTED   0x00
  84 #define L2CAP_SAR_START         0x01
  85 #define L2CAP_SAR_END           0x02
  86 #define L2CAP_SAR_CONTINUE      0x03
  87
  88 #define MAX_CHAN 64
  89
  90 struct chan_data {
  91         uint16_t index;
  92         uint16_t handle;
  93         uint8_t ident;
  94         uint16_t scid;
  95         uint16_t dcid;
  96         uint16_t psm;
  97         uint8_t  ctrlid;
  98         uint8_t  mode;
  99         uint8_t  ext_ctrl;
 100         uint8_t  seq_num;
 101         uint16_t sdu;
 102         struct packet_latency tx_l;
 103 };
 104
 105 static struct chan_data chan_list[MAX_CHAN];
 106
 107 static void assign_scid(const struct l2cap_frame *frame, uint16_t scid,
 108                         uint16_t psm, uint8_t mode, uint8_t ctrlid)
 109 {
 110         int i, n = -1;
 111         uint8_t seq_num = 1;
 112
 113         if (!scid)
 114                 return;
 115
 116         for (i = 0; i < MAX_CHAN; i++) {
 117                 if (n < 0 && chan_list[i].handle == 0x0000) {
 118                         n = i;
 119                         continue;
 120                 }
 121
 122                 if (chan_list[i].index != frame->index)
 123                         continue;
 124
 125                 if (chan_list[i].handle != frame->handle)
 126                         continue;
 127
 128                 if (chan_list[i].psm == psm)
 129                         seq_num++;
 130
 131                 /* Don't break on match - we still need to go through all
 132                  * channels to find proper seq_num.
 133                  */
 134                 if (frame->in) {
 135                         if (chan_list[i].dcid == scid)
 136                                 n = i;
 137                 } else {
 138                         if (chan_list[i].scid == scid)
 139                                 n = i;
 140                 }
 141         }
 142
 143         if (n < 0)
 144                 return;
 145
 146         memset(&chan_list[n], 0, sizeof(chan_list[n]));
 147         chan_list[n].index = frame->index;
 148         chan_list[n].handle = frame->handle;
 149         chan_list[n].ident = frame->ident;
 150
 151         if (frame->in)
 152                 chan_list[n].dcid = scid;
 153         else
 154                 chan_list[n].scid = scid;
 155
 156         chan_list[n].psm = psm;
 157         chan_list[n].ctrlid = ctrlid;
 158         chan_list[n].mode = mode;
 159
 160         chan_list[n].seq_num = seq_num;
 161 }
 162
 163 static void release_scid(const struct l2cap_frame *frame, uint16_t scid)
 164 {
 165         int i;
 166
 167         for (i = 0; i < MAX_CHAN; i++) {
 168                 if (chan_list[i].index != frame->index)
 169                         continue;
 170
 171                 if (chan_list[i].handle != frame->handle)
 172                         continue;
 173
 174                 if (frame->in) {
 175                         if (chan_list[i].scid == scid) {
 176                                 chan_list[i].handle = 0;
 177                                 break;
 178                         }
 179                 } else {
 180                         if (chan_list[i].dcid == scid) {
 181                                 chan_list[i].handle = 0;
 182                                 break;
 183                         }
 184                 }
 185         }
 186 }
 187
 188 static void assign_dcid(const struct l2cap_frame *frame, uint16_t dcid,
 189                                                                 uint16_t scid)
 190 {
 191         int i;
 192
 193         for (i = 0; i < MAX_CHAN; i++) {
 194                 if (chan_list[i].index != frame->index)
 195                         continue;
 196
 197                 if (chan_list[i].handle != frame->handle)
 198                         continue;
 199
 200                 if (frame->ident != 0 && chan_list[i].ident != frame->ident)
 201                         continue;
 202
 203                 if (frame->in) {
 204                         if (scid) {
 205                                 if (chan_list[i].scid == scid) {
 206                                         chan_list[i].dcid = dcid;
 207                                         break;
 208                                 }
 209                         } else {
 210                                 if (chan_list[i].scid && !chan_list[i].dcid) {
 211                                         chan_list[i].dcid = dcid;
 212                                         break;
 213                                 }
 214                         }
 215                 } else {
 216                         if (scid) {
 217                                 if (chan_list[i].dcid == scid) {
 218                                         chan_list[i].scid = dcid;
 219                                         break;
 220                                 }
 221                         } else {
 222                                 if (chan_list[i].dcid && !chan_list[i].scid) {
 223                                         chan_list[i].scid = dcid;
 224                                         break;
 225                                 }
 226                         }
 227                 }
 228         }
 229 }
 230
 231 static void assign_mode(const struct l2cap_frame *frame,
 232                                         uint8_t mode, uint16_t dcid)
 233 {
 234         int i;
 235
 236         for (i = 0; i < MAX_CHAN; i++) {
 237                 if (chan_list[i].index != frame->index)
 238                         continue;
 239
 240                 if (chan_list[i].handle != frame->handle)
 241                         continue;
 242
 243                 if (frame->in) {
 244                         if (chan_list[i].scid == dcid) {
 245                                 chan_list[i].mode = mode;
 246                                 break;
 247                         }
 248                 } else {
 249                         if (chan_list[i].dcid == dcid) {
 250                                 chan_list[i].mode = mode;
 251                                 break;
 252                         }
 253                 }
 254         }
 255 }
 256
 257 static int get_chan_data_index(const struct l2cap_frame *frame)
 258 {
 259         int i;
 260
 261         for (i = 0; i < MAX_CHAN; i++) {
 262                 if (chan_list[i].index != frame->index &&
 263                                         chan_list[i].ctrlid == 0)
 264                         continue;
 265
 266                 if (chan_list[i].ctrlid != 0 &&
 267                                         chan_list[i].ctrlid != frame->index)
 268                         continue;
 269
 270                 if (chan_list[i].handle != frame->handle)
 271                         continue;
 272
 273                 if (frame->in) {
 274                         if (chan_list[i].scid == frame->cid)
 275                                 return i;
 276                 } else {
 277                         if (chan_list[i].dcid == frame->cid)
 278                                 return i;
 279                 }
 280         }
 281
 282         return -1;
 283 }
 284
 285 static struct chan_data *get_chan(const struct l2cap_frame *frame)
 286 {
 287         int i;
 288
 289         if (frame->chan != UINT16_MAX)
 290                 return &chan_list[frame->chan];
 291
 292         i = get_chan_data_index(frame);
 293         if (i < 0)
 294                 return NULL;
 295
 296         return &chan_list[i];
 297 }
 298
 299 static uint16_t get_psm(const struct l2cap_frame *frame)
 300 {
 301         struct chan_data *data = get_chan(frame);
 302
 303         if (!data)
 304                 return 0;
 305
 306         return data->psm;
 307 }
 308
 309 static uint8_t get_mode(const struct l2cap_frame *frame)
 310 {
 311         struct chan_data *data = get_chan(frame);
 312
 313         if (!data)
 314                 return 0;
 315
 316         return data->mode;
 317 }
 318
 319 static uint8_t get_seq_num(const struct l2cap_frame *frame)
 320 {
 321         struct chan_data *data = get_chan(frame);
 322
 323         if (!data)
 324                 return 0;
 325
 326         return data->seq_num;
 327 }
 328
 329 static void assign_ext_ctrl(const struct l2cap_frame *frame,
 330                                         uint8_t ext_ctrl, uint16_t dcid)
 331 {
 332         int i;
 333
 334         for (i = 0; i < MAX_CHAN; i++) {
 335                 if (chan_list[i].index != frame->index)
 336                         continue;
 337
 338                 if (chan_list[i].handle != frame->handle)
 339                         continue;
 340
 341                 if (frame->in) {
 342                         if (chan_list[i].scid == dcid) {
 343                                 chan_list[i].ext_ctrl = ext_ctrl;
 344                                 break;
 345                         }
 346                 } else {
 347                         if (chan_list[i].dcid == dcid) {
 348                                 chan_list[i].ext_ctrl = ext_ctrl;
 349                                 break;
 350                         }
 351                 }
 352         }
 353 }
 354
 355 static uint8_t get_ext_ctrl(const struct l2cap_frame *frame)
 356 {
 357         struct chan_data *data = get_chan(frame);
 358
 359         if (!data)
 360                 return 0;
 361
 362         return data->ext_ctrl;
 363 }
 364
 365 static char *sar2str(uint8_t sar)
 366 {
 367         switch (sar) {
 368         case L2CAP_SAR_UNSEGMENTED:
 369                 return "Unsegmented";
 370         case L2CAP_SAR_START:
 371                 return "Start";
 372         case L2CAP_SAR_END:
 373                 return "End";
 374         case L2CAP_SAR_CONTINUE:
 375                 return "Continuation";
 376         default:
 377                 return "Bad SAR";
 378         }
 379 }
 380
 381 static char *supervisory2str(uint8_t supervisory)
 382 {
 383         switch (supervisory) {
 384         case L2CAP_SUPER_RR:
 385                 return "Receiver Ready (RR)";
 386         case L2CAP_SUPER_REJ:
 387                 return "Reject (REJ)";
 388         case L2CAP_SUPER_RNR:
 389                 return "Receiver Not Ready (RNR)";
 390         case L2CAP_SUPER_SREJ:
 391                 return "Select Reject (SREJ)";
 392         default:
 393                 return "Bad Supervisory";
 394         }
 395 }
 396
 397 static char *mode2str(uint8_t mode)
 398 {
 399         switch (mode) {
 400         case L2CAP_MODE_BASIC:
 401                 return "Basic";
 402         case L2CAP_MODE_RETRANS:
 403                 return "Retransmission";
 404         case L2CAP_MODE_FLOWCTL:
 405                 return "Flow Control";
 406         case L2CAP_MODE_ERTM:
 407                 return "Enhanced Retransmission";
 408         case L2CAP_MODE_STREAMING:
 409                 return "Streaming";
 410         case L2CAP_MODE_LE_FLOWCTL:
 411                 return "LE Flow Control";
 412         case L2CAP_MODE_ECRED:
 413                 return "Enhanced Credit";
 414         default:
 415                 return "Unknown";
 416         }
 417 }
 418
 419 static void l2cap_ctrl_ext_parse(struct l2cap_frame *frame, uint32_t ctrl)
 420 {
 421         printf("      %s:",
 422                 ctrl & L2CAP_EXT_CTRL_FRAME_TYPE ? "S-frame" : "I-frame");
 423
 424         if (ctrl & L2CAP_EXT_CTRL_FRAME_TYPE) {
 425                 printf(" %s",
 426                 supervisory2str((ctrl & L2CAP_EXT_CTRL_SUPERVISE_MASK) >>
 427                                                 L2CAP_EXT_CTRL_SUPER_SHIFT));
 428
 429                 if (ctrl & L2CAP_EXT_CTRL_POLL)
 430                         printf(" P-bit");
 431         } else {
 432                 uint8_t sar = (ctrl & L2CAP_EXT_CTRL_SAR_MASK) >>
 433                                                 L2CAP_EXT_CTRL_SAR_SHIFT;
 434                 printf(" %s", sar2str(sar));
 435                 if (sar == L2CAP_SAR_START) {
 436                         uint16_t len;
 437
 438                         if (!l2cap_frame_get_le16(frame, &len))
 439                                 return;
 440
 441                         printf(" (len %d)", len);
 442                 }
 443                 printf(" TxSeq %d", (ctrl & L2CAP_EXT_CTRL_TXSEQ_MASK) >>
 444                                                 L2CAP_EXT_CTRL_TXSEQ_SHIFT);
 445         }
 446
 447         printf(" ReqSeq %d", (ctrl & L2CAP_EXT_CTRL_REQSEQ_MASK) >>
 448                                                 L2CAP_EXT_CTRL_REQSEQ_SHIFT);
 449
 450         if (ctrl & L2CAP_EXT_CTRL_FINAL)
 451                 printf(" F-bit");
 452 }
 453
 454 static void l2cap_ctrl_parse(struct l2cap_frame *frame, uint32_t ctrl)
 455 {
 456         printf("      %s:",
 457                         ctrl & L2CAP_CTRL_FRAME_TYPE ? "S-frame" : "I-frame");
 458
 459         if (ctrl & 0x01) {
 460                 printf(" %s",
 461                         supervisory2str((ctrl & L2CAP_CTRL_SUPERVISE_MASK) >>
 462                                                 L2CAP_CTRL_SUPER_SHIFT));
 463
 464                 if (ctrl & L2CAP_CTRL_POLL)
 465                         printf(" P-bit");
 466         } else {
 467                 uint8_t sar;
 468
 469                 sar = (ctrl & L2CAP_CTRL_SAR_MASK) >> L2CAP_CTRL_SAR_SHIFT;
 470                 printf(" %s", sar2str(sar));
 471                 if (sar == L2CAP_SAR_START) {
 472                         uint16_t len;
 473
 474                         if (!l2cap_frame_get_le16(frame, &len))
 475                                 return;
 476
 477                         printf(" (len %d)", len);
 478                 }
 479                 printf(" TxSeq %d", (ctrl & L2CAP_CTRL_TXSEQ_MASK) >>
 480                                                 L2CAP_CTRL_TXSEQ_SHIFT);
 481         }
 482
 483         printf(" ReqSeq %d", (ctrl & L2CAP_CTRL_REQSEQ_MASK) >>
 484                                                 L2CAP_CTRL_REQSEQ_SHIFT);
 485
 486         if (ctrl & L2CAP_CTRL_FINAL)
 487                 printf(" F-bit");
 488 }
 489
 490 #define MAX_INDEX 16
 491
 492 struct index_data {
 493         void *frag_buf;
 494         uint16_t frag_pos;
 495         uint16_t frag_len;
 496         uint16_t frag_cid;
 497 };
 498
 499 static struct index_data index_list[MAX_INDEX][2];
 500
 501 static void clear_fragment_buffer(uint16_t index, bool in)
 502 {
 503         free(index_list[index][in].frag_buf);
 504         index_list[index][in].frag_buf = NULL;
 505         index_list[index][in].frag_pos = 0;
 506         index_list[index][in].frag_len = 0;
 507 }
 508
 509 static void print_psm(uint16_t psm)
 510 {
 511         print_field("PSM: %d (0x%4.4x)", le16_to_cpu(psm), le16_to_cpu(psm));
 512 }
 513
 514 static void print_cid(const char *type, uint16_t cid)
 515 {
 516         print_field("%s CID: %d", type, le16_to_cpu(cid));
 517 }
 518
 519 static void print_reject_reason(uint16_t reason)
 520 {
 521         const char *str;
 522
 523         switch (le16_to_cpu(reason)) {
 524         case 0x0000:
 525                 str = "Command not understood";
 526                 break;
 527         case 0x0001:
 528                 str = "Signaling MTU exceeded";
 529                 break;
 530         case 0x0002:
 531                 str = "Invalid CID in request";
 532                 break;
 533         default:
 534                 str = "Reserved";
 535                 break;
 536         }
 537
 538         print_field("Reason: %s (0x%4.4x)", str, le16_to_cpu(reason));
 539 }
 540
 541 static void print_conn_result(uint16_t result)
 542 {
 543         const char *str;
 544
 545         switch (le16_to_cpu(result)) {
 546         case 0x0000:
 547                 str = "Connection successful";
 548                 break;
 549         case 0x0001:
 550                 str = "Connection pending";
 551                 break;
 552         case 0x0002:
 553                 str = "Connection refused - PSM not supported";
 554                 break;
 555         case 0x0003:
 556                 str = "Connection refused - security block";
 557                 break;
 558         case 0x0004:
 559                 str = "Connection refused - no resources available";
 560                 break;
 561         case 0x0006:
 562                 str = "Connection refused - Invalid Source CID";
 563                 break;
 564         case 0x0007:
 565                 str = "Connection refused - Source CID already allocated";
 566                 break;
 567         default:
 568                 str = "Reserved";
 569                 break;
 570         }
 571
 572         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
 573 }
 574
 575 static void print_le_conn_result(uint16_t result)
 576 {
 577         const char *str;
 578
 579         switch (le16_to_cpu(result)) {
 580         case 0x0000:
 581                 str = "Connection successful";
 582                 break;
 583         case 0x0002:
 584                 str = "Connection refused - PSM not supported";
 585                 break;
 586         case 0x0004:
 587                 str = "Connection refused - no resources available";
 588                 break;
 589         case 0x0005:
 590                 str = "Connection refused - insufficient authentication";
 591                 break;
 592         case 0x0006:
 593                 str = "Connection refused - insufficient authorization";
 594                 break;
 595         case 0x0007:
 596                 str = "Connection refused - insufficient encryption key size";
 597                 break;
 598         case 0x0008:
 599                 str = "Connection refused - insufficient encryption";
 600                 break;
 601         case 0x0009:
 602                 str = "Connection refused - Invalid Source CID";
 603                 break;
 604         case 0x000a:
 605                 str = "Connection refused - Source CID already allocated";
 606                 break;
 607         case 0x000b:
 608                 str = "Connection refused - unacceptable parameters";
 609                 break;
 610         default:
 611                 str = "Reserved";
 612                 break;
 613         }
 614
 615         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
 616 }
 617
 618 static void print_create_chan_result(uint16_t result)
 619 {
 620         const char *str;
 621
 622         switch (le16_to_cpu(result)) {
 623         case 0x0000:
 624                 str = "Connection successful";
 625                 break;
 626         case 0x0001:
 627                 str = "Connection pending";
 628                 break;
 629         case 0x0002:
 630                 str = "Connection refused - PSM not supported";
 631                 break;
 632         case 0x0003:
 633                 str = "Connection refused - security block";
 634                 break;
 635         case 0x0004:
 636                 str = "Connection refused - no resources available";
 637                 break;
 638         case 0x0005:
 639                 str = "Connection refused - Controller ID not supported";
 640                 break;
 641         case 0x0006:
 642                 str = "Connection refused - Invalid Source CID";
 643                 break;
 644         case 0x0007:
 645                 str = "Connection refused - Source CID already allocated";
 646                 break;
 647         default:
 648                 str = "Reserved";
 649                 break;
 650         }
 651
 652         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
 653 }
 654
 655 static void print_conn_status(uint16_t status)
 656 {
 657         const char *str;
 658
 659         switch (le16_to_cpu(status)) {
 660         case 0x0000:
 661                 str = "No further information available";
 662                 break;
 663         case 0x0001:
 664                 str = "Authentication pending";
 665                 break;
 666         case 0x0002:
 667                 str = "Authorization pending";
 668                 break;
 669         default:
 670                 str = "Reserved";
 671                 break;
 672         }
 673
 674         print_field("Status: %s (0x%4.4x)", str, le16_to_cpu(status));
 675 }
 676
 677 static void print_config_flags(uint16_t flags)
 678 {
 679         const char *str;
 680
 681         if (le16_to_cpu(flags) & 0x0001)
 682                 str = " (continuation)";
 683         else
 684                 str = "";
 685
 686         print_field("Flags: 0x%4.4x%s", le16_to_cpu(flags), str);
 687 }
 688
 689 static void print_config_result(uint16_t result)
 690 {
 691         const char *str;
 692
 693         switch (le16_to_cpu(result)) {
 694         case 0x0000:
 695                 str = "Success";
 696                 break;
 697         case 0x0001:
 698                 str = "Failure - unacceptable parameters";
 699                 break;
 700         case 0x0002:
 701                 str = "Failure - rejected";
 702                 break;
 703         case 0x0003:
 704                 str = "Failure - unknown options";
 705                 break;
 706         case 0x0004:
 707                 str = "Pending";
 708                 break;
 709         case 0x0005:
 710                 str = "Failure - flow spec rejected";
 711                 break;
 712         default:
 713                 str = "Reserved";
 714                 break;
 715         }
 716
 717         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
 718 }
 719
 720 static struct {
 721         uint8_t type;
 722         uint8_t len;
 723         const char *str;
 724 } options_table[] = {
 725         { 0x01,  2, "Maximum Transmission Unit"         },
 726         { 0x02,  2, "Flush Timeout"                     },
 727         { 0x03, 22, "Quality of Service"                },
 728         { 0x04,  9, "Retransmission and Flow Control"   },
 729         { 0x05,  1, "Frame Check Sequence"              },
 730         { 0x06, 16, "Extended Flow Specification"       },
 731         { 0x07,  2, "Extended Window Size"              },
 732         { }
 733 };
 734
 735 static void print_config_options(const struct l2cap_frame *frame,
 736                                 uint8_t offset, uint16_t cid, bool response)
 737 {
 738         const uint8_t *data = frame->data + offset;
 739         uint16_t size = frame->size - offset;
 740         uint16_t consumed = 0;
 741
 742         while (consumed < size - 2) {
 743                 const char *str = "Unknown";
 744                 uint8_t type = data[consumed] & 0x7f;
 745                 uint8_t hint = data[consumed] & 0x80;
 746                 uint8_t len = data[consumed + 1];
 747                 uint8_t expect_len = 0;
 748                 int i;
 749
 750                 for (i = 0; options_table[i].str; i++) {
 751                         if (options_table[i].type == type) {
 752                                 str = options_table[i].str;
 753                                 expect_len = options_table[i].len;
 754                                 break;
 755                         }
 756                 }
 757
 758                 print_field("Option: %s (0x%2.2x) [%s]", str, type,
 759                                                 hint ? "hint" : "mandatory");
 760
 761                 if (expect_len == 0) {
 762                         consumed += 2;
 763                         break;
 764                 }
 765
 766                 if (len != expect_len) {
 767                         print_text(COLOR_ERROR, "wrong option size (%d != %d)",
 768                                                         len, expect_len);
 769                         consumed += 2;
 770                         break;
 771                 }
 772
 773                 switch (type) {
 774                 case 0x01:
 775                         print_field("  MTU: %d",
 776                                         get_le16(data + consumed + 2));
 777                         break;
 778                 case 0x02:
 779                         print_field("  Flush timeout: %d",
 780                                         get_le16(data + consumed + 2));
 781                         break;
 782                 case 0x03:
 783                         switch (data[consumed + 3]) {
 784                         case 0x00:
 785                                 str = "No Traffic";
 786                                 break;
 787                         case 0x01:
 788                                 str = "Best Effort";
 789                                 break;
 790                         case 0x02:
 791                                 str = "Guaranteed";
 792                                 break;
 793                         default:
 794                                 str = "Reserved";
 795                                 break;
 796                         }
 797                         print_field("  Flags: 0x%2.2x", data[consumed + 2]);
 798                         print_field("  Service type: %s (0x%2.2x)",
 799                                                 str, data[consumed + 3]);
 800                         print_field("  Token rate: 0x%8.8x",
 801                                         get_le32(data + consumed + 4));
 802                         print_field("  Token bucket size: 0x%8.8x",
 803                                         get_le32(data + consumed + 8));
 804                         print_field("  Peak bandwidth: 0x%8.8x",
 805                                         get_le32(data + consumed + 12));
 806                         print_field("  Latency: 0x%8.8x",
 807                                         get_le32(data + consumed + 16));
 808                         print_field("  Delay variation: 0x%8.8x",
 809                                         get_le32(data + consumed + 20));
 810                         break;
 811                 case 0x04:
 812                         if (response)
 813                                 assign_mode(frame, data[consumed + 2], cid);
 814
 815                         print_field("  Mode: %s (0x%2.2x)",
 816                                         mode2str(data[consumed + 2]),
 817                                         data[consumed + 2]);
 818                         print_field("  TX window size: %d", data[consumed + 3]);
 819                         print_field("  Max transmit: %d", data[consumed + 4]);
 820                         print_field("  Retransmission timeout: %d",
 821                                         get_le16(data + consumed + 5));
 822                         print_field("  Monitor timeout: %d",
 823                                         get_le16(data + consumed + 7));
 824                         print_field("  Maximum PDU size: %d",
 825                                         get_le16(data + consumed + 9));
 826                         break;
 827                 case 0x05:
 828                         switch (data[consumed + 2]) {
 829                         case 0x00:
 830                                 str = "No FCS";
 831                                 break;
 832                         case 0x01:
 833                                 str = "16-bit FCS";
 834                                 break;
 835                         default:
 836                                 str = "Reserved";
 837                                 break;
 838                         }
 839                         print_field("  FCS: %s (0x%2.2d)",
 840                                                 str, data[consumed + 2]);
 841                         break;
 842                 case 0x06:
 843                         switch (data[consumed + 3]) {
 844                         case 0x00:
 845                                 str = "No traffic";
 846                                 break;
 847                         case 0x01:
 848                                 str = "Best effort";
 849                                 break;
 850                         case 0x02:
 851                                 str = "Guaranteed";
 852                                 break;
 853                         default:
 854                                 str = "Reserved";
 855                                 break;
 856                         }
 857                         print_field("  Identifier: 0x%2.2x",
 858                                                 data[consumed + 2]);
 859                         print_field("  Service type: %s (0x%2.2x)",
 860                                                 str, data[consumed + 3]);
 861                         print_field("  Maximum SDU size: 0x%4.4x",
 862                                         get_le16(data + consumed + 4));
 863                         print_field("  SDU inter-arrival time: 0x%8.8x",
 864                                         get_le32(data + consumed + 6));
 865                         print_field("  Access latency: 0x%8.8x",
 866                                         get_le32(data + consumed + 10));
 867                         print_field("  Flush timeout: 0x%8.8x",
 868                                         get_le32(data + consumed + 14));
 869                         break;
 870                 case 0x07:
 871                         print_field("  Extended window size: %d",
 872                                         get_le16(data + consumed + 2));
 873                         assign_ext_ctrl(frame, 1, cid);
 874                         break;
 875                 default:
 876                         packet_hexdump(data + consumed + 2, len);
 877                         break;
 878                 }
 879
 880                 consumed += len + 2;
 881         }
 882
 883         if (consumed < size)
 884                 packet_hexdump(data + consumed, size - consumed);
 885 }
 886
 887 static void print_info_type(uint16_t type)
 888 {
 889         const char *str;
 890
 891         switch (le16_to_cpu(type)) {
 892         case 0x0001:
 893                 str = "Connectionless MTU";
 894                 break;
 895         case 0x0002:
 896                 str = "Extended features supported";
 897                 break;
 898         case 0x0003:
 899                 str = "Fixed channels supported";
 900                 break;
 901         default:
 902                 str = "Reserved";
 903                 break;
 904         }
 905
 906         print_field("Type: %s (0x%4.4x)", str, le16_to_cpu(type));
 907 }
 908
 909 static void print_info_result(uint16_t result)
 910 {
 911         const char *str;
 912
 913         switch (le16_to_cpu(result)) {
 914         case 0x0000:
 915                 str = "Success";
 916                 break;
 917         case 0x0001:
 918                 str = "Not supported";
 919                 break;
 920         default:
 921                 str = "Reserved";
 922                 break;
 923         }
 924
 925         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
 926 }
 927
 928 static const struct bitfield_data features_table[] = {
 929         {  0, "Flow control mode"                       },
 930         {  1, "Retransmission mode"                     },
 931         {  2, "Bi-directional QoS"                      },
 932         {  3, "Enhanced Retransmission Mode"            },
 933         {  4, "Streaming Mode"                          },
 934         {  5, "FCS Option"                              },
 935         {  6, "Extended Flow Specification for BR/EDR"  },
 936         {  7, "Fixed Channels"                          },
 937         {  8, "Extended Window Size"                    },
 938         {  9, "Unicast Connectionless Data Reception"   },
 939         { 31, "Reserved for feature mask extension"     },
 940         { }
 941 };
 942
 943 static void print_features(uint32_t features)
 944 {
 945         uint32_t mask;
 946
 947         print_field("Features: 0x%8.8x", features);
 948
 949         mask = print_bitfield(2, features, features_table);
 950         if (mask)
 951                 print_field("  Unknown features (0x%8.8x)", mask);
 952 }
 953
 954 static const struct bitfield_data channels_table[] = {
 955         { 0x0000, "Null identifier"             },
 956         { 0x0001, "L2CAP Signaling (BR/EDR)"    },
 957         { 0x0002, "Connectionless reception"    },
 958         { 0x0003, "AMP Manager Protocol"        },
 959         { 0x0004, "Attribute Protocol"          },
 960         { 0x0005, "L2CAP Signaling (LE)"        },
 961         { 0x0006, "Security Manager (LE)"       },
 962         { 0x0007, "Security Manager (BR/EDR)"   },
 963         { 0x003f, "AMP Test Manager"            },
 964         { }
 965 };
 966
 967 static void print_channels(uint64_t channels)
 968 {
 969         uint64_t mask;
 970
 971         print_field("Channels: 0x%16.16" PRIx64, channels);
 972
 973         mask = print_bitfield(2, channels, channels_table);
 974         if (mask)
 975                 print_field("  Unknown channels (0x%8.8" PRIx64 ")", mask);
 976 }
 977
 978 static void print_move_result(uint16_t result)
 979 {
 980         const char *str;
 981
 982         switch (le16_to_cpu(result)) {
 983         case 0x0000:
 984                 str = "Move success";
 985                 break;
 986         case 0x0001:
 987                 str = "Move pending";
 988                 break;
 989         case 0x0002:
 990                 str = "Move refused - Controller ID not supported";
 991                 break;
 992         case 0x0003:
 993                 str = "Move refused - new Controller ID is same";
 994                 break;
 995         case 0x0004:
 996                 str = "Move refused - Configuration not supported";
 997                 break;
 998         case 0x0005:
 999                 str = "Move refused - Move Channel collision";
1000                 break;
1001         case 0x0006:
1002                 str = "Move refused - Channel not allowed to be moved";
1003                 break;
1004         default:
1005                 str = "Reserved";
1006                 break;
1007         }
1008
1009         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
1010 }
1011
1012 static void print_move_cfm_result(uint16_t result)
1013 {
1014         const char *str;
1015
1016         switch (le16_to_cpu(result)) {
1017         case 0x0000:
1018                 str = "Move success - both sides succeed";
1019                 break;
1020         case 0x0001:
1021                 str = "Move failure - one or both sides refuse";
1022                 break;
1023         default:
1024                 str = "Reserved";
1025                 break;
1026         }
1027
1028         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
1029 }
1030
1031 static void print_conn_param_result(uint16_t result)
1032 {
1033         const char *str;
1034
1035         switch (le16_to_cpu(result)) {
1036         case 0x0000:
1037                 str = "Connection Parameters accepted";
1038                 break;
1039         case 0x0001:
1040                 str = "Connection Parameters rejected";
1041                 break;
1042         default:
1043                 str = "Reserved";
1044                 break;
1045         }
1046
1047         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
1048 }
1049
1050 static void sig_cmd_reject(const struct l2cap_frame *frame)
1051 {
1052         const struct bt_l2cap_pdu_cmd_reject *pdu = frame->data;
1053         const void *data = frame->data;
1054         uint16_t size = frame->size;
1055         uint16_t scid, dcid;
1056
1057         print_reject_reason(pdu->reason);
1058
1059         data += sizeof(*pdu);
1060         size -= sizeof(*pdu);
1061
1062         switch (le16_to_cpu(pdu->reason)) {
1063         case 0x0000:
1064                 if (size != 0) {
1065                         print_text(COLOR_ERROR, "invalid data size");
1066                         packet_hexdump(data, size);
1067                         break;
1068                 }
1069                 break;
1070         case 0x0001:
1071                 if (size != 2) {
1072                         print_text(COLOR_ERROR, "invalid data size");
1073                         packet_hexdump(data, size);
1074                         break;
1075                 }
1076                 print_field("MTU: %d", get_le16(data));
1077                 break;
1078         case 0x0002:
1079                 if (size != 4) {
1080                         print_text(COLOR_ERROR, "invalid data size");
1081                         packet_hexdump(data, size);
1082                         break;
1083                 }
1084                 dcid = get_le16(data);
1085                 scid = get_le16(data + 2);
1086                 print_cid("Destination", cpu_to_le16(dcid));
1087                 print_cid("Source", cpu_to_le16(scid));
1088                 break;
1089         default:
1090                 packet_hexdump(data, size);
1091                 break;
1092         }
1093 }
1094
1095 static void sig_conn_req(const struct l2cap_frame *frame)
1096 {
1097         const struct bt_l2cap_pdu_conn_req *pdu = frame->data;
1098
1099         print_psm(pdu->psm);
1100         print_cid("Source", pdu->scid);
1101
1102         assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
1103                                                 L2CAP_MODE_BASIC, 0);
1104 }
1105
1106 static void sig_conn_rsp(const struct l2cap_frame *frame)
1107 {
1108         const struct bt_l2cap_pdu_conn_rsp *pdu = frame->data;
1109
1110         print_cid("Destination", pdu->dcid);
1111         print_cid("Source", pdu->scid);
1112         print_conn_result(pdu->result);
1113         print_conn_status(pdu->status);
1114
1115         assign_dcid(frame, le16_to_cpu(pdu->dcid), le16_to_cpu(pdu->scid));
1116 }
1117
1118 static void sig_config_req(const struct l2cap_frame *frame)
1119 {
1120         const struct bt_l2cap_pdu_config_req *pdu = frame->data;
1121
1122         print_cid("Destination", pdu->dcid);
1123         print_config_flags(pdu->flags);
1124         print_config_options(frame, 4, le16_to_cpu(pdu->dcid), false);
1125 }
1126
1127 static void sig_config_rsp(const struct l2cap_frame *frame)
1128 {
1129         const struct bt_l2cap_pdu_config_rsp *pdu = frame->data;
1130
1131         print_cid("Source", pdu->scid);
1132         print_config_flags(pdu->flags);
1133         print_config_result(pdu->result);
1134         print_config_options(frame, 6, le16_to_cpu(pdu->scid), true);
1135 }
1136
1137 static void sig_disconn_req(const struct l2cap_frame *frame)
1138 {
1139         const struct bt_l2cap_pdu_disconn_req *pdu = frame->data;
1140
1141         print_cid("Destination", pdu->dcid);
1142         print_cid("Source", pdu->scid);
1143 }
1144
1145 static void sig_disconn_rsp(const struct l2cap_frame *frame)
1146 {
1147         const struct bt_l2cap_pdu_disconn_rsp *pdu = frame->data;
1148
1149         print_cid("Destination", pdu->dcid);
1150         print_cid("Source", pdu->scid);
1151
1152         release_scid(frame, le16_to_cpu(pdu->scid));
1153 }
1154
1155 static void sig_echo_req(const struct l2cap_frame *frame)
1156 {
1157         packet_hexdump(frame->data, frame->size);
1158 }
1159
1160 static void sig_echo_rsp(const struct l2cap_frame *frame)
1161 {
1162         packet_hexdump(frame->data, frame->size);
1163 }
1164
1165 static void sig_info_req(const struct l2cap_frame *frame)
1166 {
1167         const struct bt_l2cap_pdu_info_req *pdu = frame->data;
1168
1169         print_info_type(pdu->type);
1170 }
1171
1172 static void sig_info_rsp(const struct l2cap_frame *frame)
1173 {
1174         const struct bt_l2cap_pdu_info_rsp *pdu = frame->data;
1175         const void *data = frame->data;
1176         uint16_t size = frame->size;
1177
1178         print_info_type(pdu->type);
1179         print_info_result(pdu->result);
1180
1181         data += sizeof(*pdu);
1182         size -= sizeof(*pdu);
1183
1184         if (le16_to_cpu(pdu->result) != 0x0000) {
1185                 if (size > 0) {
1186                         print_text(COLOR_ERROR, "invalid data size");
1187                         packet_hexdump(data, size);
1188                 }
1189                 return;
1190         }
1191
1192         switch (le16_to_cpu(pdu->type)) {
1193         case 0x0001:
1194                 if (size != 2) {
1195                         print_text(COLOR_ERROR, "invalid data size");
1196                         packet_hexdump(data, size);
1197                         break;
1198                 }
1199                 print_field("MTU: %d", get_le16(data));
1200                 break;
1201         case 0x0002:
1202                 if (size != 4) {
1203                         print_text(COLOR_ERROR, "invalid data size");
1204                         packet_hexdump(data, size);
1205                         break;
1206                 }
1207                 print_features(get_le32(data));
1208                 break;
1209         case 0x0003:
1210                 if (size != 8) {
1211                         print_text(COLOR_ERROR, "invalid data size");
1212                         packet_hexdump(data, size);
1213                         break;
1214                 }
1215                 print_channels(get_le64(data));
1216                 break;
1217         default:
1218                 packet_hexdump(data, size);
1219                 break;
1220         }
1221 }
1222
1223 static void sig_create_chan_req(const struct l2cap_frame *frame)
1224 {
1225         const struct bt_l2cap_pdu_create_chan_req *pdu = frame->data;
1226
1227         print_psm(pdu->psm);
1228         print_cid("Source", pdu->scid);
1229         print_field("Controller ID: %d", pdu->ctrlid);
1230
1231         assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
1232                                                 L2CAP_MODE_BASIC, pdu->ctrlid);
1233 }
1234
1235 static void sig_create_chan_rsp(const struct l2cap_frame *frame)
1236 {
1237         const struct bt_l2cap_pdu_create_chan_rsp *pdu = frame->data;
1238
1239         print_cid("Destination", pdu->dcid);
1240         print_cid("Source", pdu->scid);
1241         print_create_chan_result(pdu->result);
1242         print_conn_status(pdu->status);
1243
1244         assign_dcid(frame, le16_to_cpu(pdu->dcid), le16_to_cpu(pdu->scid));
1245 }
1246
1247 static void sig_move_chan_req(const struct l2cap_frame *frame)
1248 {
1249         const struct bt_l2cap_pdu_move_chan_req *pdu = frame->data;
1250
1251         print_cid("Initiator", pdu->icid);
1252         print_field("Controller ID: %d", pdu->ctrlid);
1253 }
1254
1255 static void sig_move_chan_rsp(const struct l2cap_frame *frame)
1256 {
1257         const struct bt_l2cap_pdu_move_chan_rsp *pdu = frame->data;
1258
1259         print_cid("Initiator", pdu->icid);
1260         print_move_result(pdu->result);
1261 }
1262
1263 static void sig_move_chan_cfm(const struct l2cap_frame *frame)
1264 {
1265         const struct bt_l2cap_pdu_move_chan_cfm *pdu = frame->data;
1266
1267         print_cid("Initiator", pdu->icid);
1268         print_move_cfm_result(pdu->result);
1269 }
1270
1271 static void sig_move_chan_cfm_rsp(const struct l2cap_frame *frame)
1272 {
1273         const struct bt_l2cap_pdu_move_chan_cfm_rsp *pdu = frame->data;
1274
1275         print_cid("Initiator", pdu->icid);
1276 }
1277
1278 static void sig_conn_param_req(const struct l2cap_frame *frame)
1279 {
1280         const struct bt_l2cap_pdu_conn_param_req *pdu = frame->data;
1281
1282         print_field("Min interval: %d", le16_to_cpu(pdu->min_interval));
1283         print_field("Max interval: %d", le16_to_cpu(pdu->max_interval));
1284         print_field("Peripheral latency: %d", le16_to_cpu(pdu->latency));
1285         print_field("Timeout multiplier: %d", le16_to_cpu(pdu->timeout));
1286 }
1287
1288 static void sig_conn_param_rsp(const struct l2cap_frame *frame)
1289 {
1290         const struct bt_l2cap_pdu_conn_param_rsp *pdu = frame->data;
1291
1292         print_conn_param_result(pdu->result);
1293 }
1294
1295 static void sig_le_conn_req(const struct l2cap_frame *frame)
1296 {
1297         const struct bt_l2cap_pdu_le_conn_req *pdu = frame->data;
1298
1299         print_psm(pdu->psm);
1300         print_cid("Source", pdu->scid);
1301         print_field("MTU: %u", le16_to_cpu(pdu->mtu));
1302         print_field("MPS: %u", le16_to_cpu(pdu->mps));
1303         print_field("Credits: %u", le16_to_cpu(pdu->credits));
1304
1305         assign_scid(frame, le16_to_cpu(pdu->scid), le16_to_cpu(pdu->psm),
1306                                                 L2CAP_MODE_LE_FLOWCTL, 0);
1307 }
1308
1309 static void sig_le_conn_rsp(const struct l2cap_frame *frame)
1310 {
1311         const struct bt_l2cap_pdu_le_conn_rsp *pdu = frame->data;
1312
1313         print_cid("Destination", pdu->dcid);
1314         print_field("MTU: %u", le16_to_cpu(pdu->mtu));
1315         print_field("MPS: %u", le16_to_cpu(pdu->mps));
1316         print_field("Credits: %u", le16_to_cpu(pdu->credits));
1317         print_le_conn_result(pdu->result);
1318
1319         assign_dcid(frame, le16_to_cpu(pdu->dcid), 0);
1320 }
1321
1322 static void sig_le_flowctl_creds(const struct l2cap_frame *frame)
1323 {
1324         const struct bt_l2cap_pdu_le_flowctl_creds *pdu = frame->data;
1325
1326         print_cid("Source", pdu->cid);
1327         print_field("Credits: %u", le16_to_cpu(pdu->credits));
1328 }
1329
1330 static void sig_ecred_conn_req(const struct l2cap_frame *frame)
1331 {
1332         const struct bt_l2cap_pdu_ecred_conn_req *pdu = frame->data;
1333         uint16_t scid;
1334
1335         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
1336
1337         print_psm(pdu->psm);
1338         print_field("MTU: %u", le16_to_cpu(pdu->mtu));
1339         print_field("MPS: %u", le16_to_cpu(pdu->mps));
1340         print_field("Credits: %u", le16_to_cpu(pdu->credits));
1341
1342         while (l2cap_frame_get_le16((void *)frame, &scid)) {
1343                 print_cid("Source", scid);
1344                 assign_scid(frame, scid, le16_to_cpu(pdu->psm),
1345                                                 L2CAP_MODE_ECRED, 0);
1346         }
1347 }
1348
1349 static void print_ecred_conn_result(uint16_t result)
1350 {
1351         const char *str;
1352
1353         switch (le16_to_cpu(result)) {
1354         case 0x0000:
1355                 str = "Connection successful";
1356                 break;
1357         case 0x0002:
1358                 str = "Connection refused - PSM not supported";
1359                 break;
1360         case 0x0004:
1361                 str = "Some connections refused – not enough resources "
1362                         "available";
1363                 break;
1364         case 0x0005:
1365                 str = "All Connections refused - insufficient authentication";
1366                 break;
1367         case 0x0006:
1368                 str = "All Connections refused - insufficient authorization";
1369                 break;
1370         case 0x0007:
1371                 str = "All Connection refused - insufficient encryption key "
1372                         "size";
1373                 break;
1374         case 0x0008:
1375                 str = "All Connections refused - insufficient encryption";
1376                 break;
1377         case 0x0009:
1378                 str = "Some Connections refused - Invalid Source CID";
1379                 break;
1380         case 0x000a:
1381                 str = "Some Connections refused - Source CID already allocated";
1382                 break;
1383         case 0x000b:
1384                 str = "All Connections refused - unacceptable parameters";
1385                 break;
1386         default:
1387                 str = "Reserved";
1388                 break;
1389         }
1390
1391         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
1392 }
1393
1394 static void sig_ecred_conn_rsp(const struct l2cap_frame *frame)
1395 {
1396         const struct bt_l2cap_pdu_ecred_conn_rsp *pdu = frame->data;
1397         uint16_t dcid;
1398
1399         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
1400
1401         print_field("MTU: %u", le16_to_cpu(pdu->mtu));
1402         print_field("MPS: %u", le16_to_cpu(pdu->mps));
1403         print_field("Credits: %u", le16_to_cpu(pdu->credits));
1404         print_ecred_conn_result(pdu->result);
1405
1406         while (l2cap_frame_get_le16((void *)frame, &dcid)) {
1407                 print_cid("Destination", dcid);
1408                 assign_dcid(frame, dcid, 0);
1409         }
1410 }
1411
1412 static void sig_ecred_reconf_req(const struct l2cap_frame *frame)
1413 {
1414         const struct bt_l2cap_pdu_ecred_reconf_req *pdu = frame->data;
1415         uint16_t scid;
1416
1417         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
1418
1419         print_field("MTU: %u", le16_to_cpu(pdu->mtu));
1420         print_field("MPS: %u", le16_to_cpu(pdu->mps));
1421
1422         while (l2cap_frame_get_le16((void *)frame, &scid))
1423                 print_cid("Source", scid);
1424 }
1425
1426 static void print_ecred_reconf_result(uint16_t result)
1427 {
1428         const char *str;
1429
1430         switch (le16_to_cpu(result)) {
1431         case 0x0000:
1432                 str = "Reconfiguration successful";
1433                 break;
1434         case 0x0001:
1435                 str = "Reconfiguration failed - reduction in size of MTU not "
1436                         "allowed";
1437                 break;
1438         case 0x0002:
1439                 str = "Reconfiguration failed - reduction in size of MPS not "
1440                         "allowed for more than one channel at a time";
1441                 break;
1442         default:
1443                 str = "Reserved";
1444         }
1445
1446         print_field("Result: %s (0x%4.4x)", str, le16_to_cpu(result));
1447 }
1448
1449 static void sig_ecred_reconf_rsp(const struct l2cap_frame *frame)
1450 {
1451         const struct bt_l2cap_pdu_ecred_reconf_rsp *pdu = frame->data;
1452
1453         print_ecred_reconf_result(pdu->result);
1454 }
1455
1456 struct sig_opcode_data {
1457         uint8_t opcode;
1458         const char *str;
1459         void (*func) (const struct l2cap_frame *frame);
1460         uint16_t size;
1461         bool fixed;
1462 };
1463
1464 #define SIG_ECRED \
1465         { BT_L2CAP_PDU_ECRED_CONN_REQ,                                  \
1466         "Enhanced Credit Connection Request",                           \
1467         sig_ecred_conn_req, sizeof(struct bt_l2cap_pdu_ecred_conn_req), \
1468         false },                                                        \
1469         { BT_L2CAP_PDU_ECRED_CONN_RSP,                                  \
1470         "Enhanced Credit Connection Response",                          \
1471         sig_ecred_conn_rsp, sizeof(struct bt_l2cap_pdu_ecred_conn_rsp), \
1472         false },                                                        \
1473         { BT_L2CAP_PDU_ECRED_RECONF_REQ,                                \
1474         "Enhanced Credit Reconfigure Request",                          \
1475         sig_ecred_reconf_req, sizeof(struct bt_l2cap_pdu_ecred_reconf_req), \
1476         false },                                                        \
1477         { BT_L2CAP_PDU_ECRED_RECONF_RSP,                                \
1478         "Enhanced Credit Reconfigure Respond",                          \
1479         sig_ecred_reconf_rsp, sizeof(struct bt_l2cap_pdu_ecred_reconf_rsp), \
1480         true },
1481
1482 static const struct sig_opcode_data bredr_sig_opcode_table[] = {
1483         { 0x01, "Command Reject",
1484                         sig_cmd_reject, 2, false },
1485         { 0x02, "Connection Request",
1486                         sig_conn_req, 4, true },
1487         { 0x03, "Connection Response",
1488                         sig_conn_rsp, 8, true },
1489         { 0x04, "Configure Request",
1490                         sig_config_req, 4, false },
1491         { 0x05, "Configure Response",
1492                         sig_config_rsp, 6, false },
1493         { 0x06, "Disconnection Request",
1494                         sig_disconn_req, 4, true },
1495         { 0x07, "Disconnection Response",
1496                         sig_disconn_rsp, 4, true },
1497         { 0x08, "Echo Request",
1498                         sig_echo_req, 0, false },
1499         { 0x09, "Echo Response",
1500                         sig_echo_rsp, 0, false },
1501         { 0x0a, "Information Request",
1502                         sig_info_req, 2, true },
1503         { 0x0b, "Information Response",
1504                         sig_info_rsp, 4, false },
1505         { 0x0c, "Create Channel Request",
1506                         sig_create_chan_req, 5, true },
1507         { 0x0d, "Create Channel Response",
1508                         sig_create_chan_rsp, 8, true },
1509         { 0x0e, "Move Channel Request",
1510                         sig_move_chan_req, 3, true },
1511         { 0x0f, "Move Channel Response",
1512                         sig_move_chan_rsp, 4, true },
1513         { 0x10, "Move Channel Confirmation",
1514                         sig_move_chan_cfm, 4, true },
1515         { 0x11, "Move Channel Confirmation Response",
1516                         sig_move_chan_cfm_rsp, 2, true },
1517         SIG_ECRED
1518         { },
1519 };
1520
1521 static const struct sig_opcode_data le_sig_opcode_table[] = {
1522         { 0x01, "Command Reject",
1523                         sig_cmd_reject, 2, false },
1524         { 0x06, "Disconnection Request",
1525                         sig_disconn_req, 4, true },
1526         { 0x07, "Disconnection Response",
1527                         sig_disconn_rsp, 4, true },
1528         { 0x12, "Connection Parameter Update Request",
1529                         sig_conn_param_req, 8, true },
1530         { 0x13, "Connection Parameter Update Response",
1531                         sig_conn_param_rsp, 2, true },
1532         { 0x14, "LE Connection Request",
1533                         sig_le_conn_req, 10, true },
1534         { 0x15, "LE Connection Response",
1535                         sig_le_conn_rsp, 10, true },
1536         { 0x16, "LE Flow Control Credit",
1537                         sig_le_flowctl_creds, 4, true },
1538         SIG_ECRED
1539         { },
1540 };
1541
1542 static void l2cap_queue_frame(struct l2cap_frame *frame)
1543 {
1544         struct packet_conn_data *conn;
1545         struct l2cap_frame *tx;
1546
1547         conn = packet_get_conn_data(frame->handle);
1548         if (!conn)
1549                 return;
1550
1551         if (!conn->chan_q)
1552                 conn->chan_q = queue_new();
1553
1554         tx = new0(struct l2cap_frame, 1);
1555         memcpy(tx, frame, sizeof(*frame));
1556         queue_push_tail(conn->chan_q, tx);
1557 }
1558
1559 void l2cap_frame_init(struct l2cap_frame *frame, uint16_t index, bool in,
1560                                 uint16_t handle, uint8_t ident,
1561                                 uint16_t cid, uint16_t psm,
1562                                 const void *data, uint16_t size)
1563 {
1564         frame->index   = index;
1565         frame->in      = in;
1566         frame->handle  = handle;
1567         frame->ident   = ident;
1568         frame->cid     = cid;
1569         frame->data    = data;
1570         frame->size    = size;
1571         frame->chan    = get_chan_data_index(frame);
1572         frame->psm     = psm ? psm : get_psm(frame);
1573         frame->mode    = get_mode(frame);
1574         frame->seq_num = psm ? 1 : get_seq_num(frame);
1575
1576         if (!in)
1577                 l2cap_queue_frame(frame);
1578 }
1579
1580 static void bredr_sig_packet(uint16_t index, bool in, uint16_t handle,
1581                                 uint16_t cid, const void *data, uint16_t size)
1582 {
1583         struct l2cap_frame frame;
1584
1585         while (size > 0) {
1586                 const struct bt_l2cap_hdr_sig *hdr = data;
1587                 const struct sig_opcode_data *opcode_data = NULL;
1588                 const char *opcode_color, *opcode_str;
1589                 uint16_t len;
1590                 int i;
1591
1592                 if (size < 4) {
1593                         print_text(COLOR_ERROR, "malformed signal packet");
1594                         packet_hexdump(data, size);
1595                         return;
1596                 }
1597
1598                 len = le16_to_cpu(hdr->len);
1599
1600                 data += 4;
1601                 size -= 4;
1602
1603                 if (size < len) {
1604                         print_text(COLOR_ERROR, "invalid signal packet size");
1605                         packet_hexdump(data, size);
1606                         return;
1607                 }
1608
1609                 for (i = 0; bredr_sig_opcode_table[i].str; i++) {
1610                         if (bredr_sig_opcode_table[i].opcode == hdr->code) {
1611                                 opcode_data = &bredr_sig_opcode_table[i];
1612                                 break;
1613                         }
1614                 }
1615
1616                 if (opcode_data) {
1617                         if (opcode_data->func) {
1618                                 if (in)
1619                                         opcode_color = COLOR_MAGENTA;
1620                                 else
1621                                         opcode_color = COLOR_BLUE;
1622                         } else
1623                                 opcode_color = COLOR_WHITE_BG;
1624                         opcode_str = opcode_data->str;
1625                 } else {
1626                         opcode_color = COLOR_WHITE_BG;
1627                         opcode_str = "Unknown";
1628                 }
1629
1630                 print_indent(6, opcode_color, "L2CAP: ", opcode_str,
1631                                         COLOR_OFF,
1632                                         " (0x%2.2x) ident %d len %d",
1633                                         hdr->code, hdr->ident, len);
1634
1635                 if (!opcode_data || !opcode_data->func) {
1636                         packet_hexdump(data, len);
1637                         data += len;
1638                         size -= len;
1639                         return;
1640                 }
1641
1642                 if (opcode_data->fixed) {
1643                         if (len != opcode_data->size) {
1644                                 print_text(COLOR_ERROR, "invalid size");
1645                                 packet_hexdump(data, len);
1646                                 data += len;
1647                                 size -= len;
1648                                 continue;
1649                         }
1650                 } else {
1651                         if (len < opcode_data->size) {
1652                                 print_text(COLOR_ERROR, "too short packet");
1653                                 packet_hexdump(data, size);
1654                                 data += len;
1655                                 size -= len;
1656                                 continue;
1657                         }
1658                 }
1659
1660                 l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid, 0,
1661                                                                 data, len);
1662                 opcode_data->func(&frame);
1663
1664                 data += len;
1665                 size -= len;
1666         }
1667
1668         packet_hexdump(data, size);
1669 }
1670
1671 static void le_sig_packet(uint16_t index, bool in, uint16_t handle,
1672                                 uint16_t cid, const void *data, uint16_t size)
1673 {
1674         struct l2cap_frame frame;
1675         const struct bt_l2cap_hdr_sig *hdr = data;
1676         const struct sig_opcode_data *opcode_data = NULL;
1677         const char *opcode_color, *opcode_str;
1678         uint16_t len;
1679         int i;
1680
1681         if (size < 4) {
1682                 print_text(COLOR_ERROR, "malformed signal packet");
1683                 packet_hexdump(data, size);
1684                 return;
1685         }
1686
1687         len = le16_to_cpu(hdr->len);
1688
1689         data += 4;
1690         size -= 4;
1691
1692         if (size != len) {
1693                 print_text(COLOR_ERROR, "invalid signal packet size");
1694                 packet_hexdump(data, size);
1695                 return;
1696         }
1697
1698         for (i = 0; le_sig_opcode_table[i].str; i++) {
1699                 if (le_sig_opcode_table[i].opcode == hdr->code) {
1700                         opcode_data = &le_sig_opcode_table[i];
1701                         break;
1702                 }
1703         }
1704
1705         if (opcode_data) {
1706                 if (opcode_data->func) {
1707                         if (in)
1708                                 opcode_color = COLOR_MAGENTA;
1709                         else
1710                                 opcode_color = COLOR_BLUE;
1711                 } else
1712                         opcode_color = COLOR_WHITE_BG;
1713                 opcode_str = opcode_data->str;
1714         } else {
1715                 opcode_color = COLOR_WHITE_BG;
1716                 opcode_str = "Unknown";
1717         }
1718
1719         print_indent(6, opcode_color, "LE L2CAP: ", opcode_str, COLOR_OFF,
1720                                         " (0x%2.2x) ident %d len %d",
1721                                         hdr->code, hdr->ident, len);
1722
1723         if (!opcode_data || !opcode_data->func) {
1724                 packet_hexdump(data, len);
1725                 return;
1726         }
1727
1728         if (opcode_data->fixed) {
1729                 if (len != opcode_data->size) {
1730                         print_text(COLOR_ERROR, "invalid size");
1731                         packet_hexdump(data, len);
1732                         return;
1733                 }
1734         } else {
1735                 if (len < opcode_data->size) {
1736                         print_text(COLOR_ERROR, "too short packet");
1737                         packet_hexdump(data, size);
1738                         return;
1739                 }
1740         }
1741
1742         l2cap_frame_init(&frame, index, in, handle, hdr->ident, cid, 0,
1743                                                         data, len);
1744         opcode_data->func(&frame);
1745 }
1746
1747 static void connless_packet(uint16_t index, bool in, uint16_t handle,
1748                                 uint16_t cid, const void *data, uint16_t size)
1749 {
1750         struct l2cap_frame frame;
1751         const struct bt_l2cap_hdr_connless *hdr = data;
1752         uint16_t psm;
1753
1754         if (size < 2) {
1755                 print_text(COLOR_ERROR, "malformed connectionless packet");
1756                 packet_hexdump(data, size);
1757                 return;
1758         }
1759
1760         psm = le16_to_cpu(hdr->psm);
1761
1762         data += 2;
1763         size -= 2;
1764
1765         print_indent(6, COLOR_CYAN, "L2CAP: Connectionless", "", COLOR_OFF,
1766                                                 " len %d [PSM %d]", size, psm);
1767
1768         switch (psm) {
1769         default:
1770                 packet_hexdump(data, size);
1771                 break;
1772         }
1773
1774         l2cap_frame_init(&frame, index, in, handle, 0, cid, 0, data, size);
1775 }
1776
1777 static void print_controller_list(const uint8_t *data, uint16_t size)
1778 {
1779         while (size > 2) {
1780                 const char *str;
1781
1782                 print_field("Controller ID: %d", data[0]);
1783
1784                 switch (data[1]) {
1785                 case 0x00:
1786                         str = "Primary BR/EDR Controller";
1787                         break;
1788                 case 0x01:
1789                         str = "802.11 AMP Controller";
1790                         break;
1791                 default:
1792                         str = "Reserved";
1793                         break;
1794                 }
1795
1796                 print_field("  Type: %s (0x%2.2x)", str, data[1]);
1797
1798                 switch (data[2]) {
1799                 case 0x00:
1800                         str = "Present";
1801                         break;
1802                 case 0x01:
1803                         str = "Bluetooth only";
1804                         break;
1805                 case 0x02:
1806                         str = "No capacity";
1807                         break;
1808                 case 0x03:
1809                         str = "Low capacity";
1810                         break;
1811                 case 0x04:
1812                         str = "Medium capacity";
1813                         break;
1814                 case 0x05:
1815                         str = "High capacity";
1816                         break;
1817                 case 0x06:
1818                         str = "Full capacity";
1819                         break;
1820                 default:
1821                         str = "Reserved";
1822                         break;
1823                 }
1824
1825                 print_field("  Status: %s (0x%2.2x)", str, data[2]);
1826
1827                 data += 3;
1828                 size -= 3;
1829         }
1830
1831         packet_hexdump(data, size);
1832 }
1833
1834 static void amp_cmd_reject(const struct l2cap_frame *frame)
1835 {
1836         const struct bt_l2cap_amp_cmd_reject *pdu = frame->data;
1837
1838         print_field("Reason: 0x%4.4x", le16_to_cpu(pdu->reason));
1839 }
1840
1841 static void amp_discover_req(const struct l2cap_frame *frame)
1842 {
1843         const struct bt_l2cap_amp_discover_req *pdu = frame->data;
1844
1845         print_field("MTU/MPS size: %d", le16_to_cpu(pdu->size));
1846         print_field("Extended feature mask: 0x%4.4x",
1847                                         le16_to_cpu(pdu->features));
1848 }
1849
1850 static void amp_discover_rsp(const struct l2cap_frame *frame)
1851 {
1852         const struct bt_l2cap_amp_discover_rsp *pdu = frame->data;
1853
1854         print_field("MTU/MPS size: %d", le16_to_cpu(pdu->size));
1855         print_field("Extended feature mask: 0x%4.4x",
1856                                         le16_to_cpu(pdu->features));
1857
1858         print_controller_list(frame->data + 4, frame->size - 4);
1859 }
1860
1861 static void amp_change_notify(const struct l2cap_frame *frame)
1862 {
1863         print_controller_list(frame->data, frame->size);
1864 }
1865
1866 static void amp_change_response(const struct l2cap_frame *frame)
1867 {
1868 }
1869
1870 static void amp_get_info_req(const struct l2cap_frame *frame)
1871 {
1872         const struct bt_l2cap_amp_get_info_req *pdu = frame->data;
1873
1874         print_field("Controller ID: %d", pdu->ctrlid);
1875 }
1876
1877 static void amp_get_info_rsp(const struct l2cap_frame *frame)
1878 {
1879         const struct bt_l2cap_amp_get_info_rsp *pdu = frame->data;
1880         const char *str;
1881
1882         print_field("Controller ID: %d", pdu->ctrlid);
1883
1884         switch (pdu->status) {
1885         case 0x00:
1886                 str = "Success";
1887                 break;
1888         case 0x01:
1889                 str = "Invalid Controller ID";
1890                 break;
1891         default:
1892                 str = "Reserved";
1893                 break;
1894         }
1895
1896         print_field("Status: %s (0x%2.2x)", str, pdu->status);
1897
1898         print_field("Total bandwidth: %d kbps", le32_to_cpu(pdu->total_bw));
1899         print_field("Max guaranteed bandwidth: %d kbps",
1900                                                 le32_to_cpu(pdu->max_bw));
1901         print_field("Min latency: %d", le32_to_cpu(pdu->min_latency));
1902
1903         print_field("PAL capabilities: 0x%4.4x", le16_to_cpu(pdu->pal_cap));
1904         print_field("Max ASSOC length: %d", le16_to_cpu(pdu->max_assoc_len));
1905 }
1906
1907 static void amp_get_assoc_req(const struct l2cap_frame *frame)
1908 {
1909         const struct bt_l2cap_amp_get_assoc_req *pdu = frame->data;
1910
1911         print_field("Controller ID: %d", pdu->ctrlid);
1912 }
1913
1914 static void amp_get_assoc_rsp(const struct l2cap_frame *frame)
1915 {
1916         const struct bt_l2cap_amp_get_assoc_rsp *pdu = frame->data;
1917         const char *str;
1918
1919         print_field("Controller ID: %d", pdu->ctrlid);
1920
1921         switch (pdu->status) {
1922         case 0x00:
1923                 str = "Success";
1924                 break;
1925         case 0x01:
1926                 str = "Invalid Controller ID";
1927                 break;
1928         default:
1929                 str = "Reserved";
1930                 break;
1931         }
1932
1933         print_field("Status: %s (0x%2.2x)", str, pdu->status);
1934
1935         packet_hexdump(frame->data + 2, frame->size - 2);
1936 }
1937
1938 static void amp_create_phy_link_req(const struct l2cap_frame *frame)
1939 {
1940         const struct bt_l2cap_amp_create_phy_link_req *pdu = frame->data;
1941
1942         print_field("Local controller ID: %d", pdu->local_ctrlid);
1943         print_field("Remote controller ID: %d", pdu->remote_ctrlid);
1944
1945         packet_hexdump(frame->data + 2, frame->size - 2);
1946 }
1947
1948 static void amp_create_phy_link_rsp(const struct l2cap_frame *frame)
1949 {
1950         const struct bt_l2cap_amp_create_phy_link_rsp *pdu = frame->data;
1951         const char *str;
1952
1953         print_field("Local controller ID: %d", pdu->local_ctrlid);
1954         print_field("Remote controller ID: %d", pdu->remote_ctrlid);
1955
1956         switch (pdu->status) {
1957         case 0x00:
1958                 str = "Success";
1959                 break;
1960         case 0x01:
1961                 str = "Invalid Controller ID";
1962                 break;
1963         case 0x02:
1964                 str = "Failed - Unable to start link creation";
1965                 break;
1966         case 0x03:
1967                 str = "Failed - Collision occurred";
1968                 break;
1969         case 0x04:
1970                 str = "Failed - Disconnected link packet received";
1971                 break;
1972         case 0x05:
1973                 str = "Failed - Link already exists";
1974                 break;
1975         case 0x06:
1976                 str = "Failed - Security violation";
1977                 break;
1978         default:
1979                 str = "Reserved";
1980                 break;
1981         }
1982
1983         print_field("Status: %s (0x%2.2x)", str, pdu->status);
1984 }
1985
1986 static void amp_disconn_phy_link_req(const struct l2cap_frame *frame)
1987 {
1988         const struct bt_l2cap_amp_disconn_phy_link_req *pdu = frame->data;
1989
1990         print_field("Local controller ID: %d", pdu->local_ctrlid);
1991         print_field("Remote controller ID: %d", pdu->remote_ctrlid);
1992 }
1993
1994 static void amp_disconn_phy_link_rsp(const struct l2cap_frame *frame)
1995 {
1996         const struct bt_l2cap_amp_disconn_phy_link_rsp *pdu = frame->data;
1997         const char *str;
1998
1999         print_field("Local controller ID: %d", pdu->local_ctrlid);
2000         print_field("Remote controller ID: %d", pdu->remote_ctrlid);
2001
2002         switch (pdu->status) {
2003         case 0x00:
2004                 str = "Success";
2005                 break;
2006         case 0x01:
2007                 str = "Invalid Controller ID";
2008                 break;
2009         case 0x02:
2010                 str = "Failed - No link exists";
2011                 break;
2012         default:
2013                 str = "Reserved";
2014                 break;
2015         }
2016
2017         print_field("Status: %s (0x%2.2x)", str, pdu->status);
2018 }
2019
2020 struct amp_opcode_data {
2021         uint8_t opcode;
2022         const char *str;
2023         void (*func) (const struct l2cap_frame *frame);
2024         uint16_t size;
2025         bool fixed;
2026 };
2027
2028 static const struct amp_opcode_data amp_opcode_table[] = {
2029         { 0x01, "Command Reject",
2030                         amp_cmd_reject, 2, false },
2031         { 0x02, "Discover Request",
2032                         amp_discover_req, 4, true },
2033         { 0x03, "Discover Response",
2034                         amp_discover_rsp, 7, false },
2035         { 0x04, "Change Notify",
2036                         amp_change_notify, 3, false },
2037         { 0x05, "Change Response",
2038                         amp_change_response, 0, true },
2039         { 0x06, "Get Info Request",
2040                         amp_get_info_req, 1, true },
2041         { 0x07, "Get Info Response",
2042                         amp_get_info_rsp, 18, true },
2043         { 0x08, "Get Assoc Request",
2044                         amp_get_assoc_req, 1, true },
2045         { 0x09, "Get Assoc Response",
2046                         amp_get_assoc_rsp, 2, false },
2047         { 0x0a, "Create Physical Link Request",
2048                         amp_create_phy_link_req, 2, false },
2049         { 0x0b, "Create Physical Link Response",
2050                         amp_create_phy_link_rsp, 3, true },
2051         { 0x0c, "Disconnect Physical Link Request",
2052                         amp_disconn_phy_link_req, 2, true },
2053         { 0x0d, "Disconnect Physical Link Response",
2054                         amp_disconn_phy_link_rsp, 3, true },
2055         { },
2056 };
2057
2058 static void amp_packet(uint16_t index, bool in, uint16_t handle,
2059                         uint16_t cid, const void *data, uint16_t size)
2060 {
2061         struct l2cap_frame frame;
2062         uint16_t control, fcs, len;
2063         uint8_t opcode, ident;
2064         const struct amp_opcode_data *opcode_data = NULL;
2065         const char *opcode_color, *opcode_str;
2066         int i;
2067
2068         if (size < 4) {
2069                 print_text(COLOR_ERROR, "malformed info frame packet");
2070                 packet_hexdump(data, size);
2071                 return;
2072         }
2073
2074         control = get_le16(data);
2075         fcs = get_le16(data + size - 2);
2076
2077         print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
2078                                 " %d dlen %d control 0x%4.4x fcs 0x%4.4x",
2079                                                 3, size, control, fcs);
2080
2081         if (control & 0x01)
2082                 return;
2083
2084         if (size < 8) {
2085                 print_text(COLOR_ERROR, "malformed manager packet");
2086                 packet_hexdump(data, size);
2087                 return;
2088         }
2089
2090         opcode = *((const uint8_t *) (data + 2));
2091         ident = *((const uint8_t *) (data + 3));
2092         len = get_le16(data + 4);
2093
2094         if (len != size - 8) {
2095                 print_text(COLOR_ERROR, "invalid manager packet size");
2096                 packet_hexdump(data +  2, size - 4);
2097                 return;
2098         }
2099
2100         for (i = 0; amp_opcode_table[i].str; i++) {
2101                 if (amp_opcode_table[i].opcode == opcode) {
2102                         opcode_data = &amp_opcode_table[i];
2103                         break;
2104                 }
2105         }
2106
2107         if (opcode_data) {
2108                 if (opcode_data->func) {
2109                         if (in)
2110                                 opcode_color = COLOR_MAGENTA;
2111                         else
2112                                 opcode_color = COLOR_BLUE;
2113                 } else
2114                         opcode_color = COLOR_WHITE_BG;
2115                 opcode_str = opcode_data->str;
2116         } else {
2117                 opcode_color = COLOR_WHITE_BG;
2118                 opcode_str = "Unknown";
2119         }
2120
2121         print_indent(6, opcode_color, "AMP: ", opcode_str, COLOR_OFF,
2122                         " (0x%2.2x) ident %d len %d", opcode, ident, len);
2123
2124         if (!opcode_data || !opcode_data->func) {
2125                 packet_hexdump(data + 6, size - 8);
2126                 return;
2127         }
2128
2129         if (opcode_data->fixed) {
2130                 if (len != opcode_data->size) {
2131                         print_text(COLOR_ERROR, "invalid size");
2132                         packet_hexdump(data + 6, size - 8);
2133                         return;
2134                 }
2135         } else {
2136                 if (len < opcode_data->size) {
2137                         print_text(COLOR_ERROR, "too short packet");
2138                         packet_hexdump(data + 6, size - 8);
2139                         return;
2140                 }
2141         }
2142
2143         l2cap_frame_init(&frame, index, in, handle, 0, cid, 0, data + 6, len);
2144         opcode_data->func(&frame);
2145 }
2146
2147 static void print_smp_io_capa(uint8_t io_capa)
2148 {
2149         const char *str;
2150
2151         switch (io_capa) {
2152         case 0x00:
2153                 str = "DisplayOnly";
2154                 break;
2155         case 0x01:
2156                 str = "DisplayYesNo";
2157                 break;
2158         case 0x02:
2159                 str = "KeyboardOnly";
2160                 break;
2161         case 0x03:
2162                 str = "NoInputNoOutput";
2163                 break;
2164         case 0x04:
2165                 str = "KeyboardDisplay";
2166                 break;
2167         default:
2168                 str = "Reserved";
2169                 break;
2170         }
2171
2172         print_field("IO capability: %s (0x%2.2x)", str, io_capa);
2173 }
2174
2175 static void print_smp_oob_data(uint8_t oob_data)
2176 {
2177         const char *str;
2178
2179         switch (oob_data) {
2180         case 0x00:
2181                 str = "Authentication data not present";
2182                 break;
2183         case 0x01:
2184                 str = "Authentication data from remote device present";
2185                 break;
2186         default:
2187                 str = "Reserved";
2188                 break;
2189         }
2190
2191         print_field("OOB data: %s (0x%2.2x)", str, oob_data);
2192 }
2193
2194 static void print_smp_auth_req(uint8_t auth_req)
2195 {
2196         const char *bond, *mitm, *sc, *kp, *ct2;
2197
2198         switch (auth_req & 0x03) {
2199         case 0x00:
2200                 bond = "No bonding";
2201                 break;
2202         case 0x01:
2203                 bond = "Bonding";
2204                 break;
2205         default:
2206                 bond = "Reserved";
2207                 break;
2208         }
2209
2210         if (auth_req & 0x04)
2211                 mitm = "MITM";
2212         else
2213                 mitm = "No MITM";
2214
2215         if (auth_req & 0x08)
2216                 sc = "SC";
2217         else
2218                 sc = "Legacy";
2219
2220         if (auth_req & 0x10)
2221                 kp = "Keypresses";
2222         else
2223                 kp = "No Keypresses";
2224
2225         if (auth_req & 0x20)
2226                 ct2 = ", CT2";
2227         else
2228                 ct2 = "";
2229
2230         print_field("Authentication requirement: %s, %s, %s, %s%s (0x%2.2x)",
2231                                         bond, mitm, sc, kp, ct2, auth_req);
2232 }
2233
2234 static void print_smp_key_dist(const char *label, uint8_t dist)
2235 {
2236         char str[27];
2237
2238         if (!(dist & 0x07)) {
2239                 strcpy(str, "<none> ");
2240         } else {
2241                 str[0] = '\0';
2242                 if (dist & 0x01)
2243                         strcat(str, "EncKey ");
2244                 if (dist & 0x02)
2245                         strcat(str, "IdKey ");
2246                 if (dist & 0x04)
2247                         strcat(str, "Sign ");
2248                 if (dist & 0x08)
2249                         strcat(str, "LinkKey ");
2250         }
2251
2252         print_field("%s: %s(0x%2.2x)", label, str, dist);
2253 }
2254
2255 static void smp_pairing_request(const struct l2cap_frame *frame)
2256 {
2257         const struct bt_l2cap_smp_pairing_request *pdu = frame->data;
2258
2259         print_smp_io_capa(pdu->io_capa);
2260         print_smp_oob_data(pdu->oob_data);
2261         print_smp_auth_req(pdu->auth_req);
2262
2263         print_field("Max encryption key size: %d", pdu->max_key_size);
2264         print_smp_key_dist("Initiator key distribution", pdu->init_key_dist);
2265         print_smp_key_dist("Responder key distribution", pdu->resp_key_dist);
2266 }
2267
2268 static void smp_pairing_response(const struct l2cap_frame *frame)
2269 {
2270         const struct bt_l2cap_smp_pairing_response *pdu = frame->data;
2271
2272         print_smp_io_capa(pdu->io_capa);
2273         print_smp_oob_data(pdu->oob_data);
2274         print_smp_auth_req(pdu->auth_req);
2275
2276         print_field("Max encryption key size: %d", pdu->max_key_size);
2277         print_smp_key_dist("Initiator key distribution", pdu->init_key_dist);
2278         print_smp_key_dist("Responder key distribution", pdu->resp_key_dist);
2279 }
2280
2281 static void smp_pairing_confirm(const struct l2cap_frame *frame)
2282 {
2283         const struct bt_l2cap_smp_pairing_confirm *pdu = frame->data;
2284
2285         print_hex_field("Confim value", pdu->value, 16);
2286 }
2287
2288 static void smp_pairing_random(const struct l2cap_frame *frame)
2289 {
2290         const struct bt_l2cap_smp_pairing_random *pdu = frame->data;
2291
2292         print_hex_field("Random value", pdu->value, 16);
2293 }
2294
2295 static void smp_pairing_failed(const struct l2cap_frame *frame)
2296 {
2297         const struct bt_l2cap_smp_pairing_failed *pdu = frame->data;
2298         const char *str;
2299
2300         switch (pdu->reason) {
2301         case 0x01:
2302                 str = "Passkey entry failed";
2303                 break;
2304         case 0x02:
2305                 str = "OOB not available";
2306                 break;
2307         case 0x03:
2308                 str = "Authentication requirements";
2309                 break;
2310         case 0x04:
2311                 str = "Confirm value failed";
2312                 break;
2313         case 0x05:
2314                 str = "Pairing not supported";
2315                 break;
2316         case 0x06:
2317                 str = "Encryption key size";
2318                 break;
2319         case 0x07:
2320                 str = "Command not supported";
2321                 break;
2322         case 0x08:
2323                 str = "Unspecified reason";
2324                 break;
2325         case 0x09:
2326                 str = "Repeated attempts";
2327                 break;
2328         case 0x0a:
2329                 str = "Invalid parameters";
2330                 break;
2331         case 0x0b:
2332                 str = "DHKey check failed";
2333                 break;
2334         case 0x0c:
2335                 str = "Numeric comparison failed";
2336                 break;
2337         case 0x0d:
2338                 str = "BR/EDR pairing in progress";
2339                 break;
2340         case 0x0e:
2341                 str = "Cross-transport Key Derivation/Generation not allowed";
2342                 break;
2343         default:
2344                 str = "Reserved";
2345                 break;
2346         }
2347
2348         print_field("Reason: %s (0x%2.2x)", str, pdu->reason);
2349 }
2350
2351 static void smp_encrypt_info(const struct l2cap_frame *frame)
2352 {
2353         const struct bt_l2cap_smp_encrypt_info *pdu = frame->data;
2354
2355         print_hex_field("Long term key", pdu->ltk, 16);
2356 }
2357
2358 static void smp_central_ident(const struct l2cap_frame *frame)
2359 {
2360         const struct bt_l2cap_smp_central_ident *pdu = frame->data;
2361
2362         print_field("EDIV: 0x%4.4x", le16_to_cpu(pdu->ediv));
2363         print_field("Rand: 0x%16.16" PRIx64, le64_to_cpu(pdu->rand));
2364 }
2365
2366 static void smp_ident_info(const struct l2cap_frame *frame)
2367 {
2368         const struct bt_l2cap_smp_ident_info *pdu = frame->data;
2369
2370         print_hex_field("Identity resolving key", pdu->irk, 16);
2371
2372         keys_update_identity_key(pdu->irk);
2373 }
2374
2375 static void smp_ident_addr_info(const struct l2cap_frame *frame)
2376 {
2377         const struct bt_l2cap_smp_ident_addr_info *pdu = frame->data;
2378
2379         packet_print_addr("Address", pdu->addr, pdu->addr_type);
2380
2381         keys_update_identity_addr(pdu->addr, pdu->addr_type);
2382 }
2383
2384 static void smp_signing_info(const struct l2cap_frame *frame)
2385 {
2386         const struct bt_l2cap_smp_signing_info *pdu = frame->data;
2387
2388         print_hex_field("Signature key", pdu->csrk, 16);
2389 }
2390
2391 static void smp_security_request(const struct l2cap_frame *frame)
2392 {
2393         const struct bt_l2cap_smp_security_request *pdu = frame->data;
2394
2395         print_smp_auth_req(pdu->auth_req);
2396 }
2397
2398 static void smp_pairing_public_key(const struct l2cap_frame *frame)
2399 {
2400         const struct bt_l2cap_smp_public_key *pdu = frame->data;
2401
2402         print_hex_field("X", pdu->x, 32);
2403         print_hex_field("Y", pdu->y, 32);
2404 }
2405
2406 static void smp_pairing_dhkey_check(const struct l2cap_frame *frame)
2407 {
2408         const struct bt_l2cap_smp_dhkey_check *pdu = frame->data;
2409
2410         print_hex_field("E", pdu->e, 16);
2411 }
2412
2413 static void smp_pairing_keypress_notification(const struct l2cap_frame *frame)
2414 {
2415         const struct bt_l2cap_smp_keypress_notify *pdu = frame->data;
2416         const char *str;
2417
2418         switch (pdu->type) {
2419         case 0x00:
2420                 str = "Passkey entry started";
2421                 break;
2422         case 0x01:
2423                 str = "Passkey digit entered";
2424                 break;
2425         case 0x02:
2426                 str = "Passkey digit erased";
2427                 break;
2428         case 0x03:
2429                 str = "Passkey cleared";
2430                 break;
2431         case 0x04:
2432                 str = "Passkey entry completed";
2433                 break;
2434         default:
2435                 str = "Reserved";
2436                 break;
2437         }
2438
2439         print_field("Type: %s (0x%2.2x)", str, pdu->type);
2440 }
2441
2442 struct smp_opcode_data {
2443         uint8_t opcode;
2444         const char *str;
2445         void (*func) (const struct l2cap_frame *frame);
2446         uint8_t size;
2447         bool fixed;
2448 };
2449
2450 static const struct smp_opcode_data smp_opcode_table[] = {
2451         { 0x01, "Pairing Request",
2452                         smp_pairing_request, 6, true },
2453         { 0x02, "Pairing Response",
2454                         smp_pairing_response, 6, true },
2455         { 0x03, "Pairing Confirm",
2456                         smp_pairing_confirm, 16, true },
2457         { 0x04, "Pairing Random",
2458                         smp_pairing_random, 16, true },
2459         { 0x05, "Pairing Failed",
2460                         smp_pairing_failed, 1, true },
2461         { 0x06, "Encryption Information",
2462                         smp_encrypt_info, 16, true },
2463         { 0x07, "Central Identification",
2464                         smp_central_ident, 10, true },
2465         { 0x08, "Identity Information",
2466                         smp_ident_info, 16, true },
2467         { 0x09, "Identity Address Information",
2468                         smp_ident_addr_info, 7, true },
2469         { 0x0a, "Signing Information",
2470                         smp_signing_info, 16, true },
2471         { 0x0b, "Security Request",
2472                         smp_security_request, 1, true },
2473         { 0x0c, "Pairing Public Key",
2474                         smp_pairing_public_key, 64, true },
2475         { 0x0d, "Pairing DHKey Check",
2476                         smp_pairing_dhkey_check, 16, true },
2477         { 0x0e, "Pairing Keypress Notification",
2478                         smp_pairing_keypress_notification, 1, true },
2479         { }
2480 };
2481
2482 static void smp_packet(uint16_t index, bool in, uint16_t handle,
2483                         uint16_t cid, const void *data, uint16_t size)
2484 {
2485         struct l2cap_frame frame;
2486         uint8_t opcode = *((const uint8_t *) data);
2487         const struct smp_opcode_data *opcode_data = NULL;
2488         const char *opcode_color, *opcode_str;
2489         int i;
2490
2491         if (size < 1) {
2492                 print_text(COLOR_ERROR, "malformed attribute packet");
2493                 packet_hexdump(data, size);
2494                 return;
2495         }
2496
2497         for (i = 0; smp_opcode_table[i].str; i++) {
2498                 if (smp_opcode_table[i].opcode == opcode) {
2499                         opcode_data = &smp_opcode_table[i];
2500                         break;
2501                 }
2502         }
2503
2504         if (opcode_data) {
2505                 if (opcode_data->func) {
2506                         if (in)
2507                                 opcode_color = COLOR_MAGENTA;
2508                         else
2509                                 opcode_color = COLOR_BLUE;
2510                 } else
2511                         opcode_color = COLOR_WHITE_BG;
2512                 opcode_str = opcode_data->str;
2513         } else {
2514                 opcode_color = COLOR_WHITE_BG;
2515                 opcode_str = "Unknown";
2516         }
2517
2518         print_indent(6, opcode_color, cid == 0x0006 ? "SMP: " : "BR/EDR SMP: ",
2519                                 opcode_str, COLOR_OFF, " (0x%2.2x) len %d",
2520                                 opcode, size - 1);
2521
2522         if (!opcode_data || !opcode_data->func) {
2523                 packet_hexdump(data + 1, size - 1);
2524                 return;
2525         }
2526
2527         if (opcode_data->fixed) {
2528                 if (size - 1 != opcode_data->size) {
2529                         print_text(COLOR_ERROR, "invalid size");
2530                         packet_hexdump(data + 1, size - 1);
2531                         return;
2532                 }
2533         } else {
2534                 if (size - 1 < opcode_data->size) {
2535                         print_text(COLOR_ERROR, "too short packet");
2536                         packet_hexdump(data + 1, size - 1);
2537                         return;
2538                 }
2539         }
2540
2541         l2cap_frame_init(&frame, index, in, handle, 0, cid, 0,
2542                                                 data + 1, size - 1);
2543         opcode_data->func(&frame);
2544 }
2545
2546 void l2cap_frame(uint16_t index, bool in, uint16_t handle, uint16_t cid,
2547                         uint16_t psm, const void *data, uint16_t size)
2548 {
2549         struct l2cap_frame frame;
2550         struct chan_data *chan;
2551         uint32_t ctrl32 = 0;
2552         uint16_t ctrl16 = 0;
2553         uint8_t ext_ctrl;
2554
2555         switch (cid) {
2556         case 0x0001:
2557                 bredr_sig_packet(index, in, handle, cid, data, size);
2558                 break;
2559         case 0x0002:
2560                 connless_packet(index, in, handle, cid, data, size);
2561                 break;
2562         case 0x0003:
2563                 amp_packet(index, in, handle, cid, data, size);
2564                 break;
2565         case 0x0004:
2566                 att_packet(index, in, handle, cid, data, size);
2567                 break;
2568         case 0x0005:
2569                 le_sig_packet(index, in, handle, cid, data, size);
2570                 break;
2571         case 0x0006:
2572         case 0x0007:
2573                 smp_packet(index, in, handle, cid, data, size);
2574                 break;
2575         default:
2576                 l2cap_frame_init(&frame, index, in, handle, 0, cid, psm,
2577                                                         data, size);
2578
2579                 switch (frame.mode) {
2580                 case L2CAP_MODE_LE_FLOWCTL:
2581                 case L2CAP_MODE_ECRED:
2582                         chan = get_chan(&frame);
2583                         if (!chan)
2584                                 return;
2585
2586                         if (!chan->sdu) {
2587                                 if (!l2cap_frame_get_le16(&frame, &chan->sdu))
2588                                         return;
2589                         }
2590                         print_indent(6, COLOR_CYAN, "Channel:", "",
2591                                         COLOR_OFF, " %d len %d sdu %d"
2592                                         " [PSM %d mode %s (0x%02x)] {chan %d}",
2593                                         cid, size, chan->sdu, frame.psm,
2594                                         mode2str(frame.mode), frame.mode,
2595                                         frame.chan);
2596                         chan->sdu -= frame.size;
2597                         break;
2598                 case L2CAP_MODE_BASIC:
2599                         print_indent(6, COLOR_CYAN, "Channel:", "", COLOR_OFF,
2600                                         " %d len %d [PSM %d mode %s (0x%02x)] "
2601                                         "{chan %d}", cid, size, frame.psm,
2602                                         mode2str(frame.mode), frame.mode,
2603                                         frame.chan);
2604                         break;
2605                 default:
2606                         ext_ctrl = get_ext_ctrl(&frame);
2607
2608                         if (ext_ctrl) {
2609                                 if (!l2cap_frame_get_le32(&frame, &ctrl32))
2610                                         return;
2611
2612                                 print_indent(6, COLOR_CYAN, "Channel:", "",
2613                                                 COLOR_OFF, " %d len %d"
2614                                                 " ext_ctrl 0x%8.8x"
2615                                                 " [PSM %d mode %s (0x%02x)] "
2616                                                 "{chan %d}", cid, size, ctrl32,
2617                                                 frame.psm, mode2str(frame.mode),
2618                                                 frame.mode, frame.chan);
2619
2620                                 l2cap_ctrl_ext_parse(&frame, ctrl32);
2621                         } else {
2622                                 if (!l2cap_frame_get_le16(&frame, &ctrl16))
2623                                         return;
2624
2625                                 print_indent(6, COLOR_CYAN, "Channel:", "",
2626                                                 COLOR_OFF, " %d len %d"
2627                                                 " ctrl 0x%4.4x"
2628                                                 " [PSM %d mode %s (0x%02x)] "
2629                                                 "{chan %d}", cid, size, ctrl16,
2630                                                 frame.psm, mode2str(frame.mode),
2631                                                 frame.mode, frame.chan);
2632
2633                                 l2cap_ctrl_parse(&frame, ctrl16);
2634                         }
2635
2636                         printf("\n");
2637                         break;
2638                 }
2639
2640                 switch (frame.psm) {
2641                 case 0x0001:
2642                         sdp_packet(&frame);
2643                         break;
2644                 case 0x0003:
2645                         rfcomm_packet(&frame);
2646                         break;
2647                 case 0x000f:
2648                         bnep_packet(&frame);
2649                         break;
2650                 case 0x001f:
2651                         att_packet(index, in, handle, cid, data, size);
2652                         break;
2653                 case 0x0027:
2654                         att_packet(index, in, handle, cid, data + 2, size - 2);
2655                         break;
2656                 case 0x0017:
2657                 case 0x001B:
2658                         avctp_packet(&frame);
2659                         break;
2660                 case 0x0019:
2661                         avdtp_packet(&frame);
2662                         break;
2663                 default:
2664                         packet_hexdump(data, size);
2665                         break;
2666                 }
2667                 break;
2668         }
2669 }
2670
2671 void l2cap_packet(uint16_t index, bool in, uint16_t handle, uint8_t flags,
2672                                         const void *data, uint16_t size)
2673 {
2674         const struct bt_l2cap_hdr *hdr = data;
2675         uint16_t len, cid;
2676
2677         if (index > MAX_INDEX - 1) {
2678                 print_text(COLOR_ERROR, "controller index too large");
2679                 packet_hexdump(data, size);
2680                 return;
2681         }
2682
2683         switch (flags) {
2684         case 0x00:      /* start of a non-automatically-flushable PDU */
2685         case 0x02:      /* start of an automatically-flushable PDU */
2686                 if (index_list[index][in].frag_len) {
2687                         print_text(COLOR_ERROR, "unexpected start frame");
2688                         packet_hexdump(data, size);
2689                         clear_fragment_buffer(index, in);
2690                         return;
2691                 }
2692
2693                 if (size < sizeof(*hdr)) {
2694                         print_text(COLOR_ERROR, "frame too short");
2695                         packet_hexdump(data, size);
2696                         return;
2697                 }
2698
2699                 len = le16_to_cpu(hdr->len);
2700                 cid = le16_to_cpu(hdr->cid);
2701
2702                 data += sizeof(*hdr);
2703                 size -= sizeof(*hdr);
2704
2705                 if (len == size) {
2706                         /* complete frame */
2707                         l2cap_frame(index, in, handle, cid, 0, data, len);
2708                         return;
2709                 }
2710
2711                 if (size > len) {
2712                         print_text(COLOR_ERROR, "frame too long");
2713                         packet_hexdump(data, size);
2714                         return;
2715                 }
2716
2717                 index_list[index][in].frag_buf = malloc(len);
2718                 if (!index_list[index][in].frag_buf) {
2719                         print_text(COLOR_ERROR, "failed buffer allocation");
2720                         packet_hexdump(data, size);
2721                         return;
2722                 }
2723
2724                 memcpy(index_list[index][in].frag_buf, data, size);
2725                 index_list[index][in].frag_pos = size;
2726                 index_list[index][in].frag_len = len - size;
2727                 index_list[index][in].frag_cid = cid;
2728                 break;
2729
2730         case 0x01:      /* continuing fragment */
2731                 if (!index_list[index][in].frag_len) {
2732                         print_text(COLOR_ERROR, "unexpected continuation");
2733                         packet_hexdump(data, size);
2734                         return;
2735                 }
2736
2737                 if (size > index_list[index][in].frag_len) {
2738                         print_text(COLOR_ERROR, "fragment too long");
2739                         packet_hexdump(data, size);
2740                         clear_fragment_buffer(index, in);
2741                         return;
2742                 }
2743
2744                 memcpy(index_list[index][in].frag_buf +
2745                                 index_list[index][in].frag_pos, data, size);
2746                 index_list[index][in].frag_pos += size;
2747                 index_list[index][in].frag_len -= size;
2748
2749                 if (!index_list[index][in].frag_len) {
2750                         /* complete frame */
2751                         l2cap_frame(index, in, handle,
2752                                         index_list[index][in].frag_cid, 0,
2753                                         index_list[index][in].frag_buf,
2754                                         index_list[index][in].frag_pos);
2755                         clear_fragment_buffer(index, in);
2756                         return;
2757                 }
2758                 break;
2759
2760         case 0x03:      /* complete automatically-flushable PDU */
2761                 if (index_list[index][in].frag_len) {
2762                         print_text(COLOR_ERROR, "unexpected complete frame");
2763                         packet_hexdump(data, size);
2764                         clear_fragment_buffer(index, in);
2765                         return;
2766                 }
2767
2768                 if (size < sizeof(*hdr)) {
2769                         print_text(COLOR_ERROR, "frame too short");
2770                         packet_hexdump(data, size);
2771                         return;
2772                 }
2773
2774                 len = le16_to_cpu(hdr->len);
2775                 cid = le16_to_cpu(hdr->cid);
2776
2777                 data += sizeof(*hdr);
2778                 size -= sizeof(*hdr);
2779
2780                 if (len != size) {
2781                         print_text(COLOR_ERROR, "wrong frame size");
2782                         packet_hexdump(data, size);
2783                         return;
2784                 }
2785
2786                 /* complete frame */
2787                 l2cap_frame(index, in, handle, cid, 0, data, len);
2788                 break;
2789
2790         default:
2791                 print_text(COLOR_ERROR, "invalid packet flags (0x%2.2x)",
2792                                                                 flags);
2793                 packet_hexdump(data, size);
2794                 return;
2795         }
2796 }
2797
2798 void l2cap_dequeue_frame(struct timeval *delta, struct packet_conn_data *conn)
2799 {
2800         struct l2cap_frame *frame;
2801         struct chan_data *chan;
2802
2803         frame = queue_pop_head(conn->chan_q);
2804         if (!frame)
2805                 return;
2806
2807         chan = get_chan(frame);
2808         if (!chan)
2809                 return;
2810
2811         packet_latency_add(&chan->tx_l, delta);
2812
2813         print_field("Channel: %d [PSM %d mode %s (0x%02x)] {chan %d}",
2814                         frame->cid, frame->psm, mode2str(frame->mode),
2815                         frame->mode, frame->chan);
2816
2817         print_field("Channel Latency: %lld msec (%lld-%lld msec ~%lld msec)",
2818                         TV_MSEC(*delta), TV_MSEC(chan->tx_l.min),
2819                         TV_MSEC(chan->tx_l.max), TV_MSEC(chan->tx_l.med));
2820
2821         free(frame);
2822 }