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 #include "a2dp.h"
  41
  42 /* Message Types */
  43 #define AVDTP_MSG_TYPE_COMMAND          0x00
  44 #define AVDTP_MSG_TYPE_GENERAL_REJECT   0x01
  45 #define AVDTP_MSG_TYPE_RESPONSE_ACCEPT  0x02
  46 #define AVDTP_MSG_TYPE_RESPONSE_REJECT  0x03
  47
  48 /* Signal Identifiers */
  49 #define AVDTP_DISCOVER                  0x01
  50 #define AVDTP_GET_CAPABILITIES          0x02
  51 #define AVDTP_SET_CONFIGURATION         0x03
  52 #define AVDTP_GET_CONFIGURATION         0x04
  53 #define AVDTP_RECONFIGURE               0x05
  54 #define AVDTP_OPEN                      0x06
  55 #define AVDTP_START                     0x07
  56 #define AVDTP_CLOSE                     0x08
  57 #define AVDTP_SUSPEND                   0x09
  58 #define AVDTP_ABORT                     0x0a
  59 #define AVDTP_SECURITY_CONTROL          0x0b
  60 #define AVDTP_GET_ALL_CAPABILITIES      0x0c
  61 #define AVDTP_DELAYREPORT               0x0d
  62
  63 /* Service Categories */
  64 #define AVDTP_MEDIA_TRANSPORT           0x01
  65 #define AVDTP_REPORTING                 0x02
  66 #define AVDTP_RECOVERY                  0x03
  67 #define AVDTP_CONTENT_PROTECTION        0x04
  68 #define AVDTP_HEADER_COMPRESSION        0x05
  69 #define AVDTP_MULTIPLEXING              0x06
  70 #define AVDTP_MEDIA_CODEC               0x07
  71 #define AVDTP_DELAY_REPORTING           0x08
  72
  73 struct avdtp_frame {
  74         uint8_t hdr;
  75         uint8_t sig_id;
  76         struct l2cap_frame l2cap_frame;
  77 };
  78
  79 static inline bool is_configuration_sig_id(uint8_t sig_id)
  80 {
  81         return (sig_id == AVDTP_SET_CONFIGURATION) ||
  82                         (sig_id == AVDTP_GET_CONFIGURATION) ||
  83                         (sig_id == AVDTP_RECONFIGURE);
  84 }
  85
  86 static const char *msgtype2str(uint8_t msgtype)
  87 {
  88         switch (msgtype) {
  89         case 0:
  90                 return "Command";
  91         case 1:
  92                 return "General Reject";
  93         case 2:
  94                 return "Response Accept";
  95         case 3:
  96                 return "Response Reject";
  97         }
  98
  99         return "";
 100 }
 101
 102 static const char *sigid2str(uint8_t sigid)
 103 {
 104         switch (sigid) {
 105         case AVDTP_DISCOVER:
 106                 return "Discover";
 107         case AVDTP_GET_CAPABILITIES:
 108                 return "Get Capabilities";
 109         case AVDTP_SET_CONFIGURATION:
 110                 return "Set Configuration";
 111         case AVDTP_GET_CONFIGURATION:
 112                 return "Get Configuration";
 113         case AVDTP_RECONFIGURE:
 114                 return "Reconfigure";
 115         case AVDTP_OPEN:
 116                 return "Open";
 117         case AVDTP_START:
 118                 return "Start";
 119         case AVDTP_CLOSE:
 120                 return "Close";
 121         case AVDTP_SUSPEND:
 122                 return "Suspend";
 123         case AVDTP_ABORT:
 124                 return "Abort";
 125         case AVDTP_SECURITY_CONTROL:
 126                 return "Security Control";
 127         case AVDTP_GET_ALL_CAPABILITIES:
 128                 return "Get All Capabilities";
 129         case AVDTP_DELAYREPORT:
 130                 return "Delay Report";
 131         default:
 132                 return "Reserved";
 133         }
 134 }
 135
 136 static const char *error2str(uint8_t error)
 137 {
 138         switch (error) {
 139         case 0x01:
 140                 return "BAD_HEADER_FORMAT";
 141         case 0x11:
 142                 return "BAD_LENGTH";
 143         case 0x12:
 144                 return "BAD_ACP_SEID";
 145         case 0x13:
 146                 return "SEP_IN_USE";
 147         case 0x14:
 148                 return "SEP_NOT_IN_USER";
 149         case 0x17:
 150                 return "BAD_SERV_CATEGORY";
 151         case 0x18:
 152                 return "BAD_PAYLOAD_FORMAT";
 153         case 0x19:
 154                 return "NOT_SUPPORTED_COMMAND";
 155         case 0x1a:
 156                 return "INVALID_CAPABILITIES";
 157         case 0x22:
 158                 return "BAD_RECOVERY_TYPE";
 159         case 0x23:
 160                 return "BAD_MEDIA_TRANSPORT_FORMAT";
 161         case 0x25:
 162                 return "BAD_RECOVERY_FORMAT";
 163         case 0x26:
 164                 return "BAD_ROHC_FORMAT";
 165         case 0x27:
 166                 return "BAD_CP_FORMAT";
 167         case 0x28:
 168                 return "BAD_MULTIPLEXING_FORMAT";
 169         case 0x29:
 170                 return "UNSUPPORTED_CONFIGURATION";
 171         case 0x31:
 172                 return "BAD_STATE";
 173         default:
 174                 return "Unknown";
 175         }
 176 }
 177
 178 static const char *mediatype2str(uint8_t media_type)
 179 {
 180         switch (media_type) {
 181         case 0x00:
 182                 return "Audio";
 183         case 0x01:
 184                 return "Video";
 185         case 0x02:
 186                 return "Multimedia";
 187         default:
 188                 return "Reserved";
 189         }
 190 }
 191
 192 static const char *mediacodec2str(uint8_t codec)
 193 {
 194         switch (codec) {
 195         case 0x00:
 196                 return "SBC";
 197         case 0x01:
 198                 return "MPEG-1,2 Audio";
 199         case 0x02:
 200                 return "MPEG-2,4 AAC";
 201         case 0x04:
 202                 return "ATRAC Family";
 203         case 0xff:
 204                 return "Non-A2DP";
 205         default:
 206                 return "Reserved";
 207         }
 208 }
 209
 210 static const char *cptype2str(uint8_t cp)
 211 {
 212         switch (cp) {
 213         case 0x0001:
 214                 return "DTCP";
 215         case 0x0002:
 216                 return "SCMS-T";
 217         default:
 218                 return "Reserved";
 219         }
 220 }
 221
 222 static const char *servicecat2str(uint8_t service_cat)
 223 {
 224         switch (service_cat) {
 225         case AVDTP_MEDIA_TRANSPORT:
 226                 return "Media Transport";
 227         case AVDTP_REPORTING:
 228                 return "Reporting";
 229         case AVDTP_RECOVERY:
 230                 return "Recovery";
 231         case AVDTP_CONTENT_PROTECTION:
 232                 return "Content Protection";
 233         case AVDTP_HEADER_COMPRESSION:
 234                 return "Header Compression";
 235         case AVDTP_MULTIPLEXING:
 236                 return "Multiplexing";
 237         case AVDTP_MEDIA_CODEC:
 238                 return "Media Codec";
 239         case AVDTP_DELAY_REPORTING:
 240                 return "Delay Reporting";
 241         default:
 242                 return "Reserved";
 243         }
 244 }
 245
 246 static bool avdtp_reject_common(struct avdtp_frame *avdtp_frame)
 247 {
 248         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 249         uint8_t error;
 250
 251         if (!l2cap_frame_get_u8(frame, &error))
 252                 return false;
 253
 254         print_field("Error code: %s (0x%02x)", error2str(error), error);
 255
 256         return true;
 257 }
 258
 259 static bool service_content_protection(struct avdtp_frame *avdtp_frame,
 260                                                                 uint8_t losc)
 261 {
 262         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 263         uint16_t type = 0;
 264
 265         if (losc < 2)
 266                 return false;
 267
 268         if (!l2cap_frame_get_le16(frame, &type))
 269                 return false;
 270
 271         losc -= 2;
 272
 273         print_field("%*cContent Protection Type: %s (0x%04x)", 2, ' ',
 274                                                         cptype2str(type), type);
 275
 276         /* TODO: decode protection specific information */
 277         packet_hexdump(frame->data, losc);
 278
 279         l2cap_frame_pull(frame, frame, losc);
 280
 281         return true;
 282 }
 283
 284 static bool service_media_codec(struct avdtp_frame *avdtp_frame, uint8_t losc)
 285 {
 286         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 287         uint8_t type = 0;
 288         uint8_t codec = 0;
 289
 290         if (losc < 2)
 291                 return false;
 292
 293         l2cap_frame_get_u8(frame, &type);
 294         l2cap_frame_get_u8(frame, &codec);
 295
 296         losc -= 2;
 297
 298         print_field("%*cMedia Type: %s (0x%02x)", 2, ' ',
 299                                         mediatype2str(type >> 4), type >> 4);
 300
 301         print_field("%*cMedia Codec: %s (0x%02x)", 2, ' ',
 302                                         mediacodec2str(codec), codec);
 303
 304         if (is_configuration_sig_id(avdtp_frame->sig_id))
 305                 return a2dp_codec_cfg(codec, losc, frame);
 306         else
 307                 return a2dp_codec_cap(codec, losc, frame);
 308 }
 309
 310 static bool decode_capabilities(struct avdtp_frame *avdtp_frame)
 311 {
 312         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 313         uint8_t service_cat;
 314         uint8_t losc;
 315
 316         while (l2cap_frame_get_u8(frame, &service_cat)) {
 317                 print_field("Service Category: %s (0x%02x)",
 318                                 servicecat2str(service_cat), service_cat);
 319
 320                 if (!l2cap_frame_get_u8(frame, &losc))
 321                         return false;
 322
 323                 if (frame->size < losc)
 324                         return false;
 325
 326                 switch (service_cat) {
 327                 case AVDTP_CONTENT_PROTECTION:
 328                         if (!service_content_protection(avdtp_frame, losc))
 329                                 return false;
 330                         break;
 331                 case AVDTP_MEDIA_CODEC:
 332                         if (!service_media_codec(avdtp_frame, losc))
 333                                 return false;
 334                         break;
 335                 case AVDTP_MEDIA_TRANSPORT:
 336                 case AVDTP_REPORTING:
 337                 case AVDTP_RECOVERY:
 338                 case AVDTP_HEADER_COMPRESSION:
 339                 case AVDTP_MULTIPLEXING:
 340                 case AVDTP_DELAY_REPORTING:
 341                 default:
 342                         packet_hexdump(frame->data, losc);
 343                         l2cap_frame_pull(frame, frame, losc);
 344                 }
 345
 346         }
 347
 348         return true;
 349 }
 350
 351 static bool avdtp_discover(struct avdtp_frame *avdtp_frame)
 352 {
 353         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 354         uint8_t type = avdtp_frame->hdr & 0x03;
 355         uint8_t seid;
 356         uint8_t info;
 357
 358         switch (type) {
 359         case AVDTP_MSG_TYPE_COMMAND:
 360                 return true;
 361         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 362                 while (l2cap_frame_get_u8(frame, &seid)) {
 363                         print_field("ACP SEID: %d", seid >> 2);
 364
 365                         if (!l2cap_frame_get_u8(frame, &info))
 366                                 return false;
 367
 368                         print_field("%*cMedia Type: %s (0x%02x)", 2, ' ',
 369                                         mediatype2str(info >> 4), info >> 4);
 370                         print_field("%*cSEP Type: %s (0x%02x)", 2, ' ',
 371                                                 info & 0x04 ? "SNK" : "SRC",
 372                                                 (info >> 3) & 0x01);
 373                         print_field("%*cIn use: %s", 2, ' ',
 374                                                 seid & 0x02 ? "Yes" : "No");
 375                 }
 376                 return true;
 377         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 378                 return avdtp_reject_common(avdtp_frame);
 379         }
 380
 381         return false;
 382 }
 383
 384 static bool avdtp_get_capabilities(struct avdtp_frame *avdtp_frame)
 385 {
 386         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 387         uint8_t type = avdtp_frame->hdr & 0x03;
 388         uint8_t seid;
 389
 390         switch (type) {
 391         case AVDTP_MSG_TYPE_COMMAND:
 392                 if (!l2cap_frame_get_u8(frame, &seid))
 393                         return false;
 394
 395                 print_field("ACP SEID: %d", seid >> 2);
 396
 397                 return true;
 398         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 399                 return decode_capabilities(avdtp_frame);
 400         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 401                 return avdtp_reject_common(avdtp_frame);
 402         }
 403
 404         return false;
 405 }
 406
 407 static bool avdtp_set_configuration(struct avdtp_frame *avdtp_frame)
 408 {
 409         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 410         uint8_t type = avdtp_frame->hdr & 0x03;
 411         uint8_t acp_seid, int_seid;
 412         uint8_t service_cat;
 413
 414         switch (type) {
 415         case AVDTP_MSG_TYPE_COMMAND:
 416                 if (!l2cap_frame_get_u8(frame, &acp_seid))
 417                         return false;
 418
 419                 print_field("ACP SEID: %d", acp_seid >> 2);
 420
 421                 if (!l2cap_frame_get_u8(frame, &int_seid))
 422                         return false;
 423
 424                 print_field("INT SEID: %d", int_seid >> 2);
 425
 426                 return decode_capabilities(avdtp_frame);
 427         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 428                 return true;
 429         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 430                 if (!l2cap_frame_get_u8(frame, &service_cat))
 431                         return false;
 432
 433                 print_field("Service Category: %s (0x%02x)",
 434                                 servicecat2str(service_cat), service_cat);
 435
 436                 return avdtp_reject_common(avdtp_frame);
 437         }
 438
 439         return false;
 440 }
 441
 442 static bool avdtp_get_configuration(struct avdtp_frame *avdtp_frame)
 443 {
 444         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 445         uint8_t type = avdtp_frame->hdr & 0x03;
 446         uint8_t seid;
 447
 448         switch (type) {
 449         case AVDTP_MSG_TYPE_COMMAND:
 450                 if (!l2cap_frame_get_u8(frame, &seid))
 451                         return false;
 452
 453                 print_field("ACP SEID: %d", seid >> 2);
 454
 455                 return true;
 456         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 457                 return decode_capabilities(avdtp_frame);
 458         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 459                 return avdtp_reject_common(avdtp_frame);
 460         }
 461
 462         return false;
 463 }
 464
 465 static bool avdtp_reconfigure(struct avdtp_frame *avdtp_frame)
 466 {
 467         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 468         uint8_t type = avdtp_frame->hdr & 0x03;
 469         uint8_t seid;
 470         uint8_t service_cat;
 471
 472         switch (type) {
 473         case AVDTP_MSG_TYPE_COMMAND:
 474                 if (!l2cap_frame_get_u8(frame, &seid))
 475                         return false;
 476
 477                 print_field("ACP SEID: %d", seid >> 2);
 478
 479                 return decode_capabilities(avdtp_frame);
 480         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 481                 return true;
 482         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 483                 if (!l2cap_frame_get_u8(frame, &service_cat))
 484                         return false;
 485
 486                 print_field("Service Category: %s (0x%02x)",
 487                                 servicecat2str(service_cat), service_cat);
 488
 489                 return avdtp_reject_common(avdtp_frame);
 490         }
 491
 492         return false;
 493 }
 494
 495 static bool avdtp_open(struct avdtp_frame *avdtp_frame)
 496 {
 497         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 498         uint8_t type = avdtp_frame->hdr & 0x03;
 499         uint8_t seid;
 500
 501         switch (type) {
 502         case AVDTP_MSG_TYPE_COMMAND:
 503                 if (!l2cap_frame_get_u8(frame, &seid))
 504                         return false;
 505
 506                 print_field("ACP SEID: %d", seid >> 2);
 507
 508                 return true;
 509         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 510                 return true;
 511         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 512                 return avdtp_reject_common(avdtp_frame);
 513         }
 514
 515         return false;
 516 }
 517
 518 static bool avdtp_start(struct avdtp_frame *avdtp_frame)
 519 {
 520         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 521         uint8_t type = avdtp_frame->hdr & 0x03;
 522         uint8_t seid;
 523
 524         switch (type) {
 525         case AVDTP_MSG_TYPE_COMMAND:
 526                 if (!l2cap_frame_get_u8(frame, &seid))
 527                         return false;
 528
 529                 print_field("ACP SEID: %d", seid >> 2);
 530
 531                 while (l2cap_frame_get_u8(frame, &seid))
 532                         print_field("ACP SEID: %d", seid >> 2);
 533
 534                 return true;
 535         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 536                 return true;
 537         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 538                 if (!l2cap_frame_get_u8(frame, &seid))
 539                         return false;
 540
 541                 print_field("ACP SEID: %d", seid >> 2);
 542
 543                 return avdtp_reject_common(avdtp_frame);
 544         }
 545
 546         return false;
 547 }
 548
 549 static bool avdtp_close(struct avdtp_frame *avdtp_frame)
 550 {
 551         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 552         uint8_t type = avdtp_frame->hdr & 0x03;
 553         uint8_t seid;
 554
 555         switch (type) {
 556         case AVDTP_MSG_TYPE_COMMAND:
 557                 if (!l2cap_frame_get_u8(frame, &seid))
 558                         return false;
 559
 560                 print_field("ACP SEID: %d", seid >> 2);
 561
 562                 return true;
 563         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 564                 return true;
 565         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 566                 return avdtp_reject_common(avdtp_frame);
 567         }
 568
 569         return false;
 570 }
 571
 572 static bool avdtp_suspend(struct avdtp_frame *avdtp_frame)
 573 {
 574         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 575         uint8_t type = avdtp_frame->hdr & 0x03;
 576         uint8_t seid;
 577
 578         switch (type) {
 579         case AVDTP_MSG_TYPE_COMMAND:
 580                 if (!l2cap_frame_get_u8(frame, &seid))
 581                         return false;
 582
 583                 print_field("ACP SEID: %d", seid >> 2);
 584
 585                 while (l2cap_frame_get_u8(frame, &seid))
 586                         print_field("ACP SEID: %d", seid >> 2);
 587
 588                 return true;
 589         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 590                 return true;
 591         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 592                 if (!l2cap_frame_get_u8(frame, &seid))
 593                         return false;
 594
 595                 print_field("ACP SEID: %d", seid >> 2);
 596
 597                 return avdtp_reject_common(avdtp_frame);
 598         }
 599
 600         return false;
 601 }
 602
 603 static bool avdtp_abort(struct avdtp_frame *avdtp_frame)
 604 {
 605         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 606         uint8_t type = avdtp_frame->hdr & 0x03;
 607         uint8_t seid;
 608
 609         switch (type) {
 610         case AVDTP_MSG_TYPE_COMMAND:
 611                 if (!l2cap_frame_get_u8(frame, &seid))
 612                         return false;
 613
 614                 print_field("ACP SEID: %d", seid >> 2);
 615
 616                 return true;
 617         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 618                 return true;
 619         }
 620
 621         return false;
 622 }
 623
 624 static bool avdtp_security_control(struct avdtp_frame *avdtp_frame)
 625 {
 626         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 627         uint8_t type = avdtp_frame->hdr & 0x03;
 628         uint8_t seid;
 629
 630         switch (type) {
 631         case AVDTP_MSG_TYPE_COMMAND:
 632                 if (!l2cap_frame_get_u8(frame, &seid))
 633                         return false;
 634
 635                 print_field("ACP SEID: %d", seid >> 2);
 636
 637                 /* TODO: decode more information */
 638                 packet_hexdump(frame->data, frame->size);
 639                 return true;
 640         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 641                 /* TODO: decode more information */
 642                 packet_hexdump(frame->data, frame->size);
 643                 return true;
 644         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 645                 return avdtp_reject_common(avdtp_frame);
 646         }
 647
 648         return false;
 649 }
 650
 651 static bool avdtp_delayreport(struct avdtp_frame *avdtp_frame)
 652 {
 653         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 654         uint8_t type = avdtp_frame->hdr & 0x03;
 655         uint8_t seid;
 656         uint16_t delay;
 657
 658         switch (type) {
 659         case AVDTP_MSG_TYPE_COMMAND:
 660                 if (!l2cap_frame_get_u8(frame, &seid))
 661                         return false;
 662
 663                 print_field("ACP SEID: %d", seid >> 2);
 664
 665                 if (!l2cap_frame_get_be16(frame, &delay))
 666                         return false;
 667
 668                 print_field("Delay: %d.%dms", delay / 10, delay % 10);
 669
 670                 return true;
 671         case AVDTP_MSG_TYPE_RESPONSE_ACCEPT:
 672                 return true;
 673         case AVDTP_MSG_TYPE_RESPONSE_REJECT:
 674                 return avdtp_reject_common(avdtp_frame);
 675         }
 676
 677         return false;
 678 }
 679
 680 static bool avdtp_signalling_packet(struct avdtp_frame *avdtp_frame)
 681 {
 682         struct l2cap_frame *frame = &avdtp_frame->l2cap_frame;
 683         const char *pdu_color;
 684         uint8_t hdr;
 685         uint8_t sig_id;
 686         uint8_t nosp = 0;
 687
 688         if (frame->in)
 689                 pdu_color = COLOR_MAGENTA;
 690         else
 691                 pdu_color = COLOR_BLUE;
 692
 693         if (!l2cap_frame_get_u8(frame, &hdr))
 694                 return false;
 695
 696         avdtp_frame->hdr = hdr;
 697
 698         /* Continue Packet || End Packet */
 699         if (((hdr & 0x0c) == 0x08) || ((hdr & 0x0c) == 0x0c)) {
 700                 /* TODO: handle fragmentation */
 701                 packet_hexdump(frame->data, frame->size);
 702                 return true;
 703         }
 704
 705         /* Start Packet */
 706         if ((hdr & 0x0c) == 0x04) {
 707                 if (!l2cap_frame_get_u8(frame, &nosp))
 708                         return false;
 709         }
 710
 711         if (!l2cap_frame_get_u8(frame, &sig_id))
 712                 return false;
 713
 714         sig_id &= 0x3f;
 715
 716         avdtp_frame->sig_id = sig_id;
 717
 718         print_indent(6, pdu_color, "AVDTP: ", sigid2str(sig_id), COLOR_OFF,
 719                         " (0x%02x) %s (0x%02x) type 0x%02x label %d nosp %d",
 720                         sig_id, msgtype2str(hdr & 0x03), hdr & 0x03,
 721                         hdr & 0x0c, hdr >> 4, nosp);
 722
 723         /* Start Packet */
 724         if ((hdr & 0x0c) == 0x04) {
 725                 /* TODO: handle fragmentation */
 726                 packet_hexdump(frame->data, frame->size);
 727                 return true;
 728         }
 729
 730         /* General Reject */
 731         if ((hdr & 0x03) == 0x03)
 732                 return true;
 733
 734         switch (sig_id) {
 735         case AVDTP_DISCOVER:
 736                 return avdtp_discover(avdtp_frame);
 737         case AVDTP_GET_CAPABILITIES:
 738         case AVDTP_GET_ALL_CAPABILITIES:
 739                 return avdtp_get_capabilities(avdtp_frame);
 740         case AVDTP_SET_CONFIGURATION:
 741                 return avdtp_set_configuration(avdtp_frame);
 742         case AVDTP_GET_CONFIGURATION:
 743                 return avdtp_get_configuration(avdtp_frame);
 744         case AVDTP_RECONFIGURE:
 745                 return avdtp_reconfigure(avdtp_frame);
 746         case AVDTP_OPEN:
 747                 return avdtp_open(avdtp_frame);
 748         case AVDTP_START:
 749                 return avdtp_start(avdtp_frame);
 750         case AVDTP_CLOSE:
 751                 return avdtp_close(avdtp_frame);
 752         case AVDTP_SUSPEND:
 753                 return avdtp_suspend(avdtp_frame);
 754         case AVDTP_ABORT:
 755                 return avdtp_abort(avdtp_frame);
 756         case AVDTP_SECURITY_CONTROL:
 757                 return avdtp_security_control(avdtp_frame);
 758         case AVDTP_DELAYREPORT:
 759                 return avdtp_delayreport(avdtp_frame);
 760         }
 761
 762         packet_hexdump(frame->data, frame->size);
 763
 764         return true;
 765 }
 766
 767 void avdtp_packet(const struct l2cap_frame *frame)
 768 {
 769         struct avdtp_frame avdtp_frame;
 770         bool ret;
 771
 772         l2cap_frame_pull(&avdtp_frame.l2cap_frame, frame, 0);
 773
 774         switch (frame->seq_num) {
 775         case 1:
 776                 ret = avdtp_signalling_packet(&avdtp_frame);
 777                 break;
 778         default:
 779                 packet_hexdump(frame->data, frame->size);
 780                 return;
 781         }
 782
 783         if (!ret) {
 784                 print_text(COLOR_ERROR, "PDU malformed");
 785                 packet_hexdump(frame->data, frame->size);
 786         }
 787 }