test/avtest.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2007-2010  Marcel Holtmann <marcel@holtmann.org>
   6  *  Copyright (C) 2009-2010  Nokia Corporation
   7  *
   8  *
   9  *  This program is free software; you can redistribute it and/or modify
  10  *  it under the terms of the GNU General Public License as published by
  11  *  the Free Software Foundation; either version 2 of the License, or
  12  *  (at your option) any later version.
  13  *
  14  *  This program is distributed in the hope that it will be useful,
  15  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  16  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17  *  GNU General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU General Public License
  20  *  along with this program; if not, write to the Free Software
  21  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  22  *
  23  */
  24
  25 #ifdef HAVE_CONFIG_H
  26 #include <config.h>
  27 #endif
  28
  29 #include <stdio.h>
  30 #include <errno.h>
  31 #include <unistd.h>
  32 #include <stdlib.h>
  33 #include <string.h>
  34 #include <getopt.h>
  35 #include <sys/socket.h>
  36 #include <netinet/in.h>
  37
  38 #include <bluetooth/bluetooth.h>
  39 #include <bluetooth/hci.h>
  40 #include <bluetooth/hci_lib.h>
  41 #include <bluetooth/l2cap.h>
  42 #include <bluetooth/sdp.h>
  43
  44 #define AVDTP_PKT_TYPE_SINGLE           0x00
  45 #define AVDTP_PKT_TYPE_START            0x01
  46 #define AVDTP_PKT_TYPE_CONTINUE         0x02
  47 #define AVDTP_PKT_TYPE_END              0x03
  48
  49 #define AVDTP_MSG_TYPE_COMMAND          0x00
  50 #define AVDTP_MSG_TYPE_GEN_REJECT       0x01
  51 #define AVDTP_MSG_TYPE_ACCEPT           0x02
  52 #define AVDTP_MSG_TYPE_REJECT           0x03
  53
  54 #define AVDTP_DISCOVER                  0x01
  55 #define AVDTP_GET_CAPABILITIES          0x02
  56 #define AVDTP_SET_CONFIGURATION         0x03
  57 #define AVDTP_GET_CONFIGURATION         0x04
  58 #define AVDTP_RECONFIGURE               0x05
  59 #define AVDTP_OPEN                      0x06
  60 #define AVDTP_START                     0x07
  61 #define AVDTP_CLOSE                     0x08
  62 #define AVDTP_SUSPEND                   0x09
  63 #define AVDTP_ABORT                     0x0A
  64
  65 #define AVDTP_SEP_TYPE_SOURCE           0x00
  66 #define AVDTP_SEP_TYPE_SINK             0x01
  67
  68 #define AVDTP_MEDIA_TYPE_AUDIO          0x00
  69 #define AVDTP_MEDIA_TYPE_VIDEO          0x01
  70 #define AVDTP_MEDIA_TYPE_MULTIMEDIA     0x02
  71
  72 #if __BYTE_ORDER == __LITTLE_ENDIAN
  73
  74 struct avdtp_header {
  75         uint8_t message_type:2;
  76         uint8_t packet_type:2;
  77         uint8_t transaction:4;
  78         uint8_t signal_id:6;
  79         uint8_t rfa0:2;
  80 } __attribute__ ((packed));
  81
  82 struct seid_info {
  83         uint8_t rfa0:1;
  84         uint8_t inuse:1;
  85         uint8_t seid:6;
  86         uint8_t rfa2:3;
  87         uint8_t type:1;
  88         uint8_t media_type:4;
  89 } __attribute__ ((packed));
  90
  91 struct avdtp_start_header {
  92         uint8_t message_type:2;
  93         uint8_t packet_type:2;
  94         uint8_t transaction:4;
  95         uint8_t no_of_packets;
  96         uint8_t signal_id:6;
  97         uint8_t rfa0:2;
  98 } __attribute__ ((packed));
  99
 100 struct avdtp_continue_header {
 101         uint8_t message_type:2;
 102         uint8_t packet_type:2;
 103         uint8_t transaction:4;
 104 } __attribute__ ((packed));
 105
 106 struct avctp_header {
 107         uint8_t ipid:1;
 108         uint8_t cr:1;
 109         uint8_t packet_type:2;
 110         uint8_t transaction:4;
 111         uint16_t pid;
 112 } __attribute__ ((packed));
 113 #define AVCTP_HEADER_LENGTH 3
 114
 115 #elif __BYTE_ORDER == __BIG_ENDIAN
 116
 117 struct avdtp_header {
 118         uint8_t transaction:4;
 119         uint8_t packet_type:2;
 120         uint8_t message_type:2;
 121         uint8_t rfa0:2;
 122         uint8_t signal_id:6;
 123 } __attribute__ ((packed));
 124
 125 struct seid_info {
 126         uint8_t seid:6;
 127         uint8_t inuse:1;
 128         uint8_t rfa0:1;
 129         uint8_t media_type:4;
 130         uint8_t type:1;
 131         uint8_t rfa2:3;
 132 } __attribute__ ((packed));
 133
 134 struct avdtp_start_header {
 135         uint8_t transaction:4;
 136         uint8_t packet_type:2;
 137         uint8_t message_type:2;
 138         uint8_t no_of_packets;
 139         uint8_t rfa0:2;
 140         uint8_t signal_id:6;
 141 } __attribute__ ((packed));
 142
 143 struct avdtp_continue_header {
 144         uint8_t transaction:4;
 145         uint8_t packet_type:2;
 146         uint8_t message_type:2;
 147 } __attribute__ ((packed));
 148
 149 struct avctp_header {
 150         uint8_t transaction:4;
 151         uint8_t packet_type:2;
 152         uint8_t cr:1;
 153         uint8_t ipid:1;
 154         uint16_t pid;
 155 } __attribute__ ((packed));
 156 #define AVCTP_HEADER_LENGTH 3
 157
 158 #else
 159 #error "Unknown byte order"
 160 #endif
 161
 162 #define AVCTP_COMMAND           0
 163 #define AVCTP_RESPONSE          1
 164
 165 #define AVCTP_PACKET_SINGLE     0
 166
 167 static const unsigned char media_transport[] = {
 168                 0x01,   /* Media transport category */
 169                 0x00,
 170                 0x07,   /* Media codec category */
 171                 0x06,
 172                 0x00,   /* Media type audio */
 173                 0x00,   /* Codec SBC */
 174                 0x22,   /* 44.1 kHz, stereo */
 175                 0x15,   /* 16 blocks, 8 subbands */
 176                 0x02,
 177                 0x33,
 178 };
 179
 180 static int media_sock = -1;
 181
 182 static void dump_avctp_header(struct avctp_header *hdr)
 183 {
 184         printf("TL %d PT %d CR %d IPID %d PID 0x%04x\n", hdr->transaction,
 185                         hdr->packet_type, hdr->cr, hdr->ipid, ntohs(hdr->pid));
 186 }
 187
 188 static void dump_avdtp_header(struct avdtp_header *hdr)
 189 {
 190         printf("TL %d PT %d MT %d SI %d\n", hdr->transaction,
 191                         hdr->packet_type, hdr->message_type, hdr->signal_id);
 192 }
 193
 194 static void dump_buffer(const unsigned char *buf, int len)
 195 {
 196         int i;
 197
 198         for (i = 0; i < len; i++)
 199                 printf("%02x ", buf[i]);
 200         printf("\n");
 201 }
 202
 203 static void process_avdtp(int srv_sk, int sk, unsigned char reject,
 204                                                                 int fragment)
 205 {
 206         unsigned char buf[672];
 207         ssize_t len;
 208
 209         while (1) {
 210                 struct avdtp_header *hdr = (void *) buf;
 211
 212                 len = read(sk, buf, sizeof(buf));
 213                 if (len <= 0) {
 214                         perror("Read failed");
 215                         break;
 216                 }
 217
 218                 dump_buffer(buf, len);
 219                 dump_avdtp_header(hdr);
 220
 221                 if (hdr->packet_type != AVDTP_PKT_TYPE_SINGLE) {
 222                         fprintf(stderr, "Only single packets are supported\n");
 223                         break;
 224                 }
 225
 226                 if (hdr->message_type != AVDTP_MSG_TYPE_COMMAND) {
 227                         fprintf(stderr, "Ignoring non-command messages\n");
 228                         continue;
 229                 }
 230
 231                 switch (hdr->signal_id) {
 232                 case AVDTP_DISCOVER:
 233                         if (reject == AVDTP_DISCOVER) {
 234                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 235                                 buf[2] = 0x29; /* Unsupported configuration */
 236                                 printf("Rejecting discover command\n");
 237                                 len = write(sk, buf, 3);
 238                         } else {
 239                                 struct seid_info *sei = (void *) (buf + 2);
 240                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 241                                 buf[2] = 0x00;
 242                                 buf[3] = 0x00;
 243                                 sei->seid = 0x01;
 244                                 sei->type = AVDTP_SEP_TYPE_SINK;
 245                                 sei->media_type = AVDTP_MEDIA_TYPE_AUDIO;
 246                                 printf("Accepting discover command\n");
 247                                 len = write(sk, buf, 4);
 248                         }
 249                         break;
 250
 251                 case AVDTP_GET_CAPABILITIES:
 252                         if (reject == AVDTP_GET_CAPABILITIES) {
 253                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 254                                 buf[2] = 0x29; /* Unsupported configuration */
 255                                 printf("Rejecting get capabilties command\n");
 256                                 len = write(sk, buf, 3);
 257                         } else if (fragment) {
 258                                 struct avdtp_start_header *start = (void *) buf;
 259
 260                                 printf("Sending fragmented reply to getcap\n");
 261
 262                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 263
 264                                 /* Start packet */
 265                                 hdr->packet_type = AVDTP_PKT_TYPE_START;
 266                                 start->signal_id = AVDTP_GET_CAPABILITIES;
 267                                 start->no_of_packets = 3;
 268                                 memcpy(&buf[3], media_transport,
 269                                                 sizeof(media_transport));
 270                                 len = write(sk, buf,
 271                                                 3 + sizeof(media_transport));
 272
 273                                 /* Continue packet */
 274                                 hdr->packet_type = AVDTP_PKT_TYPE_CONTINUE;
 275                                 memcpy(&buf[1], media_transport,
 276                                                 sizeof(media_transport));
 277                                 len = write(sk, buf,
 278                                                 1 + sizeof(media_transport));
 279
 280                                 /* End packet */
 281                                 hdr->packet_type = AVDTP_PKT_TYPE_END;
 282                                 memcpy(&buf[1], media_transport,
 283                                                 sizeof(media_transport));
 284                                 len = write(sk, buf,
 285                                                 1 + sizeof(media_transport));
 286                         } else {
 287                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 288                                 memcpy(&buf[2], media_transport,
 289                                                 sizeof(media_transport));
 290                                 printf("Accepting get capabilities command\n");
 291                                 len = write(sk, buf,
 292                                                 2 + sizeof(media_transport));
 293                         }
 294                         break;
 295
 296                 case AVDTP_SET_CONFIGURATION:
 297                         if (reject == AVDTP_SET_CONFIGURATION) {
 298                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 299                                 buf[2] = buf[4];
 300                                 buf[3] = 0x13; /* SEP In Use */
 301                                 printf("Rejecting set configuration command\n");
 302                                 len = write(sk, buf, 4);
 303                         } else {
 304                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 305                                 printf("Accepting set configuration command\n");
 306                                 len = write(sk, buf, 2);
 307                         }
 308                         break;
 309
 310                 case AVDTP_GET_CONFIGURATION:
 311                         if (reject == AVDTP_GET_CONFIGURATION) {
 312                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 313                                 buf[2] = 0x12; /* Bad ACP SEID */
 314                                 printf("Rejecting get configuration command\n");
 315                                 len = write(sk, buf, 3);
 316                         } else {
 317                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 318                                 printf("Accepting get configuration command\n");
 319                                 len = write(sk, buf, 2);
 320                         }
 321                         break;
 322
 323                 case AVDTP_OPEN:
 324                         if (reject == AVDTP_OPEN) {
 325                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 326                                 buf[2] = 0x31; /* Bad State */
 327                                 printf("Rejecting open command\n");
 328                                 len = write(sk, buf, 3);
 329                         } else {
 330                                 struct sockaddr_l2 addr;
 331                                 socklen_t optlen;
 332
 333                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 334                                 printf("Accepting open command\n");
 335                                 len = write(sk, buf, 2);
 336
 337                                 memset(&addr, 0, sizeof(addr));
 338                                 optlen = sizeof(addr);
 339
 340                                 media_sock = accept(srv_sk,
 341                                                 (struct sockaddr *) &addr,
 342                                                                 &optlen);
 343                                 if (media_sock < 0) {
 344                                         perror("Accept failed");
 345                                         break;
 346                                 }
 347                         }
 348                         break;
 349
 350                 case AVDTP_START:
 351                         if (reject == AVDTP_ABORT)
 352                                 printf("Ignoring start to cause abort");
 353                         else if (reject == AVDTP_START) {
 354                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 355                                 buf[3] = 0x31; /* Bad State */
 356                                 printf("Rejecting start command\n");
 357                                 len = write(sk, buf, 4);
 358                         } else {
 359                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 360                                 printf("Accepting start command\n");
 361                                 len = write(sk, buf, 2);
 362                         }
 363                         break;
 364
 365                 case AVDTP_CLOSE:
 366                         if (reject == AVDTP_CLOSE) {
 367                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 368                                 buf[2] = 0x31; /* Bad State */
 369                                 printf("Rejecting close command\n");
 370                                 len = write(sk, buf, 3);
 371                         } else {
 372                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 373                                 printf("Accepting close command\n");
 374                                 len = write(sk, buf, 2);
 375                                 if (media_sock >= 0) {
 376                                         close(media_sock);
 377                                         media_sock = -1;
 378                                 }
 379                         }
 380                         break;
 381
 382                 case AVDTP_SUSPEND:
 383                         if (reject == AVDTP_SUSPEND) {
 384                                 hdr->message_type = AVDTP_MSG_TYPE_REJECT;
 385                                 buf[3] = 0x31; /* Bad State */
 386                                 printf("Rejecting suspend command\n");
 387                                 len = write(sk, buf, 4);
 388                         } else {
 389                                 hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 390                                 printf("Accepting suspend command\n");
 391                                 len = write(sk, buf, 2);
 392                         }
 393                         break;
 394
 395                 case AVDTP_ABORT:
 396                         hdr->message_type = AVDTP_MSG_TYPE_ACCEPT;
 397                         printf("Accepting abort command\n");
 398                         len = write(sk, buf, 2);
 399                         if (media_sock >= 0) {
 400                                 close(media_sock);
 401                                 media_sock = -1;
 402                         }
 403                         break;
 404
 405                 default:
 406                         buf[1] = 0x00;
 407                         printf("Unknown command\n");
 408                         len = write(sk, buf, 2);
 409                         break;
 410                 }
 411         }
 412 }
 413
 414 static void process_avctp(int sk, int reject)
 415 {
 416         unsigned char buf[672];
 417         ssize_t len;
 418
 419         while (1) {
 420                 struct avctp_header *hdr = (void *) buf;
 421
 422                 len = read(sk, buf, sizeof(buf));
 423                 if (len <= 0) {
 424                         perror("Read failed");
 425                         break;
 426                 }
 427
 428                 dump_buffer(buf, len);
 429
 430                 if (len >= AVCTP_HEADER_LENGTH)
 431                         dump_avctp_header(hdr);
 432         }
 433 }
 434
 435 static int set_minimum_mtu(int sk)
 436 {
 437         struct l2cap_options l2o;
 438         socklen_t optlen;
 439
 440         memset(&l2o, 0, sizeof(l2o));
 441         optlen = sizeof(l2o);
 442
 443         if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, &optlen) < 0) {
 444                 perror("getsockopt");
 445                 return -1;
 446         }
 447
 448         l2o.imtu = 48;
 449         l2o.omtu = 48;
 450
 451         if (setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &l2o, sizeof(l2o)) < 0) {
 452                 perror("setsockopt");
 453                 return -1;
 454         }
 455
 456         return 0;
 457 }
 458
 459 static void do_listen(const bdaddr_t *src, unsigned char reject, int fragment)
 460 {
 461         struct sockaddr_l2 addr;
 462         socklen_t optlen;
 463         int sk, nsk;
 464
 465         sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
 466         if (sk < 0) {
 467                 perror("Can't create socket");
 468                 return;
 469         }
 470
 471         memset(&addr, 0, sizeof(addr));
 472         addr.l2_family = AF_BLUETOOTH;
 473         bacpy(&addr.l2_bdaddr, src);
 474         addr.l2_psm = htobs(25);
 475
 476         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 477                 perror("Can't bind socket");
 478                 goto error;
 479         }
 480
 481         if (fragment)
 482                 set_minimum_mtu(sk);
 483
 484         if (listen(sk, 10)) {
 485                 perror("Can't listen on the socket");
 486                 goto error;
 487         }
 488
 489         while (1) {
 490                 memset(&addr, 0, sizeof(addr));
 491                 optlen = sizeof(addr);
 492
 493                 nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
 494                 if (nsk < 0) {
 495                         perror("Accept failed");
 496                         continue;
 497                 }
 498
 499                 process_avdtp(sk, nsk, reject, fragment);
 500
 501                 if (media_sock >= 0) {
 502                         close(media_sock);
 503                         media_sock = -1;
 504                 }
 505
 506                 close(nsk);
 507         }
 508
 509 error:
 510         close(sk);
 511 }
 512
 513 static int do_connect(const bdaddr_t *src, const bdaddr_t *dst, int avctp,
 514                                                                 int fragment)
 515 {
 516         struct sockaddr_l2 addr;
 517         int sk, err;
 518
 519         sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
 520         if (sk < 0) {
 521                 perror("Can't create socket");
 522                 return -1;
 523         }
 524
 525         memset(&addr, 0, sizeof(addr));
 526         addr.l2_family = AF_BLUETOOTH;
 527         bacpy(&addr.l2_bdaddr, src);
 528
 529         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 530                 perror("Can't bind socket");
 531                 goto error;
 532         }
 533
 534         if (fragment)
 535                 set_minimum_mtu(sk);
 536
 537         memset(&addr, 0, sizeof(addr));
 538         addr.l2_family = AF_BLUETOOTH;
 539         bacpy(&addr.l2_bdaddr, dst);
 540         addr.l2_psm = htobs(avctp ? 23 : 25);
 541
 542         err = connect(sk, (struct sockaddr *) &addr, sizeof(addr));
 543         if (err < 0) {
 544                 perror("Unable to connect");
 545                 goto error;
 546         }
 547
 548         return sk;
 549
 550 error:
 551         close(sk);
 552         return -1;
 553 }
 554
 555 static void do_avdtp_send(int sk, const bdaddr_t *src, const bdaddr_t *dst,
 556                                 unsigned char cmd, int invalid, int preconf)
 557 {
 558         unsigned char buf[672];
 559         struct avdtp_header *hdr = (void *) buf;
 560         ssize_t len;
 561
 562         memset(buf, 0, sizeof(buf));
 563
 564         switch (cmd) {
 565         case AVDTP_DISCOVER:
 566                 if (invalid)
 567                         hdr->message_type = 0x01;
 568                 else
 569                         hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 570                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 571                 hdr->signal_id = AVDTP_DISCOVER;
 572                 len = write(sk, buf, 2);
 573                 break;
 574
 575         case AVDTP_GET_CAPABILITIES:
 576                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 577                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 578                 hdr->signal_id = AVDTP_GET_CAPABILITIES;
 579                 buf[2] = 1 << 2; /* SEID 1 */
 580                 len = write(sk, buf, invalid ? 2 : 3);
 581                 break;
 582
 583         case AVDTP_SET_CONFIGURATION:
 584                 if (preconf)
 585                         do_avdtp_send(sk, src, dst, cmd, 0, 0);
 586                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 587                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 588                 hdr->signal_id = AVDTP_SET_CONFIGURATION;
 589                 buf[2] = 1 << 2; /* ACP SEID */
 590                 buf[3] = 1 << 2; /* INT SEID */
 591                 memcpy(&buf[4], media_transport, sizeof(media_transport));
 592                 if (invalid)
 593                         buf[5] = 0x01; /* LOSC != 0 */
 594                 len = write(sk, buf, 4 + sizeof(media_transport));
 595                 break;
 596
 597         case AVDTP_GET_CONFIGURATION:
 598                 if (preconf)
 599                         do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
 600                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 601                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 602                 hdr->signal_id = AVDTP_GET_CONFIGURATION;
 603                 if (invalid)
 604                         buf[2] = 13 << 2; /* Invalid ACP SEID */
 605                 else
 606                         buf[2] = 1 << 2; /* Valid ACP SEID */
 607                 len = write(sk, buf, 3);
 608                 break;
 609
 610         case AVDTP_OPEN:
 611                 if (preconf)
 612                         do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
 613                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 614                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 615                 hdr->signal_id = AVDTP_OPEN;
 616                 buf[2] = 1 << 2; /* ACP SEID */
 617                 len = write(sk, buf, 3);
 618                 break;
 619
 620         case AVDTP_START:
 621                 if (preconf)
 622                         do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
 623                 if (!invalid)
 624                         do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, 0);
 625                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 626                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 627                 hdr->signal_id = AVDTP_START;
 628                 buf[2] = 1 << 2; /* ACP SEID */
 629                 len = write(sk, buf, 3);
 630                 break;
 631
 632         case AVDTP_CLOSE:
 633                 if (preconf) {
 634                         do_avdtp_send(sk, src, dst, AVDTP_SET_CONFIGURATION, 0, 0);
 635                         do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, 0);
 636                 }
 637                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 638                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 639                 hdr->signal_id = AVDTP_CLOSE;
 640                 if (invalid)
 641                         buf[2] = 13 << 2; /* Invalid ACP SEID */
 642                 else
 643                         buf[2] = 1 << 2; /* Valid ACP SEID */
 644                 len = write(sk, buf, 3);
 645                 break;
 646
 647         case AVDTP_SUSPEND:
 648                 if (invalid)
 649                         do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, preconf);
 650                 else
 651                         do_avdtp_send(sk, src, dst, AVDTP_START, 0, preconf);
 652                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 653                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 654                 hdr->signal_id = AVDTP_SUSPEND;
 655                 buf[2] = 1 << 2; /* ACP SEID */
 656                 len = write(sk, buf, 3);
 657                 break;
 658
 659         case AVDTP_ABORT:
 660                 do_avdtp_send(sk, src, dst, AVDTP_OPEN, 0, 1);
 661                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 662                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 663                 hdr->signal_id = AVDTP_ABORT;
 664                 buf[2] = 1 << 2; /* ACP SEID */
 665                 len = write(sk, buf, 3);
 666                 break;
 667
 668         default:
 669                 hdr->message_type = AVDTP_MSG_TYPE_COMMAND;
 670                 hdr->packet_type = AVDTP_PKT_TYPE_SINGLE;
 671                 hdr->signal_id = cmd;
 672                 len = write(sk, buf, 2);
 673                 break;
 674         }
 675
 676         do {
 677                 len = read(sk, buf, sizeof(buf));
 678
 679                 dump_buffer(buf, len);
 680                 dump_avdtp_header(hdr);
 681         } while (len < 2 || (hdr->message_type != AVDTP_MSG_TYPE_ACCEPT &&
 682                                 hdr->message_type != AVDTP_MSG_TYPE_REJECT &&
 683                                 hdr->message_type != AVDTP_MSG_TYPE_GEN_REJECT));
 684
 685         if (cmd == AVDTP_OPEN && len >= 2 &&
 686                                 hdr->message_type == AVDTP_MSG_TYPE_ACCEPT)
 687                 media_sock = do_connect(src, dst, 0, 0);
 688 }
 689
 690 static void do_avctp_send(int sk, int invalid)
 691 {
 692         unsigned char buf[672];
 693         struct avctp_header *hdr = (void *) buf;
 694         unsigned char play_pressed[] = { 0x00, 0x48, 0x7c, 0x44, 0x00 };
 695         ssize_t len;
 696
 697         memset(buf, 0, sizeof(buf));
 698
 699         hdr->packet_type = AVCTP_PACKET_SINGLE;
 700         hdr->cr = AVCTP_COMMAND;
 701         if (invalid)
 702                 hdr->pid = 0xffff;
 703         else
 704                 hdr->pid = htons(AV_REMOTE_SVCLASS_ID);
 705
 706         memcpy(&buf[AVCTP_HEADER_LENGTH], play_pressed, sizeof(play_pressed));
 707
 708         len = write(sk, buf, AVCTP_HEADER_LENGTH + sizeof(play_pressed));
 709
 710         len = read(sk, buf, sizeof(buf));
 711
 712         dump_buffer(buf, len);
 713         if (len >= AVCTP_HEADER_LENGTH)
 714                 dump_avctp_header(hdr);
 715 }
 716
 717 static void usage(void)
 718 {
 719         printf("avtest - Audio/Video testing ver %s\n", VERSION);
 720         printf("Usage:\n"
 721                 "\tavtest [options] [remote address]\n");
 722         printf("Options:\n"
 723                 "\t--device <hcidev>    HCI device\n"
 724                 "\t--reject <command>   Reject command\n"
 725                 "\t--send <command>     Send command\n"
 726                 "\t--preconf            Configure stream before actual command\n"
 727                 "\t--wait <N>           Wait N seconds before exiting\n"
 728                 "\t--fragment           Use minimum MTU and fragmented messages\n"
 729                 "\t--invalid <command>  Send invalid command\n");
 730 }
 731
 732 static struct option main_options[] = {
 733         { "help",       0, 0, 'h' },
 734         { "device",     1, 0, 'i' },
 735         { "reject",     1, 0, 'r' },
 736         { "send",       1, 0, 's' },
 737         { "invalid",    1, 0, 'f' },
 738         { "preconf",    0, 0, 'c' },
 739         { "fragment",   0, 0, 'F' },
 740         { "avctp",      0, 0, 'C' },
 741         { "wait",       1, 0, 'w' },
 742         { 0, 0, 0, 0 }
 743 };
 744
 745 static unsigned char parse_cmd(const char *arg)
 746 {
 747         if (!strncmp(arg, "discov", 6))
 748                 return AVDTP_DISCOVER;
 749         else if (!strncmp(arg, "capa", 4))
 750                 return AVDTP_GET_CAPABILITIES;
 751         else if (!strncmp(arg, "getcapa", 7))
 752                 return AVDTP_GET_CAPABILITIES;
 753         else if (!strncmp(arg, "setconf", 7))
 754                 return AVDTP_SET_CONFIGURATION;
 755         else if (!strncmp(arg, "getconf", 7))
 756                 return AVDTP_GET_CONFIGURATION;
 757         else if (!strncmp(arg, "open", 4))
 758                 return AVDTP_OPEN;
 759         else if (!strncmp(arg, "start", 5))
 760                 return AVDTP_START;
 761         else if (!strncmp(arg, "close", 5))
 762                 return AVDTP_CLOSE;
 763         else if (!strncmp(arg, "suspend", 7))
 764                 return AVDTP_SUSPEND;
 765         else if (!strncmp(arg, "abort", 7))
 766                 return AVDTP_ABORT;
 767         else
 768                 return atoi(arg);
 769 }
 770
 771 enum {
 772         MODE_NONE, MODE_REJECT, MODE_SEND,
 773 };
 774
 775 int main(int argc, char *argv[])
 776 {
 777         unsigned char cmd = 0x00;
 778         bdaddr_t src, dst;
 779         int opt, mode = MODE_NONE, sk, invalid = 0, preconf = 0, fragment = 0;
 780         int avctp = 0, wait_before_exit = 0;
 781
 782         bacpy(&src, BDADDR_ANY);
 783         bacpy(&dst, BDADDR_ANY);
 784
 785         while ((opt = getopt_long(argc, argv, "+i:r:s:f:hcFCw:",
 786                                                 main_options, NULL)) != EOF) {
 787                 switch (opt) {
 788                 case 'i':
 789                         if (!strncmp(optarg, "hci", 3))
 790                                 hci_devba(atoi(optarg + 3), &src);
 791                         else
 792                                 str2ba(optarg, &src);
 793                         break;
 794
 795                 case 'r':
 796                         mode = MODE_REJECT;
 797                         cmd = parse_cmd(optarg);
 798                         break;
 799
 800                 case 'f':
 801                         invalid = 1;
 802                         /* Intentionally missing break */
 803
 804                 case 's':
 805                         mode = MODE_SEND;
 806                         cmd = parse_cmd(optarg);
 807                         break;
 808
 809                 case 'c':
 810                         preconf = 1;
 811                         break;
 812
 813                 case 'F':
 814                         fragment = 1;
 815                         break;
 816
 817                 case 'C':
 818                         avctp = 1;
 819                         break;
 820
 821                 case 'w':
 822                         wait_before_exit = atoi(optarg);
 823                         break;
 824
 825                 case 'h':
 826                 default:
 827                         usage();
 828                         exit(0);
 829                 }
 830         }
 831
 832         if (argv[optind])
 833                 str2ba(argv[optind], &dst);
 834
 835         if (avctp) {
 836                 avctp = mode;
 837                 mode = MODE_SEND;
 838         }
 839
 840         switch (mode) {
 841         case MODE_REJECT:
 842                 do_listen(&src, cmd, fragment);
 843                 break;
 844         case MODE_SEND:
 845                 sk = do_connect(&src, &dst, avctp, fragment);
 846                 if (sk < 0)
 847                         exit(1);
 848                 if (avctp) {
 849                         if (avctp == MODE_SEND)
 850                                 do_avctp_send(sk, invalid);
 851                         else
 852                                 process_avctp(sk, cmd);
 853                 } else
 854                         do_avdtp_send(sk, &src, &dst, cmd, invalid, preconf);
 855                 if (wait_before_exit) {
 856                         printf("Waiting %d seconds before exiting\n", wait_before_exit);
 857                         sleep(wait_before_exit);
 858                 }
 859                 if (media_sock >= 0)
 860                         close(media_sock);
 861                 close(sk);
 862                 break;
 863         default:
 864                 fprintf(stderr, "No operating mode specified!\n");
 865                 exit(1);
 866         }
 867
 868         return 0;
 869 }