monitor/att.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2011-2014  Intel Corporation
   7  *  Copyright (C) 2002-2010  Marcel Holtmann <marcel@holtmann.org>
   8  *  Copyright 2023 NXP
   9  *
  10  *
  11  */
  12
  13 #ifdef HAVE_CONFIG_H
  14 #include <config.h>
  15 #endif
  16
  17 #include <ctype.h>
  18 #include <stdio.h>
  19 #include <stdlib.h>
  20 #include <string.h>
  21 #include <inttypes.h>
  22 #include <stdbool.h>
  23 #include <errno.h>
  24 #include <linux/limits.h>
  25 #include <sys/stat.h>
  26
  27 #include <glib.h>
  28
  29 #include "lib/bluetooth.h"
  30 #include "lib/uuid.h"
  31 #include "lib/hci.h"
  32 #include "lib/hci_lib.h"
  33
  34 #include "src/shared/util.h"
  35 #include "src/shared/queue.h"
  36 #include "src/shared/att.h"
  37 #include "src/shared/gatt-db.h"
  38 #include "src/textfile.h"
  39 #include "src/settings.h"
  40 #include "bt.h"
  41 #include "packet.h"
  42 #include "display.h"
  43 #include "l2cap.h"
  44 #include "att.h"
  45 #include "keys.h"
  46
  47 struct att_read {
  48         struct gatt_db_attribute *attr;
  49         bool in;
  50         uint16_t chan;
  51         void (*func)(const struct l2cap_frame *frame);
  52 };
  53
  54 struct att_conn_data {
  55         struct gatt_db *ldb;
  56         struct timespec ldb_mtim;
  57         struct gatt_db *rdb;
  58         struct timespec rdb_mtim;
  59         struct queue *reads;
  60 };
  61
  62 static void print_uuid(const char *label, const void *data, uint16_t size)
  63 {
  64         const char *str;
  65         char uuidstr[MAX_LEN_UUID_STR];
  66
  67         switch (size) {
  68         case 2:
  69                 str = bt_uuid16_to_str(get_le16(data));
  70                 print_field("%s: %s (0x%4.4x)", label, str, get_le16(data));
  71                 break;
  72         case 4:
  73                 str = bt_uuid32_to_str(get_le32(data));
  74                 print_field("%s: %s (0x%8.8x)", label, str, get_le32(data));
  75                 break;
  76         case 16:
  77                 sprintf(uuidstr, "%8.8x-%4.4x-%4.4x-%4.4x-%8.8x%4.4x",
  78                                 get_le32(data + 12), get_le16(data + 10),
  79                                 get_le16(data + 8), get_le16(data + 6),
  80                                 get_le32(data + 2), get_le16(data + 0));
  81                 str = bt_uuidstr_to_str(uuidstr);
  82                 print_field("%s: %s (%s)", label, str, uuidstr);
  83                 break;
  84         default:
  85                 packet_hexdump(data, size);
  86                 break;
  87         }
  88 }
  89
  90 static void print_handle_range(const char *label, const void *data)
  91 {
  92         print_field("%s: 0x%4.4x-0x%4.4x", label,
  93                                 get_le16(data), get_le16(data + 2));
  94 }
  95
  96 static bool match_read_frame(const void *data, const void *match_data)
  97 {
  98         const struct att_read *read = data;
  99         const struct l2cap_frame *frame = match_data;
 100
 101         /* Read frame and response frame shall be in the opposite direction to
 102          * match.
 103          */
 104         if (read->in == frame->in)
 105                 return false;
 106
 107         return read->chan == frame->chan;
 108 }
 109
 110 static struct att_read *att_get_read(const struct l2cap_frame *frame)
 111 {
 112         struct packet_conn_data *conn;
 113         struct att_conn_data *data;
 114
 115         conn = packet_get_conn_data(frame->handle);
 116         if (!conn)
 117                 return NULL;
 118
 119         data = conn->data;
 120         if (!data)
 121                 return NULL;
 122
 123         return queue_remove_if(data->reads, match_read_frame, (void *)frame);
 124 }
 125
 126 static void print_value(struct gatt_db_attribute *attr)
 127 {
 128         uint16_t handle;
 129         struct gatt_db_attribute *val;
 130         const bt_uuid_t *uuid;
 131         bt_uuid_t chrc = {
 132                 .type = BT_UUID16,
 133                 .value.u16 = 0x2803,
 134         };
 135         char label[27];
 136
 137         uuid = gatt_db_attribute_get_type(attr);
 138         if (!uuid)
 139                 return;
 140
 141         /* Skip in case of characteristic declaration since it already prints
 142          * the value handle and properties.
 143          */
 144         if (!bt_uuid_cmp(uuid, &chrc))
 145                 return;
 146
 147         val = gatt_db_attribute_get_value(attr);
 148         if (!val || val == attr)
 149                 return;
 150
 151         uuid = gatt_db_attribute_get_type(val);
 152         if (!uuid)
 153                 return;
 154
 155         handle = gatt_db_attribute_get_handle(val);
 156         if (!handle)
 157                 return;
 158
 159         switch (uuid->type) {
 160         case BT_UUID16:
 161                 sprintf(label, "Value Handle: 0x%4.4x Type", handle);
 162                 print_field("%s: %s (0x%4.4x)", label,
 163                                 bt_uuid16_to_str(uuid->value.u16),
 164                                 uuid->value.u16);
 165                 return;
 166         case BT_UUID128:
 167                 sprintf(label, "Value Handle: 0x%4.4x Type", handle);
 168                 print_uuid(label, &uuid->value.u128, 16);
 169                 return;
 170         case BT_UUID_UNSPEC:
 171         case BT_UUID32:
 172                 break;
 173         }
 174 }
 175
 176 static void print_attribute(struct gatt_db_attribute *attr)
 177 {
 178         uint16_t handle;
 179         const bt_uuid_t *uuid;
 180         char label[21];
 181
 182         handle = gatt_db_attribute_get_handle(attr);
 183         if (!handle)
 184                 goto done;
 185
 186         uuid = gatt_db_attribute_get_type(attr);
 187         if (!uuid)
 188                 goto done;
 189
 190         switch (uuid->type) {
 191         case BT_UUID16:
 192                 sprintf(label, "Handle: 0x%4.4x Type", handle);
 193                 print_field("%s: %s (0x%4.4x)", label,
 194                                 bt_uuid16_to_str(uuid->value.u16),
 195                                 uuid->value.u16);
 196                 print_value(attr);
 197                 return;
 198         case BT_UUID128:
 199                 sprintf(label, "Handle: 0x%4.4x Type", handle);
 200                 print_uuid(label, &uuid->value.u128, 16);
 201                 print_value(attr);
 202                 return;
 203         case BT_UUID_UNSPEC:
 204         case BT_UUID32:
 205                 break;
 206         }
 207
 208 done:
 209         print_field("Handle: 0x%4.4x", handle);
 210 }
 211
 212 static void print_data_list(const char *label, uint8_t length,
 213                                         const struct l2cap_frame *frame)
 214 {
 215         struct att_read *read;
 216         uint8_t count;
 217
 218         if (length == 0)
 219                 return;
 220
 221         read = att_get_read(frame);
 222
 223         count = frame->size / length;
 224
 225         print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
 226
 227         while (frame->size >= length) {
 228                 if (!l2cap_frame_print_le16((void *)frame, "Handle"))
 229                         break;
 230
 231                 print_hex_field("Value", frame->data, length - 2);
 232
 233                 if (read) {
 234                         struct l2cap_frame f;
 235
 236                         l2cap_frame_clone_size(&f, frame, length - 2);
 237
 238                         read->func(&f);
 239                 }
 240
 241                 if (!l2cap_frame_pull((void *)frame, frame, length - 2))
 242                         break;
 243         }
 244
 245         packet_hexdump(frame->data, frame->size);
 246         free(read);
 247 }
 248
 249 static void print_attribute_info(uint16_t type, const void *data, uint16_t len)
 250 {
 251         const char *str = bt_uuid16_to_str(type);
 252
 253         print_field("%s: %s (0x%4.4x)", "Attribute type", str, type);
 254
 255         switch (type) {
 256         case 0x2800:    /* Primary Service */
 257         case 0x2801:    /* Secondary Service */
 258                 print_uuid("  UUID", data, len);
 259                 break;
 260         case 0x2802:    /* Include */
 261                 if (len < 4) {
 262                         print_hex_field("  Value", data, len);
 263                         break;
 264                 }
 265                 print_handle_range("  Handle range", data);
 266                 print_uuid("  UUID", data + 4, len - 4);
 267                 break;
 268         case 0x2803:    /* Characteristic */
 269                 if (len < 3) {
 270                         print_hex_field("  Value", data, len);
 271                         break;
 272                 }
 273                 print_field("  Properties: 0x%2.2x", *((uint8_t *) data));
 274                 print_field("  Handle: 0x%2.2x", get_le16(data + 1));
 275                 print_uuid("  UUID", data + 3, len - 3);
 276                 break;
 277         default:
 278                 print_hex_field("Value", data, len);
 279                 break;
 280         }
 281 }
 282
 283 static const char *att_opcode_to_str(uint8_t opcode);
 284
 285 static void att_error_response(const struct l2cap_frame *frame)
 286 {
 287         const struct bt_l2cap_att_error_response *pdu = frame->data;
 288         const char *str;
 289
 290         switch (pdu->error) {
 291         case 0x01:
 292                 str = "Invalid Handle";
 293                 break;
 294         case 0x02:
 295                 str = "Read Not Permitted";
 296                 break;
 297         case 0x03:
 298                 str = "Write Not Permitted";
 299                 break;
 300         case 0x04:
 301                 str = "Invalid PDU";
 302                 break;
 303         case 0x05:
 304                 str = "Insufficient Authentication";
 305                 break;
 306         case 0x06:
 307                 str = "Request Not Supported";
 308                 break;
 309         case 0x07:
 310                 str = "Invalid Offset";
 311                 break;
 312         case 0x08:
 313                 str = "Insufficient Authorization";
 314                 break;
 315         case 0x09:
 316                 str = "Prepare Queue Full";
 317                 break;
 318         case 0x0a:
 319                 str = "Attribute Not Found";
 320                 break;
 321         case 0x0b:
 322                 str = "Attribute Not Long";
 323                 break;
 324         case 0x0c:
 325                 str = "Insufficient Encryption Key Size";
 326                 break;
 327         case 0x0d:
 328                 str = "Invalid Attribute Value Length";
 329                 break;
 330         case 0x0e:
 331                 str = "Unlikely Error";
 332                 break;
 333         case 0x0f:
 334                 str = "Insufficient Encryption";
 335                 break;
 336         case 0x10:
 337                 str = "Unsupported Group Type";
 338                 break;
 339         case 0x11:
 340                 str = "Insufficient Resources";
 341                 break;
 342         case 0x12:
 343                 str = "Database Out of Sync";
 344                 break;
 345         case 0x13:
 346                 str = "Value Not Allowed";
 347                 break;
 348         case 0xfd:
 349                 str = "CCC Improperly Configured";
 350                 break;
 351         case 0xfe:
 352                 str = "Procedure Already in Progress";
 353                 break;
 354         case 0xff:
 355                 str = "Out of Range";
 356                 break;
 357         default:
 358                 str = "Reserved";
 359                 break;
 360         }
 361
 362         print_field("%s (0x%2.2x)", att_opcode_to_str(pdu->request),
 363                                                         pdu->request);
 364         print_field("Handle: 0x%4.4x", le16_to_cpu(pdu->handle));
 365         print_field("Error: %s (0x%2.2x)", str, pdu->error);
 366
 367         /* Read/Read By Type/Read By Group Type may create a read object which
 368          * needs to be dequeued and freed in case the operation fails.
 369          */
 370         if (pdu->request == 0x08 || pdu->request == 0x0a ||
 371                                         pdu->request == 0x10)
 372                 free(att_get_read(frame));
 373 }
 374
 375 static const struct bitfield_data chrc_prop_table[] = {
 376         {  0, "Broadcast (0x01)"                },
 377         {  1, "Read (0x02)"                     },
 378         {  2, "Write Without Response (0x04)"   },
 379         {  3, "Write (0x08)"                    },
 380         {  4, "Notify (0x10)"                   },
 381         {  5, "Indicate (0x20)"                 },
 382         {  6, "Authorize (0x40)"                },
 383         {  6, "Extended Properties (0x80)"      },
 384         { }
 385 };
 386
 387 static void att_conn_data_free(void *data)
 388 {
 389         struct att_conn_data *att_data = data;
 390
 391         gatt_db_unref(att_data->rdb);
 392         gatt_db_unref(att_data->ldb);
 393         queue_destroy(att_data->reads, free);
 394         free(att_data);
 395 }
 396
 397 static struct att_conn_data *att_get_conn_data(struct packet_conn_data *conn)
 398 {
 399         struct att_conn_data *data;
 400
 401         if (!conn)
 402                 return NULL;
 403
 404         data = conn->data;
 405
 406         if (data)
 407                 return data;
 408
 409         data = new0(struct att_conn_data, 1);
 410         data->rdb = gatt_db_new();
 411         data->ldb = gatt_db_new();
 412         conn->data = data;
 413         conn->destroy = att_conn_data_free;
 414
 415         return data;
 416 }
 417
 418 static void gatt_load_db(struct gatt_db *db, const char *filename,
 419                                                 struct timespec *mtim)
 420 {
 421         struct stat st;
 422
 423         if (lstat(filename, &st))
 424                 return;
 425
 426         if (!gatt_db_isempty(db)) {
 427                 /* Check if file has been modified since last time */
 428                 if (st.st_mtim.tv_sec == mtim->tv_sec &&
 429                                     st.st_mtim.tv_nsec == mtim->tv_nsec)
 430                         return;
 431                 /* Clear db before reloading */
 432                 gatt_db_clear(db);
 433         }
 434
 435         *mtim = st.st_mtim;
 436
 437         btd_settings_gatt_db_load(db, filename);
 438 }
 439
 440 static void load_gatt_db(struct packet_conn_data *conn)
 441 {
 442         struct att_conn_data *data = att_get_conn_data(conn);
 443         char filename[PATH_MAX];
 444         char local[18];
 445         char peer[18];
 446         uint8_t id[6], id_type;
 447
 448         ba2str((bdaddr_t *)conn->src, local);
 449
 450         if (keys_resolve_identity(conn->dst, id, &id_type))
 451                 ba2str((bdaddr_t *)id, peer);
 452         else
 453                 ba2str((bdaddr_t *)conn->dst, peer);
 454
 455         create_filename(filename, PATH_MAX, "/%s/attributes", local);
 456         gatt_load_db(data->ldb, filename, &data->ldb_mtim);
 457
 458         create_filename(filename, PATH_MAX, "/%s/cache/%s", local, peer);
 459         gatt_load_db(data->rdb, filename, &data->rdb_mtim);
 460 }
 461
 462 static struct gatt_db *get_db(const struct l2cap_frame *frame, bool rsp)
 463 {
 464         struct packet_conn_data *conn;
 465         struct att_conn_data *data;
 466         struct gatt_db *db;
 467
 468         conn = packet_get_conn_data(frame->handle);
 469         if (!conn)
 470                 return NULL;
 471
 472         /* Try loading local and remote gatt_db if not loaded yet */
 473         load_gatt_db(conn);
 474
 475         data = conn->data;
 476         if (!data)
 477                 return NULL;
 478
 479         if (frame->in) {
 480                 if (rsp)
 481                         db = data->rdb;
 482                 else
 483                         db = data->ldb;
 484         } else {
 485                 if (rsp)
 486                         db = data->ldb;
 487                 else
 488                         db = data->rdb;
 489         }
 490
 491         return db;
 492 }
 493
 494 static struct gatt_db_attribute *insert_chrc(const struct l2cap_frame *frame,
 495                                                 uint16_t handle,
 496                                                 bt_uuid_t *uuid, uint8_t prop,
 497                                                 bool rsp)
 498 {
 499         struct gatt_db *db;
 500
 501         db = get_db(frame, rsp);
 502         if (!db)
 503                 return NULL;
 504
 505         return gatt_db_insert_characteristic(db, handle, uuid, 0, prop, NULL,
 506                                                         NULL, NULL);
 507 }
 508
 509 static int bt_uuid_from_data(bt_uuid_t *uuid, const void *data, uint16_t size)
 510 {
 511         uint128_t u128;
 512
 513         if (!uuid)
 514                 return -EINVAL;
 515
 516         switch (size) {
 517         case 2:
 518                 return bt_uuid16_create(uuid, get_le16(data));
 519         case 4:
 520                 return bt_uuid32_create(uuid, get_le32(data));
 521         case 16:
 522                 memcpy(u128.data, data, sizeof(u128.data));
 523                 return bt_uuid128_create(uuid, u128);
 524         }
 525
 526         return -EINVAL;
 527 }
 528
 529 static bool svc_read(const struct l2cap_frame *frame, uint16_t *start,
 530                         uint16_t *end, bt_uuid_t *uuid)
 531 {
 532         if (!l2cap_frame_get_le16((void *)frame, start))
 533                 return false;
 534
 535         if (!l2cap_frame_get_le16((void *)frame, end))
 536                 return false;
 537
 538         return !bt_uuid_from_data(uuid, frame->data, frame->size);
 539 }
 540
 541 static struct gatt_db_attribute *insert_svc(const struct l2cap_frame *frame,
 542                                                 uint16_t handle,
 543                                                 bt_uuid_t *uuid, bool primary,
 544                                                 bool rsp, uint16_t num_handles)
 545 {
 546         struct gatt_db *db;
 547
 548         db = get_db(frame, rsp);
 549         if (!db)
 550                 return NULL;
 551
 552         return gatt_db_insert_service(db, handle, uuid, primary, num_handles);
 553 }
 554
 555 static void pri_svc_read(const struct l2cap_frame *frame)
 556 {
 557         uint16_t start, end;
 558         bt_uuid_t uuid;
 559
 560         if (!svc_read(frame, &start, &end, &uuid))
 561                 return;
 562
 563         insert_svc(frame, start, &uuid, true, true, end - start + 1);
 564 }
 565
 566 static void sec_svc_read(const struct l2cap_frame *frame)
 567 {
 568         uint16_t start, end;
 569         bt_uuid_t uuid;
 570
 571         if (!svc_read(frame, &start, &end, &uuid))
 572                 return;
 573
 574         insert_svc(frame, start, &uuid, true, false, end - start + 1);
 575 }
 576
 577 static void print_chrc(const struct l2cap_frame *frame)
 578 {
 579         uint8_t prop;
 580         uint8_t mask;
 581         uint16_t handle;
 582         bt_uuid_t uuid;
 583
 584         if (!l2cap_frame_get_u8((void *)frame, &prop)) {
 585                 print_text(COLOR_ERROR, "Property: invalid size");
 586                 return;
 587         }
 588
 589         print_field("    Properties: 0x%2.2x", prop);
 590
 591         mask = print_bitfield(6, prop, chrc_prop_table);
 592         if (mask)
 593                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
 594                                                                 mask);
 595
 596         if (!l2cap_frame_get_le16((void *)frame, &handle)) {
 597                 print_text(COLOR_ERROR, "    Value Handle: invalid size");
 598                 return;
 599         }
 600
 601         print_field("    Value Handle: 0x%4.4x", handle);
 602         print_uuid("    Value UUID", frame->data, frame->size);
 603         bt_uuid_from_data(&uuid, frame->data, frame->size);
 604
 605         insert_chrc(frame, handle, &uuid, prop, true);
 606 }
 607
 608 static void chrc_read(const struct l2cap_frame *frame)
 609 {
 610         print_chrc(frame);
 611 }
 612
 613 static const struct bitfield_data ccc_value_table[] = {
 614         {  0, "Notification (0x01)"             },
 615         {  1, "Indication (0x02)"               },
 616         { }
 617 };
 618
 619 static void print_ccc_value(const struct l2cap_frame *frame)
 620 {
 621         uint8_t value;
 622         uint8_t mask;
 623
 624         if (!l2cap_frame_get_u8((void *)frame, &value)) {
 625                 print_text(COLOR_ERROR, "invalid size");
 626                 return;
 627         }
 628
 629         mask = print_bitfield(4, value, ccc_value_table);
 630         if (mask)
 631                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
 632                                                                 mask);
 633 }
 634
 635 static void ccc_read(const struct l2cap_frame *frame)
 636 {
 637         print_ccc_value(frame);
 638 }
 639
 640 static void ccc_write(const struct l2cap_frame *frame)
 641 {
 642         print_ccc_value(frame);
 643 }
 644
 645 static bool print_ase_codec(const struct l2cap_frame *frame)
 646 {
 647         uint8_t codec_id;
 648         uint16_t codec_cid, codec_vid;
 649
 650         if (!l2cap_frame_get_u8((void *)frame, &codec_id)) {
 651                 print_text(COLOR_ERROR, "Codec: invalid size");
 652                 return false;
 653         }
 654
 655         packet_print_codec_id("    Codec", codec_id);
 656
 657         if (!l2cap_frame_get_le16((void *)frame, &codec_cid)) {
 658                 print_text(COLOR_ERROR, "Codec Company ID: invalid size");
 659                 return false;
 660         }
 661
 662         if (!l2cap_frame_get_le16((void *)frame, &codec_vid)) {
 663                 print_text(COLOR_ERROR, "Codec Vendor ID: invalid size");
 664                 return false;
 665         }
 666
 667         if (codec_id == 0xff) {
 668                 print_field("    Codec Company ID: %s (0x%04x)",
 669                                                 bt_compidtostr(codec_cid),
 670                                                 codec_cid);
 671                 print_field("    Codec Vendor ID: 0x%04x", codec_vid);
 672         }
 673
 674         return true;
 675 }
 676
 677 static bool print_ase_lv(const struct l2cap_frame *frame, const char *label,
 678                         struct packet_ltv_decoder *decoder, size_t decoder_len)
 679 {
 680         struct bt_hci_lv_data *lv;
 681
 682         lv = l2cap_frame_pull((void *)frame, frame, sizeof(*lv));
 683         if (!lv) {
 684                 print_text(COLOR_ERROR, "%s: invalid size", label);
 685                 return false;
 686         }
 687
 688         if (!l2cap_frame_pull((void *)frame, frame, lv->len)) {
 689                 print_text(COLOR_ERROR, "%s: invalid size", label);
 690                 return false;
 691         }
 692
 693         packet_print_ltv(label, lv->data, lv->len, decoder, decoder_len);
 694
 695         return true;
 696 }
 697
 698 static bool print_ase_cc(const struct l2cap_frame *frame, const char *label,
 699                         struct packet_ltv_decoder *decoder, size_t decoder_len)
 700 {
 701         return print_ase_lv(frame, label, decoder, decoder_len);
 702 }
 703
 704 static const struct bitfield_data pac_context_table[] = {
 705         {  0, "Unspecified (0x0001)"                    },
 706         {  1, "Conversational (0x0002)"                 },
 707         {  2, "Media (0x0004)"                          },
 708         {  3, "Game (0x0008)"                           },
 709         {  4, "Instructional (0x0010)"                  },
 710         {  5, "Voice Assistants (0x0020)"               },
 711         {  6, "Live (0x0040)"                           },
 712         {  7, "Sound Effects (0x0080)"                  },
 713         {  8, "Notifications (0x0100)"                  },
 714         {  9, "Ringtone (0x0200)"                       },
 715         {  10, "Alerts (0x0400)"                        },
 716         {  11, "Emergency alarm (0x0800)"               },
 717         {  12, "RFU (0x1000)"                           },
 718         {  13, "RFU (0x2000)"                           },
 719         {  14, "RFU (0x4000)"                           },
 720         {  15, "RFU (0x8000)"                           },
 721         { }
 722 };
 723
 724 static void print_context(const struct l2cap_frame *frame, const char *label)
 725 {
 726         uint16_t value;
 727         uint16_t mask;
 728
 729         if (!l2cap_frame_get_le16((void *)frame, &value)) {
 730                 print_text(COLOR_ERROR, "    value: invalid size");
 731                 goto done;
 732         }
 733
 734         print_field("%s: 0x%4.4x", label, value);
 735
 736         mask = print_bitfield(8, value, pac_context_table);
 737         if (mask)
 738                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%4.4x)",
 739                                                                 mask);
 740
 741 done:
 742         if (frame->size)
 743                 print_hex_field("    Data", frame->data, frame->size);
 744 }
 745
 746 static void ase_decode_preferred_context(const uint8_t *data, uint8_t len)
 747 {
 748         struct l2cap_frame frame;
 749
 750         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 751
 752         print_context(&frame, "      Preferred Context");
 753 }
 754
 755 static void ase_decode_context(const uint8_t *data, uint8_t len)
 756 {
 757         struct l2cap_frame frame;
 758
 759         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 760
 761         print_context(&frame, "      Context");
 762 }
 763
 764 static void ase_decode_program_info(const uint8_t *data, uint8_t len)
 765 {
 766         struct l2cap_frame frame;
 767         const char *str;
 768
 769         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 770
 771         str = l2cap_frame_pull(&frame, &frame, len);
 772         if (!str) {
 773                 print_text(COLOR_ERROR, "    value: invalid size");
 774                 goto done;
 775         }
 776
 777         print_field("      Program Info: %s", str);
 778
 779 done:
 780         if (frame.size)
 781                 print_hex_field("    Data", frame.data, frame.size);
 782 }
 783
 784 static void ase_decode_language(const uint8_t *data, uint8_t len)
 785 {
 786         struct l2cap_frame frame;
 787         uint32_t value;
 788
 789         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 790
 791         if (!l2cap_frame_get_le24(&frame, &value)) {
 792                 print_text(COLOR_ERROR, "    value: invalid size");
 793                 goto done;
 794         }
 795
 796         print_field("      Language: 0x%6.6x", value);
 797
 798 done:
 799         if (frame.size)
 800                 print_hex_field("    Data", frame.data, frame.size);
 801 }
 802
 803 struct packet_ltv_decoder ase_metadata_table[] = {
 804         LTV_DEC(0x01, ase_decode_preferred_context),
 805         LTV_DEC(0x02, ase_decode_context),
 806         LTV_DEC(0x03, ase_decode_program_info),
 807         LTV_DEC(0x04, ase_decode_language)
 808 };
 809
 810 static bool print_ase_metadata(const struct l2cap_frame *frame)
 811 {
 812         return print_ase_lv(frame, "    Metadata", NULL, 0);
 813 }
 814
 815 static const struct bitfield_data pac_freq_table[] = {
 816         {  0, "8 Khz (0x0001)"                          },
 817         {  1, "11.25 Khz (0x0002)"                      },
 818         {  2, "16 Khz (0x0004)"                         },
 819         {  3, "22.05 Khz (0x0008)"                      },
 820         {  4, "24 Khz (0x0010)"                         },
 821         {  5, "32 Khz (0x0020)"                         },
 822         {  6, "44.1 Khz (0x0040)"                       },
 823         {  7, "48 Khz (0x0080)"                         },
 824         {  8, "88.2 Khz (0x0100)"                       },
 825         {  9, "96 Khz (0x0200)"                         },
 826         {  10, "176.4 Khz (0x0400)"                     },
 827         {  11, "192 Khz (0x0800)"                       },
 828         {  12, "384 Khz (0x1000)"                       },
 829         {  13, "RFU (0x2000)"                           },
 830         {  14, "RFU (0x4000)"                           },
 831         {  15, "RFU (0x8000)"                           },
 832         { }
 833 };
 834
 835 static void pac_decode_freq(const uint8_t *data, uint8_t len)
 836 {
 837         struct l2cap_frame frame;
 838         uint16_t value;
 839         uint16_t mask;
 840
 841         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 842
 843         if (!l2cap_frame_get_le16(&frame, &value)) {
 844                 print_text(COLOR_ERROR, "    value: invalid size");
 845                 goto done;
 846         }
 847
 848         print_field("      Sampling Frequencies: 0x%4.4x", value);
 849
 850         mask = print_bitfield(8, value, pac_freq_table);
 851         if (mask)
 852                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%4.4x)",
 853                                                                 mask);
 854
 855 done:
 856         if (frame.size)
 857                 print_hex_field("    Data", frame.data, frame.size);
 858 }
 859
 860 static const struct bitfield_data pac_duration_table[] = {
 861         {  0, "7.5 ms (0x01)"                           },
 862         {  1, "10 ms (0x02)"                            },
 863         {  2, "RFU (0x04)"                              },
 864         {  3, "RFU (0x08)"                              },
 865         {  4, "7.5 ms preferred (0x10)"                 },
 866         {  5, "10 ms preferred (0x20)"                  },
 867         {  6, "RFU (0x40)"                              },
 868         {  7, "RFU (0x80)"                              },
 869         { }
 870 };
 871
 872 static void pac_decode_duration(const uint8_t *data, uint8_t len)
 873 {
 874         struct l2cap_frame frame;
 875         uint8_t value;
 876         uint8_t mask;
 877
 878         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 879
 880         if (!l2cap_frame_get_u8(&frame, &value)) {
 881                 print_text(COLOR_ERROR, "    value: invalid size");
 882                 goto done;
 883         }
 884
 885         print_field("      Frame Duration: 0x%4.4x", value);
 886
 887         mask = print_bitfield(8, value, pac_duration_table);
 888         if (mask)
 889                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
 890                                                                 mask);
 891
 892 done:
 893         if (frame.size)
 894                 print_hex_field("    Data", frame.data, frame.size);
 895 }
 896
 897 static const struct bitfield_data pac_channel_table[] = {
 898         {  0, "1 channel (0x01)"                        },
 899         {  1, "2 channels (0x02)"                       },
 900         {  2, "3 channels (0x04)"                       },
 901         {  3, "4 chanenls (0x08)"                       },
 902         {  4, "5 channels (0x10)"                       },
 903         {  5, "6 channels (0x20)"                       },
 904         {  6, "7 channels (0x40)"                       },
 905         {  7, "8 channels (0x80)"                       },
 906         { }
 907 };
 908
 909 static void pac_decode_channels(const uint8_t *data, uint8_t len)
 910 {
 911         struct l2cap_frame frame;
 912         uint8_t value;
 913         uint8_t mask;
 914
 915         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 916
 917         if (!l2cap_frame_get_u8(&frame, &value)) {
 918                 print_text(COLOR_ERROR, "    value: invalid size");
 919                 goto done;
 920         }
 921
 922         print_field("      Audio Channel Count: 0x%2.2x", value);
 923
 924         mask = print_bitfield(8, value, pac_channel_table);
 925         if (mask)
 926                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
 927                                                                 mask);
 928
 929 done:
 930         if (frame.size)
 931                 print_hex_field("    Data", frame.data, frame.size);
 932 }
 933
 934 static void pac_decode_frame_length(const uint8_t *data, uint8_t len)
 935 {
 936         struct l2cap_frame frame;
 937         uint16_t min, max;
 938
 939         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 940
 941         if (!l2cap_frame_get_le16(&frame, &min)) {
 942                 print_text(COLOR_ERROR, "    min: invalid size");
 943                 goto done;
 944         }
 945
 946         if (!l2cap_frame_get_le16(&frame, &max)) {
 947                 print_text(COLOR_ERROR, "    min: invalid size");
 948                 goto done;
 949         }
 950
 951         print_field("      Frame Length: %u (0x%4.4x) - %u (0x%4.4x)",
 952                                                         min, min, max, max);
 953
 954 done:
 955         if (frame.size)
 956                 print_hex_field("    Data", frame.data, frame.size);
 957 }
 958
 959 static void pac_decode_sdu(const uint8_t *data, uint8_t len)
 960 {
 961         struct l2cap_frame frame;
 962         uint8_t value;
 963
 964         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
 965
 966         if (!l2cap_frame_get_u8(&frame, &value)) {
 967                 print_text(COLOR_ERROR, "    value: invalid size");
 968                 goto done;
 969         }
 970
 971         print_field("      Max SDU: %u (0x%2.2x)", value, value);
 972
 973 done:
 974         if (frame.size)
 975                 print_hex_field("    Data", frame.data, frame.size);
 976 }
 977
 978 struct packet_ltv_decoder pac_cap_table[] = {
 979         LTV_DEC(0x01, pac_decode_freq),
 980         LTV_DEC(0x02, pac_decode_duration),
 981         LTV_DEC(0x03, pac_decode_channels),
 982         LTV_DEC(0x04, pac_decode_frame_length),
 983         LTV_DEC(0x05, pac_decode_sdu)
 984 };
 985
 986 static void print_pac(const struct l2cap_frame *frame)
 987 {
 988         uint8_t num = 0, i;
 989
 990         if (!l2cap_frame_get_u8((void *)frame, &num)) {
 991                 print_text(COLOR_ERROR, "Number of PAC(s): invalid size");
 992                 goto done;
 993         }
 994
 995         print_field("  Number of PAC(s): %u", num);
 996
 997         for (i = 0; i < num; i++) {
 998                 print_field("  PAC #%u:", i);
 999
1000                 if (!print_ase_codec(frame))
1001                         goto done;
1002
1003                 if (!print_ase_cc(frame, "    Codec Specific Capabilities",
1004                                 pac_cap_table, ARRAY_SIZE(pac_cap_table)))
1005                         break;
1006
1007                 if (!print_ase_metadata(frame))
1008                         break;
1009         }
1010
1011 done:
1012         if (frame->size)
1013                 print_hex_field("  Data", frame->data, frame->size);
1014 }
1015
1016 static void pac_read(const struct l2cap_frame *frame)
1017 {
1018         print_pac(frame);
1019 }
1020
1021 static void pac_notify(const struct l2cap_frame *frame)
1022 {
1023         print_pac(frame);
1024 }
1025
1026 static bool print_prefer_framing(const struct l2cap_frame *frame)
1027 {
1028         uint8_t framing;
1029
1030         if (!l2cap_frame_get_u8((void *)frame, &framing)) {
1031                 print_text(COLOR_ERROR, "    Framing: invalid size");
1032                 return false;
1033         }
1034
1035         switch (framing) {
1036         case 0x00:
1037                 print_field("    Framing: Unframed PDUs supported (0x00)");
1038                 break;
1039         case 0x01:
1040                 print_field("    Framing: Unframed PDUs not supported (0x01)");
1041                 break;
1042         default:
1043                 print_field("    Framing: Reserved (0x%2.2x)", framing);
1044                 break;
1045         }
1046
1047         return true;
1048 }
1049
1050 static const struct bitfield_data prefer_phy_table[] = {
1051         {  0, "LE 1M PHY preffered (0x01)"              },
1052         {  1, "LE 2M PHY preffered (0x02)"              },
1053         {  2, "LE Codec PHY preffered (0x04)"           },
1054         { }
1055 };
1056
1057 static bool print_prefer_phy(const struct l2cap_frame *frame)
1058 {
1059         uint8_t phy, mask;
1060
1061         if (!l2cap_frame_get_u8((void *)frame, &phy)) {
1062                 print_text(COLOR_ERROR, "PHY: invalid size");
1063                 return false;
1064         }
1065
1066         print_field("    PHY: 0x%2.2x", phy);
1067
1068         mask = print_bitfield(4, phy, prefer_phy_table);
1069         if (mask)
1070                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
1071                                                                 mask);
1072
1073         return true;
1074 }
1075
1076 static bool print_ase_rtn(const struct l2cap_frame *frame, const char *label)
1077 {
1078         uint8_t rtn;
1079
1080         if (!l2cap_frame_get_u8((void *)frame, &rtn)) {
1081                 print_text(COLOR_ERROR, "%s: invalid size", label);
1082                 return false;
1083         }
1084
1085         print_field("%s: %u", label, rtn);
1086
1087         return true;
1088 }
1089
1090 static bool print_ase_latency(const struct l2cap_frame *frame,
1091                                                 const char *label)
1092 {
1093         uint16_t latency;
1094
1095         if (!l2cap_frame_get_le16((void *)frame, &latency)) {
1096                 print_text(COLOR_ERROR, "%s: invalid size", label);
1097                 return false;
1098         }
1099
1100         print_field("%s: %u", label, latency);
1101
1102         return true;
1103 }
1104
1105 static bool print_ase_pd(const struct l2cap_frame *frame, const char *label)
1106 {
1107         uint32_t pd;
1108
1109         if (!l2cap_frame_get_le24((void *)frame, &pd)) {
1110                 print_text(COLOR_ERROR, "%s: invalid size", label);
1111                 return false;
1112         }
1113
1114         print_field("%s: %u us", label, pd);
1115
1116         return true;
1117 }
1118
1119 static void ase_decode_freq(const uint8_t *data, uint8_t len)
1120 {
1121         struct l2cap_frame frame;
1122         uint8_t value;
1123
1124         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
1125
1126         if (!l2cap_frame_get_u8(&frame, &value)) {
1127                 print_text(COLOR_ERROR, "    value: invalid size");
1128                 goto done;
1129         }
1130
1131         switch (value) {
1132         case 0x01:
1133                 print_field("      Sampling Frequency: 8 Khz (0x01)");
1134                 break;
1135         case 0x02:
1136                 print_field("      Sampling Frequency: 11.25 Khz (0x02)");
1137                 break;
1138         case 0x03:
1139                 print_field("      Sampling Frequency: 16 Khz (0x03)");
1140                 break;
1141         case 0x04:
1142                 print_field("      Sampling Frequency: 22.05 Khz (0x04)");
1143                 break;
1144         case 0x05:
1145                 print_field("      Sampling Frequency: 24 Khz (0x05)");
1146                 break;
1147         case 0x06:
1148                 print_field("      Sampling Frequency: 32 Khz (0x06)");
1149                 break;
1150         case 0x07:
1151                 print_field("      Sampling Frequency: 44.1 Khz (0x07)");
1152                 break;
1153         case 0x08:
1154                 print_field("      Sampling Frequency: 48 Khz (0x08)");
1155                 break;
1156         case 0x09:
1157                 print_field("      Sampling Frequency: 88.2 Khz (0x09)");
1158                 break;
1159         case 0x0a:
1160                 print_field("      Sampling Frequency: 96 Khz (0x0a)");
1161                 break;
1162         case 0x0b:
1163                 print_field("      Sampling Frequency: 176.4 Khz (0x0b)");
1164                 break;
1165         case 0x0c:
1166                 print_field("      Sampling Frequency: 192 Khz (0x0c)");
1167                 break;
1168         case 0x0d:
1169                 print_field("      Sampling Frequency: 384 Khz (0x0d)");
1170                 break;
1171         default:
1172                 print_field("      Sampling Frequency: RFU (0x%2.2x)", value);
1173                 break;
1174         }
1175
1176 done:
1177         if (frame.size)
1178                 print_hex_field("    Data", frame.data, frame.size);
1179 }
1180
1181 static void ase_decode_duration(const uint8_t *data, uint8_t len)
1182 {
1183         struct l2cap_frame frame;
1184         uint8_t value;
1185
1186         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
1187
1188         if (!l2cap_frame_get_u8(&frame, &value)) {
1189                 print_text(COLOR_ERROR, "    value: invalid size");
1190                 goto done;
1191         }
1192
1193         switch (value) {
1194         case 0x00:
1195                 print_field("      Frame Duration: 7.5 ms (0x00)");
1196                 break;
1197         case 0x01:
1198                 print_field("      Frame Duration: 10 ms (0x01)");
1199                 break;
1200         default:
1201                 print_field("      Frame Duration: RFU (0x%2.2x)", value);
1202                 break;
1203         }
1204
1205 done:
1206         if (frame.size)
1207                 print_hex_field("    Data", frame.data, frame.size);
1208 }
1209
1210 static const struct bitfield_data channel_location_table[] = {
1211         {  0, "Front Left (0x00000001)"                 },
1212         {  1, "Front Right (0x00000002)"                },
1213         {  2, "Front Center (0x00000004)"               },
1214         {  3, "Low Frequency Effects 1 (0x00000008)"    },
1215         {  4, "Back Left (0x00000010)"                  },
1216         {  5, "Back Right (0x00000020)"                 },
1217         {  6, "Front Left of Center (0x00000040)"       },
1218         {  7, "Front Right of Center (0x00000080)"      },
1219         {  8, "Back Center (0x00000100)"                },
1220         {  9, "Low Frequency Effects 2 (0x00000200)"    },
1221         {  10, "Side Left (0x00000400)"                 },
1222         {  11, "Side Right (0x00000800)"                },
1223         {  12, "Top Front Left (0x00001000)"            },
1224         {  13, "Top Front Right (0x00002000)"           },
1225         {  14, "Top Front Center (0x00004000)"          },
1226         {  15, "Top Center (0x00008000)"                },
1227         {  16, "Top Back Left (0x00010000)"             },
1228         {  17, "Top Back Right (0x00020000)"            },
1229         {  18, "Top Side Left (0x00040000)"             },
1230         {  19, "Top Side Right (0x00080000)"            },
1231         {  20, "Top Back Center (0x00100000)"           },
1232         {  21, "Bottom Front Center (0x00200000)"       },
1233         {  22, "Bottom Front Left (0x00400000)"         },
1234         {  23, "Bottom Front Right (0x00800000)"        },
1235         {  24, "Front Left Wide (0x01000000)"           },
1236         {  25, "Front Right Wide (0x02000000)"          },
1237         {  26, "Left Surround (0x04000000)"             },
1238         {  27, "Right Surround (0x08000000)"            },
1239         {  28, "RFU (0x10000000)"                       },
1240         {  29, "RFU (0x20000000)"                       },
1241         {  30, "RFU (0x40000000)"                       },
1242         {  31, "RFU (0x80000000)"                       },
1243         { }
1244 };
1245
1246 static void print_location(const struct l2cap_frame *frame)
1247 {
1248         uint32_t value;
1249         uint32_t mask;
1250
1251         if (!l2cap_frame_get_le32((void *)frame, &value)) {
1252                 print_text(COLOR_ERROR, "    value: invalid size");
1253                 goto done;
1254         }
1255
1256         print_field("   Location: 0x%8.8x", value);
1257
1258         mask = print_bitfield(6, value, channel_location_table);
1259         if (mask)
1260                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%8.8x)",
1261                                                                 mask);
1262
1263 done:
1264         if (frame->size)
1265                 print_hex_field("  Data", frame->data, frame->size);
1266 }
1267
1268 static void ase_decode_location(const uint8_t *data, uint8_t len)
1269 {
1270         struct l2cap_frame frame;
1271
1272         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
1273
1274         print_location(&frame);
1275 }
1276
1277 static void ase_decode_frame_length(const uint8_t *data, uint8_t len)
1278 {
1279         struct l2cap_frame frame;
1280         uint16_t value;
1281
1282         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
1283
1284         if (!l2cap_frame_get_le16(&frame, &value)) {
1285                 print_text(COLOR_ERROR, "    value: invalid size");
1286                 goto done;
1287         }
1288
1289         print_field("      Frame Length: %u (0x%4.4x)", value, value);
1290
1291 done:
1292         if (frame.size)
1293                 print_hex_field("    Data", frame.data, frame.size);
1294 }
1295
1296 static void ase_decode_blocks(const uint8_t *data, uint8_t len)
1297 {
1298         struct l2cap_frame frame;
1299         uint8_t value;
1300
1301         l2cap_frame_init(&frame, 0, 0, 0, 0, 0, 0, data, len);
1302
1303         if (!l2cap_frame_get_u8(&frame, &value)) {
1304                 print_text(COLOR_ERROR, "    value: invalid size");
1305                 goto done;
1306         }
1307
1308         print_field("      Frame Blocks per SDU: %u (0x%2.2x)", value, value);
1309
1310 done:
1311         if (frame.size)
1312                 print_hex_field("    Data", frame.data, frame.size);
1313 }
1314
1315 struct packet_ltv_decoder ase_cc_table[] = {
1316         LTV_DEC(0x01, ase_decode_freq),
1317         LTV_DEC(0x02, ase_decode_duration),
1318         LTV_DEC(0x03, ase_decode_location),
1319         LTV_DEC(0x04, ase_decode_frame_length),
1320         LTV_DEC(0x05, ase_decode_blocks)
1321 };
1322
1323 static void print_ase_config(const struct l2cap_frame *frame)
1324 {
1325         if (!print_prefer_framing(frame))
1326                 return;
1327
1328         if (!print_prefer_phy(frame))
1329                 return;
1330
1331         if (!print_ase_rtn(frame, "    RTN"))
1332                 return;
1333
1334         if (!print_ase_latency(frame, "    Max Transport Latency"))
1335                 return;
1336
1337         if (!print_ase_pd(frame, "    Presentation Delay Min"))
1338                 return;
1339
1340         if (!print_ase_pd(frame, "    Presentation Delay Max"))
1341                 return;
1342
1343         if (!print_ase_pd(frame, "    Preferred Presentation Delay Min"))
1344                 return;
1345
1346         if (!print_ase_pd(frame, "    Preferred Presentation Delay Max"))
1347                 return;
1348
1349         if (!print_ase_codec(frame))
1350                 return;
1351
1352         print_ase_cc(frame, "    Codec Specific Configuration",
1353                         ase_cc_table, ARRAY_SIZE(ase_cc_table));
1354 }
1355
1356 static bool print_ase_framing(const struct l2cap_frame *frame,
1357                                                 const char *label)
1358 {
1359         uint8_t framing;
1360
1361         if (!l2cap_frame_get_u8((void *)frame, &framing)) {
1362                 print_text(COLOR_ERROR, "%s: invalid size", label);
1363                 return false;
1364         }
1365
1366         switch (framing) {
1367         case 0x00:
1368                 print_field("%s: Unframed (0x00)", label);
1369                 break;
1370         case 0x01:
1371                 print_field("%s: Framed (0x01)", label);
1372                 break;
1373         default:
1374                 print_field("%s: Reserved (0x%2.2x)", label, framing);
1375         }
1376
1377         return true;
1378 }
1379
1380 static const struct bitfield_data phy_table[] = {
1381         {  0, "LE 1M PHY (0x01)"                },
1382         {  1, "LE 2M PHY (0x02)"                },
1383         {  2, "LE Codec PHY (0x04)"             },
1384         { }
1385 };
1386
1387 static bool print_ase_phy(const struct l2cap_frame *frame, const char *label)
1388 {
1389         uint8_t phy, mask;
1390
1391         if (!l2cap_frame_get_u8((void *)frame, &phy)) {
1392                 print_text(COLOR_ERROR, "%s: invalid size", label);
1393                 return false;
1394         }
1395
1396         print_field("%s: 0x%2.2x", label, phy);
1397
1398         mask = print_bitfield(4, phy, phy_table);
1399         if (mask)
1400                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%2.2x)",
1401                                                                 mask);
1402
1403         return true;
1404 }
1405
1406 static bool print_ase_interval(const struct l2cap_frame *frame,
1407                                                 const char *label)
1408 {
1409         uint32_t interval;
1410
1411         if (!l2cap_frame_get_le24((void *)frame, &interval)) {
1412                 print_text(COLOR_ERROR, "%s: invalid size", label);
1413                 return false;
1414         }
1415
1416         print_field("%s: %u usec", label, interval);
1417
1418         return true;
1419 }
1420
1421 static bool print_ase_sdu(const struct l2cap_frame *frame, const char *label)
1422 {
1423         uint16_t sdu;
1424
1425         if (!l2cap_frame_get_le16((void *)frame, &sdu)) {
1426                 print_text(COLOR_ERROR, "%s: invalid size", label);
1427                 return false;
1428         }
1429
1430         print_field("%s: %u", label, sdu);
1431
1432         return true;
1433 }
1434
1435 static void print_ase_qos(const struct l2cap_frame *frame)
1436 {
1437         if (!l2cap_frame_print_u8((void *)frame, "    CIG ID"))
1438                 return;
1439
1440         if (!l2cap_frame_print_u8((void *)frame, "    CIS ID"))
1441                 return;
1442
1443         if (!print_ase_interval(frame, "    SDU Interval"))
1444                 return;
1445
1446         if (!print_ase_framing(frame, "    Framing"))
1447                 return;
1448
1449         if (!print_ase_phy(frame, "    PHY"))
1450                 return;
1451
1452         if (!print_ase_sdu(frame, "    Max SDU"))
1453                 return;
1454
1455         if (!print_ase_rtn(frame, "    RTN"))
1456                 return;
1457
1458         if (!print_ase_latency(frame, "    Max Transport Latency"))
1459                 return;
1460
1461         print_ase_pd(frame, "    Presentation Delay");
1462 }
1463
1464 static void print_ase_metadata_status(const struct l2cap_frame *frame)
1465 {
1466         if (!l2cap_frame_print_u8((void *)frame, "    CIG ID"))
1467                 return;
1468
1469         if (!l2cap_frame_print_u8((void *)frame, "    CIS ID"))
1470                 return;
1471
1472         print_ase_metadata(frame);
1473 }
1474
1475 static void print_ase_status(const struct l2cap_frame *frame)
1476 {
1477         uint8_t id, state;
1478
1479         if (!l2cap_frame_get_u8((void *)frame, &id)) {
1480                 print_text(COLOR_ERROR, "ASE ID: invalid size");
1481                 goto done;
1482         }
1483
1484         print_field("    ASE ID: %u", id);
1485
1486         if (!l2cap_frame_get_u8((void *)frame, &state)) {
1487                 print_text(COLOR_ERROR, "ASE State: invalid size");
1488                 goto done;
1489         }
1490
1491         switch (state) {
1492         /* ASE_State = 0x00 (Idle) */
1493         case 0x00:
1494                 print_field("    State: Idle (0x00)");
1495                 break;
1496         /* ASE_State = 0x01 (Codec Configured) */
1497         case 0x01:
1498                 print_field("    State: Codec Configured (0x01)");
1499                 print_ase_config(frame);
1500                 break;
1501         /* ASE_State = 0x02 (QoS Configured) */
1502         case 0x02:
1503                 print_field("    State: QoS Configured (0x02)");
1504                 print_ase_qos(frame);
1505                 break;
1506         /* ASE_Status = 0x03 (Enabling) */
1507         case 0x03:
1508                 print_field("    State: Enabling (0x03)");
1509                 print_ase_metadata_status(frame);
1510                 break;
1511         /* ASE_Status = 0x04 (Streaming) */
1512         case 0x04:
1513                 print_field("    State: Streaming (0x04)");
1514                 print_ase_metadata_status(frame);
1515                 break;
1516         /* ASE_Status = 0x05 (Disabling) */
1517         case 0x05:
1518                 print_field("    State: Disabling (0x05)");
1519                 print_ase_metadata_status(frame);
1520                 break;
1521         /* ASE_Status = 0x06 (Releasing) */
1522         case 0x06:
1523                 print_field("    State: Releasing (0x06)");
1524                 break;
1525         default:
1526                 print_field("    State: Reserved (0x%2.2x)", state);
1527                 break;
1528         }
1529
1530 done:
1531         if (frame->size)
1532                 print_hex_field("  Data", frame->data, frame->size);
1533 }
1534
1535 static void ase_read(const struct l2cap_frame *frame)
1536 {
1537         print_ase_status(frame);
1538 }
1539
1540 static void ase_notify(const struct l2cap_frame *frame)
1541 {
1542         print_ase_status(frame);
1543 }
1544
1545 static bool print_ase_target_latency(const struct l2cap_frame *frame)
1546 {
1547         uint8_t latency;
1548
1549         if (!l2cap_frame_get_u8((void *)frame, &latency)) {
1550                 print_text(COLOR_ERROR, "    Target Latency: invalid size");
1551                 return false;
1552         }
1553
1554         switch (latency) {
1555         case 0x01:
1556                 print_field("    Target Latency: Low Latency (0x01)");
1557                 break;
1558         case 0x02:
1559                 print_field("    Target Latency: Balance Latency/Reliability "
1560                                                                 "(0x02)");
1561                 break;
1562         case 0x03:
1563                 print_field("    Target Latency: High Reliability (0x03)");
1564                 break;
1565         default:
1566                 print_field("    Target Latency: Reserved (0x%2.2x)", latency);
1567                 break;
1568         }
1569
1570         return true;
1571 }
1572
1573 static bool ase_config_cmd(const struct l2cap_frame *frame)
1574 {
1575         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1576                 return false;
1577
1578         if (!print_ase_target_latency(frame))
1579                 return false;
1580
1581         if (!print_ase_phy(frame, "    PHY"))
1582                 return false;
1583
1584         if (!print_ase_codec(frame))
1585                 return false;
1586
1587         if (!print_ase_cc(frame, "    Codec Specific Configuration",
1588                                 ase_cc_table, ARRAY_SIZE(ase_cc_table)))
1589                 return false;
1590
1591         return true;
1592 }
1593
1594 static bool ase_qos_cmd(const struct l2cap_frame *frame)
1595 {
1596         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1597                 return false;
1598
1599         if (!l2cap_frame_print_u8((void *)frame, "    CIG ID"))
1600                 return false;
1601
1602         if (!l2cap_frame_print_u8((void *)frame, "    CIS ID"))
1603                 return false;
1604
1605         if (!print_ase_interval(frame, "    SDU Interval"))
1606                 return false;
1607
1608         if (!print_ase_framing(frame, "    Framing"))
1609                 return false;
1610
1611         if (!print_ase_phy(frame, "    PHY"))
1612                 return false;
1613
1614         if (!print_ase_sdu(frame, "    Max SDU"))
1615                 return false;
1616
1617         if (!print_ase_rtn(frame, "    RTN"))
1618                 return false;
1619
1620         if (!print_ase_latency(frame, "    Max Transport Latency"))
1621                 return false;
1622
1623         if (!print_ase_pd(frame, "    Presentation Delay"))
1624                 return false;
1625
1626         return true;
1627 }
1628
1629 static bool ase_enable_cmd(const struct l2cap_frame *frame)
1630 {
1631         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1632                 return false;
1633
1634         if (!print_ase_metadata(frame))
1635                 return false;
1636
1637         return true;
1638 }
1639
1640 static bool ase_start_cmd(const struct l2cap_frame *frame)
1641 {
1642         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1643                 return false;
1644
1645         return true;
1646 }
1647
1648 static bool ase_disable_cmd(const struct l2cap_frame *frame)
1649 {
1650         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1651                 return false;
1652
1653         return true;
1654 }
1655
1656 static bool ase_stop_cmd(const struct l2cap_frame *frame)
1657 {
1658         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1659                 return false;
1660
1661         return true;
1662 }
1663
1664 static bool ase_metadata_cmd(const struct l2cap_frame *frame)
1665 {
1666         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1667                 return false;
1668
1669         if (!print_ase_metadata(frame))
1670                 return false;
1671
1672         return true;
1673 }
1674
1675 static bool ase_release_cmd(const struct l2cap_frame *frame)
1676 {
1677         if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1678                 return false;
1679
1680         return true;
1681 }
1682
1683 #define ASE_CMD(_op, _desc, _func) \
1684 [_op] = { \
1685         .desc = _desc, \
1686         .func = _func, \
1687 }
1688
1689 struct ase_cmd {
1690         const char *desc;
1691         bool (*func)(const struct l2cap_frame *frame);
1692 } ase_cmd_table[] = {
1693         /* Opcode = 0x01 (Codec Configuration) */
1694         ASE_CMD(0x01, "Codec Configuration", ase_config_cmd),
1695         /* Opcode = 0x02 (QoS Configuration) */
1696         ASE_CMD(0x02, "QoS Configuration", ase_qos_cmd),
1697         /* Opcode = 0x03 (Enable) */
1698         ASE_CMD(0x03, "Enable", ase_enable_cmd),
1699         /* Opcode = 0x04 (Receiver Start Ready) */
1700         ASE_CMD(0x04, "Receiver Start Ready", ase_start_cmd),
1701         /* Opcode = 0x05 (Disable) */
1702         ASE_CMD(0x05, "Disable", ase_disable_cmd),
1703         /* Opcode = 0x06 (Receiver Stop Ready) */
1704         ASE_CMD(0x06, "Receiver Stop Ready", ase_stop_cmd),
1705         /* Opcode = 0x07 (Update Metadata) */
1706         ASE_CMD(0x07, "Update Metadata", ase_metadata_cmd),
1707         /* Opcode = 0x08 (Release) */
1708         ASE_CMD(0x08, "Release", ase_release_cmd),
1709 };
1710
1711 static struct ase_cmd *ase_get_cmd(uint8_t op)
1712 {
1713         if (op > ARRAY_SIZE(ase_cmd_table))
1714                 return NULL;
1715
1716         return &ase_cmd_table[op];
1717 }
1718
1719 static void print_ase_cmd(const struct l2cap_frame *frame)
1720 {
1721         uint8_t op, num, i;
1722         struct ase_cmd *cmd;
1723
1724         if (!l2cap_frame_get_u8((void *)frame, &op)) {
1725                 print_text(COLOR_ERROR, "opcode: invalid size");
1726                 goto done;
1727         }
1728
1729         if (!l2cap_frame_get_u8((void *)frame, &num)) {
1730                 print_text(COLOR_ERROR, "num: invalid size");
1731                 goto done;
1732         }
1733
1734         cmd = ase_get_cmd(op);
1735         if (!cmd) {
1736                 print_field("    Opcode: Reserved (0x%2.2x)", op);
1737                 goto done;
1738         }
1739
1740         print_field("    Opcode: %s (0x%2.2x)", cmd->desc, op);
1741         print_field("    Number of ASE(s): %u", num);
1742
1743         for (i = 0; i < num && frame->size; i++) {
1744                 print_field("    ASE: #%u", i);
1745
1746                 if (!cmd->func(frame))
1747                         break;
1748         }
1749
1750 done:
1751         if (frame->size)
1752                 print_hex_field("  Data", frame->data, frame->size);
1753 }
1754
1755 static void ase_cp_write(const struct l2cap_frame *frame)
1756 {
1757         print_ase_cmd(frame);
1758 }
1759
1760 static bool print_ase_cp_rsp_code(const struct l2cap_frame *frame)
1761 {
1762         uint8_t code;
1763
1764         if (!l2cap_frame_get_u8((void *)frame, &code)) {
1765                 print_text(COLOR_ERROR, "    ASE Response Code: invalid size");
1766                 return false;
1767         }
1768
1769         switch (code) {
1770         case 0x00:
1771                 print_field("    ASE Response Code: Success (0x00)");
1772                 break;
1773         case 0x01:
1774                 print_field("    ASE Response Code: Unsupported Opcode (0x01)");
1775                 break;
1776         case 0x02:
1777                 print_field("    ASE Response Code: Invalid Length (0x02)");
1778                 break;
1779         case 0x03:
1780                 print_field("    ASE Response Code: Invalid ASE ID (0x03)");
1781                 break;
1782         case 0x04:
1783                 print_field("    ASE Response Code: Invalid ASE State (0x04)");
1784                 break;
1785         case 0x05:
1786                 print_field("    ASE Response Code: Invalid ASE Direction "
1787                                                                 "(0x05)");
1788                 break;
1789         case 0x06:
1790                 print_field("    ASE Response Code: Unsupported Audio "
1791                                                         "Capabilities (0x06)");
1792                 break;
1793         case 0x07:
1794                 print_field("    ASE Response Code: Unsupported Configuration "
1795                                                                 "(0x07)");
1796                 break;
1797         case 0x08:
1798                 print_field("    ASE Response Code: Rejected Configuration "
1799                                                                 "(0x08)");
1800                 break;
1801         case 0x09:
1802                 print_field("    ASE Response Code: Invalid Configuration "
1803                                                                 "(0x09)");
1804                 break;
1805         case 0x0a:
1806                 print_field("    ASE Response Code: Unsupported Metadata "
1807                                                                 "(0x0a)");
1808                 break;
1809         case 0x0b:
1810                 print_field("    ASE Response Code: Rejected Metadata (0x0b)");
1811                 break;
1812         case 0x0c:
1813                 print_field("    ASE Response Code: Invalid Metadata (0x0c)");
1814                 break;
1815         case 0x0d:
1816                 print_field("    ASE Response Code: Insufficient Resources "
1817                                                                 "(0x0d)");
1818                 break;
1819         case 0x0e:
1820                 print_field("    ASE Response Code: Unspecified Error (0x0e)");
1821                 break;
1822         default:
1823                 print_field("    ASE Response Code: Reserved (0x%2.2x)", code);
1824                 break;
1825         }
1826
1827         return true;
1828 }
1829
1830 static bool print_ase_cp_rsp_reason(const struct l2cap_frame *frame)
1831 {
1832         uint8_t reason;
1833
1834         if (!l2cap_frame_get_u8((void *)frame, &reason)) {
1835                 print_text(COLOR_ERROR,
1836                                 "    ASE Response Reason: invalid size");
1837                 return false;
1838         }
1839
1840         switch (reason) {
1841         case 0x00:
1842                 print_field("    ASE Response Reason: None (0x00)");
1843                 break;
1844         case 0x01:
1845                 print_field("    ASE Response Reason: ASE ID (0x01)");
1846                 break;
1847         case 0x02:
1848                 print_field("    ASE Response Reason: Codec Specific "
1849                                                 "Configuration (0x02)");
1850                 break;
1851         case 0x03:
1852                 print_field("    ASE Response Reason: SDU Interval (0x03)");
1853                 break;
1854         case 0x04:
1855                 print_field("    ASE Response Reason: Framing (0x04)");
1856                 break;
1857         case 0x05:
1858                 print_field("    ASE Response Reason: PHY (0x05)");
1859                 break;
1860         case 0x06:
1861                 print_field("    ASE Response Reason: Max SDU (0x06)");
1862                 break;
1863         case 0x07:
1864                 print_field("    ASE Response Reason: RTN (0x07)");
1865                 break;
1866         case 0x08:
1867                 print_field("    ASE Response Reason: Max Transport Latency "
1868                                                                 "(0x08)");
1869                 break;
1870         case 0x09:
1871                 print_field("    ASE Response Reason: Presentation Delay "
1872                                                                 "(0x09)");
1873                 break;
1874         case 0x0a:
1875                 print_field("    ASE Response Reason: Invalid ASE/CIS Mapping "
1876                                                                 "(0x0a)");
1877                 break;
1878         default:
1879                 print_field("    ASE Response Reason: Reserved (0x%2.2x)",
1880                                                                 reason);
1881                 break;
1882         }
1883
1884         return true;
1885 }
1886
1887 static void print_ase_cp_rsp(const struct l2cap_frame *frame)
1888 {
1889         uint8_t op, num, i;
1890         struct ase_cmd *cmd;
1891
1892         if (!l2cap_frame_get_u8((void *)frame, &op)) {
1893                 print_text(COLOR_ERROR, "    opcode: invalid size");
1894                 goto done;
1895         }
1896
1897         if (!l2cap_frame_get_u8((void *)frame, &num)) {
1898                 print_text(COLOR_ERROR, "    Number of ASE(s): invalid size");
1899                 goto done;
1900         }
1901
1902         cmd = ase_get_cmd(op);
1903         if (!cmd) {
1904                 print_field("    Opcode: Reserved (0x%2.2x)", op);
1905                 goto done;
1906         }
1907
1908         print_field("    Opcode: %s (0x%2.2x)", cmd->desc, op);
1909         print_field("    Number of ASE(s): %u", num);
1910
1911         for (i = 0; i < num && frame->size; i++) {
1912                 print_field("    ASE: #%u", i);
1913
1914                 if (!l2cap_frame_print_u8((void *)frame, "    ASE ID"))
1915                         break;
1916
1917                 if (!print_ase_cp_rsp_code(frame))
1918                         break;
1919
1920                 if (!print_ase_cp_rsp_reason(frame))
1921                         break;
1922         }
1923
1924 done:
1925         if (frame->size)
1926                 print_hex_field("  Data", frame->data, frame->size);
1927 }
1928
1929 static void ase_cp_notify(const struct l2cap_frame *frame)
1930 {
1931         print_ase_cp_rsp(frame);
1932 }
1933
1934 static void pac_loc_read(const struct l2cap_frame *frame)
1935 {
1936         print_location(frame);
1937 }
1938
1939 static void pac_loc_notify(const struct l2cap_frame *frame)
1940 {
1941         print_location(frame);
1942 }
1943
1944 static void print_pac_context(const struct l2cap_frame *frame)
1945 {
1946         uint16_t snk, src;
1947         uint16_t mask;
1948
1949         if (!l2cap_frame_get_le16((void *)frame, &snk)) {
1950                 print_text(COLOR_ERROR, "  sink: invalid size");
1951                 goto done;
1952         }
1953
1954         print_field("  Sink Context: 0x%4.4x", snk);
1955
1956         mask = print_bitfield(4, snk, pac_context_table);
1957         if (mask)
1958                 print_text(COLOR_WHITE_BG, "  Unknown fields (0x%4.4x)",
1959                                                                 mask);
1960
1961         if (!l2cap_frame_get_le16((void *)frame, &src)) {
1962                 print_text(COLOR_ERROR, "  source: invalid size");
1963                 goto done;
1964         }
1965
1966         print_field("  Source Context: 0x%4.4x", src);
1967
1968         mask = print_bitfield(4, src, pac_context_table);
1969         if (mask)
1970                 print_text(COLOR_WHITE_BG, "  Unknown fields (0x%4.4x)",
1971                                                                 mask);
1972
1973 done:
1974         if (frame->size)
1975                 print_hex_field("  Data", frame->data, frame->size);
1976 }
1977
1978 static void pac_context_read(const struct l2cap_frame *frame)
1979 {
1980         print_pac_context(frame);
1981 }
1982
1983 static void pac_context_notify(const struct l2cap_frame *frame)
1984 {
1985         print_pac_context(frame);
1986 }
1987
1988 static void csip_rank_read(const struct l2cap_frame *frame)
1989 {
1990         uint8_t rank;
1991
1992         if (!l2cap_frame_get_u8((void *)frame, &rank)) {
1993                 print_text(COLOR_ERROR, "Rank: invalid size");
1994                 goto done;
1995         }
1996
1997         print_field("    Rank: 0x%02x", rank);
1998
1999 done:
2000         if (frame->size)
2001                 print_hex_field("  Data", frame->data, frame->size);
2002 }
2003
2004 static void csip_lock_read(const struct l2cap_frame *frame)
2005 {
2006         uint8_t lock;
2007
2008         if (!l2cap_frame_get_u8((void *)frame, &lock)) {
2009                 print_text(COLOR_ERROR, "Lock: invalid size");
2010                 goto done;
2011         }
2012
2013         switch (lock) {
2014         case 0x01:
2015                 print_field("    Unlocked (0x%02x)", lock);
2016                 break;
2017         case 0x02:
2018                 print_field("    Locked (0x%02x)", lock);
2019                 break;
2020         default:
2021                 print_field("    RFU (0x%02x)", lock);
2022                 break;
2023         }
2024
2025 done:
2026         if (frame->size)
2027                 print_hex_field("  Data", frame->data, frame->size);
2028 }
2029
2030 static void print_csip_size(const struct l2cap_frame *frame)
2031 {
2032         uint8_t size;
2033
2034         if (!l2cap_frame_get_u8((void *)frame, &size)) {
2035                 print_text(COLOR_ERROR, "Size: invalid size");
2036                 goto done;
2037         }
2038         print_field("    Size: 0x%02x", size);
2039
2040 done:
2041         if (frame->size)
2042                 print_hex_field("  Data", frame->data, frame->size);
2043 }
2044
2045 static void csip_size_read(const struct l2cap_frame *frame)
2046 {
2047         print_csip_size(frame);
2048 }
2049
2050 static void csip_size_notify(const struct l2cap_frame *frame)
2051 {
2052         print_csip_size(frame);
2053 }
2054
2055 static void csip_sirk_read(const struct l2cap_frame *frame)
2056 {
2057         if (frame->size)
2058                 print_hex_field("  SIRK", frame->data, frame->size);
2059 }
2060
2061 static void csip_sirk_notify(const struct l2cap_frame *frame)
2062 {
2063         if (frame->size)
2064                 print_hex_field("  SIRK", frame->data, frame->size);
2065 }
2066
2067 static void print_vcs_state(const struct l2cap_frame *frame)
2068 {
2069         uint8_t vol_set, mute, chng_ctr;
2070
2071         if (!l2cap_frame_get_u8((void *)frame, &vol_set)) {
2072                 print_text(COLOR_ERROR, "Volume Settings: invalid size");
2073                 goto done;
2074         }
2075         print_field("    Volume Setting: %u", vol_set);
2076
2077         if (!l2cap_frame_get_u8((void *)frame, &mute)) {
2078                 print_text(COLOR_ERROR, "Mute Filed: invalid size");
2079                 goto done;
2080         }
2081
2082         switch (mute) {
2083         case 0x00:
2084                 print_field("    Not Muted: %u", mute);
2085                 break;
2086         case 0x01:
2087                 print_field("    Muted: %u", mute);
2088                 break;
2089         default:
2090                 print_field("    Unknown Mute Value: %u", mute);
2091                 break;
2092         }
2093
2094         if (!l2cap_frame_get_u8((void *)frame, &chng_ctr)) {
2095                 print_text(COLOR_ERROR, "Change Counter: invalid size");
2096                 goto done;
2097         }
2098         print_field("    Change Counter: %u", chng_ctr);
2099
2100 done:
2101         if (frame->size)
2102                 print_hex_field("  Data", frame->data, frame->size);
2103 }
2104
2105 static void vol_state_read(const struct l2cap_frame *frame)
2106 {
2107         print_vcs_state(frame);
2108 }
2109
2110 static void vol_state_notify(const struct l2cap_frame *frame)
2111 {
2112         print_vcs_state(frame);
2113 }
2114
2115 static bool vcs_config_cmd(const struct l2cap_frame *frame)
2116 {
2117         if (!l2cap_frame_print_u8((void *)frame, "    Change Counter"))
2118                 return false;
2119
2120         return true;
2121 }
2122
2123 static bool vcs_absolute_cmd(const struct l2cap_frame *frame)
2124 {
2125         if (!l2cap_frame_print_u8((void *)frame, "    Change Counter"))
2126                 return false;
2127
2128         if (!l2cap_frame_print_u8((void *)frame, "    Volume Setting"))
2129                 return false;
2130
2131         return true;
2132 }
2133
2134 #define VCS_CMD(_op, _desc, _func) \
2135 [_op] = { \
2136         .desc = _desc, \
2137         .func = _func, \
2138 }
2139
2140 struct vcs_cmd {
2141         const char *desc;
2142         bool (*func)(const struct l2cap_frame *frame);
2143 } vcs_cmd_table[] = {
2144         /* Opcode = 0x00 (Relative Volume Down) */
2145         VCS_CMD(0x00, "Relative Volume Down", vcs_config_cmd),
2146         /* Opcode = 0x01 (Relative Volume Up) */
2147         VCS_CMD(0x01, "Relative Volume Up", vcs_config_cmd),
2148         /* Opcode = 0x02 (Unmute/Relative Volume Down) */
2149         VCS_CMD(0x02, "Unmute/Relative Volume Down", vcs_config_cmd),
2150         /* Opcode = 0x03 (Unmute/Relative Volume Up) */
2151         VCS_CMD(0x03, "Unmute/Relative Volume Up", vcs_config_cmd),
2152         /* Opcode = 0x04 (Set Absolute Volume) */
2153         VCS_CMD(0x04, "Set Absolute Volume", vcs_absolute_cmd),
2154         /* Opcode = 0x05 (Unmute) */
2155         VCS_CMD(0x05, "Unmute", vcs_config_cmd),
2156         /* Opcode = 0x06 (Mute) */
2157         VCS_CMD(0x06, "Mute", vcs_config_cmd),
2158 };
2159
2160 static struct vcs_cmd *vcs_get_cmd(uint8_t op)
2161 {
2162         if (op > ARRAY_SIZE(vcs_cmd_table))
2163                 return NULL;
2164
2165         return &vcs_cmd_table[op];
2166 }
2167
2168 static void print_vcs_cmd(const struct l2cap_frame *frame)
2169 {
2170         uint8_t op;
2171         struct vcs_cmd *cmd;
2172
2173         if (!l2cap_frame_get_u8((void *)frame, &op)) {
2174                 print_text(COLOR_ERROR, "opcode: invalid size");
2175                 goto done;
2176         }
2177
2178         cmd = vcs_get_cmd(op);
2179         if (!cmd) {
2180                 print_field("    Opcode: Reserved (0x%2.2x)", op);
2181                 goto done;
2182         }
2183
2184         print_field("    Opcode: %s (0x%2.2x)", cmd->desc, op);
2185         if (!cmd->func(frame))
2186                 print_field("    Unknown Opcode");
2187
2188 done:
2189         if (frame->size)
2190                 print_hex_field("  Data", frame->data, frame->size);
2191 }
2192
2193 static void vol_cp_write(const struct l2cap_frame *frame)
2194 {
2195         print_vcs_cmd(frame);
2196 }
2197
2198 static void print_vcs_flag(const struct l2cap_frame *frame)
2199 {
2200         uint8_t vol_flag;
2201
2202         if (!l2cap_frame_get_u8((void *)frame, &vol_flag)) {
2203                 print_text(COLOR_ERROR, "Volume Flag: invalid size");
2204                 goto done;
2205         }
2206         print_field("    Volume Falg: %u", vol_flag);
2207
2208 done:
2209         if (frame->size)
2210                 print_hex_field("  Data", frame->data, frame->size);
2211 }
2212
2213 static void vol_flag_read(const struct l2cap_frame *frame)
2214 {
2215         print_vcs_flag(frame);
2216 }
2217
2218 static void vol_flag_notify(const struct l2cap_frame *frame)
2219 {
2220         print_vcs_flag(frame);
2221 }
2222
2223 static char *name2utf8(const uint8_t *name, uint16_t len)
2224 {
2225         char utf8_name[HCI_MAX_NAME_LENGTH + 2];
2226         int i;
2227
2228         if (g_utf8_validate((const char *) name, len, NULL))
2229                 return g_strndup((char *) name, len);
2230
2231         len = MIN(len, sizeof(utf8_name) - 1);
2232
2233         memset(utf8_name, 0, sizeof(utf8_name));
2234         strncpy(utf8_name, (char *) name, len);
2235
2236         /* Assume ASCII, and replace all non-ASCII with spaces */
2237         for (i = 0; utf8_name[i] != '\0'; i++) {
2238                 if (!isascii(utf8_name[i]))
2239                         utf8_name[i] = ' ';
2240         }
2241
2242         /* Remove leading and trailing whitespace characters */
2243         g_strstrip(utf8_name);
2244
2245         return g_strdup(utf8_name);
2246 }
2247
2248 static void print_mp_name(const struct l2cap_frame *frame)
2249 {
2250         char *name;
2251
2252         name = name2utf8((uint8_t *)frame->data, frame->size);
2253
2254         print_field("  Media Player Name: %s", name);
2255 }
2256
2257 static void mp_name_read(const struct l2cap_frame *frame)
2258 {
2259         print_mp_name(frame);
2260 }
2261
2262 static void mp_name_notify(const struct l2cap_frame *frame)
2263 {
2264         print_mp_name(frame);
2265 }
2266
2267 static void print_track_changed(const struct l2cap_frame *frame)
2268 {
2269         print_field("  Track Changed");
2270 }
2271
2272 static void track_changed_notify(const struct l2cap_frame *frame)
2273 {
2274         print_track_changed(frame);
2275 }
2276
2277 static void print_track_title(const struct l2cap_frame *frame)
2278 {
2279         char *name;
2280
2281         name = name2utf8((uint8_t *)frame->data, frame->size);
2282
2283         print_field("  Track Title: %s", name);
2284 }
2285
2286 static void track_title_read(const struct l2cap_frame *frame)
2287 {
2288         print_track_title(frame);
2289 }
2290
2291 static void track_title_notify(const struct l2cap_frame *frame)
2292 {
2293         print_track_title(frame);
2294 }
2295
2296 static void print_track_duration(const struct l2cap_frame *frame)
2297 {
2298         int32_t duration;
2299
2300         if (!l2cap_frame_get_le32((void *)frame, (uint32_t *)&duration)) {
2301                 print_text(COLOR_ERROR, "  Track Duration: invalid size");
2302                 goto done;
2303         }
2304
2305         print_field("  Track Duration: %u", duration);
2306
2307 done:
2308         if (frame->size)
2309                 print_hex_field("  Data", frame->data, frame->size);
2310 }
2311
2312 static void track_duration_read(const struct l2cap_frame *frame)
2313 {
2314         print_track_duration(frame);
2315 }
2316
2317 static void track_duration_notify(const struct l2cap_frame *frame)
2318 {
2319         print_track_duration(frame);
2320 }
2321
2322 static void print_track_position(const struct l2cap_frame *frame)
2323 {
2324         int32_t position;
2325
2326         if (!l2cap_frame_get_le32((void *)frame, (uint32_t *)&position)) {
2327                 print_text(COLOR_ERROR, "  Track Position: invalid size");
2328                 goto done;
2329         }
2330
2331         print_field("  Track Position: %u", position);
2332
2333 done:
2334         if (frame->size)
2335                 print_hex_field("  Data", frame->data, frame->size);
2336 }
2337
2338 static void track_position_read(const struct l2cap_frame *frame)
2339 {
2340         print_track_position(frame);
2341 }
2342
2343 static void track_position_write(const struct l2cap_frame *frame)
2344 {
2345         print_track_position(frame);
2346 }
2347
2348 static void track_position_notify(const struct l2cap_frame *frame)
2349 {
2350         print_track_position(frame);
2351 }
2352
2353 static void print_playback_speed(const struct l2cap_frame *frame)
2354 {
2355         int8_t playback_speed;
2356
2357         if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&playback_speed)) {
2358                 print_text(COLOR_ERROR, "  Playback Speed: invalid size");
2359                 goto done;
2360         }
2361
2362         print_field("  Playback Speed: %u", playback_speed);
2363
2364 done:
2365         if (frame->size)
2366                 print_hex_field("  Data", frame->data, frame->size);
2367 }
2368
2369 static void playback_speed_read(const struct l2cap_frame *frame)
2370 {
2371         print_playback_speed(frame);
2372 }
2373
2374 static void playback_speed_write(const struct l2cap_frame *frame)
2375 {
2376         print_playback_speed(frame);
2377 }
2378
2379 static void playback_speed_notify(const struct l2cap_frame *frame)
2380 {
2381         print_playback_speed(frame);
2382 }
2383
2384 static void print_seeking_speed(const struct l2cap_frame *frame)
2385 {
2386         int8_t seeking_speed;
2387
2388         if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&seeking_speed)) {
2389                 print_text(COLOR_ERROR, "  Seeking Speed: invalid size");
2390                 goto done;
2391         }
2392
2393         print_field("  Seeking Speed: %u", seeking_speed);
2394
2395 done:
2396         if (frame->size)
2397                 print_hex_field("  Data", frame->data, frame->size);
2398 }
2399
2400 static void seeking_speed_read(const struct l2cap_frame *frame)
2401 {
2402         print_seeking_speed(frame);
2403 }
2404
2405 static void seeking_speed_notify(const struct l2cap_frame *frame)
2406 {
2407         print_seeking_speed(frame);
2408 }
2409
2410 static const char *play_order_str(uint8_t order)
2411 {
2412         switch (order) {
2413         case 0x01:
2414                 return "Single once";
2415         case 0x02:
2416                 return "Single repeat";
2417         case 0x03:
2418                 return "In order once";
2419         case 0x04:
2420                 return "In order repeat";
2421         case 0x05:
2422                 return "Oldest once";
2423         case 0x06:
2424                 return "Oldest repeat";
2425         case 0x07:
2426                 return "Newest once";
2427         case 0x08:
2428                 return "Newest repeat";
2429         case 0x09:
2430                 return "Shuffle once";
2431         case 0x0A:
2432                 return "Shuffle repeat";
2433         default:
2434                 return "RFU";
2435         }
2436 }
2437
2438 static void print_playing_order(const struct l2cap_frame *frame)
2439 {
2440         int8_t playing_order;
2441
2442         if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&playing_order)) {
2443                 print_text(COLOR_ERROR, "  Playing Order: invalid size");
2444                 goto done;
2445         }
2446
2447         print_field("  Playing Order: %s", play_order_str(playing_order));
2448
2449 done:
2450         if (frame->size)
2451                 print_hex_field("  Data", frame->data, frame->size);
2452 }
2453
2454 static void playing_order_read(const struct l2cap_frame *frame)
2455 {
2456         print_playing_order(frame);
2457 }
2458
2459 static void playing_order_write(const struct l2cap_frame *frame)
2460 {
2461         print_playing_order(frame);
2462 }
2463
2464 static void playing_order_notify(const struct l2cap_frame *frame)
2465 {
2466         print_playing_order(frame);
2467 }
2468
2469 static const struct bitfield_data playing_orders_table[] = {
2470         {  0, "Single once (0x0001)"        },
2471         {  1, "Single repeat (0x0002)"          },
2472         {  2, "In order once (0x0004)"          },
2473         {  3, "In Order Repeat (0x0008)"        },
2474         {  4, "Oldest once (0x0010)"            },
2475         {  5, "Oldest repeat (0x0020)"          },
2476         {  6, "Newest once (0x0040)"            },
2477         {  7, "Newest repeat (0x0080)"      },
2478         {  8, "Shuffle once (0x0100)"           },
2479         {  9, "Shuffle repeat (0x0200)"         },
2480         {  10, "RFU (0x0400)"                       },
2481         {  11, "RFU (0x0800)"                   },
2482         {  12, "RFU (0x1000)"                           },
2483         {  13, "RFU (0x2000)"                           },
2484         {  14, "RFU (0x4000)"                           },
2485         {  15, "RFU (0x8000)"                           },
2486         { }
2487 };
2488
2489 static void print_playing_orders_supported(const struct l2cap_frame *frame)
2490 {
2491         uint16_t supported_orders;
2492         uint16_t mask;
2493
2494         if (!l2cap_frame_get_le16((void *)frame, &supported_orders)) {
2495                 print_text(COLOR_ERROR,
2496                                 "    Supported Playing Orders: invalid size");
2497                 goto done;
2498         }
2499
2500         print_field("      Supported Playing Orders: 0x%4.4x",
2501                                 supported_orders);
2502
2503         mask = print_bitfield(8, supported_orders, playing_orders_table);
2504         if (mask)
2505                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%4.4x)",
2506                                                                 mask);
2507
2508 done:
2509         if (frame->size)
2510                 print_hex_field("    Data", frame->data, frame->size);
2511 }
2512
2513 static void playing_orders_supported_read(const struct l2cap_frame *frame)
2514 {
2515         print_playing_orders_supported(frame);
2516 }
2517
2518 static const char *media_state_str(uint8_t state)
2519 {
2520         switch (state) {
2521         case 0x00:
2522                 return "Inactive";
2523         case 0x01:
2524                 return "Playing";
2525         case 0x02:
2526                 return "Paused";
2527         case 0x03:
2528                 return "Seeking";
2529         default:
2530                 return "RFU";
2531         }
2532 }
2533
2534 static void print_media_state(const struct l2cap_frame *frame)
2535 {
2536         int8_t state;
2537
2538         if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&state)) {
2539                 print_text(COLOR_ERROR, "  Media State: invalid size");
2540                 goto done;
2541         }
2542
2543         print_field("  Media State: %s", media_state_str(state));
2544
2545 done:
2546         if (frame->size)
2547                 print_hex_field("  Data", frame->data, frame->size);
2548 }
2549
2550 static void media_state_read(const struct l2cap_frame *frame)
2551 {
2552         print_media_state(frame);
2553 }
2554
2555 static void media_state_notify(const struct l2cap_frame *frame)
2556 {
2557         print_media_state(frame);
2558 }
2559
2560 struct media_cp_opcode {
2561         uint8_t opcode;
2562         const char *opcode_str;
2563 } media_cp_opcode_table[] = {
2564         {0x01,  "Play"},
2565         {0x02,  "Pause"},
2566         {0x03,  "Fast Rewind"},
2567         {0x04,  "Fast Forward"},
2568         {0x05,  "Stop"},
2569         {0x10,  "Move Relative"},
2570         {0x20,  "Previous Segment"},
2571         {0x21,  "Next Segment"},
2572         {0x22,  "First Segment"},
2573         {0x23,  "Last Segment"},
2574         {0x24,  "Goto Segment"},
2575         {0x30,  "Previous Track"},
2576         {0x31,  "Next Track"},
2577         {0x32,  "First Track"},
2578         {0x33,  "Last Track"},
2579         {0x34,  "Goto Track"},
2580         {0x40,  "Previous Group"},
2581         {0x41,  "Next Group"},
2582         {0x42,  "First Group"},
2583         {0x43,  "Last Group"},
2584         {0x44,  "Goto Group"},
2585 };
2586
2587 static const char *cp_opcode_str(uint8_t opcode)
2588 {
2589         size_t i;
2590
2591         for (i = 0; i < ARRAY_SIZE(media_cp_opcode_table); i++) {
2592                 const char *str = media_cp_opcode_table[i].opcode_str;
2593
2594                 if (opcode == media_cp_opcode_table[i].opcode)
2595                         return str;
2596         }
2597
2598         return "RFU";
2599 }
2600
2601 static void print_media_cp(const struct l2cap_frame *frame)
2602 {
2603         int8_t opcode;
2604
2605         if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&opcode)) {
2606                 print_text(COLOR_ERROR, "  Media Control Point: invalid size");
2607                 goto done;
2608         }
2609
2610         print_field("  Media Control Point: %s", cp_opcode_str(opcode));
2611
2612 done:
2613         if (frame->size)
2614                 print_hex_field("  Data", frame->data, frame->size);
2615 }
2616
2617 static void media_cp_write(const struct l2cap_frame *frame)
2618 {
2619         print_media_cp(frame);
2620 }
2621
2622 static void media_cp_notify(const struct l2cap_frame *frame)
2623 {
2624         print_media_cp(frame);
2625 }
2626
2627 static const struct bitfield_data supported_opcodes_table[] = {
2628         {0, "Play (0x00000001)"                         },
2629         {1, "Pause (0x00000002)"                        },
2630         {2, "Fast Rewind        (0x00000004)"   },
2631         {3, "Fast Forward (0x00000008)"         },
2632         {4, "Stop (0x00000010)"                         },
2633         {5, "Move Relative (0x00000020)"        },
2634         {6, "Previous Segment (0x00000040)"     },
2635         {7, "Next Segment (0x00000080)"         },
2636         {8, "First Segment (0x00000100)"        },
2637         {9, "Last Segment (0x00000200)"         },
2638         {10, "Goto Segment (0x00000400)"        },
2639         {11, "Previous Track (0x00000800)"      },
2640         {12, "Next Track (0x00001000)"          },
2641         {13, "First Track (0x00002000)"         },
2642         {14, "Last Track (0x00004000)"          },
2643         {15, "Goto Track (0x00008000)"          },
2644         {16, "Previous Group (0x00010000)"      },
2645         {17, "Next Group (0x00020000)"          },
2646         {18, "First Group (0x00040000)"         },
2647         {19, "Last Group (0x00080000)"          },
2648         {20, "Goto Group (0x00100000)"          },
2649         {21, "RFU (0x00200000)"                         },
2650         {22, "RFU (0x00400000)"                         },
2651         {23, "RFU (0x00800000)"                         },
2652         {24, "RFU (0x01000000)"                         },
2653         {25, "RFU (0x02000000)"                         },
2654         {26, "RFU (0x04000000)"                         },
2655         {27, "RFU (0x08000000)"                         },
2656         {28, "RFU (0x10000000)"                         },
2657         {29, "RFU (0x20000000)"                         },
2658         {30, "RFU (0x40000000)"                         },
2659         {31, "RFU (0x80000000)"                         },
2660         { }
2661 };
2662
2663 static void print_media_cp_op_supported(const struct l2cap_frame *frame)
2664 {
2665         uint32_t supported_opcodes;
2666         uint32_t mask;
2667
2668         if (!l2cap_frame_get_le32((void *)frame, &supported_opcodes)) {
2669                 print_text(COLOR_ERROR, "    value: invalid size");
2670                 goto done;
2671         }
2672
2673         print_field("      Supported Opcodes: 0x%8.8x", supported_opcodes);
2674
2675         mask = print_bitfield(8, supported_opcodes, supported_opcodes_table);
2676         if (mask)
2677                 print_text(COLOR_WHITE_BG, "    Unknown fields (0x%4.4x)",
2678                                                                 mask);
2679
2680 done:
2681         if (frame->size)
2682                 print_hex_field("    Data", frame->data, frame->size);
2683 }
2684
2685 static void media_cp_op_supported_read(const struct l2cap_frame *frame)
2686 {
2687         print_media_cp_op_supported(frame);
2688 }
2689
2690 static void media_cp_op_supported_notify(const struct l2cap_frame *frame)
2691 {
2692         print_media_cp_op_supported(frame);
2693 }
2694
2695 static void print_content_control_id(const struct l2cap_frame *frame)
2696 {
2697         int8_t ccid;
2698
2699         if (!l2cap_frame_get_u8((void *)frame, (uint8_t *)&ccid)) {
2700                 print_text(COLOR_ERROR, "  Content Control ID: invalid size");
2701                 goto done;
2702         }
2703
2704         print_field("  Content Control ID: 0x%2.2x", ccid);
2705
2706 done:
2707         if (frame->size)
2708                 print_hex_field("  Data", frame->data, frame->size);
2709 }
2710
2711 static void content_control_id_read(const struct l2cap_frame *frame)
2712 {
2713         print_content_control_id(frame);
2714 }
2715
2716 static const struct pa_sync_state_decoder {
2717         uint8_t code;
2718         char *value;
2719 } pa_sync_state_decoders[] = {
2720         { 0x00, "Not synchronized to PA" },
2721         { 0x01, "SyncInfo Request" },
2722         { 0x02, "Synchronized to PA" },
2723         { 0x03, "Failed to synchronize to PA" },
2724         { 0x04, "No PAST" },
2725 };
2726
2727 static const struct cp_pa_sync_state_decoder {
2728         uint8_t code;
2729         char *value;
2730 } cp_pa_sync_state_decoders[] = {
2731         { 0x00, "Do not synchronize to PA" },
2732         { 0x01, "Synchronize to PA - PAST available" },
2733         { 0x02, "Synchronize to PA - PAST not available" },
2734 };
2735
2736 static const struct big_enc_decoder {
2737         uint8_t code;
2738         char *value;
2739 } big_enc_decoders[] = {
2740         { 0x00, "Not encrypted" },
2741         { 0x01, "Broadcast_Code required" },
2742         { 0x02, "Decrypting" },
2743         { 0x03, "Bad_Code (incorrect encryption key)" },
2744 };
2745
2746 static bool print_subgroup_lv(const struct l2cap_frame *frame,
2747                 const char *label, struct packet_ltv_decoder *decoder,
2748                 size_t decoder_len)
2749 {
2750         struct bt_hci_lv_data *lv;
2751
2752         lv = l2cap_frame_pull((void *)frame, frame, sizeof(*lv));
2753         if (!lv) {
2754                 print_text(COLOR_ERROR, "%s: invalid size", label);
2755                 return false;
2756         }
2757
2758         if (!l2cap_frame_pull((void *)frame, frame, lv->len)) {
2759                 print_text(COLOR_ERROR, "%s: invalid size", label);
2760                 return false;
2761         }
2762
2763         packet_print_ltv(label, lv->data, lv->len, decoder, decoder_len);
2764
2765         return true;
2766 }
2767
2768 static bool print_subgroup_metadata(const char *label,
2769                                 const struct l2cap_frame *frame)
2770 {
2771         return print_subgroup_lv(frame, label, NULL, 0);
2772 }
2773
2774 static void print_bcast_recv_state(const struct l2cap_frame *frame)
2775 {
2776         uint8_t i;
2777         uint8_t id;
2778         uint8_t addr_type;
2779         uint8_t *addr;
2780         uint8_t sid;
2781         uint32_t bid;
2782         uint8_t pa_sync_state;
2783         uint8_t enc;
2784         uint8_t *bad_code;
2785         uint8_t num_subgroups = 0;
2786         uint32_t bis_sync_state;
2787
2788         if (frame->size == 0) {
2789                 print_field("  Empty characteristic");
2790                 goto done;
2791         }
2792
2793         if (!l2cap_frame_get_u8((void *)frame, &id)) {
2794                 print_text(COLOR_ERROR, "Source_ID: invalid size");
2795                 goto done;
2796         }
2797
2798         print_field("  Source_ID: %u", id);
2799
2800         if (!l2cap_frame_get_u8((void *)frame, &addr_type)) {
2801                 print_text(COLOR_ERROR, "Source_Address_Type: invalid size");
2802                 goto done;
2803         }
2804
2805         print_field("  Source_Address_Type: %u", addr_type);
2806
2807         addr = l2cap_frame_pull((void *)frame, frame, sizeof(bdaddr_t));
2808         if (!addr) {
2809                 print_text(COLOR_ERROR, "Source_Address: invalid size");
2810                 goto done;
2811         }
2812
2813         print_field("  Source_Address: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
2814                                         addr[5], addr[4],
2815                                         addr[3], addr[2],
2816                                         addr[1], addr[0]);
2817
2818         if (!l2cap_frame_get_u8((void *)frame, &sid)) {
2819                 print_text(COLOR_ERROR, "Source_Adv_SID: invalid size");
2820                 goto done;
2821         }
2822
2823         print_field("  Source_Adv_SID: %u", sid);
2824
2825         if (!l2cap_frame_get_le24((void *)frame, &bid)) {
2826                 print_text(COLOR_ERROR, "Broadcast_ID: invalid size");
2827                 goto done;
2828         }
2829
2830         print_field("  Broadcast_ID: 0x%06x", bid);
2831
2832         if (!l2cap_frame_get_u8((void *)frame, &pa_sync_state)) {
2833                 print_text(COLOR_ERROR, "PA_Sync_State: invalid size");
2834                 goto done;
2835         }
2836
2837         for (i = 0; i < ARRAY_SIZE(pa_sync_state_decoders); i++) {
2838                 const struct pa_sync_state_decoder *decoder;
2839
2840                 decoder = &pa_sync_state_decoders[i];
2841
2842                 if (decoder->code == pa_sync_state) {
2843                         print_field("  PA_Sync_State: %s", decoder->value);
2844                         break;
2845                 }
2846         }
2847
2848         if (i == ARRAY_SIZE(pa_sync_state_decoders))
2849                 print_field("  PA_Sync_State: %s", "Invalid value");
2850
2851         if (!l2cap_frame_get_u8((void *)frame, &enc)) {
2852                 print_text(COLOR_ERROR, "BIG_Encryption: invalid size");
2853                 goto done;
2854         }
2855
2856         for (i = 0; i < ARRAY_SIZE(big_enc_decoders); i++) {
2857                 const struct big_enc_decoder *decoder;
2858
2859                 decoder = &big_enc_decoders[i];
2860
2861                 if (decoder->code == enc) {
2862                         print_field("  BIG_Encryption: %s", decoder->value);
2863                         break;
2864                 }
2865         }
2866
2867         if (i == ARRAY_SIZE(big_enc_decoders))
2868                 print_field("  BIG_Encryption: %s", "Invalid value");
2869
2870         if (enc == 0x03) {
2871                 bad_code = l2cap_frame_pull((void *)frame, frame, 16);
2872                 if (!bad_code) {
2873                         print_text(COLOR_ERROR, "Bad_Code: invalid size");
2874                         goto done;
2875                 }
2876
2877                 print_hex_field("  Bad_Code", bad_code, 16);
2878         }
2879
2880         if (!l2cap_frame_get_u8((void *)frame, &num_subgroups)) {
2881                 print_text(COLOR_ERROR, "Num_Subgroups: invalid size");
2882                 goto done;
2883         }
2884
2885         print_field("  Num_Subgroups: %u", num_subgroups);
2886
2887         for (i = 0; i < num_subgroups; i++) {
2888                 print_field("  Subgroup #%u:", i);
2889
2890                 if (!l2cap_frame_get_le32((void *)frame, &bis_sync_state)) {
2891                         print_text(COLOR_ERROR, "BIS_Sync State: invalid size");
2892                         goto done;
2893                 }
2894
2895                 print_field("    BIS_Sync State: 0x%8.8x", bis_sync_state);
2896
2897                 if (!print_subgroup_metadata("    Metadata", frame))
2898                         goto done;
2899         }
2900
2901 done:
2902         if (frame->size)
2903                 print_hex_field("  Data", frame->data, frame->size);
2904 }
2905
2906 static void bcast_recv_state_read(const struct l2cap_frame *frame)
2907 {
2908         print_bcast_recv_state(frame);
2909 }
2910
2911 static void bcast_recv_state_notify(const struct l2cap_frame *frame)
2912 {
2913         print_bcast_recv_state(frame);
2914 }
2915
2916 #define BCAST_AUDIO_SCAN_CP_CMD(_op, _desc, _func) \
2917 [_op] = { \
2918         .desc = _desc, \
2919         .func = _func, \
2920 }
2921
2922 static void bcast_audio_scan_cp_add_src_cmd(const struct l2cap_frame *frame)
2923 {
2924         uint8_t i;
2925         uint8_t addr_type;
2926         uint8_t *addr;
2927         uint8_t sid;
2928         uint32_t bid;
2929         uint8_t pa_sync_state;
2930         uint16_t pa_interval;
2931         uint8_t num_subgroups = 0;
2932         uint32_t bis_sync_state;
2933
2934         if (!l2cap_frame_get_u8((void *)frame, &addr_type)) {
2935                 print_text(COLOR_ERROR, "Source_Address_Type: invalid size");
2936                 return;
2937         }
2938
2939         print_field("    Source_Address_Type: %u", addr_type);
2940
2941         addr = l2cap_frame_pull((void *)frame, frame, sizeof(bdaddr_t));
2942         if (!addr) {
2943                 print_text(COLOR_ERROR, "Source_Address: invalid size");
2944                 return;
2945         }
2946
2947         print_field("    Source_Address: %2.2X:%2.2X:%2.2X:%2.2X:%2.2X:%2.2X",
2948                                         addr[5], addr[4],
2949                                         addr[3], addr[2],
2950                                         addr[1], addr[0]);
2951
2952         if (!l2cap_frame_get_u8((void *)frame, &sid)) {
2953                 print_text(COLOR_ERROR, "Source_Adv_SID: invalid size");
2954                 return;
2955         }
2956
2957         print_field("    Source_Adv_SID: %u", sid);
2958
2959         if (!l2cap_frame_get_le24((void *)frame, &bid)) {
2960                 print_text(COLOR_ERROR, "Broadcast_ID: invalid size");
2961                 return;
2962         }
2963
2964         print_field("    Broadcast_ID: 0x%06x", bid);
2965
2966         if (!l2cap_frame_get_u8((void *)frame, &pa_sync_state)) {
2967                 print_text(COLOR_ERROR, "PA_Sync_State: invalid size");
2968                 return;
2969         }
2970
2971         for (i = 0; i < ARRAY_SIZE(cp_pa_sync_state_decoders); i++) {
2972                 const struct cp_pa_sync_state_decoder *decoder;
2973
2974                 decoder = &cp_pa_sync_state_decoders[i];
2975
2976                 if (decoder->code == pa_sync_state) {
2977                         print_field("    PA_Sync_State: %s", decoder->value);
2978                         break;
2979                 }
2980         }
2981
2982         if (i == ARRAY_SIZE(cp_pa_sync_state_decoders))
2983                 print_field("    PA_Sync_State: %s", "Invalid value");
2984
2985         if (!l2cap_frame_get_le16((void *)frame, &pa_interval)) {
2986                 print_text(COLOR_ERROR, "PA_Interval: invalid size");
2987                 return;
2988         }
2989
2990         print_field("    PA_Interval: 0x%04x", pa_interval);
2991
2992         if (!l2cap_frame_get_u8((void *)frame, &num_subgroups)) {
2993                 print_text(COLOR_ERROR, "Num_Subgroups: invalid size");
2994                 return;
2995         }
2996
2997         print_field("    Num_Subgroups: %u", num_subgroups);
2998
2999         for (i = 0; i < num_subgroups; i++) {
3000                 print_field("    Subgroup #%u:", i);
3001
3002                 if (!l2cap_frame_get_le32((void *)frame, &bis_sync_state)) {
3003                         print_text(COLOR_ERROR, "BIS_Sync State: invalid size");
3004                         return;
3005                 }
3006
3007                 print_field("      BIS_Sync State: 0x%8.8x", bis_sync_state);
3008
3009                 if (!print_subgroup_metadata("      Metadata", frame))
3010                         return;
3011         }
3012 }
3013
3014 static void bcast_audio_scan_cp_mod_src_cmd(const struct l2cap_frame *frame)
3015 {
3016         uint8_t i;
3017         uint8_t id;
3018         uint8_t pa_sync_state;
3019         uint16_t pa_interval;
3020         uint8_t num_subgroups = 0;
3021         uint32_t bis_sync_state;
3022
3023         if (!l2cap_frame_get_u8((void *)frame, &id)) {
3024                 print_text(COLOR_ERROR, "Source_ID: invalid size");
3025                 return;
3026         }
3027
3028         print_field("    Source_ID: %u", id);
3029
3030         if (!l2cap_frame_get_u8((void *)frame, &pa_sync_state)) {
3031                 print_text(COLOR_ERROR, "PA_Sync_State: invalid size");
3032                 return;
3033         }
3034
3035         for (i = 0; i < ARRAY_SIZE(cp_pa_sync_state_decoders); i++) {
3036                 const struct cp_pa_sync_state_decoder *decoder;
3037
3038                 decoder = &cp_pa_sync_state_decoders[i];
3039
3040                 if (decoder->code == pa_sync_state) {
3041                         print_field("    PA_Sync_State: %s", decoder->value);
3042                         break;
3043                 }
3044         }
3045
3046         if (i == ARRAY_SIZE(cp_pa_sync_state_decoders))
3047                 print_field("    PA_Sync_State: %s", "Invalid value");
3048
3049         if (!l2cap_frame_get_le16((void *)frame, &pa_interval)) {
3050                 print_text(COLOR_ERROR, "PA_Interval: invalid size");
3051                 return;
3052         }
3053
3054         print_field("    PA_Interval: 0x%04x", pa_interval);
3055
3056         if (!l2cap_frame_get_u8((void *)frame, &num_subgroups)) {
3057                 print_text(COLOR_ERROR, "Num_Subgroups: invalid size");
3058                 return;
3059         }
3060
3061         print_field("    Num_Subgroups: %u", num_subgroups);
3062
3063         for (i = 0; i < num_subgroups; i++) {
3064                 print_field("    Subgroup #%u:", i);
3065
3066                 if (!l2cap_frame_get_le32((void *)frame, &bis_sync_state)) {
3067                         print_text(COLOR_ERROR, "BIS_Sync State: invalid size");
3068                         return;
3069                 }
3070
3071                 print_field("      BIS_Sync State: 0x%8.8x", bis_sync_state);
3072
3073                 if (!print_subgroup_metadata("      Metadata", frame))
3074                         return;
3075         }
3076 }
3077
3078 static void bcast_audio_scan_cp_set_bcode_cmd(const struct l2cap_frame *frame)
3079 {
3080         uint8_t id;
3081         uint8_t *bcast_code;
3082
3083         if (!l2cap_frame_get_u8((void *)frame, &id)) {
3084                 print_text(COLOR_ERROR, "Source_ID: invalid size");
3085                 return;
3086         }
3087
3088         print_field("    Source_ID: %u", id);
3089
3090         bcast_code = l2cap_frame_pull((void *)frame, frame, 16);
3091         if (!bcast_code) {
3092                 print_text(COLOR_ERROR, "Broadcast_Code: invalid size");
3093                 return;
3094         }
3095
3096         print_hex_field("    Broadcast_Code", bcast_code, 16);
3097
3098 }
3099
3100 static void bcast_audio_scan_cp_remove_src_cmd(const struct l2cap_frame *frame)
3101 {
3102         uint8_t id;
3103
3104         if (!l2cap_frame_get_u8((void *)frame, &id)) {
3105                 print_text(COLOR_ERROR, "Source_ID: invalid size");
3106                 return;
3107         }
3108
3109         print_field("    Source_ID: %u", id);
3110 }
3111
3112 struct bcast_audio_scan_cp_cmd {
3113         const char *desc;
3114         void (*func)(const struct l2cap_frame *frame);
3115 } bcast_audio_scan_cp_cmd_table[] = {
3116         /* Opcode = 0x00 (Remote Scan Stopped) */
3117         BCAST_AUDIO_SCAN_CP_CMD(0x00, "Remote Scan Stopped", NULL),
3118         /* Opcode = 0x01 (Remote Scan Started) */
3119         BCAST_AUDIO_SCAN_CP_CMD(0x01, "Remote Scan Started", NULL),
3120         /* Opcode = 0x02 (Add Source) */
3121         BCAST_AUDIO_SCAN_CP_CMD(0x02, "Add Source",
3122                                         bcast_audio_scan_cp_add_src_cmd),
3123         /* Opcode = 0x03 (Modify Source) */
3124         BCAST_AUDIO_SCAN_CP_CMD(0x03, "Modify Source",
3125                                         bcast_audio_scan_cp_mod_src_cmd),
3126         /* Opcode = 0x04 (Set Broadcast_Code) */
3127         BCAST_AUDIO_SCAN_CP_CMD(0x04, "Set Broadcast_Code",
3128                                         bcast_audio_scan_cp_set_bcode_cmd),
3129         /* Opcode = 0x05 (Remove Source) */
3130         BCAST_AUDIO_SCAN_CP_CMD(0x05, "Remove Source",
3131                                         bcast_audio_scan_cp_remove_src_cmd),
3132 };
3133
3134 static struct bcast_audio_scan_cp_cmd *bcast_audio_scan_cp_get_cmd(uint8_t op)
3135 {
3136         if (op > ARRAY_SIZE(bcast_audio_scan_cp_cmd_table))
3137                 return NULL;
3138
3139         return &bcast_audio_scan_cp_cmd_table[op];
3140 }
3141
3142 static void print_bcast_audio_scan_cp_cmd(const struct l2cap_frame *frame)
3143 {
3144         uint8_t op;
3145         struct bcast_audio_scan_cp_cmd *cmd;
3146
3147         if (!l2cap_frame_get_u8((void *)frame, &op)) {
3148                 print_text(COLOR_ERROR, "Opcode: invalid size");
3149                 goto done;
3150         }
3151
3152         cmd = bcast_audio_scan_cp_get_cmd(op);
3153         if (!cmd) {
3154                 print_field("    Opcode: Reserved (0x%2.2x)", op);
3155                 goto done;
3156         }
3157
3158         print_field("    Opcode: %s (0x%2.2x)", cmd->desc, op);
3159         if (cmd->func)
3160                 cmd->func(frame);
3161
3162 done:
3163         if (frame->size)
3164                 print_hex_field("  Data", frame->data, frame->size);
3165 }
3166
3167 static void bcast_audio_scan_cp_write(const struct l2cap_frame *frame)
3168 {
3169         print_bcast_audio_scan_cp_cmd(frame);
3170 }
3171
3172 #define GATT_HANDLER(_uuid, _read, _write, _notify) \
3173 { \
3174         .uuid = { \
3175                 .type = BT_UUID16, \
3176                 .value.u16 = _uuid, \
3177         }, \
3178         .read = _read, \
3179         .write = _write, \
3180         .notify = _notify \
3181 }
3182
3183 struct gatt_handler {
3184         bt_uuid_t uuid;
3185         void (*read)(const struct l2cap_frame *frame);
3186         void (*write)(const struct l2cap_frame *frame);
3187         void (*notify)(const struct l2cap_frame *frame);
3188 } gatt_handlers[] = {
3189         GATT_HANDLER(0x2800, pri_svc_read, NULL, NULL),
3190         GATT_HANDLER(0x2801, sec_svc_read, NULL, NULL),
3191         GATT_HANDLER(0x2803, chrc_read, NULL, NULL),
3192         GATT_HANDLER(0x2902, ccc_read, ccc_write, NULL),
3193         GATT_HANDLER(0x2bc4, ase_read, NULL, ase_notify),
3194         GATT_HANDLER(0x2bc5, ase_read, NULL, ase_notify),
3195         GATT_HANDLER(0x2bc6, NULL, ase_cp_write, ase_cp_notify),
3196         GATT_HANDLER(0x2bc9, pac_read, NULL, pac_notify),
3197         GATT_HANDLER(0x2bca, pac_loc_read, NULL, pac_loc_notify),
3198         GATT_HANDLER(0x2bcb, pac_read, NULL, pac_notify),
3199         GATT_HANDLER(0x2bcc, pac_loc_read, NULL, pac_loc_notify),
3200         GATT_HANDLER(0x2bcd, pac_context_read, NULL, pac_context_notify),
3201         GATT_HANDLER(0x2bce, pac_context_read, NULL, pac_context_notify),
3202         GATT_HANDLER(0x2b7d, vol_state_read, NULL, vol_state_notify),
3203         GATT_HANDLER(0x2b7e, NULL, vol_cp_write, NULL),
3204         GATT_HANDLER(0x2b7f, vol_flag_read, NULL, vol_flag_notify),
3205
3206         GATT_HANDLER(0x2b84, csip_sirk_read, NULL, csip_sirk_notify),
3207         GATT_HANDLER(0x2b85, csip_size_read, NULL, csip_size_notify),
3208         GATT_HANDLER(0x2b86, csip_lock_read, NULL, NULL),
3209         GATT_HANDLER(0x2b87, csip_rank_read, NULL, NULL),
3210
3211         GATT_HANDLER(0x2b93, mp_name_read, NULL, mp_name_notify),
3212         GATT_HANDLER(0x2b96, NULL, NULL, track_changed_notify),
3213         GATT_HANDLER(0x2b97, track_title_read, NULL, track_title_notify),
3214         GATT_HANDLER(0x2b98, track_duration_read, NULL, track_duration_notify),
3215         GATT_HANDLER(0x2b99, track_position_read, track_position_write,
3216                                         track_position_notify),
3217         GATT_HANDLER(0x2b9a, playback_speed_read, playback_speed_write,
3218                                         playback_speed_notify),
3219         GATT_HANDLER(0x2b9b, seeking_speed_read, NULL, seeking_speed_notify),
3220         GATT_HANDLER(0x2ba1, playing_order_read, playing_order_write,
3221                                         playing_order_notify),
3222         GATT_HANDLER(0x2ba2, playing_orders_supported_read, NULL, NULL),
3223         GATT_HANDLER(0x2ba3, media_state_read, NULL, media_state_notify),
3224         GATT_HANDLER(0x2ba4, NULL, media_cp_write, media_cp_notify),
3225         GATT_HANDLER(0x2ba5, media_cp_op_supported_read, NULL,
3226                                         media_cp_op_supported_notify),
3227         GATT_HANDLER(0x2bba, content_control_id_read, NULL, NULL),
3228
3229         GATT_HANDLER(0x2bc7, NULL, bcast_audio_scan_cp_write, NULL),
3230         GATT_HANDLER(0x2bc8, bcast_recv_state_read, NULL,
3231                                         bcast_recv_state_notify),
3232 };
3233
3234 static struct gatt_handler *get_handler_uuid(const bt_uuid_t *uuid)
3235 {
3236         size_t i;
3237
3238         if (!uuid)
3239                 return NULL;
3240
3241         for (i = 0; i < ARRAY_SIZE(gatt_handlers); i++) {
3242                 struct gatt_handler *handler = &gatt_handlers[i];
3243
3244                 if (!bt_uuid_cmp(&handler->uuid, uuid))
3245                         return handler;
3246         }
3247
3248         return NULL;
3249 }
3250
3251 static struct gatt_handler *get_handler(struct gatt_db_attribute *attr)
3252 {
3253         return get_handler_uuid(gatt_db_attribute_get_type(attr));
3254 }
3255
3256 static void att_exchange_mtu_req(const struct l2cap_frame *frame)
3257 {
3258         const struct bt_l2cap_att_exchange_mtu_req *pdu = frame->data;
3259
3260         print_field("Client RX MTU: %d", le16_to_cpu(pdu->mtu));
3261 }
3262
3263 static void att_exchange_mtu_rsp(const struct l2cap_frame *frame)
3264 {
3265         const struct bt_l2cap_att_exchange_mtu_rsp *pdu = frame->data;
3266
3267         print_field("Server RX MTU: %d", le16_to_cpu(pdu->mtu));
3268 }
3269
3270 static void att_find_info_req(const struct l2cap_frame *frame)
3271 {
3272         print_handle_range("Handle range", frame->data);
3273 }
3274
3275 static const char *att_format_str(uint8_t format)
3276 {
3277         switch (format) {
3278         case 0x01:
3279                 return "UUID-16";
3280         case 0x02:
3281                 return "UUID-128";
3282         default:
3283                 return "unknown";
3284         }
3285 }
3286
3287 static struct gatt_db_attribute *insert_desc(const struct l2cap_frame *frame,
3288                                                 uint16_t handle,
3289                                                 bt_uuid_t *uuid, bool rsp)
3290 {
3291         struct gatt_db *db;
3292
3293         db = get_db(frame, rsp);
3294         if (!db)
3295                 return NULL;
3296
3297         return gatt_db_insert_descriptor(db, handle, uuid, 0, NULL, NULL, NULL);
3298 }
3299
3300 static void att_find_info_rsp_16(const struct l2cap_frame *frame)
3301 {
3302         while (frame->size >= 4) {
3303                 uint16_t handle;
3304                 uint16_t u16;
3305                 bt_uuid_t uuid;
3306
3307                 if (!l2cap_frame_get_le16((void *)frame, &handle)) {
3308                         print_text(COLOR_ERROR, "    Handle: invalid size");
3309                         return;
3310                 }
3311
3312                 if (!l2cap_frame_get_le16((void *)frame, &u16)) {
3313                         print_text(COLOR_ERROR, "    UUID: invalid size");
3314                         return;
3315                 }
3316
3317                 print_field("Handle: 0x%4.4x", handle);
3318                 print_uuid("UUID", &u16, 2);
3319
3320                 bt_uuid16_create(&uuid, u16);
3321
3322                 insert_desc(frame, handle, &uuid, true);
3323         }
3324 }
3325
3326 static void att_find_info_rsp_128(const struct l2cap_frame *frame)
3327 {
3328         while (frame->size >= 18) {
3329                 uint16_t handle;
3330                 bt_uuid_t uuid;
3331
3332                 if (!l2cap_frame_get_le16((void *)frame, &handle)) {
3333                         print_text(COLOR_ERROR, "    Handle: invalid size");
3334                         return;
3335                 }
3336
3337                 if (frame->size < 16) {
3338                         print_text(COLOR_ERROR, "    UUID: invalid size");
3339                         return;
3340                 }
3341
3342                 print_field("Handle: 0x%4.4x", handle);
3343                 print_uuid("UUID", frame->data, 16);
3344
3345                 bt_uuid_from_data(&uuid, frame->data, 16);
3346
3347                 if (!l2cap_frame_pull((void *)frame, frame, 16))
3348                         return;
3349
3350                 insert_desc(frame, handle, &uuid, true);
3351         }
3352 }
3353
3354 static void att_find_info_rsp(const struct l2cap_frame *frame)
3355 {
3356         uint8_t format;
3357
3358         if (!l2cap_frame_get_u8((void *)frame, &format)) {
3359                 print_text(COLOR_ERROR, "    Format: invalid size");
3360                 goto done;
3361         }
3362
3363         print_field("Format: %s (0x%2.2x)", att_format_str(format), format);
3364
3365         switch (format) {
3366         case 0x01:
3367                 att_find_info_rsp_16(frame);
3368                 break;
3369         case 0x02:
3370                 att_find_info_rsp_128(frame);
3371                 break;
3372         }
3373
3374 done:
3375         if (frame->size)
3376                 packet_hexdump(frame->data, frame->size);
3377 }
3378
3379 static void att_find_by_type_val_req(const struct l2cap_frame *frame)
3380 {
3381         uint16_t type;
3382
3383         print_handle_range("Handle range", frame->data);
3384
3385         type = get_le16(frame->data + 4);
3386         print_attribute_info(type, frame->data + 6, frame->size - 6);
3387 }
3388
3389 static void att_find_by_type_val_rsp(const struct l2cap_frame *frame)
3390 {
3391         const uint8_t *ptr = frame->data;
3392         uint16_t len = frame->size;
3393
3394         while (len >= 4) {
3395                 print_handle_range("Handle range", ptr);
3396                 ptr += 4;
3397                 len -= 4;
3398         }
3399
3400         packet_hexdump(ptr, len);
3401 }
3402
3403 static struct gatt_db_attribute *get_attribute(const struct l2cap_frame *frame,
3404                                                 uint16_t handle, bool rsp)
3405 {
3406         struct gatt_db *db;
3407
3408         db = get_db(frame, rsp);
3409         if (!db)
3410                 return NULL;
3411
3412         return gatt_db_get_attribute(db, handle);
3413 }
3414
3415 static void queue_read(const struct l2cap_frame *frame, bt_uuid_t *uuid,
3416                                         uint16_t handle)
3417 {
3418         struct packet_conn_data *conn;
3419         struct att_conn_data *data;
3420         struct att_read *read;
3421         struct gatt_db_attribute *attr = NULL;
3422         struct gatt_handler *handler;
3423
3424         if (handle) {
3425                 attr = get_attribute(frame, handle, false);
3426                 if (!attr)
3427                         return;
3428         }
3429
3430         handler = attr ? get_handler(attr) : get_handler_uuid(uuid);
3431         if (!handler || !handler->read)
3432                 return;
3433
3434         conn = packet_get_conn_data(frame->handle);
3435         data = att_get_conn_data(conn);
3436         if (!data)
3437                 return;
3438
3439         if (!data->reads)
3440                 data->reads = queue_new();
3441
3442         read = new0(struct att_read, 1);
3443         read->attr = attr;
3444         read->in = frame->in;
3445         read->chan = frame->chan;
3446         read->func = handler->read;
3447
3448         queue_push_tail(data->reads, read);
3449 }
3450
3451 static void att_read_type_req(const struct l2cap_frame *frame)
3452 {
3453         bt_uuid_t uuid;
3454
3455         print_handle_range("Handle range", frame->data);
3456         print_uuid("Attribute type", frame->data + 4, frame->size - 4);
3457
3458         if (bt_uuid_from_data(&uuid, frame->data + 4, frame->size - 4))
3459                 return;
3460
3461         queue_read(frame, &uuid, 0x0000);
3462 }
3463
3464 static void att_read_type_rsp(const struct l2cap_frame *frame)
3465 {
3466         uint8_t len;
3467
3468         if (!l2cap_frame_get_u8((void *)frame, &len)) {
3469                 print_text(COLOR_ERROR, "invalid size");
3470                 return;
3471         }
3472
3473         print_field("Attribute data length: %d", len);
3474         print_data_list("Attribute data list", len, frame);
3475 }
3476
3477 static void print_handle(const struct l2cap_frame *frame, uint16_t handle,
3478                                                                 bool rsp)
3479 {
3480         struct gatt_db_attribute *attr;
3481
3482         attr = get_attribute(frame, handle, rsp);
3483         if (!attr) {
3484                 print_field("Handle: 0x%4.4x", handle);
3485                 return;
3486         }
3487
3488         print_attribute(attr);
3489 }
3490
3491 static void att_read_req(const struct l2cap_frame *frame)
3492 {
3493         const struct bt_l2cap_att_read_req *pdu = frame->data;
3494         uint16_t handle;
3495
3496         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
3497
3498         handle = le16_to_cpu(pdu->handle);
3499         print_handle(frame, handle, false);
3500
3501         queue_read(frame, NULL, handle);
3502 }
3503
3504 static void att_read_rsp(const struct l2cap_frame *frame)
3505 {
3506         struct att_read *read;
3507
3508         read = att_get_read(frame);
3509         if (!read)
3510                 return;
3511
3512         print_attribute(read->attr);
3513         print_hex_field("Value", frame->data, frame->size);
3514
3515         read->func(frame);
3516
3517         free(read);
3518 }
3519
3520 static void att_read_blob_req(const struct l2cap_frame *frame)
3521 {
3522         print_handle(frame, get_le16(frame->data), false);
3523         print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
3524 }
3525
3526 static void att_read_blob_rsp(const struct l2cap_frame *frame)
3527 {
3528         packet_hexdump(frame->data, frame->size);
3529 }
3530
3531 static void att_read_multiple_req(const struct l2cap_frame *frame)
3532 {
3533         int i, count;
3534
3535         count = frame->size / 2;
3536
3537         for (i = 0; i < count; i++)
3538                 print_handle(frame, get_le16(frame->data + (i * 2)), false);
3539 }
3540
3541 static void att_read_group_type_req(const struct l2cap_frame *frame)
3542 {
3543         bt_uuid_t uuid;
3544
3545         print_handle_range("Handle range", frame->data);
3546         print_uuid("Attribute group type", frame->data + 4, frame->size - 4);
3547
3548         if (bt_uuid_from_data(&uuid, frame->data + 4, frame->size - 4))
3549                 return;
3550
3551         queue_read(frame, &uuid, 0x0000);
3552 }
3553
3554 static void print_group_list(const char *label, uint8_t length,
3555                                         const struct l2cap_frame *frame)
3556 {
3557         struct att_read *read;
3558         uint8_t count;
3559
3560         if (length == 0)
3561                 return;
3562
3563         read = att_get_read(frame);
3564
3565         count = frame->size / length;
3566
3567         print_field("%s: %u entr%s", label, count, count == 1 ? "y" : "ies");
3568
3569         while (frame->size >= length) {
3570                 print_handle_range("Handle range", frame->data);
3571                 print_uuid("UUID", frame->data + 4, length - 4);
3572
3573                 if (read) {
3574                         struct l2cap_frame f;
3575
3576                         l2cap_frame_clone_size(&f, frame, length);
3577
3578                         read->func(&f);
3579                 }
3580
3581                 if (!l2cap_frame_pull((void *)frame, frame, length))
3582                         break;
3583         }
3584
3585         packet_hexdump(frame->data, frame->size);
3586         free(read);
3587 }
3588
3589 static void att_read_group_type_rsp(const struct l2cap_frame *frame)
3590 {
3591         const struct bt_l2cap_att_read_group_type_rsp *pdu = frame->data;
3592
3593         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
3594
3595         print_field("Attribute data length: %d", pdu->length);
3596         print_group_list("Attribute group list", pdu->length, frame);
3597 }
3598
3599 static void print_write(const struct l2cap_frame *frame, uint16_t handle,
3600                                                         size_t len)
3601 {
3602         struct gatt_db_attribute *attr;
3603         struct gatt_handler *handler;
3604
3605         print_handle(frame, handle, false);
3606
3607         if (len > frame->size) {
3608                 print_text(COLOR_ERROR, "invalid size");
3609                 return;
3610         }
3611
3612         print_hex_field("  Data", frame->data, len);
3613
3614         attr = get_attribute(frame, handle, false);
3615         if (!attr)
3616                 return;
3617
3618         handler = get_handler(attr);
3619         if (!handler || !handler->write)
3620                 return;
3621
3622         handler->write(frame);
3623 }
3624
3625 static void att_write_req(const struct l2cap_frame *frame)
3626 {
3627         uint16_t handle;
3628
3629         if (!l2cap_frame_get_le16((void *)frame, &handle)) {
3630                 print_text(COLOR_ERROR, "invalid size");
3631                 return;
3632         }
3633
3634         print_write(frame, handle, frame->size);
3635 }
3636
3637 static void att_write_rsp(const struct l2cap_frame *frame)
3638 {
3639 }
3640
3641 static void att_prepare_write_req(const struct l2cap_frame *frame)
3642 {
3643         print_handle(frame, get_le16(frame->data), false);
3644         print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
3645         print_hex_field("  Data", frame->data + 4, frame->size - 4);
3646 }
3647
3648 static void att_prepare_write_rsp(const struct l2cap_frame *frame)
3649 {
3650         print_handle(frame, get_le16(frame->data), true);
3651         print_field("Offset: 0x%4.4x", get_le16(frame->data + 2));
3652         print_hex_field("  Data", frame->data + 4, frame->size - 4);
3653 }
3654
3655 static void att_execute_write_req(const struct l2cap_frame *frame)
3656 {
3657         uint8_t flags = *(uint8_t *) frame->data;
3658         const char *flags_str;
3659
3660         switch (flags) {
3661         case 0x00:
3662                 flags_str = "Cancel all prepared writes";
3663                 break;
3664         case 0x01:
3665                 flags_str = "Immediately write all pending values";
3666                 break;
3667         default:
3668                 flags_str = "Unknown";
3669                 break;
3670         }
3671
3672         print_field("Flags: %s (0x%02x)", flags_str, flags);
3673 }
3674
3675 static void print_notify(const struct l2cap_frame *frame, uint16_t handle,
3676                                                                 size_t len)
3677 {
3678         struct gatt_db_attribute *attr;
3679         struct gatt_handler *handler;
3680         struct l2cap_frame clone;
3681
3682         print_handle(frame, handle, true);
3683         print_hex_field("  Data", frame->data, len);
3684
3685         if (len > frame->size) {
3686                 print_text(COLOR_ERROR, "invalid size");
3687                 return;
3688         }
3689
3690         attr = get_attribute(frame, handle, true);
3691         if (!attr)
3692                 return;
3693
3694         handler = get_handler(attr);
3695         if (!handler)
3696                 return;
3697
3698         /* Use a clone if the callback is not expected to parse the whole
3699          * frame.
3700          */
3701         if (len != frame->size) {
3702                 l2cap_frame_clone(&clone, frame);
3703                 clone.size = len;
3704                 frame = &clone;
3705         }
3706
3707         handler->notify(frame);
3708 }
3709
3710 static void att_handle_value_notify(const struct l2cap_frame *frame)
3711 {
3712         uint16_t handle;
3713         const struct bt_l2cap_att_handle_value_notify *pdu = frame->data;
3714
3715         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
3716
3717         handle = le16_to_cpu(pdu->handle);
3718         print_notify(frame, handle, frame->size);
3719 }
3720
3721 static void att_handle_value_ind(const struct l2cap_frame *frame)
3722 {
3723         const struct bt_l2cap_att_handle_value_ind *pdu = frame->data;
3724
3725         l2cap_frame_pull((void *)frame, frame, sizeof(*pdu));
3726
3727         print_notify(frame, le16_to_cpu(pdu->handle), frame->size);
3728 }
3729
3730 static void att_handle_value_conf(const struct l2cap_frame *frame)
3731 {
3732 }
3733
3734 static void att_multiple_vl_rsp(const struct l2cap_frame *frame)
3735 {
3736         struct l2cap_frame *f = (void *) frame;
3737
3738         while (frame->size) {
3739                 uint16_t handle;
3740                 uint16_t len;
3741
3742                 if (!l2cap_frame_get_le16(f, &handle))
3743                         return;
3744
3745                 if (!l2cap_frame_get_le16(f, &len))
3746                         return;
3747
3748                 print_field("Length: 0x%4.4x", len);
3749
3750                 print_notify(frame, handle, len);
3751
3752                 l2cap_frame_pull(f, f, len);
3753         }
3754 }
3755
3756 static void att_write_command(const struct l2cap_frame *frame)
3757 {
3758         uint16_t handle;
3759
3760         if (!l2cap_frame_get_le16((void *)frame, &handle)) {
3761                 print_text(COLOR_ERROR, "invalid size");
3762                 return;
3763         }
3764
3765         print_write(frame, handle, frame->size);
3766 }
3767
3768 static void att_signed_write_command(const struct l2cap_frame *frame)
3769 {
3770         uint16_t handle;
3771
3772         if (!l2cap_frame_get_le16((void *)frame, &handle)) {
3773                 print_text(COLOR_ERROR, "invalid size");
3774                 return;
3775         }
3776
3777         print_write(frame, handle, frame->size - 12);
3778         print_hex_field("  Signature", frame->data + frame->size - 12, 12);
3779 }
3780
3781 struct att_opcode_data {
3782         uint8_t opcode;
3783         const char *str;
3784         void (*func) (const struct l2cap_frame *frame);
3785         uint8_t size;
3786         bool fixed;
3787 };
3788
3789 static const struct att_opcode_data att_opcode_table[] = {
3790         { 0x01, "Error Response",
3791                         att_error_response, 4, true },
3792         { 0x02, "Exchange MTU Request",
3793                         att_exchange_mtu_req, 2, true },
3794         { 0x03, "Exchange MTU Response",
3795                         att_exchange_mtu_rsp, 2, true },
3796         { 0x04, "Find Information Request",
3797                         att_find_info_req, 4, true },
3798         { 0x05, "Find Information Response",
3799                         att_find_info_rsp, 5, false },
3800         { 0x06, "Find By Type Value Request",
3801                         att_find_by_type_val_req, 6, false },
3802         { 0x07, "Find By Type Value Response",
3803                         att_find_by_type_val_rsp, 4, false },
3804         { 0x08, "Read By Type Request",
3805                         att_read_type_req, 6, false },
3806         { 0x09, "Read By Type Response",
3807                         att_read_type_rsp, 3, false },
3808         { 0x0a, "Read Request",
3809                         att_read_req, 2, true },
3810         { 0x0b, "Read Response",
3811                         att_read_rsp, 0, false },
3812         { 0x0c, "Read Blob Request",
3813                         att_read_blob_req, 4, true },
3814         { 0x0d, "Read Blob Response",
3815                         att_read_blob_rsp, 0, false },
3816         { 0x0e, "Read Multiple Request",
3817                         att_read_multiple_req, 4, false },
3818         { 0x0f, "Read Multiple Response"        },
3819         { 0x10, "Read By Group Type Request",
3820                         att_read_group_type_req, 6, false },
3821         { 0x11, "Read By Group Type Response",
3822                         att_read_group_type_rsp, 4, false },
3823         { 0x12, "Write Request" ,
3824                         att_write_req, 2, false },
3825         { 0x13, "Write Response",
3826                         att_write_rsp, 0, true  },
3827         { 0x16, "Prepare Write Request",
3828                         att_prepare_write_req, 4, false },
3829         { 0x17, "Prepare Write Response",
3830                         att_prepare_write_rsp, 4, false },
3831         { 0x18, "Execute Write Request",
3832                         att_execute_write_req, 1, true },
3833         { 0x19, "Execute Write Response"        },
3834         { 0x1b, "Handle Value Notification",
3835                         att_handle_value_notify, 2, false },
3836         { 0x1d, "Handle Value Indication",
3837                         att_handle_value_ind, 2, false },
3838         { 0x1e, "Handle Value Confirmation",
3839                         att_handle_value_conf, 0, true },
3840         { 0x20, "Read Multiple Request Variable Length",
3841                         att_read_multiple_req, 4, false },
3842         { 0x21, "Read Multiple Response Variable Length",
3843                         att_multiple_vl_rsp, 4, false },
3844         { 0x23, "Handle Multiple Value Notification",
3845                         att_multiple_vl_rsp, 4, false },
3846         { 0x52, "Write Command",
3847                         att_write_command, 2, false },
3848         { 0xd2, "Signed Write Command", att_signed_write_command, 14, false },
3849         { }
3850 };
3851
3852 static const char *att_opcode_to_str(uint8_t opcode)
3853 {
3854         int i;
3855
3856         for (i = 0; att_opcode_table[i].str; i++) {
3857                 if (att_opcode_table[i].opcode == opcode)
3858                         return att_opcode_table[i].str;
3859         }
3860
3861         return "Unknown";
3862 }
3863
3864 void att_packet(uint16_t index, bool in, uint16_t handle, uint16_t cid,
3865                                         const void *data, uint16_t size)
3866 {
3867         struct l2cap_frame frame;
3868         uint8_t opcode = *((const uint8_t *) data);
3869         const struct att_opcode_data *opcode_data = NULL;
3870         const char *opcode_color, *opcode_str;
3871         int i;
3872
3873         if (size < 1) {
3874                 print_text(COLOR_ERROR, "malformed attribute packet");
3875                 packet_hexdump(data, size);
3876                 return;
3877         }
3878
3879         for (i = 0; att_opcode_table[i].str; i++) {
3880                 if (att_opcode_table[i].opcode == opcode) {
3881                         opcode_data = &att_opcode_table[i];
3882                         break;
3883                 }
3884         }
3885
3886         if (opcode_data) {
3887                 if (opcode_data->func) {
3888                         if (in)
3889                                 opcode_color = COLOR_MAGENTA;
3890                         else
3891                                 opcode_color = COLOR_BLUE;
3892                 } else
3893                         opcode_color = COLOR_WHITE_BG;
3894                 opcode_str = opcode_data->str;
3895         } else {
3896                 opcode_color = COLOR_WHITE_BG;
3897                 opcode_str = "Unknown";
3898         }
3899
3900         print_indent(6, opcode_color, "ATT: ", opcode_str, COLOR_OFF,
3901                                 " (0x%2.2x) len %d", opcode, size - 1);
3902
3903         if (!opcode_data || !opcode_data->func) {
3904                 packet_hexdump(data + 1, size - 1);
3905                 return;
3906         }
3907
3908         if (opcode_data->fixed) {
3909                 if (size - 1 != opcode_data->size) {
3910                         print_text(COLOR_ERROR, "invalid size");
3911                         packet_hexdump(data + 1, size - 1);
3912                         return;
3913                 }
3914         } else {
3915                 if (size - 1 < opcode_data->size) {
3916                         print_text(COLOR_ERROR, "too short packet");
3917                         packet_hexdump(data + 1, size - 1);
3918                         return;
3919                 }
3920         }
3921
3922         l2cap_frame_init(&frame, index, in, handle, 0, cid, 0,
3923                                                 data + 1, size - 1);
3924         opcode_data->func(&frame);
3925 }