monitor/a2dp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2015  Andrzej Kaczmarek <andrzej.kaczmarek@codecoup.pl>
   6  *
   7  *
   8  *  This library is free software; you can redistribute it and/or
   9  *  modify it under the terms of the GNU Lesser General Public
  10  *  License as published by the Free Software Foundation; either
  11  *  version 2.1 of the License, or (at your option) any later version.
  12  *
  13  *  This library is distributed in the hope that it will be useful,
  14  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  16  *  Lesser General Public License for more details.
  17  *
  18  *  You should have received a copy of the GNU Lesser General Public
  19  *  License along with this library; if not, write to the Free Software
  20  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  21  *
  22  */
  23
  24 #ifdef HAVE_CONFIG_H
  25 #include <config.h>
  26 #endif
  27
  28 #include <stdio.h>
  29 #include <stdlib.h>
  30 #include <string.h>
  31
  32 #include "lib/bluetooth.h"
  33
  34 #include "src/shared/util.h"
  35 #include "bt.h"
  36 #include "packet.h"
  37 #include "display.h"
  38 #include "l2cap.h"
  39 #include "a2dp.h"
  40
  41 #define BASE_INDENT     4
  42
  43 /* Codec Types */
  44 #define A2DP_CODEC_SBC          0x00
  45 #define A2DP_CODEC_MPEG12       0x01
  46 #define A2DP_CODEC_MPEG24       0x02
  47 #define A2DP_CODEC_ATRAC        0x04
  48 #define A2DP_CODEC_VENDOR       0xff
  49
  50 /* Vendor Specific A2DP Codecs */
  51 #define APTX_VENDOR_ID          0x0000004f
  52 #define APTX_CODEC_ID           0x0001
  53 #define LDAC_VENDOR_ID          0x0000012d
  54 #define LDAC_CODEC_ID           0x00aa
  55
  56 struct bit_desc {
  57         uint8_t bit_num;
  58         const char *str;
  59 };
  60
  61 static const struct bit_desc sbc_frequency_table[] = {
  62         {  7, "16000" },
  63         {  6, "32000" },
  64         {  5, "44100" },
  65         {  4, "48000" },
  66         { }
  67 };
  68
  69 static const struct bit_desc sbc_channel_mode_table[] = {
  70         {  3, "Mono" },
  71         {  2, "Dual Channel" },
  72         {  1, "Stereo" },
  73         {  0, "Joint Stereo" },
  74         { }
  75 };
  76
  77 static const struct bit_desc sbc_blocklen_table[] = {
  78         {  7, "4" },
  79         {  6, "8" },
  80         {  5, "12" },
  81         {  4, "16" },
  82         { }
  83 };
  84
  85 static const struct bit_desc sbc_subbands_table[] = {
  86         {  3, "4" },
  87         {  2, "8" },
  88         { }
  89 };
  90
  91 static const struct bit_desc sbc_allocation_table[] = {
  92         {  1, "SNR" },
  93         {  0, "Loudness" },
  94         { }
  95 };
  96
  97 static const struct bit_desc mpeg12_layer_table[] = {
  98         {  7, "Layer I (mp1)" },
  99         {  6, "Layer II (mp2)" },
 100         {  5, "Layer III (mp3)" },
 101         { }
 102 };
 103
 104 static const struct bit_desc mpeg12_channel_mode_table[] = {
 105         {  3, "Mono" },
 106         {  2, "Dual Channel" },
 107         {  1, "Stereo" },
 108         {  0, "Joint Stereo" },
 109         { }
 110 };
 111
 112 static const struct bit_desc mpeg12_frequency_table[] = {
 113         {  5, "16000" },
 114         {  4, "22050" },
 115         {  3, "24000" },
 116         {  2, "32000" },
 117         {  1, "44100" },
 118         {  0, "48000" },
 119         { }
 120 };
 121
 122 static const struct bit_desc mpeg12_bitrate_table[] = {
 123         { 14, "1110" },
 124         { 13, "1101" },
 125         { 12, "1100" },
 126         { 11, "1011" },
 127         { 10, "1010" },
 128         {  9, "1001" },
 129         {  8, "1000" },
 130         {  7, "0111" },
 131         {  6, "0110" },
 132         {  5, "0101" },
 133         {  4, "0100" },
 134         {  3, "0011" },
 135         {  2, "0010" },
 136         {  1, "0001" },
 137         {  0, "0000" },
 138         { }
 139 };
 140
 141 static const struct bit_desc aac_object_type_table[] = {
 142         {  7, "MPEG-2 AAC LC" },
 143         {  6, "MPEG-4 AAC LC" },
 144         {  5, "MPEG-4 AAC LTP" },
 145         {  4, "MPEG-4 AAC scalable" },
 146         {  3, "RFA (b3)" },
 147         {  2, "RFA (b2)" },
 148         {  1, "RFA (b1)" },
 149         {  0, "RFA (b0)" },
 150         { }
 151 };
 152
 153 static const struct bit_desc aac_frequency_table[] = {
 154         { 15, "8000" },
 155         { 14, "11025" },
 156         { 13, "12000" },
 157         { 12, "16000" },
 158         { 11, "22050" },
 159         { 10, "24000" },
 160         {  9, "32000" },
 161         {  8, "44100" },
 162         {  7, "48000" },
 163         {  6, "64000" },
 164         {  5, "88200" },
 165         {  4, "96000" },
 166         { }
 167 };
 168
 169 static const struct bit_desc aac_channels_table[] = {
 170         {  3, "1" },
 171         {  2, "2" },
 172         { }
 173 };
 174
 175 static const struct bit_desc aptx_frequency_table[] = {
 176         {  7, "16000" },
 177         {  6, "32000" },
 178         {  5, "44100" },
 179         {  4, "48000" },
 180         { }
 181 };
 182
 183 static const struct bit_desc aptx_channel_mode_table[] = {
 184         {  0, "Mono" },
 185         {  1, "Stereo" },
 186         { }
 187 };
 188
 189 static void print_value_bits(uint8_t indent, uint32_t value,
 190                                                 const struct bit_desc *table)
 191 {
 192         int i;
 193
 194         for (i = 0; table[i].str; i++) {
 195                 if (value & (1 << table[i].bit_num))
 196                         print_field("%*c%s", indent + 2, ' ', table[i].str);
 197         }
 198 }
 199
 200 static const char *find_value_bit(uint32_t value,
 201                                                 const struct bit_desc *table)
 202 {
 203         int i;
 204
 205         for (i = 0; table[i].str; i++) {
 206                 if (value & (1 << table[i].bit_num))
 207                         return table[i].str;
 208         }
 209
 210         return "Unknown";
 211 }
 212
 213 static const char *vndcodec2str(uint32_t vendor_id, uint16_t codec_id)
 214 {
 215         if (vendor_id == APTX_VENDOR_ID && codec_id == APTX_CODEC_ID)
 216                 return "aptX";
 217         else if (vendor_id == LDAC_VENDOR_ID && codec_id == LDAC_CODEC_ID)
 218                 return "LDAC";
 219
 220         return "Unknown";
 221 }
 222
 223 static bool codec_sbc_cap(uint8_t losc, struct l2cap_frame *frame)
 224 {
 225         uint8_t cap = 0;
 226
 227         if (losc != 4)
 228                 return false;
 229
 230         l2cap_frame_get_u8(frame, &cap);
 231
 232         print_field("%*cFrequency: 0x%02x", BASE_INDENT, ' ', cap & 0xf0);
 233         print_value_bits(BASE_INDENT, cap & 0xf0, sbc_frequency_table);
 234
 235         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT, ' ', cap & 0x0f);
 236         print_value_bits(BASE_INDENT, cap & 0x0f, sbc_channel_mode_table);
 237
 238         l2cap_frame_get_u8(frame, &cap);
 239
 240         print_field("%*cBlock Length: 0x%02x", BASE_INDENT, ' ', cap & 0xf0);
 241         print_value_bits(BASE_INDENT, cap & 0xf0, sbc_blocklen_table);
 242
 243         print_field("%*cSubbands: 0x%02x", BASE_INDENT, ' ', cap & 0x0c);
 244         print_value_bits(BASE_INDENT, cap & 0x0c, sbc_subbands_table);
 245
 246         print_field("%*cAllocation Method: 0x%02x", BASE_INDENT, ' ',
 247                                                                 cap & 0x03);
 248         print_value_bits(BASE_INDENT, cap & 0x03, sbc_allocation_table);
 249
 250         l2cap_frame_get_u8(frame, &cap);
 251
 252         print_field("%*cMinimum Bitpool: %d", BASE_INDENT, ' ', cap);
 253
 254         l2cap_frame_get_u8(frame, &cap);
 255
 256         print_field("%*cMaximum Bitpool: %d", BASE_INDENT, ' ', cap);
 257
 258         return true;
 259 }
 260
 261 static bool codec_sbc_cfg(uint8_t losc, struct l2cap_frame *frame)
 262 {
 263         uint8_t cap = 0;
 264
 265         if (losc != 4)
 266                 return false;
 267
 268         l2cap_frame_get_u8(frame, &cap);
 269
 270         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT, ' ',
 271                         find_value_bit(cap & 0xf0, sbc_frequency_table),
 272                         cap & 0xf0);
 273
 274         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT, ' ',
 275                         find_value_bit(cap & 0x0f, sbc_channel_mode_table),
 276                         cap & 0x0f);
 277
 278         l2cap_frame_get_u8(frame, &cap);
 279
 280         print_field("%*cBlock Length: %s (0x%02x)", BASE_INDENT, ' ',
 281                         find_value_bit(cap & 0xf0, sbc_blocklen_table),
 282                         cap & 0xf0);
 283
 284         print_field("%*cSubbands: %s (0x%02x)", BASE_INDENT, ' ',
 285                         find_value_bit(cap & 0x0c, sbc_subbands_table),
 286                         cap & 0x0c);
 287
 288         print_field("%*cAllocation Method: %s (0x%02x)", BASE_INDENT, ' ',
 289                         find_value_bit(cap & 0x03, sbc_allocation_table),
 290                         cap & 0x03);
 291
 292         l2cap_frame_get_u8(frame, &cap);
 293
 294         print_field("%*cMinimum Bitpool: %d", BASE_INDENT, ' ', cap);
 295
 296         l2cap_frame_get_u8(frame, &cap);
 297
 298         print_field("%*cMaximum Bitpool: %d", BASE_INDENT, ' ', cap);
 299
 300         return true;
 301 }
 302
 303 static bool codec_mpeg12_cap(uint8_t losc, struct l2cap_frame *frame)
 304 {
 305         uint16_t cap = 0;
 306         uint8_t layer;
 307         uint8_t chan;
 308         uint8_t freq;
 309         uint16_t bitrate;
 310         bool crc, mpf, vbr;
 311
 312         if (losc != 4)
 313                 return false;
 314
 315         l2cap_frame_get_be16(frame, &cap);
 316
 317         layer = (cap >> 8) & 0xe0;
 318         crc = cap & 0x1000;
 319         chan = (cap >> 8) & 0x0f;
 320         mpf = cap & 0x0040;
 321         freq = cap & 0x003f;
 322
 323         l2cap_frame_get_be16(frame, &cap);
 324
 325         vbr = cap & 0x8000;
 326         bitrate = cap & 0x7fff;
 327
 328         print_field("%*cLayer: 0x%02x", BASE_INDENT, ' ', layer);
 329         print_value_bits(BASE_INDENT, layer, mpeg12_layer_table);
 330
 331         print_field("%*cCRC: %s", BASE_INDENT, ' ', crc ? "Yes" : "No");
 332
 333         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT, ' ', chan);
 334         print_value_bits(BASE_INDENT, chan, mpeg12_channel_mode_table);
 335
 336         print_field("%*cMedia Payload Format: %s", BASE_INDENT, ' ',
 337                                         mpf ? "RFC-2250 RFC-3119" : "RFC-2250");
 338
 339         print_field("%*cFrequency: 0x%02x", BASE_INDENT, ' ', freq);
 340         print_value_bits(BASE_INDENT, freq, mpeg12_frequency_table);
 341
 342         if (!vbr) {
 343                 print_field("%*cBitrate Index: 0x%04x", BASE_INDENT, ' ',
 344                                                                 bitrate);
 345                 print_value_bits(BASE_INDENT, freq, mpeg12_bitrate_table);
 346         }
 347
 348         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 349
 350         return true;
 351 }
 352
 353 static bool codec_mpeg12_cfg(uint8_t losc, struct l2cap_frame *frame)
 354 {
 355         uint16_t cap = 0;
 356         uint8_t layer;
 357         uint8_t chan;
 358         uint8_t freq;
 359         uint16_t bitrate;
 360         bool crc, mpf, vbr;
 361
 362         if (losc != 4)
 363                 return false;
 364
 365         l2cap_frame_get_be16(frame, &cap);
 366
 367         layer = (cap >> 8) & 0xe0;
 368         crc = cap & 0x1000;
 369         chan = (cap >> 8) & 0x0f;
 370         mpf = cap & 0x0040;
 371         freq = cap & 0x003f;
 372
 373         l2cap_frame_get_be16(frame, &cap);
 374
 375         vbr = cap & 0x8000;
 376         bitrate = cap & 0x7fff;
 377
 378         print_field("%*cLayer: %s (0x%02x)", BASE_INDENT, ' ',
 379                                 find_value_bit(layer, mpeg12_layer_table),
 380                                 layer);
 381
 382         print_field("%*cCRC: %s", BASE_INDENT, ' ', crc ? "Yes" : "No");
 383
 384         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT, ' ',
 385                                 find_value_bit(chan, mpeg12_channel_mode_table),
 386                                 chan);
 387
 388         print_field("%*cMedia Payload Format: %s", BASE_INDENT, ' ',
 389                                         mpf ? "RFC-2250 RFC-3119" : "RFC-2250");
 390
 391         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT, ' ',
 392                                 find_value_bit(freq, mpeg12_frequency_table),
 393                                 freq);
 394
 395         if (!vbr)
 396                 print_field("%*cBitrate Index: %s (0x%04x)", BASE_INDENT, ' ',
 397                                 find_value_bit(freq, mpeg12_bitrate_table),
 398                                 bitrate);
 399
 400         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 401
 402         return true;
 403 }
 404
 405 static bool codec_aac_cap(uint8_t losc, struct l2cap_frame *frame)
 406 {
 407         uint16_t cap = 0;
 408         uint8_t type;
 409         uint16_t freq;
 410         uint8_t chan;
 411         uint32_t bitrate;
 412         bool vbr;
 413
 414         if (losc != 6)
 415                 return false;
 416
 417         l2cap_frame_get_be16(frame, &cap);
 418
 419         type = cap >> 8;
 420         freq = cap << 8;
 421
 422         l2cap_frame_get_be16(frame, &cap);
 423
 424         freq |= (cap >> 8) & 0xf0;
 425         chan = (cap >> 8) & 0x0c;
 426         bitrate = (cap << 16) & 0x7f0000;
 427         vbr = cap & 0x0080;
 428
 429         l2cap_frame_get_be16(frame, &cap);
 430
 431         bitrate |= cap;
 432
 433         print_field("%*cObject Type: 0x%02x", BASE_INDENT, ' ', type);
 434         print_value_bits(BASE_INDENT, type, aac_object_type_table);
 435
 436         print_field("%*cFrequency: 0x%02x", BASE_INDENT, ' ', freq);
 437         print_value_bits(BASE_INDENT, freq, aac_frequency_table);
 438
 439         print_field("%*cChannels: 0x%02x", BASE_INDENT, ' ', chan);
 440         print_value_bits(BASE_INDENT, chan, aac_channels_table);
 441
 442         print_field("%*cBitrate: %ubps", BASE_INDENT, ' ', bitrate);
 443         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 444
 445         return true;
 446 }
 447
 448 static bool codec_aac_cfg(uint8_t losc, struct l2cap_frame *frame)
 449 {
 450         uint16_t cap = 0;
 451         uint8_t type;
 452         uint16_t freq;
 453         uint8_t chan;
 454         uint32_t bitrate;
 455         bool vbr;
 456
 457         if (losc != 6)
 458                 return false;
 459
 460         l2cap_frame_get_be16(frame, &cap);
 461
 462         type = cap >> 8;
 463         freq = cap << 8;
 464
 465         l2cap_frame_get_be16(frame, &cap);
 466
 467         freq |= (cap >> 8) & 0xf0;
 468         chan = (cap >> 8) & 0x0c;
 469         bitrate = (cap << 16) & 0x7f0000;
 470         vbr = cap & 0x0080;
 471
 472         l2cap_frame_get_be16(frame, &cap);
 473
 474         bitrate |= cap;
 475
 476         print_field("%*cObject Type: %s (0x%02x)", BASE_INDENT, ' ',
 477                         find_value_bit(type, aac_object_type_table), type);
 478
 479         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT, ' ',
 480                         find_value_bit(freq, aac_frequency_table), freq);
 481
 482         print_field("%*cChannels: %s (0x%02x)", BASE_INDENT, ' ',
 483                         find_value_bit(chan, aac_channels_table), chan);
 484
 485         print_field("%*cBitrate: %ubps", BASE_INDENT, ' ', bitrate);
 486         print_field("%*cVBR: %s", BASE_INDENT, ' ', vbr ? "Yes" : "No");
 487
 488         return true;
 489 }
 490
 491 static bool codec_vendor_aptx_cap(uint8_t losc, struct l2cap_frame *frame)
 492 {
 493         uint8_t cap = 0;
 494
 495         if (losc != 1)
 496                 return false;
 497
 498         l2cap_frame_get_u8(frame, &cap);
 499
 500         print_field("%*cFrequency: 0x%02x", BASE_INDENT + 2, ' ', cap & 0xf0);
 501         print_value_bits(BASE_INDENT + 2, cap & 0xf0, aptx_frequency_table);
 502
 503         print_field("%*cChannel Mode: 0x%02x", BASE_INDENT + 2, ' ',
 504                                                                 cap & 0x0f);
 505         print_value_bits(BASE_INDENT + 2, cap & 0x0f, aptx_channel_mode_table);
 506
 507         return true;
 508 }
 509
 510 static bool codec_vendor_ldac(uint8_t losc, struct l2cap_frame *frame)
 511 {
 512         uint16_t cap = 0;
 513
 514         if (losc != 2)
 515                 return false;
 516
 517         l2cap_frame_get_le16(frame, &cap);
 518
 519         print_field("%*cUnknown: 0x%04x", BASE_INDENT + 2, ' ', cap);
 520
 521         return true;
 522 }
 523
 524 static bool codec_vendor_cap(uint8_t losc, struct l2cap_frame *frame)
 525 {
 526         uint32_t vendor_id = 0;
 527         uint16_t codec_id = 0;
 528
 529         if (losc < 6)
 530                 return false;
 531
 532         l2cap_frame_get_le32(frame, &vendor_id);
 533         l2cap_frame_get_le16(frame, &codec_id);
 534
 535         losc -= 6;
 536
 537         print_field("%*cVendor ID: %s (0x%08x)", BASE_INDENT, ' ',
 538                                         bt_compidtostr(vendor_id),  vendor_id);
 539
 540         print_field("%*cVendor Specific Codec ID: %s (0x%04x)", BASE_INDENT,
 541                         ' ', vndcodec2str(vendor_id, codec_id), codec_id);
 542
 543         if (vendor_id == APTX_VENDOR_ID && codec_id == APTX_CODEC_ID)
 544                 return codec_vendor_aptx_cap(losc, frame);
 545         else if (vendor_id == LDAC_VENDOR_ID && codec_id == LDAC_CODEC_ID)
 546                 return codec_vendor_ldac(losc, frame);
 547
 548         packet_hexdump(frame->data, losc);
 549         l2cap_frame_pull(frame, frame, losc);
 550
 551         return true;
 552 }
 553
 554 static bool codec_vendor_aptx_cfg(uint8_t losc, struct l2cap_frame *frame)
 555 {
 556         uint8_t cap = 0;
 557
 558         if (losc != 1)
 559                 return false;
 560
 561         l2cap_frame_get_u8(frame, &cap);
 562
 563         print_field("%*cFrequency: %s (0x%02x)", BASE_INDENT + 2, ' ',
 564                         find_value_bit(cap & 0xf0, aptx_frequency_table),
 565                         cap & 0xf0);
 566
 567         print_field("%*cChannel Mode: %s (0x%02x)", BASE_INDENT + 2, ' ',
 568                         find_value_bit(cap & 0x0f, aptx_channel_mode_table),
 569                         cap & 0x0f);
 570
 571         return true;
 572 }
 573
 574 static bool codec_vendor_cfg(uint8_t losc, struct l2cap_frame *frame)
 575 {
 576         uint32_t vendor_id = 0;
 577         uint16_t codec_id = 0;
 578
 579         if (losc < 6)
 580                 return false;
 581
 582         l2cap_frame_get_le32(frame, &vendor_id);
 583         l2cap_frame_get_le16(frame, &codec_id);
 584
 585         losc -= 6;
 586
 587         print_field("%*cVendor ID: %s (0x%08x)", BASE_INDENT, ' ',
 588                                         bt_compidtostr(vendor_id),  vendor_id);
 589
 590         print_field("%*cVendor Specific Codec ID: %s (0x%04x)", BASE_INDENT,
 591                         ' ', vndcodec2str(vendor_id, codec_id), codec_id);
 592
 593         if (vendor_id == APTX_VENDOR_ID && codec_id == APTX_CODEC_ID)
 594                 return codec_vendor_aptx_cfg(losc, frame);
 595         else if (vendor_id == LDAC_VENDOR_ID && codec_id == LDAC_CODEC_ID)
 596                 return codec_vendor_ldac(losc, frame);
 597
 598         packet_hexdump(frame->data, losc);
 599         l2cap_frame_pull(frame, frame, losc);
 600
 601         return true;
 602 }
 603
 604 bool a2dp_codec_cap(uint8_t codec, uint8_t losc, struct l2cap_frame *frame)
 605 {
 606         switch (codec) {
 607         case A2DP_CODEC_SBC:
 608                 return codec_sbc_cap(losc, frame);
 609         case A2DP_CODEC_MPEG12:
 610                 return codec_mpeg12_cap(losc, frame);
 611         case A2DP_CODEC_MPEG24:
 612                 return codec_aac_cap(losc, frame);
 613         case A2DP_CODEC_VENDOR:
 614                 return codec_vendor_cap(losc, frame);
 615         default:
 616                 packet_hexdump(frame->data, losc);
 617                 l2cap_frame_pull(frame, frame, losc);
 618                 return true;
 619         }
 620 }
 621
 622 bool a2dp_codec_cfg(uint8_t codec, uint8_t losc, struct l2cap_frame *frame)
 623 {
 624         switch (codec) {
 625         case A2DP_CODEC_SBC:
 626                 return codec_sbc_cfg(losc, frame);
 627         case A2DP_CODEC_MPEG12:
 628                 return codec_mpeg12_cfg(losc, frame);
 629         case A2DP_CODEC_MPEG24:
 630                 return codec_aac_cfg(losc, frame);
 631         case A2DP_CODEC_VENDOR:
 632                 return codec_vendor_cfg(losc, frame);
 633         default:
 634                 packet_hexdump(frame->data, losc);
 635                 l2cap_frame_pull(frame, frame, losc);
 636                 return true;
 637         }
 638 }