monitor/avdtp.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 "avdtp.h"
  40
  41 /* Message Types */
  42 #define AVDTP_MSG_TYPE_COMMAND          0x00
  43 #define AVDTP_MSG_TYPE_GENERAL_REJECT   0x01
  44 #define AVDTP_MSG_TYPE_RESPONSE_ACCEPT  0x02
  45 #define AVDTP_MSG_TYPE_RESPONSE_REJECT  0x03
  46
  47 /* Signal Identifiers */
  48 #define AVDTP_DISCOVER                  0x01
  49 #define AVDTP_GET_CAPABILITIES          0x02
  50 #define AVDTP_SET_CONFIGURATION         0x03
  51 #define AVDTP_GET_CONFIGURATION         0x04
  52 #define AVDTP_RECONFIGURE               0x05
  53 #define AVDTP_OPEN                      0x06
  54 #define AVDTP_START                     0x07
  55 #define AVDTP_CLOSE                     0x08
  56 #define AVDTP_SUSPEND                   0x09
  57 #define AVDTP_ABORT                     0x0a
  58 #define AVDTP_SECURITY_CONTROL          0x0b
  59 #define AVDTP_GET_ALL_CAPABILITIES      0x0c
  60 #define AVDTP_DELAYREPORT               0x0d
  61
  62 /* Service Categories */
  63 #define AVDTP_MEDIA_TRANSPORT           0x01
  64 #define AVDTP_REPORTING                 0x02
  65 #define AVDTP_RECOVERY                  0x03
  66 #define AVDTP_CONTENT_PROTECTION        0x04
  67 #define AVDTP_HEADER_COMPRESSION        0x05
  68 #define AVDTP_MULTIPLEXING              0x06
  69 #define AVDTP_MEDIA_CODEC               0x07
  70 #define AVDTP_DELAY_REPORTING           0x08
  71
  72 struct avdtp_frame {
  73         uint8_t hdr;
  74         uint8_t sig_id;
  75         struct l2cap_frame l2cap_frame;
  76 };
  77
  78 static const char *msgtype2str(uint8_t msgtype)
  79 {
  80         switch (msgtype) {
  81         case 0:
  82                 return "Command";
  83         case 1:
  84                 return "General Reject";
  85         case 2:
  86                 return "Response Accept";
  87         case 3:
  88                 return "Response Reject";
  89         }
  90
  91         return "";
  92 }
  93
  94 static const char *sigid2str(uint8_t sigid)
  95 {
  96         switch (sigid) {
  97         case AVDTP_DISCOVER:
  98                 return "Discover";
  99         case AVDTP_GET_CAPABILITIES:
 100                 return "Get Capabilities";
 101         case AVDTP_SET_CONFIGURATION:
 102                 return "Set Configuration";
 103         case AVDTP_GET_CONFIGURATION:
 104                 return "Get Configuration";
 105         case AVDTP_RECONFIGURE:
 106                 return "Reconfigure";
 107         case AVDTP_OPEN:
 108                 return "Open";
 109         case AVDTP_START:
 110                 return "Start";
 111         case AVDTP_CLOSE:
 112                 return "Close";
 113         case AVDTP_SUSPEND:
 114                 return "Suspend";
 115         case AVDTP_ABORT:
 116                 return "Abort";
 117         case AVDTP_SECURITY_CONTROL:
 118                 return "Security Control";
 119         case AVDTP_GET_ALL_CAPABILITIES:
 120                 return "Get All Capabilities";
 121         case AVDTP_DELAYREPORT:
 122                 return "Delay Report";
 123         default:
 124                 return "Reserved";
 125         }
 126 }
 127
 128 static const char *error2str(uint8_t error)
 129 {
 130         switch (error) {
 131         case 0x01:
 132                 return "BAD_HEADER_FORMAT";
 133         case 0x11:
 134                 return "BAD_LENGTH";
 135         case 0x12:
 136                 return "BAD_ACP_SEID";
 137         case 0x13:
 138                 return "SEP_IN_USE";
 139         case 0x14:
 140                 return "SEP_NOT_IN_USER";
 141         case 0x17:
 142                 return "BAD_SERV_CATEGORY";
 143         case 0x18:
 144                 return "BAD_PAYLOAD_FORMAT";
 145         case 0x19:
 146                 return "NOT_SUPPORTED_COMMAND";
 147         case 0x1a:
 148                 return "INVALID_CAPABILITIES";
 149         case 0x22:
 150                 return "BAD_RECOVERY_TYPE";
 151         case 0x23:
 152                 return "BAD_MEDIA_TRANSPORT_FORMAT";
 153         case 0x25:
 154                 return "BAD_RECOVERY_FORMAT";
 155         case 0x26:
 156                 return "BAD_ROHC_FORMAT";
 157         case 0x27:
 158                 return "BAD_CP_FORMAT";
 159         case 0x28:
 160                 return "BAD_MULTIPLEXING_FORMAT";
 161         case 0x29:
 162                 return "UNSUPPORTED_CONFIGURATION";
 163         case 0x31:
 164                 return "BAD_STATE";
 165         default:
 166                 return "Unknown";
 167         }
 168 }
 169
 170 static const char *mediatype2str(uint8_t media_type)
 171 {
 172         switch (media_type) {
 173         case 0x00:
 174                 return "Audio";
 175         case 0x01:
 176                 return "Video";
 177         case 0x02:
 178                 return "Multimedia";
 179         default:
 180                 return "Reserved";
 181         }
 182 }
 183
 184 static const char *servicecat2str(uint8_t service_cat)
 185 {
 186         switch (service_cat) {
 187         case AVDTP_MEDIA_TRANSPORT:
 188                 return "Media Transport";
 189         case AVDTP_REPORTING:
 190                 return "Reporting";
 191         case AVDTP_RECOVERY:
 192                 return "Recovery";
 193         case AVDTP_CONTENT_PROTECTION:
 194                 return "Content Protection";
 195         case AVDTP_HEADER_COMPRESSION:
 196                 return "Header Compression";
 197         case AVDTP_MULTIPLEXING:
 198                 return "Multiplexing";
 199         case AVDTP_MEDIA_CODEC:
 200                 return "Media Codec";
 201         case AVDTP_DELAY_REPORTING:
 202                 return "Delay Reporting";
 203         default:
 204                 return "Reserved";
 205         }
 206 }
 207
 208 static bool avdtp_reject_common(struct avdtp_frame *avdtp_frame)
 209 {
 210         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 211         uint8_t error;
 212
 213         if (!l2cap_frame_get_u8(frame, &error))
 214                 return false;
 215
 216         print_field("Error code: %s (0x%02x)", error2str(error), error);
 217
 218         return true;
 219 }
 220
 221 static bool decode_capabilities(struct avdtp_frame *avdtp_frame)
 222 {
 223         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 224         uint8_t service_cat;
 225         uint8_t losc;
 226
 227         while (l2cap_frame_get_u8(frame, &service_cat)) {
 228                 print_field("Service Category: %s (0x%02x)",
 229                                 servicecat2str(service_cat), service_cat);
 230
 231                 if (!l2cap_frame_get_u8(frame, &losc))
 232                         return false;
 233
 234                 if (frame->size < losc)
 235                         return false;
 236
 237                 /* TODO: decode service capabilities */
 238
 239                 packet_hexdump(frame->data, losc);
 240
 241                 l2cap_frame_pull(frame, frame, losc);
 242         }
 243
 244         return true;
 245 }
 246
 247 static bool avdtp_discover(struct avdtp_frame *avdtp_frame)
 248 {
 249         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 250         uint8_t type = avdtp_frame->hdr & 0x03;
 251         uint8_t seid;
 252         uint8_t info;
 253
 254         switch (type) {
 255         case AVDTP_MSG_TYPE_COMMAND:
 256                 return true;
 257         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 258                 while (l2cap_frame_get_u8(frame, &seid)) {
 259                         print_field("ACP SEID: %d", seid >> 2);
 260
 261                         if (!l2cap_frame_get_u8(frame, &info))
 262                                 return false;
 263
 264                         print_field("%*cMedia Type: %s (0x%02x)", 2, ' ',
 265                                         mediatype2str(info >> 4), info >> 4);
 266                         print_field("%*cSEP Type: %s (0x%02x)", 2, ' ',
 267                                                 info & 0x04 ? "SNK" : "SRC",
 268                                                 (info >> 3) & 0x01);
 269                         print_field("%*cIn use: %s", 2, ' ',
 270                                                 seid & 0x02 ? "Yes" : "No");
 271                 }
 272                 return true;
 273         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 274                 return avdtp_reject_common(avdtp_frame);
 275         }
 276
 277         return false;
 278 }
 279
 280 static bool avdtp_get_capabilities(struct avdtp_frame *avdtp_frame)
 281 {
 282         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 283         uint8_t type = avdtp_frame->hdr & 0x03;
 284         uint8_t seid;
 285
 286         switch (type) {
 287         case AVDTP_MSG_TYPE_COMMAND:
 288                 if (!l2cap_frame_get_u8(frame, &seid))
 289                         return false;
 290
 291                 print_field("ACP SEID: %d", seid >> 2);
 292
 293                 return true;
 294         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 295                 return decode_capabilities(avdtp_frame);
 296         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 297                 return avdtp_reject_common(avdtp_frame);
 298         }
 299
 300         return false;
 301 }
 302
 303 static bool avdtp_set_configuration(struct avdtp_frame *avdtp_frame)
 304 {
 305         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 306         uint8_t type = avdtp_frame->hdr & 0x03;
 307         uint8_t acp_seid, int_seid;
 308         uint8_t service_cat;
 309
 310         switch (type) {
 311         case AVDTP_MSG_TYPE_COMMAND:
 312                 if (!l2cap_frame_get_u8(frame, &acp_seid))
 313                         return false;
 314
 315                 print_field("ACP SEID: %d", acp_seid >> 2);
 316
 317                 if (!l2cap_frame_get_u8(frame, &int_seid))
 318                         return false;
 319
 320                 print_field("INT SEID: %d", int_seid >> 2);
 321
 322                 return decode_capabilities(avdtp_frame);
 323         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 324                 return true;
 325         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 326                 if (!l2cap_frame_get_u8(frame, &service_cat))
 327                         return false;
 328
 329                 print_field("Service Category: %s (0x%02x)",
 330                                 servicecat2str(service_cat), service_cat);
 331
 332                 return avdtp_reject_common(avdtp_frame);
 333         }
 334
 335         return false;
 336 }
 337
 338 static bool avdtp_get_configuration(struct avdtp_frame *avdtp_frame)
 339 {
 340         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 341         uint8_t type = avdtp_frame->hdr & 0x03;
 342         uint8_t seid;
 343
 344         switch (type) {
 345         case AVDTP_MSG_TYPE_COMMAND:
 346                 if (!l2cap_frame_get_u8(frame, &seid))
 347                         return false;
 348
 349                 print_field("ACP SEID: %d", seid >> 2);
 350
 351                 return true;
 352         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 353                 return decode_capabilities(avdtp_frame);
 354         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 355                 return avdtp_reject_common(avdtp_frame);
 356         }
 357
 358         return false;
 359 }
 360
 361 static bool avdtp_reconfigure(struct avdtp_frame *avdtp_frame)
 362 {
 363         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 364         uint8_t type = avdtp_frame->hdr & 0x03;
 365         uint8_t seid;
 366         uint8_t service_cat;
 367
 368         switch (type) {
 369         case AVDTP_MSG_TYPE_COMMAND:
 370                 if (!l2cap_frame_get_u8(frame, &seid))
 371                         return false;
 372
 373                 print_field("ACP SEID: %d", seid >> 2);
 374
 375                 return decode_capabilities(avdtp_frame);
 376         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 377                 return true;
 378         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 379                 if (!l2cap_frame_get_u8(frame, &service_cat))
 380                         return false;
 381
 382                 print_field("Service Category: %s (0x%02x)",
 383                                 servicecat2str(service_cat), service_cat);
 384
 385                 return avdtp_reject_common(avdtp_frame);
 386         }
 387
 388         return false;
 389 }
 390
 391 static bool avdtp_open(struct avdtp_frame *avdtp_frame)
 392 {
 393         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 394         uint8_t type = avdtp_frame->hdr & 0x03;
 395         uint8_t seid;
 396
 397         switch (type) {
 398         case AVDTP_MSG_TYPE_COMMAND:
 399                 if (!l2cap_frame_get_u8(frame, &seid))
 400                         return false;
 401
 402                 print_field("ACP SEID: %d", seid >> 2);
 403
 404                 return true;
 405         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 406                 return true;
 407         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 408                 return avdtp_reject_common(avdtp_frame);
 409         }
 410
 411         return false;
 412 }
 413
 414 static bool avdtp_start(struct avdtp_frame *avdtp_frame)
 415 {
 416         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 417         uint8_t type = avdtp_frame->hdr & 0x03;
 418         uint8_t seid;
 419
 420         switch (type) {
 421         case AVDTP_MSG_TYPE_COMMAND:
 422                 if (!l2cap_frame_get_u8(frame, &seid))
 423                         return false;
 424
 425                 print_field("ACP SEID: %d", seid >> 2);
 426
 427                 while (l2cap_frame_get_u8(frame, &seid))
 428                         print_field("ACP SEID: %d", seid >> 2);
 429
 430                 return true;
 431         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 432                 return true;
 433         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 434                 if (!l2cap_frame_get_u8(frame, &seid))
 435                         return false;
 436
 437                 print_field("ACP SEID: %d", seid >> 2);
 438
 439                 return avdtp_reject_common(avdtp_frame);
 440         }
 441
 442         return false;
 443 }
 444
 445 static bool avdtp_close(struct avdtp_frame *avdtp_frame)
 446 {
 447         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 448         uint8_t type = avdtp_frame->hdr & 0x03;
 449         uint8_t seid;
 450
 451         switch (type) {
 452         case AVDTP_MSG_TYPE_COMMAND:
 453                 if (!l2cap_frame_get_u8(frame, &seid))
 454                         return false;
 455
 456                 print_field("ACP SEID: %d", seid >> 2);
 457
 458                 return true;
 459         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 460                 return true;
 461         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 462                 return avdtp_reject_common(avdtp_frame);
 463         }
 464
 465         return false;
 466 }
 467
 468 static bool avdtp_signalling_packet(struct avdtp_frame *avdtp_frame)
 469 {
 470         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 471         const char *pdu_color;
 472         uint8_t hdr;
 473         uint8_t sig_id;
 474         uint8_t nosp = 0;
 475
 476         if (frame->in)
 477                 pdu_color = COLOR_MAGENTA;
 478         else
 479                 pdu_color = COLOR_BLUE;
 480
 481         if (!l2cap_frame_get_u8(frame, &hdr))
 482                 return false;
 483
 484         avdtp_frame->hdr = hdr;
 485
 486         /* Continue Packet || End Packet */
 487         if (((hdr & 0x0c) == 0x08) || ((hdr & 0x0c) == 0x0c)) {
 488                 /* TODO: handle fragmentation */
 489                 packet_hexdump(frame->data, frame->size);
 490                 return true;
 491         }
 492
 493         /* Start Packet */
 494         if ((hdr & 0x0c) == 0x04) {
 495                 if (!l2cap_frame_get_u8(frame, &nosp))
 496                         return false;
 497         }
 498
 499         if (!l2cap_frame_get_u8(frame, &sig_id))
 500                 return false;
 501
 502         sig_id &= 0x3f;
 503
 504         avdtp_frame->sig_id = sig_id;
 505
 506         print_indent(6, pdu_color, "AVDTP: ", sigid2str(sig_id), COLOR_OFF,
 507                         " (0x%02x) %s (0x%02x) type 0x%02x label %d nosp %d",
 508                         sig_id, msgtype2str(hdr & 0x03), hdr & 0x03,
 509                         hdr & 0x0c, hdr >> 4, nosp);
 510
 511         /* Start Packet */
 512         if ((hdr & 0x0c) == 0x04) {
 513                 /* TODO: handle fragmentation */
 514                 packet_hexdump(frame->data, frame->size);
 515                 return true;
 516         }
 517
 518         /* General Reject */
 519         if ((hdr & 0x03) == 0x03)
 520                 return true;
 521
 522         switch (sig_id) {
 523         case AVDTP_DISCOVER:
 524                 return avdtp_discover(avdtp_frame);
 525         case AVDTP_GET_CAPABILITIES:
 526                 return avdtp_get_capabilities(avdtp_frame);
 527         case AVDTP_SET_CONFIGURATION:
 528                 return avdtp_set_configuration(avdtp_frame);
 529         case AVDTP_GET_CONFIGURATION:
 530                 return avdtp_get_configuration(avdtp_frame);
 531         case AVDTP_RECONFIGURE:
 532                 return avdtp_reconfigure(avdtp_frame);
 533         case AVDTP_OPEN:
 534                 return avdtp_open(avdtp_frame);
 535         case AVDTP_START:
 536                 return avdtp_start(avdtp_frame);
 537         case AVDTP_CLOSE:
 538                 return avdtp_close(avdtp_frame);
 539         }
 540
 541         packet_hexdump(frame->data, frame->size);
 542
 543         return true;
 544 }
 545
 546 void avdtp_packet(const struct l2cap_frame *frame)
 547 {
 548         struct avdtp_frame avdtp_frame;
 549         bool ret;
 550
 551         l2cap_frame_pull(&avdtp_frame.l2cap_frame, frame, 0);
 552
 553         switch (frame->seq_num) {
 554         case 1:
 555                 ret = avdtp_signalling_packet(&avdtp_frame);
 556                 break;
 557         default:
 558                 packet_hexdump(frame->data, frame->size);
 559                 return;
 560         }
 561
 562         if (!ret) {
 563                 print_text(COLOR_ERROR, "PDU malformed");
 564                 packet_hexdump(frame->data, frame->size);
 565         }
 566 }