monitor/a2dp.c

   1 // SPDX-License-Identifier: LGPL-2.1-or-later
   2 /*
   3  *
   4  *  BlueZ - Bluetooth protocol stack for Linux
   5  *
   6  *  Copyright (C) 2015  Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
   7  *  Copyright (C) 2018  Pali Rohár <pali.rohar@gmail.com>
   8  *
   9  *
  10  */
  11
  12 #ifdef HAVE_CONFIG_H
  13 #include <config.h>
  14 #endif
  15
  16 #include <stdio.h>
  17 #include <stdlib.h>
  18 #include <string.h>
  19
  20 #include "lib/bluetooth.h"
  21
  22 #include "src/shared/util.h"
  23 #include "bt.h"
  24 #include "packet.h"
  25 #include "display.h"
  26 #include "l2cap.h"
  27 #include "a2dp.h"
  28
  29 #define BASE_INDENT     4
  30
  31 /* Codec Types */
  32 #define A2DP_CODEC_SBC          0x00
  33 #define A2DP_CODEC_MPEG12       0x01
  34 #define A2DP_CODEC_MPEG24       0x02
  35 #define A2DP_CODEC_ATRAC        0x04
  36 #define A2DP_CODEC_VENDOR       0xff
  37
  38 /* Vendor Specific A2DP Codecs */
  39 #define APTX_VENDOR_ID          0x0000004f
  40 #define APTX_CODEC_ID           0x0001
  41 #define FASTSTREAM_VENDOR_ID    0x0000000a
  42 #define FASTSTREAM_CODEC_ID     0x0001
  43 #define APTX_LL_VENDOR_ID       0x0000000a
  44 #define APTX_LL_CODEC_ID        0x0002
  45 #define APTX_HD_VENDOR_ID       0x000000D7
  46 #define APTX_HD_CODEC_ID        0x0024
  47 #define LDAC_VENDOR_ID          0x0000012d
  48 #define LDAC_CODEC_ID           0x00aa
  49
  50 struct bit_desc {
  51         uint8_t bit_num;
  52         const char *str;
  53 };
  54
  55 static const struct bit_desc sbc_frequency_table[] = {
  56         {  7, "16000" },
  57         {  6, "32000" },
  58         {  5, "44100" },
  59         {  4, "48000" },
  60         { }
  61 };
  62
  63 static const struct bit_desc sbc_channel_mode_table[] = {
  64         {  3, "Mono" },
  65         {  2, "Dual Channel" },
  66         {  1, "Stereo" },
  67         {  0, "Joint Stereo" },
  68         { }
  69 };
  70
  71 static const struct bit_desc sbc_blocklen_table[] = {
  72         {  7, "4" },
  73         {  6, "8" },
  74         {  5, "12" },
  75         {  4, "16" },
  76         { }
  77 };
  78
  79 static const struct bit_desc sbc_subbands_table[] = {
  80         {  3, "4" },
  81         {  2, "8" },
  82         { }
  83 };
  84
  85 static const struct bit_desc sbc_allocation_table[] = {
  86         {  1, "SNR" },
  87         {  0, "Loudness" },
  88         { }
  89 };
  90
  91 static const struct bit_desc mpeg12_layer_table[] = {
  92         {  7, "Layer I (mp1)" },
  93         {  6, "Layer II (mp2)" },
  94         {  5, "Layer III (mp3)" },
  95         { }
  96 };
  97
  98 static const struct bit_desc mpeg12_channel_mode_table[] = {
  99         {  3, "Mono" },
 100         {  2, "Dual Channel" },
 101         {  1, "Stereo" },
 102         {  0, "Joint Stereo" },
 103         { }
 104 };
 105
 106 static const struct bit_desc mpeg12_frequency_table[] = {
 107         {  5, "16000" },
 108         {  4, "22050" },
 109         {  3, "24000" },
 110         {  2, "32000" },
 111         {  1, "44100" },
 112         {  0, "48000" },
 113         { }
 114 };
 115
 116 static const struct bit_desc mpeg12_bitrate_table[] = {
 117         { 14, "1110" },
 118         { 13, "1101" },
 119         { 12, "1100" },
 120         { 11, "1011" },
 121         { 10, "1010" },
 122         {  9, "1001" },
 123         {  8, "1000" },
 124         {  7, "0111" },
 125         {  6, "0110" },
 126         {  5, "0101" },
 127         {  4, "0100" },
 128         {  3, "0011" },
 129         {  2, "0010" },
 130         {  1, "0001" },
 131         {  0, "0000" },
 132         { }
 133 };
 134
 135 static const struct bit_desc aac_object_type_table[] = {
 136         {  7, "MPEG-2 AAC LC" },
 137         {  6, "MPEG-4 AAC LC" },
 138         {  5, "MPEG-4 AAC LTP" },
 139         {  4, "MPEG-4 AAC scalable" },
 140         {  3, "RFA (b3)" },
 141         {  2, "RFA (b2)" },
 142         {  1, "RFA (b1)" },
 143         {  0, "RFA (b0)" },
 144         { }
 145 };
 146
 147 static const struct bit_desc aac_frequency_table[] = {
 148         { 15, "8000" },
 149         { 14, "11025" },
 150         { 13, "12000" },
 151         { 12, "16000" },
 152         { 11, "22050" },
 153         { 10, "24000" },
 154         {  9, "32000" },
 155         {  8, "44100" },
 156         {  7, "48000" },
 157         {  6, "64000" },
 158         {  5, "88200" },
 159         {  4, "96000" },
 160         { }
 161 };
 162
 163 static const struct bit_desc aac_channels_table[] = {
 164         {  3, "1" },
 165         {  2, "2" },
 166         { }
 167 };
 168
 169 static const struct bit_desc aptx_frequency_table[] = {
 170         {  7, "16000" },
 171         {  6, "32000" },
 172         {  5, "44100" },
 173         {  4, "48000" },
 174         { }
 175 };
 176
 177 static const struct bit_desc aptx_channel_mode_table[] = {
 178         {  0, "Mono" },
 179         {  1, "Stereo" },
 180         { }
 181 };
 182
 183 static const struct bit_desc faststream_direction_table[] = {
 184         {  0, "Sink" },
 185         {  1, "Source" },
 186         { }
 187 };
 188
 189 static const struct bit_desc faststream_sink_frequency_table[] = {
 190         /* in config buffer, there may be more frequency bits
 191          * and 48kHz takes precedence over 41kHz
 192          */
 193         {  0, "48000" },
 194         {  1, "44100" },
 195         { }
 196 };
 197
 198 static const struct bit_desc faststream_source_frequency_table[] = {
 199         {  5, "16000" },
 200         { }
 201 };
 202
 203 static void print_value_bits(uint8_t indent, uint32_t value,
 204                                                 const struct bit_desc *table)
 205 {
 206         int i;
 207
 208         for (i = 0; table[i].str; i++) {
 209                 if (value & (1 << table[i].bit_num))
 210                         print_field("%*c%s", indent + 2, ' ', table[i].str);
 211         }
 212 }
 213
 214 static const char *find_value_bit(uint32_t value,
 215                                                 const struct bit_desc *table)
 216 {
 217         int i;
 218
 219         for (i = 0; table[i].str; i++) {
 220                 if (value & (1 << table[i].bit_num))
 221                         return table[i].str;
 222         }
 223
 224         return "Unknown";
 225 }
 226
 227 struct vndcodec {
 228         uint32_t vendor_id;
 229         uint16_t codec_id;
 230         char *codec_name;
 231         bool (*codec_vendor_cap)(uint8_t losc, struct l2cap_frame *frame);
 232         bool (*codec_vendor_cfg)(uint8_t losc, struct l2cap_frame *frame);
 233 };
 234
 235 static bool codec_vendor_aptx_cap(uint8_t losc, struct l2cap_frame *frame);
 236 static bool codec_vendor_aptx_cfg(uint8_t losc, struct l2cap_frame *frame);
 237 static bool codec_vendor_faststream_cap(uint8_t losc,
 238                                         struct l2cap_frame *frame);
 239 static bool codec_vendor_faststream_cfg(uint8_t losc,
 240                                         struct l2cap_frame *frame);
 241 static bool codec_vendor_aptx_ll_cap(uint8_t losc, struct l2cap_frame *frame);
 242 static bool codec_vendor_aptx_ll_cfg(uint8_t losc, struct l2cap_frame *frame);
 243 static bool codec_vendor_aptx_hd_cap(uint8_t losc, struct l2cap_frame *frame);
 244 static bool codec_vendor_aptx_hd_cfg(uint8_t losc, struct l2cap_frame *frame);
 245 static bool codec_vendor_ldac(uint8_t losc, struct l2cap_frame *frame);
 246
 247 static const struct vndcodec vndcodecs[] = {
 248         { APTX_VENDOR_ID, APTX_CODEC_ID, "aptX",
 249           codec_vendor_aptx_cap, codec_vendor_aptx_cfg },
 250         { FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID, "FastStream",
 251           codec_vendor_faststream_cap, codec_vendor_faststream_cfg },
 252         { APTX_LL_VENDOR_ID, APTX_LL_CODEC_ID, "aptX Low Latency",
 253           codec_vendor_aptx_ll_cap, codec_vendor_aptx_ll_cfg },
 254         { APTX_HD_VENDOR_ID, APTX_HD_CODEC_ID, "aptX HD",
 255           codec_vendor_aptx_hd_cap, codec_vendor_aptx_hd_cfg },
 256         { LDAC_VENDOR_ID, LDAC_CODEC_ID, "LDAC",
 257           codec_vendor_ldac, codec_vendor_ldac },
 258         { }
 259 };
 260
 261 static const char *vndcodec2str(uint32_t vendor_id, uint16_t codec_id)
 262 {
 263         size_t i;
 264
 265         for (i = 0; i < sizeof(vndcodecs)/sizeof(*vndcodecs); i++) {
 266                 if (vndcodecs[i].vendor_id == vendor_id &&
 267                     vndcodecs[i].codec_id == codec_id)
 268                         return vndcodecs[i].codec_name;
 269         }
 270
 271         return "Unknown";
 272 }
 273
 274 static bool codec_sbc_cap(uint8_t losc, struct l2cap_frame *frame)
 275 {
 276         uint8_t cap = 0;
 277
 278         if (losc != 4)
 279                 return false;
 280
 281         l2cap_frame_get_u8(frame, &cap);
 282
 283         print_field("%*cFrequency: 0x%02x", BASE_INDENT, ' ', cap & 0xf0);
 284         print_value_bits(BASE_INDENT, cap & 0xf0, sbc_frequency_table);
 285
 286         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT, ' ', cap & 0x0f);
 287         print_value_bits(BASE_INDENT, cap & 0x0f, sbc_channel_mode_table);
 288
 289         l2cap_frame_get_u8(frame, &cap);
 290
 291         print_field("%*cBlock Length: 0x%02x", BASE_INDENT, ' ', cap & 0xf0);
 292         print_value_bits(BASE_INDENT, cap & 0xf0, sbc_blocklen_table);
 293
 294         print_field("%*cSubbands: 0x%02x", BASE_INDENT, ' ', cap & 0x0c);
 295         print_value_bits(BASE_INDENT, cap & 0x0c, sbc_subbands_table);
 296
 297         print_field("%*cAllocation Method: 0x%02x", BASE_INDENT, ' ',
 298                                                                 cap & 0x03);
 299         print_value_bits(BASE_INDENT, cap & 0x03, sbc_allocation_table);
 300
 301         l2cap_frame_get_u8(frame, &cap);
 302
 303         print_field("%*cMinimum Bitpool: %d", BASE_INDENT, ' ', cap);
 304
 305         l2cap_frame_get_u8(frame, &cap);
 306
 307         print_field("%*cMaximum Bitpool: %d", BASE_INDENT, ' ', cap);
 308
 309         return true;
 310 }
 311
 312 static bool codec_sbc_cfg(uint8_t losc, struct l2cap_frame *frame)
 313 {
 314         uint8_t cap = 0;
 315
 316         if (losc != 4)
 317                 return false;
 318
 319         l2cap_frame_get_u8(frame, &cap);
 320
 321         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT, ' ',
 322                         find_value_bit(cap & 0xf0, sbc_frequency_table),
 323                         cap & 0xf0);
 324
 325         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT, ' ',
 326                         find_value_bit(cap & 0x0f, sbc_channel_mode_table),
 327                         cap & 0x0f);
 328
 329         l2cap_frame_get_u8(frame, &cap);
 330
 331         print_field("%*cBlock Length: %s (0x%02x)", BASE_INDENT, ' ',
 332                         find_value_bit(cap & 0xf0, sbc_blocklen_table),
 333                         cap & 0xf0);
 334
 335         print_field("%*cSubbands: %s (0x%02x)", BASE_INDENT, ' ',
 336                         find_value_bit(cap & 0x0c, sbc_subbands_table),
 337                         cap & 0x0c);
 338
 339         print_field("%*cAllocation Method: %s (0x%02x)", BASE_INDENT, ' ',
 340                         find_value_bit(cap & 0x03, sbc_allocation_table),
 341                         cap & 0x03);
 342
 343         l2cap_frame_get_u8(frame, &cap);
 344
 345         print_field("%*cMinimum Bitpool: %d", BASE_INDENT, ' ', cap);
 346
 347         l2cap_frame_get_u8(frame, &cap);
 348
 349         print_field("%*cMaximum Bitpool: %d", BASE_INDENT, ' ', cap);
 350
 351         return true;
 352 }
 353
 354 static bool codec_mpeg12_cap(uint8_t losc, struct l2cap_frame *frame)
 355 {
 356         uint16_t cap = 0;
 357         uint8_t layer;
 358         uint8_t chan;
 359         uint8_t freq;
 360         uint16_t bitrate;
 361         bool crc, mpf, vbr;
 362
 363         if (losc != 4)
 364                 return false;
 365
 366         if (!l2cap_frame_get_be16(frame, &cap))
 367                 return false;
 368
 369         layer = (cap >> 8) & 0xe0;
 370         crc = cap & 0x1000;
 371         chan = (cap >> 8) & 0x0f;
 372         mpf = cap & 0x0040;
 373         freq = cap & 0x003f;
 374
 375         if (!l2cap_frame_get_be16(frame, &cap))
 376                 return false;
 377
 378         vbr = cap & 0x8000;
 379         bitrate = cap & 0x7fff;
 380
 381         print_field("%*cLayer: 0x%02x", BASE_INDENT, ' ', layer);
 382         print_value_bits(BASE_INDENT, layer, mpeg12_layer_table);
 383
 384         print_field("%*cCRC: %s", BASE_INDENT, ' ', crc ? "Yes" : "No");
 385
 386         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT, ' ', chan);
 387         print_value_bits(BASE_INDENT, chan, mpeg12_channel_mode_table);
 388
 389         print_field("%*cMedia Payload Format: %s", BASE_INDENT, ' ',
 390                                         mpf ? "RFC-2250 RFC-3119" : "RFC-2250");
 391
 392         print_field("%*cFrequency: 0x%02x", BASE_INDENT, ' ', freq);
 393         print_value_bits(BASE_INDENT, freq, mpeg12_frequency_table);
 394
 395         if (!vbr) {
 396                 print_field("%*cBitrate Index: 0x%04x", BASE_INDENT, ' ',
 397                                                                 bitrate);
 398                 print_value_bits(BASE_INDENT, freq, mpeg12_bitrate_table);
 399         }
 400
 401         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 402
 403         return true;
 404 }
 405
 406 static bool codec_mpeg12_cfg(uint8_t losc, struct l2cap_frame *frame)
 407 {
 408         uint16_t cap = 0;
 409         uint8_t layer;
 410         uint8_t chan;
 411         uint8_t freq;
 412         uint16_t bitrate;
 413         bool crc, mpf, vbr;
 414
 415         if (losc != 4)
 416                 return false;
 417
 418         if (!l2cap_frame_get_be16(frame, &cap))
 419                 return false;
 420
 421         layer = (cap >> 8) & 0xe0;
 422         crc = cap & 0x1000;
 423         chan = (cap >> 8) & 0x0f;
 424         mpf = cap & 0x0040;
 425         freq = cap & 0x003f;
 426
 427         if (!l2cap_frame_get_be16(frame, &cap))
 428                 return false;
 429
 430         vbr = cap & 0x8000;
 431         bitrate = cap & 0x7fff;
 432
 433         print_field("%*cLayer: %s (0x%02x)", BASE_INDENT, ' ',
 434                                 find_value_bit(layer, mpeg12_layer_table),
 435                                 layer);
 436
 437         print_field("%*cCRC: %s", BASE_INDENT, ' ', crc ? "Yes" : "No");
 438
 439         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT, ' ',
 440                                 find_value_bit(chan, mpeg12_channel_mode_table),
 441                                 chan);
 442
 443         print_field("%*cMedia Payload Format: %s", BASE_INDENT, ' ',
 444                                         mpf ? "RFC-2250 RFC-3119" : "RFC-2250");
 445
 446         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT, ' ',
 447                                 find_value_bit(freq, mpeg12_frequency_table),
 448                                 freq);
 449
 450         if (!vbr)
 451                 print_field("%*cBitrate Index: %s (0x%04x)", BASE_INDENT, ' ',
 452                                 find_value_bit(freq, mpeg12_bitrate_table),
 453                                 bitrate);
 454
 455         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 456
 457         return true;
 458 }
 459
 460 static bool codec_aac_cap(uint8_t losc, struct l2cap_frame *frame)
 461 {
 462         uint16_t cap = 0;
 463         uint8_t type;
 464         uint16_t freq;
 465         uint8_t chan;
 466         uint32_t bitrate;
 467         bool vbr;
 468
 469         if (losc != 6)
 470                 return false;
 471
 472         if (!l2cap_frame_get_be16(frame, &cap))
 473                 return false;
 474
 475         type = cap >> 8;
 476         freq = cap << 8;
 477
 478         if (!l2cap_frame_get_be16(frame, &cap))
 479                 return false;
 480
 481         freq |= (cap >> 8) & 0xf0;
 482         chan = (cap >> 8) & 0x0c;
 483         bitrate = (cap << 16) & 0x7f0000;
 484         vbr = cap & 0x0080;
 485
 486         if (!l2cap_frame_get_be16(frame, &cap))
 487                 return false;
 488
 489         bitrate |= cap;
 490
 491         print_field("%*cObject Type: 0x%02x", BASE_INDENT, ' ', type);
 492         print_value_bits(BASE_INDENT, type, aac_object_type_table);
 493
 494         print_field("%*cFrequency: 0x%02x", BASE_INDENT, ' ', freq);
 495         print_value_bits(BASE_INDENT, freq, aac_frequency_table);
 496
 497         print_field("%*cChannels: 0x%02x", BASE_INDENT, ' ', chan);
 498         print_value_bits(BASE_INDENT, chan, aac_channels_table);
 499
 500         print_field("%*cBitrate: %ubps", BASE_INDENT, ' ', bitrate);
 501         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 502
 503         return true;
 504 }
 505
 506 static bool codec_aac_cfg(uint8_t losc, struct l2cap_frame *frame)
 507 {
 508         uint16_t cap = 0;
 509         uint8_t type;
 510         uint16_t freq;
 511         uint8_t chan;
 512         uint32_t bitrate;
 513         bool vbr;
 514
 515         if (losc != 6)
 516                 return false;
 517
 518         if (!l2cap_frame_get_be16(frame, &cap))
 519                 return false;
 520
 521         type = cap >> 8;
 522         freq = cap << 8;
 523
 524         if (!l2cap_frame_get_be16(frame, &cap))
 525                 return false;
 526
 527         freq |= (cap >> 8) & 0xf0;
 528         chan = (cap >> 8) & 0x0c;
 529         bitrate = (cap << 16) & 0x7f0000;
 530         vbr = cap & 0x0080;
 531
 532         if (!l2cap_frame_get_be16(frame, &cap))
 533                 return false;
 534
 535         bitrate |= cap;
 536
 537         print_field("%*cObject Type: %s (0x%02x)", BASE_INDENT, ' ',
 538                         find_value_bit(type, aac_object_type_table), type);
 539
 540         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT, ' ',
 541                         find_value_bit(freq, aac_frequency_table), freq);
 542
 543         print_field("%*cChannels: %s (0x%02x)", BASE_INDENT, ' ',
 544                         find_value_bit(chan, aac_channels_table), chan);
 545
 546         print_field("%*cBitrate: %ubps", BASE_INDENT, ' ', bitrate);
 547         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 548
 549         return true;
 550 }
 551
 552 static bool codec_vendor_aptx_cap(uint8_t losc, struct l2cap_frame *frame)
 553 {
 554         uint8_t cap = 0;
 555
 556         if (losc != 1)
 557                 return false;
 558
 559         l2cap_frame_get_u8(frame, &cap);
 560
 561         print_field("%*cFrequency: 0x%02x", BASE_INDENT + 2, ' ', cap & 0xf0);
 562         print_value_bits(BASE_INDENT + 2, cap & 0xf0, aptx_frequency_table);
 563
 564         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT + 2, ' ',
 565                                                                 cap & 0x0f);
 566         print_value_bits(BASE_INDENT + 2, cap & 0x0f, aptx_channel_mode_table);
 567
 568         return true;
 569 }
 570
 571 static bool codec_vendor_faststream_cap(uint8_t losc, struct l2cap_frame *frame)
 572 {
 573         uint8_t cap = 0;
 574
 575         if (losc != 2)
 576                 return false;
 577
 578         l2cap_frame_get_u8(frame, &cap);
 579
 580         print_field("%*cDirection: 0x%02x", BASE_INDENT + 2, ' ', cap);
 581         print_value_bits(BASE_INDENT + 2, cap, faststream_direction_table);
 582
 583         l2cap_frame_get_u8(frame, &cap);
 584
 585         print_field("%*cSink Frequency: 0x%02x", BASE_INDENT + 2, ' ',
 586                         cap & 0x0f);
 587         print_value_bits(BASE_INDENT + 2, cap & 0x0f,
 588                         faststream_sink_frequency_table);
 589
 590         print_field("%*cSource Frequency: 0x%02x", BASE_INDENT + 2, ' ',
 591                         cap & 0xf0);
 592         print_value_bits(BASE_INDENT + 2, cap & 0xf0,
 593                         faststream_source_frequency_table);
 594
 595         return true;
 596 }
 597
 598 static bool codec_vendor_aptx_ll_cap(uint8_t losc, struct l2cap_frame *frame)
 599 {
 600         uint8_t cap = 0;
 601         uint16_t level = 0;
 602
 603         if (losc != 2 && losc != 11)
 604                 return false;
 605
 606         l2cap_frame_get_u8(frame, &cap);
 607
 608         print_field("%*cFrequency: 0x%02x", BASE_INDENT + 2, ' ', cap & 0xf0);
 609         print_value_bits(BASE_INDENT + 2, cap & 0xf0, aptx_frequency_table);
 610
 611         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT + 2, ' ',
 612                                                                 cap & 0x0f);
 613         print_value_bits(BASE_INDENT + 2, cap & 0x0f, aptx_channel_mode_table);
 614
 615         l2cap_frame_get_u8(frame, &cap);
 616
 617         print_field("%*cBidirectional link: %s", BASE_INDENT, ' ',
 618                         (cap & 1) ? "Yes" : "No");
 619
 620         if ((cap & 2) && losc == 11) {
 621                 /* reserved */
 622                 l2cap_frame_get_u8(frame, &cap);
 623
 624                 l2cap_frame_get_le16(frame, &level);
 625                 print_field("%*cTarget codec buffer level: %u (0x%02x)",
 626                                 BASE_INDENT + 2, ' ', level, level);
 627
 628                 l2cap_frame_get_le16(frame, &level);
 629                 print_field("%*cInitial codec buffer level: %u (0x%02x)",
 630                                 BASE_INDENT + 2, ' ', level, level);
 631
 632                 l2cap_frame_get_u8(frame, &cap);
 633                 print_field("%*cSRA max rate: %g (0x%02x)",
 634                                 BASE_INDENT + 2, ' ', cap / 10000.0, cap);
 635
 636                 l2cap_frame_get_u8(frame, &cap);
 637                 print_field("%*cSRA averaging time: %us (0x%02x)",
 638                                 BASE_INDENT + 2, ' ', cap, cap);
 639
 640                 l2cap_frame_get_le16(frame, &level);
 641                 print_field("%*cGood working codec buffer level: %u (0x%02x)",
 642                                 BASE_INDENT + 2, ' ', level, level);
 643         }
 644
 645         return true;
 646 }
 647
 648 static bool codec_vendor_aptx_hd_cap(uint8_t losc, struct l2cap_frame *frame)
 649 {
 650         uint8_t cap = 0;
 651
 652         if (losc != 5)
 653                 return false;
 654
 655         l2cap_frame_get_u8(frame, &cap);
 656
 657         print_field("%*cFrequency: 0x%02x", BASE_INDENT + 2, ' ', cap & 0xf0);
 658         print_value_bits(BASE_INDENT + 2, cap & 0xf0, aptx_frequency_table);
 659
 660         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT + 2, ' ',
 661                                                                 cap & 0x0f);
 662         print_value_bits(BASE_INDENT + 2, cap & 0x0f, aptx_channel_mode_table);
 663
 664         /* reserved */
 665         l2cap_frame_get_u8(frame, &cap);
 666         l2cap_frame_get_u8(frame, &cap);
 667         l2cap_frame_get_u8(frame, &cap);
 668         l2cap_frame_get_u8(frame, &cap);
 669
 670         return true;
 671 }
 672
 673 static bool codec_vendor_ldac(uint8_t losc, struct l2cap_frame *frame)
 674 {
 675         uint16_t cap = 0;
 676
 677         if (losc != 2)
 678                 return false;
 679
 680         l2cap_frame_get_le16(frame, &cap);
 681
 682         print_field("%*cUnknown: 0x%04x", BASE_INDENT + 2, ' ', cap);
 683
 684         return true;
 685 }
 686
 687 static bool codec_vendor_cap(uint8_t losc, struct l2cap_frame *frame)
 688 {
 689         uint32_t vendor_id = 0;
 690         uint16_t codec_id = 0;
 691         size_t i;
 692
 693         if (losc < 6)
 694                 return false;
 695
 696         l2cap_frame_get_le32(frame, &vendor_id);
 697         l2cap_frame_get_le16(frame, &codec_id);
 698
 699         losc -= 6;
 700
 701         print_field("%*cVendor ID: %s (0x%08x)", BASE_INDENT, ' ',
 702                                         bt_compidtostr(vendor_id),  vendor_id);
 703
 704         print_field("%*cVendor Specific Codec ID: %s (0x%04x)", BASE_INDENT,
 705                         ' ', vndcodec2str(vendor_id, codec_id), codec_id);
 706
 707         for (i = 0; i < sizeof(vndcodecs)/sizeof(*vndcodecs); i++) {
 708                 if (vndcodecs[i].vendor_id == vendor_id &&
 709                     vndcodecs[i].codec_id == codec_id)
 710                         return vndcodecs[i].codec_vendor_cap(losc, frame);
 711         }
 712
 713         packet_hexdump(frame->data, losc);
 714         l2cap_frame_pull(frame, frame, losc);
 715
 716         return true;
 717 }
 718
 719 static bool codec_vendor_aptx_cfg(uint8_t losc, struct l2cap_frame *frame)
 720 {
 721         uint8_t cap = 0;
 722
 723         if (losc != 1)
 724                 return false;
 725
 726         l2cap_frame_get_u8(frame, &cap);
 727
 728         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT + 2, ' ',
 729                         find_value_bit(cap & 0xf0, aptx_frequency_table),
 730                         cap & 0xf0);
 731
 732         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT + 2, ' ',
 733                         find_value_bit(cap & 0x0f, aptx_channel_mode_table),
 734                         cap & 0x0f);
 735
 736         return true;
 737 }
 738
 739 static bool codec_vendor_faststream_cfg(uint8_t losc, struct l2cap_frame *frame)
 740 {
 741         uint8_t cap = 0;
 742
 743         if (losc != 2)
 744                 return false;
 745
 746         l2cap_frame_get_u8(frame, &cap);
 747
 748         /* FastStream codec is bi-directional */
 749         print_field("%*cDirection: 0x%02x", BASE_INDENT + 2, ' ', cap);
 750         print_value_bits(BASE_INDENT + 2, cap, faststream_direction_table);
 751
 752         l2cap_frame_get_u8(frame, &cap);
 753
 754         print_field("%*cSink Frequency: %s (0x%02x)", BASE_INDENT + 2, ' ',
 755                         find_value_bit(cap & 0x0f,
 756                                 faststream_sink_frequency_table),
 757                         cap & 0x0f);
 758
 759         print_field("%*cSource Frequency: %s (0x%02x)", BASE_INDENT + 2, ' ',
 760                         find_value_bit(cap & 0xf0,
 761                                 faststream_source_frequency_table),
 762                         cap & 0xf0);
 763
 764         return true;
 765 }
 766
 767 static bool codec_vendor_aptx_ll_cfg(uint8_t losc, struct l2cap_frame *frame)
 768 {
 769         uint8_t cap = 0;
 770         uint16_t level = 0;
 771
 772         if (losc != 2 && losc != 11)
 773                 return false;
 774
 775         l2cap_frame_get_u8(frame, &cap);
 776
 777         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT + 2, ' ',
 778                         find_value_bit(cap & 0xf0, aptx_frequency_table),
 779                         cap & 0xf0);
 780
 781         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT + 2, ' ',
 782                         find_value_bit(cap & 0x0f, aptx_channel_mode_table),
 783                         cap & 0x0f);
 784
 785         l2cap_frame_get_u8(frame, &cap);
 786
 787         print_field("%*cBidirectional link: %s", BASE_INDENT, ' ',
 788                         (cap & 1) ? "Yes" : "No");
 789
 790         if ((cap & 2) && losc == 11) {
 791                 /* reserved */
 792                 l2cap_frame_get_u8(frame, &cap);
 793
 794                 l2cap_frame_get_le16(frame, &level);
 795                 print_field("%*cTarget codec buffer level: %u (0x%02x)",
 796                                 BASE_INDENT + 2, ' ', level, level);
 797
 798                 l2cap_frame_get_le16(frame, &level);
 799                 print_field("%*cInitial codec buffer level: %u (0x%02x)",
 800                                 BASE_INDENT + 2, ' ', level, level);
 801
 802                 l2cap_frame_get_u8(frame, &cap);
 803                 print_field("%*cSRA max rate: %g (0x%02x)",
 804                                 BASE_INDENT + 2, ' ', cap / 10000.0, cap);
 805
 806                 l2cap_frame_get_u8(frame, &cap);
 807                 print_field("%*cSRA averaging time: %us (0x%02x)",
 808                                 BASE_INDENT + 2, ' ', cap, cap);
 809
 810                 l2cap_frame_get_le16(frame, &level);
 811                 print_field("%*cGood working codec buffer level: %u (0x%02x)",
 812                                 BASE_INDENT + 2, ' ', level, level);
 813         }
 814
 815         return true;
 816 }
 817
 818 static bool codec_vendor_aptx_hd_cfg(uint8_t losc, struct l2cap_frame *frame)
 819 {
 820         uint8_t cap = 0;
 821
 822         if (losc != 5)
 823                 return false;
 824
 825         l2cap_frame_get_u8(frame, &cap);
 826
 827         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT + 2, ' ',
 828                         find_value_bit(cap & 0xf0, aptx_frequency_table),
 829                         cap & 0xf0);
 830
 831         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT + 2, ' ',
 832                         find_value_bit(cap & 0x0f, aptx_channel_mode_table),
 833                         cap & 0x0f);
 834
 835         /* reserved */
 836         l2cap_frame_get_u8(frame, &cap);
 837         l2cap_frame_get_u8(frame, &cap);
 838         l2cap_frame_get_u8(frame, &cap);
 839         l2cap_frame_get_u8(frame, &cap);
 840
 841         return true;
 842 }
 843
 844 static bool codec_vendor_cfg(uint8_t losc, struct l2cap_frame *frame)
 845 {
 846         uint32_t vendor_id = 0;
 847         uint16_t codec_id = 0;
 848         size_t i;
 849
 850         if (losc < 6)
 851                 return false;
 852
 853         l2cap_frame_get_le32(frame, &vendor_id);
 854         l2cap_frame_get_le16(frame, &codec_id);
 855
 856         losc -= 6;
 857
 858         print_field("%*cVendor ID: %s (0x%08x)", BASE_INDENT, ' ',
 859                                         bt_compidtostr(vendor_id),  vendor_id);
 860
 861         print_field("%*cVendor Specific Codec ID: %s (0x%04x)", BASE_INDENT,
 862                         ' ', vndcodec2str(vendor_id, codec_id), codec_id);
 863
 864         for (i = 0; i < sizeof(vndcodecs)/sizeof(*vndcodecs); i++) {
 865                 if (vndcodecs[i].vendor_id == vendor_id &&
 866                     vndcodecs[i].codec_id == codec_id)
 867                         return vndcodecs[i].codec_vendor_cfg(losc, frame);
 868         }
 869
 870         packet_hexdump(frame->data, losc);
 871         l2cap_frame_pull(frame, frame, losc);
 872
 873         return true;
 874 }
 875
 876 bool a2dp_codec_cap(uint8_t codec, uint8_t losc, struct l2cap_frame *frame)
 877 {
 878         switch (codec) {
 879         case A2DP_CODEC_SBC:
 880                 return codec_sbc_cap(losc, frame);
 881         case A2DP_CODEC_MPEG12:
 882                 return codec_mpeg12_cap(losc, frame);
 883         case A2DP_CODEC_MPEG24:
 884                 return codec_aac_cap(losc, frame);
 885         case A2DP_CODEC_VENDOR:
 886                 return codec_vendor_cap(losc, frame);
 887         default:
 888                 packet_hexdump(frame->data, losc);
 889                 l2cap_frame_pull(frame, frame, losc);
 890                 return true;
 891         }
 892 }
 893
 894 bool a2dp_codec_cfg(uint8_t codec, uint8_t losc, struct l2cap_frame *frame)
 895 {
 896         switch (codec) {
 897         case A2DP_CODEC_SBC:
 898                 return codec_sbc_cfg(losc, frame);
 899         case A2DP_CODEC_MPEG12:
 900                 return codec_mpeg12_cfg(losc, frame);
 901         case A2DP_CODEC_MPEG24:
 902                 return codec_aac_cfg(losc, frame);
 903         case A2DP_CODEC_VENDOR:
 904                 return codec_vendor_cfg(losc, frame);
 905         default:
 906                 packet_hexdump(frame->data, losc);
 907                 l2cap_frame_pull(frame, frame, losc);
 908                 return true;
 909         }
 910 }