monitor/l2cap.c

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