android/avdtp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2006-2010  Nokia Corporation
   6  *  Copyright (C) 2004-2010  Marcel Holtmann <marcel@holtmann.org>
   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 <stdlib.h>
  30 #include <stdio.h>
  31 #include <stdint.h>
  32 #include <stdbool.h>
  33 #include <errno.h>
  34 #include <unistd.h>
  35 #include <assert.h>
  36 #include <string.h>
  37 #include <sys/socket.h>
  38 #include <netinet/in.h>
  39
  40 #include <glib.h>
  41
  42 #include "lib/bluetooth.h"
  43 #include "src/log.h"
  44 #include "src/shared/util.h"
  45 #include "src/shared/queue.h"
  46 #include "avdtp.h"
  47 #include "../profiles/audio/a2dp-codecs.h"
  48
  49 #define MAX_SEID 0x3E
  50 static unsigned int seids;
  51
  52 #ifndef MAX
  53 # define MAX(x, y) ((x) > (y) ? (x) : (y))
  54 #endif
  55
  56 #define AVDTP_DISCOVER                          0x01
  57 #define AVDTP_GET_CAPABILITIES                  0x02
  58 #define AVDTP_SET_CONFIGURATION                 0x03
  59 #define AVDTP_GET_CONFIGURATION                 0x04
  60 #define AVDTP_RECONFIGURE                       0x05
  61 #define AVDTP_OPEN                              0x06
  62 #define AVDTP_START                             0x07
  63 #define AVDTP_CLOSE                             0x08
  64 #define AVDTP_SUSPEND                           0x09
  65 #define AVDTP_ABORT                             0x0A
  66 #define AVDTP_SECURITY_CONTROL                  0x0B
  67 #define AVDTP_GET_ALL_CAPABILITIES              0x0C
  68 #define AVDTP_DELAY_REPORT                      0x0D
  69
  70 #define AVDTP_PKT_TYPE_SINGLE                   0x00
  71 #define AVDTP_PKT_TYPE_START                    0x01
  72 #define AVDTP_PKT_TYPE_CONTINUE                 0x02
  73 #define AVDTP_PKT_TYPE_END                      0x03
  74
  75 #define AVDTP_MSG_TYPE_COMMAND                  0x00
  76 #define AVDTP_MSG_TYPE_GEN_REJECT               0x01
  77 #define AVDTP_MSG_TYPE_ACCEPT                   0x02
  78 #define AVDTP_MSG_TYPE_REJECT                   0x03
  79
  80 #define REQ_TIMEOUT 6
  81 #define ABORT_TIMEOUT 2
  82 #define DISCONNECT_TIMEOUT 1
  83 #define START_TIMEOUT 1
  84
  85 #if __BYTE_ORDER == __LITTLE_ENDIAN
  86
  87 struct avdtp_common_header {
  88         uint8_t message_type:2;
  89         uint8_t packet_type:2;
  90         uint8_t transaction:4;
  91 } __attribute__ ((packed));
  92
  93 struct avdtp_single_header {
  94         uint8_t message_type:2;
  95         uint8_t packet_type:2;
  96         uint8_t transaction:4;
  97         uint8_t signal_id:6;
  98         uint8_t rfa0:2;
  99 } __attribute__ ((packed));
 100
 101 struct avdtp_start_header {
 102         uint8_t message_type:2;
 103         uint8_t packet_type:2;
 104         uint8_t transaction:4;
 105         uint8_t no_of_packets;
 106         uint8_t signal_id:6;
 107         uint8_t rfa0:2;
 108 } __attribute__ ((packed));
 109
 110 struct avdtp_continue_header {
 111         uint8_t message_type:2;
 112         uint8_t packet_type:2;
 113         uint8_t transaction:4;
 114 } __attribute__ ((packed));
 115
 116 struct seid_info {
 117         uint8_t rfa0:1;
 118         uint8_t inuse:1;
 119         uint8_t seid:6;
 120         uint8_t rfa2:3;
 121         uint8_t type:1;
 122         uint8_t media_type:4;
 123 } __attribute__ ((packed));
 124
 125 struct seid {
 126         uint8_t rfa0:2;
 127         uint8_t seid:6;
 128 } __attribute__ ((packed));
 129
 130 #elif __BYTE_ORDER == __BIG_ENDIAN
 131
 132 struct avdtp_common_header {
 133         uint8_t transaction:4;
 134         uint8_t packet_type:2;
 135         uint8_t message_type:2;
 136 } __attribute__ ((packed));
 137
 138 struct avdtp_single_header {
 139         uint8_t transaction:4;
 140         uint8_t packet_type:2;
 141         uint8_t message_type:2;
 142         uint8_t rfa0:2;
 143         uint8_t signal_id:6;
 144 } __attribute__ ((packed));
 145
 146 struct avdtp_start_header {
 147         uint8_t transaction:4;
 148         uint8_t packet_type:2;
 149         uint8_t message_type:2;
 150         uint8_t no_of_packets;
 151         uint8_t rfa0:2;
 152         uint8_t signal_id:6;
 153 } __attribute__ ((packed));
 154
 155 struct avdtp_continue_header {
 156         uint8_t transaction:4;
 157         uint8_t packet_type:2;
 158         uint8_t message_type:2;
 159 } __attribute__ ((packed));
 160
 161 struct seid_info {
 162         uint8_t seid:6;
 163         uint8_t inuse:1;
 164         uint8_t rfa0:1;
 165         uint8_t media_type:4;
 166         uint8_t type:1;
 167         uint8_t rfa2:3;
 168 } __attribute__ ((packed));
 169
 170 struct seid {
 171         uint8_t seid:6;
 172         uint8_t rfa0:2;
 173 } __attribute__ ((packed));
 174
 175 #else
 176 #error "Unknown byte order"
 177 #endif
 178
 179 /* packets */
 180
 181 struct discover_resp {
 182         struct seid_info seps[0];
 183 } __attribute__ ((packed));
 184
 185 struct getcap_resp {
 186         uint8_t caps[0];
 187 } __attribute__ ((packed));
 188
 189 struct start_req {
 190         struct seid first_seid;
 191         struct seid other_seids[0];
 192 } __attribute__ ((packed));
 193
 194 struct suspend_req {
 195         struct seid first_seid;
 196         struct seid other_seids[0];
 197 } __attribute__ ((packed));
 198
 199 struct seid_rej {
 200         uint8_t error;
 201 } __attribute__ ((packed));
 202
 203 struct conf_rej {
 204         uint8_t category;
 205         uint8_t error;
 206 } __attribute__ ((packed));
 207
 208 #if __BYTE_ORDER == __LITTLE_ENDIAN
 209
 210 struct seid_req {
 211         uint8_t rfa0:2;
 212         uint8_t acp_seid:6;
 213 } __attribute__ ((packed));
 214
 215 struct setconf_req {
 216         uint8_t rfa0:2;
 217         uint8_t acp_seid:6;
 218         uint8_t rfa1:2;
 219         uint8_t int_seid:6;
 220
 221         uint8_t caps[0];
 222 } __attribute__ ((packed));
 223
 224 struct stream_rej {
 225         uint8_t rfa0:2;
 226         uint8_t acp_seid:6;
 227         uint8_t error;
 228 } __attribute__ ((packed));
 229
 230 struct reconf_req {
 231         uint8_t rfa0:2;
 232         uint8_t acp_seid:6;
 233
 234         uint8_t serv_cap;
 235         uint8_t serv_cap_len;
 236
 237         uint8_t caps[0];
 238 } __attribute__ ((packed));
 239
 240 struct delay_req {
 241         uint8_t rfa0:2;
 242         uint8_t acp_seid:6;
 243         uint16_t delay;
 244 } __attribute__ ((packed));
 245
 246 #elif __BYTE_ORDER == __BIG_ENDIAN
 247
 248 struct seid_req {
 249         uint8_t acp_seid:6;
 250         uint8_t rfa0:2;
 251 } __attribute__ ((packed));
 252
 253 struct setconf_req {
 254         uint8_t acp_seid:6;
 255         uint8_t rfa0:2;
 256         uint8_t int_seid:6;
 257         uint8_t rfa1:2;
 258
 259         uint8_t caps[0];
 260 } __attribute__ ((packed));
 261
 262 struct stream_rej {
 263         uint8_t acp_seid:6;
 264         uint8_t rfa0:2;
 265         uint8_t error;
 266 } __attribute__ ((packed));
 267
 268 struct reconf_req {
 269         uint8_t acp_seid:6;
 270         uint8_t rfa0:2;
 271
 272         uint8_t serv_cap;
 273         uint8_t serv_cap_len;
 274
 275         uint8_t caps[0];
 276 } __attribute__ ((packed));
 277
 278 struct delay_req {
 279         uint8_t acp_seid:6;
 280         uint8_t rfa0:2;
 281         uint16_t delay;
 282 } __attribute__ ((packed));
 283
 284 #else
 285 #error "Unknown byte order"
 286 #endif
 287
 288 struct in_buf {
 289         gboolean active;
 290         int no_of_packets;
 291         uint8_t transaction;
 292         uint8_t message_type;
 293         uint8_t signal_id;
 294         uint8_t buf[1024];
 295         uint8_t data_size;
 296 };
 297
 298 struct pending_req {
 299         uint8_t transaction;
 300         uint8_t signal_id;
 301         void *data;
 302         size_t data_size;
 303         struct avdtp_stream *stream; /* Set if the request targeted a stream */
 304         guint timeout;
 305         gboolean collided;
 306 };
 307
 308 struct avdtp_remote_sep {
 309         uint8_t seid;
 310         uint8_t type;
 311         uint8_t media_type;
 312         struct avdtp_service_capability *codec;
 313         gboolean delay_reporting;
 314         GSList *caps; /* of type struct avdtp_service_capability */
 315         struct avdtp_stream *stream;
 316 };
 317
 318 struct avdtp_local_sep {
 319         avdtp_state_t state;
 320         struct avdtp_stream *stream;
 321         struct seid_info info;
 322         uint8_t codec;
 323         uint32_t vndcodec_vendor;
 324         uint16_t vndcodec_codec;
 325         gboolean delay_reporting;
 326         GSList *caps;
 327         struct avdtp_sep_ind *ind;
 328         struct avdtp_sep_cfm *cfm;
 329         void *user_data;
 330 };
 331
 332 struct stream_callback {
 333         avdtp_stream_state_cb cb;
 334         void *user_data;
 335         unsigned int id;
 336 };
 337
 338 struct discover_callback {
 339         unsigned int id;
 340         avdtp_discover_cb_t cb;
 341         void *user_data;
 342 };
 343
 344 struct disconnect_callback {
 345         unsigned int id;
 346         avdtp_disconnect_cb_t cb;
 347         void *user_data;
 348 };
 349
 350 struct avdtp_stream {
 351         GIOChannel *io;
 352         uint16_t imtu;
 353         uint16_t omtu;
 354         struct avdtp *session;
 355         struct avdtp_local_sep *lsep;
 356         uint8_t rseid;
 357         GSList *caps;
 358         GSList *callbacks;
 359         struct avdtp_service_capability *codec;
 360         guint io_id;            /* Transport GSource ID */
 361         guint timer;            /* Waiting for other side to close or open
 362                                  * the transport channel */
 363         gboolean open_acp;      /* If we are in ACT role for Open */
 364         gboolean close_int;     /* If we are in INT role for Close */
 365         gboolean abort_int;     /* If we are in INT role for Abort */
 366         guint start_timer;      /* Wait START command timer */
 367         gboolean delay_reporting;
 368         uint16_t delay;         /* AVDTP 1.3 Delay Reporting feature */
 369         gboolean starting;      /* only valid while sep state == OPEN */
 370 };
 371
 372 /* Structure describing an AVDTP connection between two devices */
 373
 374 struct avdtp {
 375         unsigned int ref;
 376
 377         uint16_t version;
 378
 379         guint auth_id;
 380
 381         GIOChannel *io;
 382         guint io_id;
 383
 384         GSList *seps; /* Elements of type struct avdtp_remote_sep * */
 385         struct queue *lseps; /* Elements of type struct avdtp_local_sep * */
 386
 387         GSList *streams; /* Elements of type struct avdtp_stream * */
 388
 389         GSList *req_queue; /* Elements of type struct pending_req * */
 390         GSList *prio_queue; /* Same as req_queue but is processed before it */
 391
 392         struct avdtp_stream *pending_open;
 393
 394         uint16_t imtu;
 395         uint16_t omtu;
 396
 397         struct in_buf in;
 398
 399         char *buf;
 400
 401         struct discover_callback *discover;
 402         struct pending_req *req;
 403
 404         GSList *disconnect;
 405
 406         bool shutdown;
 407 };
 408
 409 static int send_request(struct avdtp *session, gboolean priority,
 410                         struct avdtp_stream *stream, uint8_t signal_id,
 411                         void *buffer, size_t size);
 412 static gboolean avdtp_parse_resp(struct avdtp *session,
 413                                         struct avdtp_stream *stream,
 414                                         uint8_t transaction, uint8_t signal_id,
 415                                         void *buf, int size);
 416 static gboolean avdtp_parse_rej(struct avdtp *session,
 417                                         struct avdtp_stream *stream,
 418                                         uint8_t transaction, uint8_t signal_id,
 419                                         void *buf, int size);
 420 static int process_queue(struct avdtp *session);
 421 static void avdtp_sep_set_state(struct avdtp *session,
 422                                 struct avdtp_local_sep *sep,
 423                                 avdtp_state_t state);
 424
 425 static const char *avdtp_statestr(avdtp_state_t state)
 426 {
 427         switch (state) {
 428         case AVDTP_STATE_IDLE:
 429                 return "IDLE";
 430         case AVDTP_STATE_CONFIGURED:
 431                 return "CONFIGURED";
 432         case AVDTP_STATE_OPEN:
 433                 return "OPEN";
 434         case AVDTP_STATE_STREAMING:
 435                 return "STREAMING";
 436         case AVDTP_STATE_CLOSING:
 437                 return "CLOSING";
 438         case AVDTP_STATE_ABORTING:
 439                 return "ABORTING";
 440         default:
 441                 return "<unknown state>";
 442         }
 443 }
 444
 445 static gboolean try_send(int sk, void *data, size_t len)
 446 {
 447         int err;
 448
 449         do {
 450                 err = send(sk, data, len, 0);
 451         } while (err < 0 && errno == EINTR);
 452
 453         if (err < 0) {
 454                 error("send: %s (%d)", strerror(errno), errno);
 455                 return FALSE;
 456         } else if ((size_t) err != len) {
 457                 error("try_send: complete buffer not sent (%d/%zu bytes)",
 458                                                                 err, len);
 459                 return FALSE;
 460         }
 461
 462         return TRUE;
 463 }
 464
 465 static gboolean avdtp_send(struct avdtp *session, uint8_t transaction,
 466                                 uint8_t message_type, uint8_t signal_id,
 467                                 void *data, size_t len)
 468 {
 469         unsigned int cont_fragments, sent;
 470         struct avdtp_start_header start;
 471         struct avdtp_continue_header cont;
 472         int sock;
 473
 474         if (session->io == NULL) {
 475                 error("avdtp_send: session is closed");
 476                 return FALSE;
 477         }
 478
 479         sock = g_io_channel_unix_get_fd(session->io);
 480
 481         /* Single packet - no fragmentation */
 482         if (sizeof(struct avdtp_single_header) + len <= session->omtu) {
 483                 struct avdtp_single_header single;
 484
 485                 memset(&single, 0, sizeof(single));
 486
 487                 single.transaction = transaction;
 488                 single.packet_type = AVDTP_PKT_TYPE_SINGLE;
 489                 single.message_type = message_type;
 490                 single.signal_id = signal_id;
 491
 492                 memcpy(session->buf, &single, sizeof(single));
 493                 memcpy(session->buf + sizeof(single), data, len);
 494
 495                 return try_send(sock, session->buf, sizeof(single) + len);
 496         }
 497
 498         /* Check if there is enough space to start packet */
 499         if (session->omtu < sizeof(start)) {
 500                 error("No enough space to fragment packet");
 501                 return FALSE;
 502         }
 503
 504         /* Count the number of needed fragments */
 505         cont_fragments = (len - (session->omtu - sizeof(start))) /
 506                                         (session->omtu - sizeof(cont)) + 1;
 507
 508         DBG("%zu bytes split into %d fragments", len, cont_fragments + 1);
 509
 510         /* Send the start packet */
 511         memset(&start, 0, sizeof(start));
 512         start.transaction = transaction;
 513         start.packet_type = AVDTP_PKT_TYPE_START;
 514         start.message_type = message_type;
 515         start.no_of_packets = cont_fragments + 1;
 516         start.signal_id = signal_id;
 517
 518         memcpy(session->buf, &start, sizeof(start));
 519         memcpy(session->buf + sizeof(start), data,
 520                                         session->omtu - sizeof(start));
 521
 522         if (!try_send(sock, session->buf, session->omtu))
 523                 return FALSE;
 524
 525         DBG("first packet with %zu bytes sent", session->omtu - sizeof(start));
 526
 527         sent = session->omtu - sizeof(start);
 528
 529         /* Send the continue fragments and the end packet */
 530         while (sent < len) {
 531                 int left, to_copy;
 532
 533                 left = len - sent;
 534                 if (left + sizeof(cont) > session->omtu) {
 535                         cont.packet_type = AVDTP_PKT_TYPE_CONTINUE;
 536                         to_copy = session->omtu - sizeof(cont);
 537                         DBG("sending continue with %d bytes", to_copy);
 538                 } else {
 539                         cont.packet_type = AVDTP_PKT_TYPE_END;
 540                         to_copy = left;
 541                         DBG("sending end with %d bytes", to_copy);
 542                 }
 543
 544                 cont.transaction = transaction;
 545                 cont.message_type = message_type;
 546
 547                 memcpy(session->buf, &cont, sizeof(cont));
 548                 memcpy(session->buf + sizeof(cont), data + sent, to_copy);
 549
 550                 if (!try_send(sock, session->buf, to_copy + sizeof(cont)))
 551                         return FALSE;
 552
 553                 sent += to_copy;
 554         }
 555
 556         return TRUE;
 557 }
 558
 559 static void pending_req_free(void *data)
 560 {
 561         struct pending_req *req = data;
 562
 563         if (req->timeout)
 564                 g_source_remove(req->timeout);
 565         g_free(req->data);
 566         g_free(req);
 567 }
 568
 569 static void close_stream(struct avdtp_stream *stream)
 570 {
 571         int sock;
 572
 573         if (stream->io == NULL)
 574                 return;
 575
 576         sock = g_io_channel_unix_get_fd(stream->io);
 577
 578         shutdown(sock, SHUT_RDWR);
 579
 580         g_io_channel_shutdown(stream->io, FALSE, NULL);
 581
 582         g_io_channel_unref(stream->io);
 583         stream->io = NULL;
 584 }
 585
 586 static gboolean stream_close_timeout(gpointer user_data)
 587 {
 588         struct avdtp_stream *stream = user_data;
 589
 590         DBG("Timed out waiting for peer to close the transport channel");
 591
 592         stream->timer = 0;
 593
 594         close_stream(stream);
 595
 596         return FALSE;
 597 }
 598
 599 static gboolean stream_open_timeout(gpointer user_data)
 600 {
 601         struct avdtp_stream *stream = user_data;
 602
 603         DBG("Timed out waiting for peer to open the transport channel");
 604
 605         stream->timer = 0;
 606
 607         stream->session->pending_open = NULL;
 608
 609         avdtp_abort(stream->session, stream);
 610
 611         return FALSE;
 612 }
 613
 614 void avdtp_error_init(struct avdtp_error *err, uint8_t category, int id)
 615 {
 616         err->category = category;
 617
 618         if (category == AVDTP_ERRNO)
 619                 err->err.posix_errno = id;
 620         else
 621                 err->err.error_code = id;
 622 }
 623
 624 uint8_t avdtp_error_category(struct avdtp_error *err)
 625 {
 626         return err->category;
 627 }
 628
 629 int avdtp_error_error_code(struct avdtp_error *err)
 630 {
 631         assert(err->category != AVDTP_ERRNO);
 632         return err->err.error_code;
 633 }
 634
 635 int avdtp_error_posix_errno(struct avdtp_error *err)
 636 {
 637         assert(err->category == AVDTP_ERRNO);
 638         return err->err.posix_errno;
 639 }
 640
 641 static struct avdtp_stream *find_stream_by_rseid(struct avdtp *session,
 642                                                         uint8_t rseid)
 643 {
 644         GSList *l;
 645
 646         for (l = session->streams; l != NULL; l = g_slist_next(l)) {
 647                 struct avdtp_stream *stream = l->data;
 648
 649                 if (stream->rseid == rseid)
 650                         return stream;
 651         }
 652
 653         return NULL;
 654 }
 655
 656 static struct avdtp_remote_sep *find_remote_sep(GSList *seps, uint8_t seid)
 657 {
 658         GSList *l;
 659
 660         for (l = seps; l != NULL; l = g_slist_next(l)) {
 661                 struct avdtp_remote_sep *sep = l->data;
 662
 663                 if (sep->seid == seid)
 664                         return sep;
 665         }
 666
 667         return NULL;
 668 }
 669
 670 static void stream_free(void *data)
 671 {
 672         struct avdtp_stream *stream = data;
 673         struct avdtp_remote_sep *rsep;
 674
 675         stream->lsep->info.inuse = 0;
 676         stream->lsep->stream = NULL;
 677
 678         rsep = find_remote_sep(stream->session->seps, stream->rseid);
 679         if (rsep)
 680                 rsep->stream = NULL;
 681
 682         if (stream->timer)
 683                 g_source_remove(stream->timer);
 684
 685         if (stream->start_timer > 0)
 686                 g_source_remove(stream->start_timer);
 687
 688         if (stream->io)
 689                 close_stream(stream);
 690
 691         if (stream->io_id)
 692                 g_source_remove(stream->io_id);
 693
 694         g_slist_free_full(stream->callbacks, g_free);
 695         g_slist_free_full(stream->caps, g_free);
 696
 697         g_free(stream);
 698 }
 699
 700 static gboolean transport_cb(GIOChannel *chan, GIOCondition cond,
 701                                 gpointer data)
 702 {
 703         struct avdtp_stream *stream = data;
 704         struct avdtp_local_sep *sep = stream->lsep;
 705
 706         if (stream->close_int && sep->cfm && sep->cfm->close)
 707                 sep->cfm->close(stream->session, sep, stream, NULL,
 708                                 sep->user_data);
 709
 710         if (!(cond & G_IO_NVAL))
 711                 close_stream(stream);
 712
 713         stream->io_id = 0;
 714
 715         if (!stream->abort_int)
 716                 avdtp_sep_set_state(stream->session, sep, AVDTP_STATE_IDLE);
 717
 718         return FALSE;
 719 }
 720
 721 static void handle_transport_connect(struct avdtp *session, GIOChannel *io,
 722                                         uint16_t imtu, uint16_t omtu)
 723 {
 724         struct avdtp_stream *stream = session->pending_open;
 725         struct avdtp_local_sep *sep = stream->lsep;
 726
 727         session->pending_open = NULL;
 728
 729         if (stream->timer) {
 730                 g_source_remove(stream->timer);
 731                 stream->timer = 0;
 732         }
 733
 734         if (io == NULL)
 735                 return;
 736
 737         if (stream->io == NULL)
 738                 stream->io = g_io_channel_ref(io);
 739
 740         stream->omtu = omtu;
 741         stream->imtu = imtu;
 742
 743         avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);
 744
 745         stream->io_id = g_io_add_watch(io, G_IO_ERR | G_IO_HUP | G_IO_NVAL,
 746                                         (GIOFunc) transport_cb, stream);
 747 }
 748
 749 static int pending_req_cmp(gconstpointer a, gconstpointer b)
 750 {
 751         const struct pending_req *req = a;
 752         const struct avdtp_stream *stream = b;
 753
 754         if (req->stream == stream)
 755                 return 0;
 756
 757         return -1;
 758 }
 759
 760 static void cleanup_queue(struct avdtp *session, struct avdtp_stream *stream)
 761 {
 762         GSList *l;
 763         struct pending_req *req;
 764
 765         while ((l = g_slist_find_custom(session->prio_queue, stream,
 766                                                         pending_req_cmp))) {
 767                 req = l->data;
 768                 pending_req_free(req);
 769                 session->prio_queue = g_slist_remove(session->prio_queue, req);
 770         }
 771
 772         while ((l = g_slist_find_custom(session->req_queue, stream,
 773                                                         pending_req_cmp))) {
 774                 req = l->data;
 775                 pending_req_free(req);
 776                 session->req_queue = g_slist_remove(session->req_queue, req);
 777         }
 778 }
 779
 780 static void handle_unanswered_req(struct avdtp *session,
 781                                                 struct avdtp_stream *stream)
 782 {
 783         struct pending_req *req;
 784         struct avdtp_local_sep *lsep;
 785         struct avdtp_error err;
 786
 787         if (!session->req->timeout)
 788                 /* Request is in process */
 789                 return;
 790
 791         if (session->req->signal_id == AVDTP_ABORT) {
 792                 /* Avoid freeing the Abort request here */
 793                 DBG("handle_unanswered_req: Abort req, returning");
 794                 session->req->stream = NULL;
 795                 return;
 796         }
 797
 798         req = session->req;
 799         session->req = NULL;
 800
 801         avdtp_error_init(&err, AVDTP_ERRNO, EIO);
 802
 803         lsep = stream->lsep;
 804
 805         switch (req->signal_id) {
 806         case AVDTP_RECONFIGURE:
 807                 error("No reply to Reconfigure request");
 808                 if (lsep && lsep->cfm && lsep->cfm->reconfigure)
 809                         lsep->cfm->reconfigure(session, lsep, stream, &err,
 810                                                 lsep->user_data);
 811                 break;
 812         case AVDTP_OPEN:
 813                 error("No reply to Open request");
 814                 if (lsep && lsep->cfm && lsep->cfm->open)
 815                         lsep->cfm->open(session, lsep, stream, &err,
 816                                         lsep->user_data);
 817                 break;
 818         case AVDTP_START:
 819                 error("No reply to Start request");
 820                 if (lsep && lsep->cfm && lsep->cfm->start)
 821                         lsep->cfm->start(session, lsep, stream, &err,
 822                                                 lsep->user_data);
 823                 break;
 824         case AVDTP_SUSPEND:
 825                 error("No reply to Suspend request");
 826                 if (lsep && lsep->cfm && lsep->cfm->suspend)
 827                         lsep->cfm->suspend(session, lsep, stream, &err,
 828                                                 lsep->user_data);
 829                 break;
 830         case AVDTP_CLOSE:
 831                 error("No reply to Close request");
 832                 if (lsep && lsep->cfm && lsep->cfm->close)
 833                         lsep->cfm->close(session, lsep, stream, &err,
 834                                                 lsep->user_data);
 835                 break;
 836         case AVDTP_SET_CONFIGURATION:
 837                 error("No reply to SetConfiguration request");
 838                 if (lsep && lsep->cfm && lsep->cfm->set_configuration)
 839                         lsep->cfm->set_configuration(session, lsep, stream,
 840                                                         &err, lsep->user_data);
 841         }
 842
 843         pending_req_free(req);
 844 }
 845
 846 static void avdtp_sep_set_state(struct avdtp *session,
 847                                 struct avdtp_local_sep *sep,
 848                                 avdtp_state_t state)
 849 {
 850         struct avdtp_stream *stream = sep->stream;
 851         avdtp_state_t old_state;
 852         struct avdtp_error err, *err_ptr = NULL;
 853         GSList *l;
 854
 855         if (!stream) {
 856                 error("Error changing sep state: stream not available");
 857                 return;
 858         }
 859
 860         if (sep->state == state) {
 861                 avdtp_error_init(&err, AVDTP_ERRNO, EIO);
 862                 DBG("stream state change failed: %s", avdtp_strerror(&err));
 863                 err_ptr = &err;
 864         } else {
 865                 err_ptr = NULL;
 866                 DBG("stream state changed: %s -> %s",
 867                                 avdtp_statestr(sep->state),
 868                                 avdtp_statestr(state));
 869         }
 870
 871         old_state = sep->state;
 872         sep->state = state;
 873
 874         switch (state) {
 875         case AVDTP_STATE_CONFIGURED:
 876                 if (sep->info.type == AVDTP_SEP_TYPE_SINK)
 877                         avdtp_delay_report(session, stream, stream->delay);
 878                 break;
 879         case AVDTP_STATE_OPEN:
 880                 stream->starting = FALSE;
 881                 break;
 882         case AVDTP_STATE_STREAMING:
 883                 if (stream->start_timer) {
 884                         g_source_remove(stream->start_timer);
 885                         stream->start_timer = 0;
 886                 }
 887                 stream->open_acp = FALSE;
 888                 break;
 889         case AVDTP_STATE_CLOSING:
 890         case AVDTP_STATE_ABORTING:
 891                 if (stream->start_timer) {
 892                         g_source_remove(stream->start_timer);
 893                         stream->start_timer = 0;
 894                 }
 895                 break;
 896         case AVDTP_STATE_IDLE:
 897                 if (stream->start_timer) {
 898                         g_source_remove(stream->start_timer);
 899                         stream->start_timer = 0;
 900                 }
 901                 if (session->pending_open == stream)
 902                         handle_transport_connect(session, NULL, 0, 0);
 903                 if (session->req && session->req->stream == stream)
 904                         handle_unanswered_req(session, stream);
 905                 /* Remove pending commands for this stream from the queue */
 906                 cleanup_queue(session, stream);
 907                 break;
 908         default:
 909                 break;
 910         }
 911
 912         l = stream->callbacks;
 913         while (l != NULL) {
 914                 struct stream_callback *cb = l->data;
 915                 l = g_slist_next(l);
 916                 cb->cb(stream, old_state, state, err_ptr, cb->user_data);
 917         }
 918
 919         if (state == AVDTP_STATE_IDLE &&
 920                                 g_slist_find(session->streams, stream)) {
 921                 session->streams = g_slist_remove(session->streams, stream);
 922                 stream_free(stream);
 923         }
 924
 925         if (session->io && session->shutdown && session->streams == NULL) {
 926                 int sock = g_io_channel_unix_get_fd(session->io);
 927                 shutdown(sock, SHUT_RDWR);
 928         }
 929 }
 930
 931 static void finalize_discovery(struct avdtp *session, int err)
 932 {
 933         struct discover_callback *discover = session->discover;
 934         struct avdtp_error avdtp_err;
 935
 936         if (!discover)
 937                 return;
 938
 939         session->discover = NULL;
 940
 941         avdtp_error_init(&avdtp_err, AVDTP_ERRNO, err);
 942
 943         if (discover->id > 0)
 944                 g_source_remove(discover->id);
 945
 946         if (discover->cb)
 947                 discover->cb(session, session->seps, err ? &avdtp_err : NULL,
 948                                                         discover->user_data);
 949         g_free(discover);
 950 }
 951
 952 static void release_stream(struct avdtp_stream *stream, struct avdtp *session)
 953 {
 954         struct avdtp_local_sep *sep = stream->lsep;
 955
 956         if (sep->cfm && sep->cfm->abort &&
 957                                 (sep->state != AVDTP_STATE_ABORTING ||
 958                                                         stream->abort_int))
 959                 sep->cfm->abort(session, sep, stream, NULL, sep->user_data);
 960
 961         avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);
 962 }
 963
 964 static void sep_free(gpointer data)
 965 {
 966         struct avdtp_remote_sep *sep = data;
 967
 968         g_slist_free_full(sep->caps, g_free);
 969         g_free(sep);
 970 }
 971
 972 static void avdtp_free(void *data)
 973 {
 974         struct avdtp *session = data;
 975
 976         DBG("%p", session);
 977
 978         g_slist_free_full(session->streams, stream_free);
 979
 980         if (session->io) {
 981                 g_io_channel_shutdown(session->io, FALSE, NULL);
 982                 g_io_channel_unref(session->io);
 983         }
 984
 985         if (session->io_id) {
 986                 g_source_remove(session->io_id);
 987                 session->io_id = 0;
 988         }
 989
 990         if (session->req)
 991                 pending_req_free(session->req);
 992
 993         g_slist_free_full(session->req_queue, pending_req_free);
 994         g_slist_free_full(session->prio_queue, pending_req_free);
 995         g_slist_free_full(session->seps, sep_free);
 996         g_slist_free_full(session->disconnect, g_free);
 997
 998         /* Free copy of the SEP list */
 999         session->lseps = NULL;
1000
1001         g_free(session->buf);
1002
1003         g_free(session);
1004 }
1005
1006 static void process_disconnect(void *data)
1007 {
1008         struct disconnect_callback *callback = data;
1009
1010         callback->cb(callback->user_data);
1011
1012         g_free(callback);
1013 }
1014
1015 static void connection_lost(struct avdtp *session, int err)
1016 {
1017         DBG("Disconnected: %s (%d)", strerror(err), err);
1018
1019         g_slist_foreach(session->streams, (GFunc) release_stream, session);
1020         session->streams = NULL;
1021
1022         avdtp_ref(session);
1023
1024         finalize_discovery(session, err);
1025
1026         g_slist_free_full(session->disconnect, process_disconnect);
1027         session->disconnect = NULL;
1028
1029         avdtp_unref(session);
1030 }
1031
1032 void avdtp_unref(struct avdtp *session)
1033 {
1034         if (!session)
1035                 return;
1036
1037         session->ref--;
1038
1039         DBG("%p: ref=%d", session, session->ref);
1040
1041         if (session->ref > 0)
1042                 return;
1043
1044         finalize_discovery(session, ECONNABORTED);
1045
1046         avdtp_free(session);
1047 }
1048
1049 struct avdtp *avdtp_ref(struct avdtp *session)
1050 {
1051         session->ref++;
1052
1053         DBG("%p: ref=%d", session, session->ref);
1054
1055         return session;
1056 }
1057
1058 static bool match_by_seid(const void *data, const void *user_data)
1059 {
1060         const struct avdtp_local_sep *sep = data;
1061         uint8_t seid = PTR_TO_UINT(user_data);
1062
1063         return sep->info.seid == seid;
1064 }
1065
1066 static struct avdtp_local_sep *find_local_sep_by_seid(struct avdtp *session,
1067                                                                 uint8_t seid)
1068 {
1069         return queue_find(session->lseps, match_by_seid, INT_TO_PTR(seid));
1070 }
1071
1072 struct avdtp_remote_sep *avdtp_find_remote_sep(struct avdtp *session,
1073                                                 struct avdtp_local_sep *lsep)
1074 {
1075         GSList *l;
1076
1077         if (lsep->info.inuse)
1078                 return NULL;
1079
1080         for (l = session->seps; l != NULL; l = g_slist_next(l)) {
1081                 struct avdtp_remote_sep *sep = l->data;
1082                 struct avdtp_service_capability *cap;
1083                 struct avdtp_media_codec_capability *codec_data;
1084
1085                 /* Type must be different: source <-> sink */
1086                 if (sep->type == lsep->info.type)
1087                         continue;
1088
1089                 if (sep->media_type != lsep->info.media_type)
1090                         continue;
1091
1092                 if (!sep->codec)
1093                         continue;
1094
1095                 cap = sep->codec;
1096                 codec_data = (void *) cap->data;
1097
1098                 if (codec_data->media_codec_type != lsep->codec)
1099                         continue;
1100
1101                 /* FIXME: Add Vendor Specific Codec match to SEP callback */
1102                 if (lsep->codec == A2DP_CODEC_VENDOR) {
1103                         a2dp_vendor_codec_t *vndcodec =
1104                                                 (void *) codec_data->data;
1105
1106                         if (btohl(vndcodec->vendor_id) != lsep->vndcodec_vendor)
1107                                 continue;
1108
1109                         if (btohs(vndcodec->codec_id) != lsep->vndcodec_codec)
1110                                 continue;
1111                 }
1112
1113                 if (sep->stream == NULL)
1114                         return sep;
1115         }
1116
1117         return NULL;
1118 }
1119
1120 static GSList *caps_to_list(uint8_t *data, int size,
1121                                 struct avdtp_service_capability **codec,
1122                                 gboolean *delay_reporting)
1123 {
1124         GSList *caps;
1125         int processed;
1126
1127         if (delay_reporting)
1128                 *delay_reporting = FALSE;
1129
1130         for (processed = 0, caps = NULL; processed + 2 <= size;) {
1131                 struct avdtp_service_capability *cap;
1132                 uint8_t length, category;
1133
1134                 category = data[0];
1135                 length = data[1];
1136
1137                 if (processed + 2 + length > size) {
1138                         error("Invalid capability data in getcap resp");
1139                         break;
1140                 }
1141
1142                 cap = g_malloc(sizeof(struct avdtp_service_capability) +
1143                                         length);
1144                 memcpy(cap, data, 2 + length);
1145
1146                 processed += 2 + length;
1147                 data += 2 + length;
1148
1149                 caps = g_slist_append(caps, cap);
1150
1151                 if (category == AVDTP_MEDIA_CODEC &&
1152                                 length >=
1153                                 sizeof(struct avdtp_media_codec_capability))
1154                         *codec = cap;
1155                 else if (category == AVDTP_DELAY_REPORTING && delay_reporting)
1156                         *delay_reporting = TRUE;
1157         }
1158
1159         return caps;
1160 }
1161
1162 static gboolean avdtp_unknown_cmd(struct avdtp *session, uint8_t transaction,
1163                                                         uint8_t signal_id)
1164 {
1165         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_GEN_REJECT,
1166                                                         signal_id, NULL, 0);
1167 }
1168
1169 static void copy_seps(void *data, void *user_data)
1170 {
1171         struct avdtp_local_sep *sep = data;
1172         struct seid_info **p = user_data;
1173
1174         memcpy(*p, &sep->info, sizeof(struct seid_info));
1175         *p = *p + 1;
1176 }
1177
1178 static gboolean avdtp_discover_cmd(struct avdtp *session, uint8_t transaction,
1179                                                         void *buf, int size)
1180 {
1181         unsigned int rsp_size, sep_count;
1182         struct seid_info *seps, *p;
1183         gboolean ret;
1184
1185         sep_count = queue_length(session->lseps);
1186
1187         if (sep_count == 0) {
1188                 uint8_t err = AVDTP_NOT_SUPPORTED_COMMAND;
1189                 return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1190                                         AVDTP_DISCOVER, &err, sizeof(err));
1191         }
1192
1193         rsp_size = sep_count * sizeof(struct seid_info);
1194
1195         seps = g_new0(struct seid_info, sep_count);
1196         p = seps;
1197
1198         queue_foreach(session->lseps, copy_seps, &p);
1199
1200         ret = avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1201                                 AVDTP_DISCOVER, seps, rsp_size);
1202         g_free(seps);
1203
1204         return ret;
1205 }
1206
1207 static gboolean avdtp_getcap_cmd(struct avdtp *session, uint8_t transaction,
1208                                         struct seid_req *req, unsigned int size,
1209                                         gboolean get_all)
1210 {
1211         GSList *l, *caps;
1212         struct avdtp_local_sep *sep = NULL;
1213         unsigned int rsp_size;
1214         uint8_t err, buf[1024], *ptr = buf;
1215         uint8_t cmd;
1216
1217         cmd = get_all ? AVDTP_GET_ALL_CAPABILITIES : AVDTP_GET_CAPABILITIES;
1218
1219         if (size < sizeof(struct seid_req)) {
1220                 err = AVDTP_BAD_LENGTH;
1221                 goto failed;
1222         }
1223
1224         sep = find_local_sep_by_seid(session, req->acp_seid);
1225         if (!sep) {
1226                 err = AVDTP_BAD_ACP_SEID;
1227                 goto failed;
1228         }
1229
1230         if (!sep->ind->get_capability(session, sep, &caps, &err,
1231                                                         sep->user_data))
1232                 goto failed;
1233
1234         for (l = caps, rsp_size = 0; l != NULL; l = g_slist_next(l)) {
1235                 struct avdtp_service_capability *cap = l->data;
1236
1237                 if (rsp_size + cap->length + 2 > sizeof(buf))
1238                         break;
1239
1240                 memcpy(ptr, cap, cap->length + 2);
1241                 rsp_size += cap->length + 2;
1242                 ptr += cap->length + 2;
1243
1244                 g_free(cap);
1245         }
1246
1247         if (get_all && sep->delay_reporting) {
1248                 ptr[0] = AVDTP_DELAY_REPORTING;
1249                 ptr[1] = 0x00;
1250                 rsp_size += 2;
1251         }
1252
1253         g_slist_free(caps);
1254
1255         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT, cmd,
1256                                                                 buf, rsp_size);
1257
1258 failed:
1259         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT, cmd,
1260                                                         &err, sizeof(err));
1261 }
1262
1263 static void setconf_cb(struct avdtp *session, struct avdtp_stream *stream,
1264                                                 struct avdtp_error *err)
1265 {
1266         struct conf_rej rej;
1267         struct avdtp_local_sep *sep;
1268
1269         if (err != NULL) {
1270                 rej.error = AVDTP_UNSUPPORTED_CONFIGURATION;
1271                 rej.category = err->err.error_code;
1272                 avdtp_send(session, session->in.transaction,
1273                                 AVDTP_MSG_TYPE_REJECT, AVDTP_SET_CONFIGURATION,
1274                                 &rej, sizeof(rej));
1275                 return;
1276         }
1277
1278         if (!avdtp_send(session, session->in.transaction, AVDTP_MSG_TYPE_ACCEPT,
1279                                         AVDTP_SET_CONFIGURATION, NULL, 0)) {
1280                 stream_free(stream);
1281                 return;
1282         }
1283
1284         sep = stream->lsep;
1285         sep->stream = stream;
1286         sep->info.inuse = 1;
1287         session->streams = g_slist_append(session->streams, stream);
1288
1289         avdtp_sep_set_state(session, sep, AVDTP_STATE_CONFIGURED);
1290 }
1291
1292 static gboolean avdtp_setconf_cmd(struct avdtp *session, uint8_t transaction,
1293                                 struct setconf_req *req, unsigned int size)
1294 {
1295         struct conf_rej rej;
1296         struct avdtp_local_sep *sep;
1297         struct avdtp_stream *stream;
1298         uint8_t err, category = 0x00;
1299         GSList *l;
1300
1301         if (size < sizeof(struct setconf_req)) {
1302                 error("Too short getcap request");
1303                 return FALSE;
1304         }
1305
1306         sep = find_local_sep_by_seid(session, req->acp_seid);
1307         if (!sep) {
1308                 err = AVDTP_BAD_ACP_SEID;
1309                 goto failed;
1310         }
1311
1312         if (sep->stream) {
1313                 err = AVDTP_SEP_IN_USE;
1314                 goto failed;
1315         }
1316
1317         stream = g_new0(struct avdtp_stream, 1);
1318         stream->session = session;
1319         stream->lsep = sep;
1320         stream->rseid = req->int_seid;
1321         stream->caps = caps_to_list(req->caps,
1322                                         size - sizeof(struct setconf_req),
1323                                         &stream->codec,
1324                                         &stream->delay_reporting);
1325
1326         /*
1327          * Verify that the Media Transport capability's length = 0.
1328          * Reject otherwise
1329          */
1330         for (l = stream->caps; l != NULL; l = g_slist_next(l)) {
1331                 struct avdtp_service_capability *cap = l->data;
1332
1333                 if (cap->category == AVDTP_MEDIA_TRANSPORT &&
1334                                                         cap->length != 0) {
1335                         err = AVDTP_BAD_MEDIA_TRANSPORT_FORMAT;
1336                         goto failed_stream;
1337                 }
1338         }
1339
1340         if (stream->delay_reporting && session->version < 0x0103)
1341                 session->version = 0x0103;
1342
1343         if (sep->ind && sep->ind->set_configuration) {
1344                 if (!sep->ind->set_configuration(session, sep, stream,
1345                                                         stream->caps,
1346                                                         setconf_cb,
1347                                                         sep->user_data)) {
1348                         err = AVDTP_UNSUPPORTED_CONFIGURATION;
1349                         category = 0x00;
1350                         goto failed_stream;
1351                 }
1352         } else {
1353                 if (!avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1354                                         AVDTP_SET_CONFIGURATION, NULL, 0)) {
1355                         stream_free(stream);
1356                         return FALSE;
1357                 }
1358
1359                 sep->stream = stream;
1360                 sep->info.inuse = 1;
1361                 session->streams = g_slist_append(session->streams, stream);
1362
1363                 avdtp_sep_set_state(session, sep, AVDTP_STATE_CONFIGURED);
1364         }
1365
1366         return TRUE;
1367
1368 failed_stream:
1369         stream_free(stream);
1370 failed:
1371         rej.error = err;
1372         rej.category = category;
1373         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1374                                 AVDTP_SET_CONFIGURATION, &rej, sizeof(rej));
1375 }
1376
1377 static gboolean avdtp_getconf_cmd(struct avdtp *session, uint8_t transaction,
1378                                         struct seid_req *req, int size)
1379 {
1380         GSList *l;
1381         struct avdtp_local_sep *sep = NULL;
1382         int rsp_size;
1383         uint8_t err;
1384         uint8_t buf[1024];
1385         uint8_t *ptr = buf;
1386
1387         if (size < (int) sizeof(struct seid_req)) {
1388                 error("Too short getconf request");
1389                 return FALSE;
1390         }
1391
1392         memset(buf, 0, sizeof(buf));
1393
1394         sep = find_local_sep_by_seid(session, req->acp_seid);
1395         if (!sep) {
1396                 err = AVDTP_BAD_ACP_SEID;
1397                 goto failed;
1398         }
1399         if (!sep->stream || !sep->stream->caps) {
1400                 err = AVDTP_UNSUPPORTED_CONFIGURATION;
1401                 goto failed;
1402         }
1403
1404         for (l = sep->stream->caps, rsp_size = 0; l; l = g_slist_next(l)) {
1405                 struct avdtp_service_capability *cap = l->data;
1406
1407                 if (rsp_size + cap->length + 2 > (int) sizeof(buf))
1408                         break;
1409
1410                 memcpy(ptr, cap, cap->length + 2);
1411                 rsp_size += cap->length + 2;
1412                 ptr += cap->length + 2;
1413         }
1414
1415         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1416                                 AVDTP_GET_CONFIGURATION, buf, rsp_size);
1417
1418 failed:
1419         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1420                                 AVDTP_GET_CONFIGURATION, &err, sizeof(err));
1421 }
1422
1423 static gboolean avdtp_reconf_cmd(struct avdtp *session, uint8_t transaction,
1424                                         struct seid_req *req, int size)
1425 {
1426         struct conf_rej rej;
1427
1428         rej.error = AVDTP_NOT_SUPPORTED_COMMAND;
1429         rej.category = 0x00;
1430
1431         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1432                                         AVDTP_RECONFIGURE, &rej, sizeof(rej));
1433 }
1434
1435 static void check_seid_collision(struct pending_req *req, uint8_t id)
1436 {
1437         struct seid_req *seid = req->data;
1438
1439         if (seid->acp_seid == id)
1440                 req->collided = TRUE;
1441 }
1442
1443 static void check_start_collision(struct pending_req *req, uint8_t id)
1444 {
1445         struct start_req *start = req->data;
1446         struct seid *seid = &start->first_seid;
1447         int count = 1 + req->data_size - sizeof(struct start_req);
1448         int i;
1449
1450         for (i = 0; i < count; i++, seid++) {
1451                 if (seid->seid == id) {
1452                         req->collided = TRUE;
1453                         return;
1454                 }
1455         }
1456 }
1457
1458 static void check_suspend_collision(struct pending_req *req, uint8_t id)
1459 {
1460         struct suspend_req *suspend = req->data;
1461         struct seid *seid = &suspend->first_seid;
1462         int count = 1 + req->data_size - sizeof(struct suspend_req);
1463         int i;
1464
1465         for (i = 0; i < count; i++, seid++) {
1466                 if (seid->seid == id) {
1467                         req->collided = TRUE;
1468                         return;
1469                 }
1470         }
1471 }
1472
1473 static void avdtp_check_collision(struct avdtp *session, uint8_t cmd,
1474                                         struct avdtp_stream *stream)
1475 {
1476         struct pending_req *req = session->req;
1477
1478         if (req == NULL || (req->signal_id != cmd && cmd != AVDTP_ABORT))
1479                 return;
1480
1481         if (cmd == AVDTP_ABORT)
1482                 cmd = req->signal_id;
1483
1484         switch (cmd) {
1485         case AVDTP_OPEN:
1486         case AVDTP_CLOSE:
1487                 check_seid_collision(req, stream->rseid);
1488                 break;
1489         case AVDTP_START:
1490                 check_start_collision(req, stream->rseid);
1491                 break;
1492         case AVDTP_SUSPEND:
1493                 check_suspend_collision(req, stream->rseid);
1494                 break;
1495         }
1496 }
1497
1498 static gboolean avdtp_open_cmd(struct avdtp *session, uint8_t transaction,
1499                                 struct seid_req *req, unsigned int size)
1500 {
1501         struct avdtp_local_sep *sep;
1502         struct avdtp_stream *stream;
1503         uint8_t err;
1504
1505         if (size < sizeof(struct seid_req)) {
1506                 error("Too short abort request");
1507                 return FALSE;
1508         }
1509
1510         sep = find_local_sep_by_seid(session, req->acp_seid);
1511         if (!sep) {
1512                 err = AVDTP_BAD_ACP_SEID;
1513                 goto failed;
1514         }
1515
1516         if (sep->state != AVDTP_STATE_CONFIGURED) {
1517                 err = AVDTP_BAD_STATE;
1518                 goto failed;
1519         }
1520
1521         stream = sep->stream;
1522
1523         if (sep->ind && sep->ind->open) {
1524                 if (!sep->ind->open(session, sep, stream, &err,
1525                                         sep->user_data))
1526                         goto failed;
1527         }
1528
1529         avdtp_check_collision(session, AVDTP_OPEN, stream);
1530
1531         if (!avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1532                                                 AVDTP_OPEN, NULL, 0))
1533                 return FALSE;
1534
1535         stream->open_acp = TRUE;
1536         session->pending_open = stream;
1537         stream->timer = g_timeout_add_seconds(REQ_TIMEOUT,
1538                                                 stream_open_timeout,
1539                                                 stream);
1540
1541         return TRUE;
1542
1543 failed:
1544         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1545                                 AVDTP_OPEN, &err, sizeof(err));
1546 }
1547
1548 static gboolean avdtp_start_cmd(struct avdtp *session, uint8_t transaction,
1549                                 struct start_req *req, unsigned int size)
1550 {
1551         struct avdtp_local_sep *sep;
1552         struct avdtp_stream *stream;
1553         struct stream_rej rej;
1554         struct seid *seid;
1555         uint8_t err, failed_seid;
1556         int seid_count, i;
1557
1558         if (size < sizeof(struct start_req)) {
1559                 error("Too short start request");
1560                 return FALSE;
1561         }
1562
1563         seid_count = 1 + size - sizeof(struct start_req);
1564
1565         seid = &req->first_seid;
1566
1567         for (i = 0; i < seid_count; i++, seid++) {
1568                 failed_seid = seid->seid;
1569
1570                 sep = find_local_sep_by_seid(session, req->first_seid.seid);
1571                 if (!sep || !sep->stream) {
1572                         err = AVDTP_BAD_ACP_SEID;
1573                         goto failed;
1574                 }
1575
1576                 stream = sep->stream;
1577
1578                 /* Also reject start cmd if state is not open */
1579                 if (sep->state != AVDTP_STATE_OPEN) {
1580                         err = AVDTP_BAD_STATE;
1581                         goto failed;
1582                 }
1583                 stream->starting = TRUE;
1584
1585                 if (sep->ind && sep->ind->start) {
1586                         if (!sep->ind->start(session, sep, stream, &err,
1587                                                 sep->user_data))
1588                                 goto failed;
1589                 }
1590
1591                 avdtp_check_collision(session, AVDTP_START, stream);
1592
1593                 avdtp_sep_set_state(session, sep, AVDTP_STATE_STREAMING);
1594         }
1595
1596         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1597                                                 AVDTP_START, NULL, 0);
1598
1599 failed:
1600         DBG("Rejecting (%d)", err);
1601         memset(&rej, 0, sizeof(rej));
1602         rej.acp_seid = failed_seid;
1603         rej.error = err;
1604         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1605                                 AVDTP_START, &rej, sizeof(rej));
1606 }
1607
1608 static gboolean avdtp_close_cmd(struct avdtp *session, uint8_t transaction,
1609                                 struct seid_req *req, unsigned int size)
1610 {
1611         struct avdtp_local_sep *sep;
1612         struct avdtp_stream *stream;
1613         uint8_t err;
1614
1615         if (size < sizeof(struct seid_req)) {
1616                 error("Too short close request");
1617                 return FALSE;
1618         }
1619
1620         sep = find_local_sep_by_seid(session, req->acp_seid);
1621         if (!sep || !sep->stream) {
1622                 err = AVDTP_BAD_ACP_SEID;
1623                 goto failed;
1624         }
1625
1626         if (sep->state != AVDTP_STATE_OPEN &&
1627                         sep->state != AVDTP_STATE_STREAMING) {
1628                 err = AVDTP_BAD_STATE;
1629                 goto failed;
1630         }
1631
1632         stream = sep->stream;
1633
1634         if (sep->ind && sep->ind->close) {
1635                 if (!sep->ind->close(session, sep, stream, &err,
1636                                         sep->user_data))
1637                         goto failed;
1638         }
1639
1640         avdtp_check_collision(session, AVDTP_CLOSE, stream);
1641
1642         avdtp_sep_set_state(session, sep, AVDTP_STATE_CLOSING);
1643
1644         if (!avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1645                                                 AVDTP_CLOSE, NULL, 0))
1646                 return FALSE;
1647
1648         stream->timer = g_timeout_add_seconds(REQ_TIMEOUT,
1649                                         stream_close_timeout,
1650                                         stream);
1651
1652         return TRUE;
1653
1654 failed:
1655         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1656                                         AVDTP_CLOSE, &err, sizeof(err));
1657 }
1658
1659 static gboolean avdtp_suspend_cmd(struct avdtp *session, uint8_t transaction,
1660                                 struct suspend_req *req, unsigned int size)
1661 {
1662         struct avdtp_local_sep *sep;
1663         struct avdtp_stream *stream;
1664         struct stream_rej rej;
1665         struct seid *seid;
1666         uint8_t err, failed_seid;
1667         int seid_count, i;
1668
1669         if (size < sizeof(struct suspend_req)) {
1670                 error("Too short suspend request");
1671                 return FALSE;
1672         }
1673
1674         seid_count = 1 + size - sizeof(struct suspend_req);
1675
1676         seid = &req->first_seid;
1677
1678         for (i = 0; i < seid_count; i++, seid++) {
1679                 failed_seid = seid->seid;
1680
1681                 sep = find_local_sep_by_seid(session, req->first_seid.seid);
1682                 if (!sep || !sep->stream) {
1683                         err = AVDTP_BAD_ACP_SEID;
1684                         goto failed;
1685                 }
1686
1687                 stream = sep->stream;
1688
1689                 if (sep->state != AVDTP_STATE_STREAMING) {
1690                         err = AVDTP_BAD_STATE;
1691                         goto failed;
1692                 }
1693
1694                 if (sep->ind && sep->ind->suspend) {
1695                         if (!sep->ind->suspend(session, sep, stream, &err,
1696                                                 sep->user_data))
1697                                 goto failed;
1698                 }
1699
1700                 avdtp_check_collision(session, AVDTP_SUSPEND, stream);
1701
1702                 avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);
1703         }
1704
1705         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1706                                                 AVDTP_SUSPEND, NULL, 0);
1707
1708 failed:
1709         memset(&rej, 0, sizeof(rej));
1710         rej.acp_seid = failed_seid;
1711         rej.error = err;
1712         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1713                                 AVDTP_SUSPEND, &rej, sizeof(rej));
1714 }
1715
1716 static gboolean avdtp_abort_cmd(struct avdtp *session, uint8_t transaction,
1717                                 struct seid_req *req, unsigned int size)
1718 {
1719         struct avdtp_local_sep *sep;
1720         uint8_t err;
1721         gboolean ret;
1722
1723         if (size < sizeof(struct seid_req)) {
1724                 error("Too short abort request");
1725                 return FALSE;
1726         }
1727
1728         sep = find_local_sep_by_seid(session, req->acp_seid);
1729         if (!sep || !sep->stream)
1730                 return TRUE;
1731
1732         if (sep->ind && sep->ind->abort)
1733                 sep->ind->abort(session, sep, sep->stream, &err,
1734                                                         sep->user_data);
1735
1736         avdtp_check_collision(session, AVDTP_ABORT, sep->stream);
1737
1738         ret = avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1739                                                 AVDTP_ABORT, NULL, 0);
1740         if (ret)
1741                 avdtp_sep_set_state(session, sep, AVDTP_STATE_ABORTING);
1742
1743         return ret;
1744 }
1745
1746 static gboolean avdtp_secctl_cmd(struct avdtp *session, uint8_t transaction,
1747                                         struct seid_req *req, int size)
1748 {
1749         return avdtp_unknown_cmd(session, transaction, AVDTP_SECURITY_CONTROL);
1750 }
1751
1752 static gboolean avdtp_delayreport_cmd(struct avdtp *session,
1753                                         uint8_t transaction,
1754                                         struct delay_req *req,
1755                                         unsigned int size)
1756 {
1757         struct avdtp_local_sep *sep;
1758         struct avdtp_stream *stream;
1759         uint8_t err;
1760
1761         if (size < sizeof(struct delay_req)) {
1762                 error("Too short delay report request");
1763                 return FALSE;
1764         }
1765
1766         sep = find_local_sep_by_seid(session, req->acp_seid);
1767         if (!sep || !sep->stream) {
1768                 err = AVDTP_BAD_ACP_SEID;
1769                 goto failed;
1770         }
1771
1772         stream = sep->stream;
1773
1774         switch (sep->state) {
1775         case AVDTP_STATE_IDLE:
1776         case AVDTP_STATE_ABORTING:
1777         case AVDTP_STATE_CLOSING:
1778                 err = AVDTP_BAD_STATE;
1779                 goto failed;
1780         case AVDTP_STATE_CONFIGURED:
1781         case AVDTP_STATE_OPEN:
1782         case AVDTP_STATE_STREAMING:
1783         default:
1784                 break;
1785         }
1786
1787         stream->delay = ntohs(req->delay);
1788
1789         if (sep->ind && sep->ind->delayreport) {
1790                 if (!sep->ind->delayreport(session, sep, stream->rseid,
1791                                                 stream->delay, &err,
1792                                                 sep->user_data))
1793                         goto failed;
1794         }
1795
1796         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_ACCEPT,
1797                                                 AVDTP_DELAY_REPORT, NULL, 0);
1798
1799 failed:
1800         return avdtp_send(session, transaction, AVDTP_MSG_TYPE_REJECT,
1801                                         AVDTP_DELAY_REPORT, &err, sizeof(err));
1802 }
1803
1804 static gboolean avdtp_parse_cmd(struct avdtp *session, uint8_t transaction,
1805                                 uint8_t signal_id, void *buf, int size)
1806 {
1807         switch (signal_id) {
1808         case AVDTP_DISCOVER:
1809                 DBG("Received DISCOVER_CMD");
1810                 return avdtp_discover_cmd(session, transaction, buf, size);
1811         case AVDTP_GET_CAPABILITIES:
1812                 DBG("Received  GET_CAPABILITIES_CMD");
1813                 return avdtp_getcap_cmd(session, transaction, buf, size,
1814                                                                         FALSE);
1815         case AVDTP_GET_ALL_CAPABILITIES:
1816                 DBG("Received  GET_ALL_CAPABILITIES_CMD");
1817                 return avdtp_getcap_cmd(session, transaction, buf, size, TRUE);
1818         case AVDTP_SET_CONFIGURATION:
1819                 DBG("Received SET_CONFIGURATION_CMD");
1820                 return avdtp_setconf_cmd(session, transaction, buf, size);
1821         case AVDTP_GET_CONFIGURATION:
1822                 DBG("Received GET_CONFIGURATION_CMD");
1823                 return avdtp_getconf_cmd(session, transaction, buf, size);
1824         case AVDTP_RECONFIGURE:
1825                 DBG("Received RECONFIGURE_CMD");
1826                 return avdtp_reconf_cmd(session, transaction, buf, size);
1827         case AVDTP_OPEN:
1828                 DBG("Received OPEN_CMD");
1829                 return avdtp_open_cmd(session, transaction, buf, size);
1830         case AVDTP_START:
1831                 DBG("Received START_CMD");
1832                 return avdtp_start_cmd(session, transaction, buf, size);
1833         case AVDTP_CLOSE:
1834                 DBG("Received CLOSE_CMD");
1835                 return avdtp_close_cmd(session, transaction, buf, size);
1836         case AVDTP_SUSPEND:
1837                 DBG("Received SUSPEND_CMD");
1838                 return avdtp_suspend_cmd(session, transaction, buf, size);
1839         case AVDTP_ABORT:
1840                 DBG("Received ABORT_CMD");
1841                 return avdtp_abort_cmd(session, transaction, buf, size);
1842         case AVDTP_SECURITY_CONTROL:
1843                 DBG("Received SECURITY_CONTROL_CMD");
1844                 return avdtp_secctl_cmd(session, transaction, buf, size);
1845         case AVDTP_DELAY_REPORT:
1846                 DBG("Received DELAY_REPORT_CMD");
1847                 return avdtp_delayreport_cmd(session, transaction, buf, size);
1848         default:
1849                 DBG("Received unknown request id %u", signal_id);
1850                 return avdtp_unknown_cmd(session, transaction, signal_id);
1851         }
1852 }
1853
1854 enum avdtp_parse_result { PARSE_ERROR, PARSE_FRAGMENT, PARSE_SUCCESS };
1855
1856 static enum avdtp_parse_result avdtp_parse_data(struct avdtp *session,
1857                                                         void *buf, size_t size)
1858 {
1859         struct avdtp_common_header *header = buf;
1860         struct avdtp_single_header *single = (void *) session->buf;
1861         struct avdtp_start_header *start = (void *) session->buf;
1862         void *payload;
1863         gsize payload_size;
1864
1865         switch (header->packet_type) {
1866         case AVDTP_PKT_TYPE_SINGLE:
1867                 if (size < sizeof(*single)) {
1868                         error("Received too small single packet (%zu bytes)",
1869                                                                         size);
1870                         return PARSE_ERROR;
1871                 }
1872                 if (session->in.active) {
1873                         error("SINGLE: Invalid AVDTP packet fragmentation");
1874                         return PARSE_ERROR;
1875                 }
1876
1877                 payload = session->buf + sizeof(*single);
1878                 payload_size = size - sizeof(*single);
1879
1880                 session->in.active = TRUE;
1881                 session->in.data_size = 0;
1882                 session->in.no_of_packets = 1;
1883                 session->in.transaction = header->transaction;
1884                 session->in.message_type = header->message_type;
1885                 session->in.signal_id = single->signal_id;
1886
1887                 break;
1888         case AVDTP_PKT_TYPE_START:
1889                 if (size < sizeof(*start)) {
1890                         error("Received too small start packet (%zu bytes)",
1891                                                                         size);
1892                         return PARSE_ERROR;
1893                 }
1894                 if (session->in.active) {
1895                         error("START: Invalid AVDTP packet fragmentation");
1896                         return PARSE_ERROR;
1897                 }
1898
1899                 session->in.active = TRUE;
1900                 session->in.data_size = 0;
1901                 session->in.transaction = header->transaction;
1902                 session->in.message_type = header->message_type;
1903                 session->in.no_of_packets = start->no_of_packets;
1904                 session->in.signal_id = start->signal_id;
1905
1906                 payload = session->buf + sizeof(*start);
1907                 payload_size = size - sizeof(*start);
1908
1909                 break;
1910         case AVDTP_PKT_TYPE_CONTINUE:
1911                 if (size < sizeof(struct avdtp_continue_header)) {
1912                         error("Received too small continue packet (%zu bytes)",
1913                                                                         size);
1914                         return PARSE_ERROR;
1915                 }
1916                 if (!session->in.active) {
1917                         error("CONTINUE: Invalid AVDTP packet fragmentation");
1918                         return PARSE_ERROR;
1919                 }
1920                 if (session->in.transaction != header->transaction) {
1921                         error("Continue transaction id doesn't match");
1922                         return PARSE_ERROR;
1923                 }
1924                 if (session->in.no_of_packets <= 1) {
1925                         error("Too few continue packets");
1926                         return PARSE_ERROR;
1927                 }
1928
1929                 payload = session->buf + sizeof(struct avdtp_continue_header);
1930                 payload_size = size - sizeof(struct avdtp_continue_header);
1931
1932                 break;
1933         case AVDTP_PKT_TYPE_END:
1934                 if (size < sizeof(struct avdtp_continue_header)) {
1935                         error("Received too small end packet (%zu bytes)",
1936                                                                         size);
1937                         return PARSE_ERROR;
1938                 }
1939                 if (!session->in.active) {
1940                         error("END: Invalid AVDTP packet fragmentation");
1941                         return PARSE_ERROR;
1942                 }
1943                 if (session->in.transaction != header->transaction) {
1944                         error("End transaction id doesn't match");
1945                         return PARSE_ERROR;
1946                 }
1947                 if (session->in.no_of_packets > 1) {
1948                         error("Got an end packet too early");
1949                         return PARSE_ERROR;
1950                 }
1951
1952                 payload = session->buf + sizeof(struct avdtp_continue_header);
1953                 payload_size = size - sizeof(struct avdtp_continue_header);
1954
1955                 break;
1956         default:
1957                 error("Invalid AVDTP packet type 0x%02X", header->packet_type);
1958                 return PARSE_ERROR;
1959         }
1960
1961         if (session->in.data_size + payload_size >
1962                                         sizeof(session->in.buf)) {
1963                 error("Not enough incoming buffer space!");
1964                 return PARSE_ERROR;
1965         }
1966
1967         memcpy(session->in.buf + session->in.data_size, payload, payload_size);
1968         session->in.data_size += payload_size;
1969
1970         if (session->in.no_of_packets > 1) {
1971                 session->in.no_of_packets--;
1972                 DBG("Received AVDTP fragment. %d to go",
1973                                                 session->in.no_of_packets);
1974                 return PARSE_FRAGMENT;
1975         }
1976
1977         session->in.active = FALSE;
1978
1979         return PARSE_SUCCESS;
1980 }
1981
1982 static gboolean session_cb(GIOChannel *chan, GIOCondition cond,
1983                                 gpointer data)
1984 {
1985         struct avdtp *session = data;
1986         struct avdtp_common_header *header;
1987         ssize_t size;
1988         int fd;
1989
1990         DBG("");
1991
1992         if (cond & G_IO_NVAL) {
1993                 session->io_id = 0;
1994
1995                 return FALSE;
1996         }
1997
1998         header = (void *) session->buf;
1999
2000         if (cond & (G_IO_HUP | G_IO_ERR))
2001                 goto failed;
2002
2003         fd = g_io_channel_unix_get_fd(chan);
2004         size = read(fd, session->buf, session->imtu);
2005         if (size < 0) {
2006                 error("IO Channel read error");
2007                 goto failed;
2008         }
2009
2010         if ((size_t) size < sizeof(struct avdtp_common_header)) {
2011                 error("Received too small packet (%zu bytes)", size);
2012                 goto failed;
2013         }
2014
2015         switch (avdtp_parse_data(session, session->buf, size)) {
2016         case PARSE_ERROR:
2017                 goto failed;
2018         case PARSE_FRAGMENT:
2019                 return TRUE;
2020         case PARSE_SUCCESS:
2021                 break;
2022         }
2023
2024         /* Take a reference to protect against callback destroying session */
2025         avdtp_ref(session);
2026
2027         if (session->in.message_type == AVDTP_MSG_TYPE_COMMAND) {
2028                 if (!avdtp_parse_cmd(session, session->in.transaction,
2029                                         session->in.signal_id,
2030                                         session->in.buf,
2031                                         session->in.data_size)) {
2032                         error("Unable to handle command. Disconnecting");
2033                         goto failed;
2034                 }
2035
2036                 if (session->req && session->req->collided) {
2037                         DBG("Collision detected");
2038                         goto next;
2039                 }
2040
2041                 avdtp_unref(session);
2042                 return TRUE;
2043         }
2044
2045         if (session->req == NULL) {
2046                 error("No pending request, ignoring message");
2047                 avdtp_unref(session);
2048                 return TRUE;
2049         }
2050
2051         if (header->transaction != session->req->transaction) {
2052                 error("Transaction label doesn't match");
2053                 avdtp_unref(session);
2054                 return TRUE;
2055         }
2056
2057         if (session->in.signal_id != session->req->signal_id) {
2058                 error("Response signal doesn't match");
2059                 avdtp_unref(session);
2060                 return TRUE;
2061         }
2062
2063         g_source_remove(session->req->timeout);
2064         session->req->timeout = 0;
2065
2066         switch (header->message_type) {
2067         case AVDTP_MSG_TYPE_ACCEPT:
2068                 if (!avdtp_parse_resp(session, session->req->stream,
2069                                                 session->in.transaction,
2070                                                 session->in.signal_id,
2071                                                 session->in.buf,
2072                                                 session->in.data_size)) {
2073                         error("Unable to parse accept response");
2074                         goto failed;
2075                 }
2076                 break;
2077         case AVDTP_MSG_TYPE_REJECT:
2078                 if (!avdtp_parse_rej(session, session->req->stream,
2079                                                 session->in.transaction,
2080                                                 session->in.signal_id,
2081                                                 session->in.buf,
2082                                                 session->in.data_size)) {
2083                         error("Unable to parse reject response");
2084                         goto failed;
2085                 }
2086                 break;
2087         case AVDTP_MSG_TYPE_GEN_REJECT:
2088                 error("Received a General Reject message");
2089                 break;
2090         default:
2091                 error("Unknown message type 0x%02X", header->message_type);
2092                 break;
2093         }
2094
2095 next:
2096         pending_req_free(session->req);
2097         session->req = NULL;
2098
2099         if (session->ref > 1)
2100                 process_queue(session);
2101
2102         avdtp_unref(session);
2103
2104         return TRUE;
2105
2106 failed:
2107         session->io_id = 0;
2108         connection_lost(session, EIO);
2109
2110         return FALSE;
2111 }
2112
2113 static int set_priority(int fd, int priority)
2114 {
2115         int err;
2116
2117         err = setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &priority,
2118                                                         sizeof(priority));
2119         if (err == 0 || errno == ENOTSOCK)
2120                 return 0;
2121
2122         err = -errno;
2123         error("setsockopt(SO_PRIORITY): %s (%d)", strerror(-err), -err);
2124
2125         return err;
2126 }
2127
2128 struct avdtp *avdtp_new(int fd, size_t imtu, size_t omtu, uint16_t version,
2129                                                         struct queue *lseps)
2130 {
2131         struct avdtp *session;
2132         GIOCondition cond = G_IO_IN | G_IO_ERR | G_IO_HUP | G_IO_NVAL;
2133         int new_fd;
2134
2135         if (!lseps)
2136                 return NULL;
2137
2138         new_fd = dup(fd);
2139         if (new_fd < 0) {
2140                 error("dup(): %s (%d)", strerror(errno), errno);
2141                 return NULL;
2142         }
2143
2144         if (set_priority(new_fd, 6) < 0)
2145                 return NULL;
2146
2147         session = g_new0(struct avdtp, 1);
2148         session->io = g_io_channel_unix_new(new_fd);
2149         session->version = version;
2150         session->imtu = imtu;
2151         session->omtu = omtu;
2152         session->buf = g_malloc0(MAX(session->imtu, session->omtu));
2153
2154         /* This watch should be low priority since otherwise the
2155          * connect callback might be dispatched before the session
2156          * callback if the kernel wakes us up at the same time for
2157          * them. This could happen if a headset is very quick in
2158          * sending the Start command after connecting the stream
2159          * transport channel.
2160          */
2161         session->io_id = g_io_add_watch_full(session->io, G_PRIORITY_LOW, cond,
2162                                                 (GIOFunc) session_cb, session,
2163                                                 NULL);
2164
2165         session->lseps = lseps;
2166
2167         return avdtp_ref(session);
2168 }
2169
2170 unsigned int avdtp_add_disconnect_cb(struct avdtp *session,
2171                                                 avdtp_disconnect_cb_t cb,
2172                                                 void *user_data)
2173 {
2174         struct disconnect_callback *callback;
2175         static unsigned int id = 0;
2176
2177         callback = g_new0(struct disconnect_callback, 1);
2178         callback->id = ++id;
2179         callback->cb = cb;
2180         callback->user_data = user_data;
2181         session->disconnect = g_slist_append(session->disconnect, callback);
2182
2183         return id;
2184 }
2185
2186 gboolean avdtp_remove_disconnect_cb(struct avdtp *session, unsigned int id)
2187 {
2188         GSList *l;
2189
2190         for (l = session->disconnect; l; l = g_slist_next(l)) {
2191                 struct disconnect_callback *callback = l->data;
2192
2193                 if (callback->id != id)
2194                         continue;
2195
2196                 session->disconnect = g_slist_remove(session->disconnect,
2197                                                                 callback);
2198                 g_free(callback);
2199                 return TRUE;
2200         }
2201
2202         return FALSE;
2203 }
2204
2205 void avdtp_shutdown(struct avdtp *session)
2206 {
2207         GSList *l;
2208         bool aborting = false;
2209
2210         if (!session->io)
2211                 return;
2212
2213         for (l = session->streams; l; l = g_slist_next(l)) {
2214                 struct avdtp_stream *stream = l->data;
2215
2216                 if (stream->abort_int ||
2217                                         avdtp_close(session, stream, TRUE) == 0)
2218                         aborting = true;
2219         }
2220
2221         if (aborting) {
2222                 /* defer shutdown until all streams are aborted properly */
2223                 session->shutdown = true;
2224         } else {
2225                 int sock = g_io_channel_unix_get_fd(session->io);
2226
2227                 shutdown(sock, SHUT_RDWR);
2228         }
2229 }
2230
2231 static void queue_request(struct avdtp *session, struct pending_req *req,
2232                         gboolean priority)
2233 {
2234         if (priority)
2235                 session->prio_queue = g_slist_append(session->prio_queue, req);
2236         else
2237                 session->req_queue = g_slist_append(session->req_queue, req);
2238 }
2239
2240 static uint8_t req_get_seid(struct pending_req *req)
2241 {
2242         if (req->signal_id == AVDTP_DISCOVER)
2243                 return 0;
2244
2245         return ((struct seid_req *) (req->data))->acp_seid;
2246 }
2247
2248 static int cancel_request(struct avdtp *session, int err)
2249 {
2250         struct pending_req *req;
2251         struct seid_req sreq;
2252         struct avdtp_local_sep *lsep;
2253         struct avdtp_stream *stream;
2254         uint8_t seid;
2255         struct avdtp_error averr;
2256
2257         req = session->req;
2258         session->req = NULL;
2259
2260         avdtp_error_init(&averr, AVDTP_ERRNO, err);
2261
2262         seid = req_get_seid(req);
2263         if (seid)
2264                 stream = find_stream_by_rseid(session, seid);
2265         else
2266                 stream = NULL;
2267
2268         if (stream) {
2269                 stream->abort_int = TRUE;
2270                 lsep = stream->lsep;
2271         } else
2272                 lsep = NULL;
2273
2274         switch (req->signal_id) {
2275         case AVDTP_RECONFIGURE:
2276                 error("Reconfigure: %s (%d)", strerror(err), err);
2277                 if (lsep && lsep->cfm && lsep->cfm->reconfigure)
2278                         lsep->cfm->reconfigure(session, lsep, stream, &averr,
2279                                                 lsep->user_data);
2280                 break;
2281         case AVDTP_OPEN:
2282                 error("Open: %s (%d)", strerror(err), err);
2283                 if (lsep && lsep->cfm && lsep->cfm->open)
2284                         lsep->cfm->open(session, lsep, stream, &averr,
2285                                         lsep->user_data);
2286                 break;
2287         case AVDTP_START:
2288                 error("Start: %s (%d)", strerror(err), err);
2289                 if (lsep && lsep->cfm && lsep->cfm->start) {
2290                         lsep->cfm->start(session, lsep, stream, &averr,
2291                                                 lsep->user_data);
2292                         if (stream)
2293                                 stream->starting = FALSE;
2294                 }
2295                 break;
2296         case AVDTP_SUSPEND:
2297                 error("Suspend: %s (%d)", strerror(err), err);
2298                 if (lsep && lsep->cfm && lsep->cfm->suspend)
2299                         lsep->cfm->suspend(session, lsep, stream, &averr,
2300                                                 lsep->user_data);
2301                 break;
2302         case AVDTP_CLOSE:
2303                 error("Close: %s (%d)", strerror(err), err);
2304                 if (lsep && lsep->cfm && lsep->cfm->close) {
2305                         lsep->cfm->close(session, lsep, stream, &averr,
2306                                                 lsep->user_data);
2307                         if (stream)
2308                                 stream->close_int = FALSE;
2309                 }
2310                 break;
2311         case AVDTP_SET_CONFIGURATION:
2312                 error("SetConfiguration: %s (%d)", strerror(err), err);
2313                 if (lsep && lsep->cfm && lsep->cfm->set_configuration)
2314                         lsep->cfm->set_configuration(session, lsep, stream,
2315                                                 &averr, lsep->user_data);
2316                 goto failed;
2317         case AVDTP_DISCOVER:
2318                 error("Discover: %s (%d)", strerror(err), err);
2319                 goto failed;
2320         case AVDTP_GET_CAPABILITIES:
2321                 error("GetCapabilities: %s (%d)", strerror(err), err);
2322                 goto failed;
2323         case AVDTP_ABORT:
2324                 error("Abort: %s (%d)", strerror(err), err);
2325                 goto failed;
2326         }
2327
2328         if (!stream)
2329                 goto failed;
2330
2331         memset(&sreq, 0, sizeof(sreq));
2332         sreq.acp_seid = seid;
2333
2334         err = send_request(session, TRUE, stream, AVDTP_ABORT, &sreq,
2335                                 sizeof(sreq));
2336         if (err < 0) {
2337                 error("Unable to send abort request");
2338                 goto failed;
2339         }
2340
2341         goto done;
2342
2343 failed:
2344         connection_lost(session, err);
2345 done:
2346         pending_req_free(req);
2347         return err;
2348 }
2349
2350 static gboolean request_timeout(gpointer user_data)
2351 {
2352         struct avdtp *session = user_data;
2353
2354         cancel_request(session, ETIMEDOUT);
2355
2356         return FALSE;
2357 }
2358
2359 static int send_req(struct avdtp *session, gboolean priority,
2360                         struct pending_req *req)
2361 {
2362         static int transaction = 0;
2363         int err;
2364
2365         if (session->req != NULL) {
2366                 queue_request(session, req, priority);
2367                 return 0;
2368         }
2369
2370         req->transaction = transaction++;
2371         transaction %= 16;
2372
2373         /* FIXME: Should we retry to send if the buffer
2374         was not totally sent or in case of EINTR? */
2375         if (!avdtp_send(session, req->transaction, AVDTP_MSG_TYPE_COMMAND,
2376                                 req->signal_id, req->data, req->data_size)) {
2377                 err = -EIO;
2378                 goto failed;
2379         }
2380
2381         session->req = req;
2382
2383         req->timeout = g_timeout_add_seconds(req->signal_id == AVDTP_ABORT ?
2384                                         ABORT_TIMEOUT : REQ_TIMEOUT,
2385                                         request_timeout,
2386                                         session);
2387         return 0;
2388
2389 failed:
2390         g_free(req->data);
2391         g_free(req);
2392         return err;
2393 }
2394
2395 static int send_request(struct avdtp *session, gboolean priority,
2396                         struct avdtp_stream *stream, uint8_t signal_id,
2397                         void *buffer, size_t size)
2398 {
2399         struct pending_req *req;
2400
2401         if (size > 0 && !buffer) {
2402                 DBG("Invalid buffer %p", buffer);
2403                 return -EINVAL;
2404         }
2405
2406         if (stream && stream->abort_int && signal_id != AVDTP_ABORT) {
2407                 DBG("Unable to send requests while aborting");
2408                 return -EINVAL;
2409         }
2410
2411         req = g_new0(struct pending_req, 1);
2412         req->signal_id = signal_id;
2413         req->data_size = size;
2414         req->stream = stream;
2415
2416         if (size > 0) {
2417                 req->data = g_malloc(size);
2418                 memcpy(req->data, buffer, size);
2419         }
2420
2421         return send_req(session, priority, req);
2422 }
2423
2424 static gboolean avdtp_discover_resp(struct avdtp *session,
2425                                         struct discover_resp *resp, int size)
2426 {
2427         int sep_count, i;
2428         uint8_t getcap_cmd;
2429         int ret = 0;
2430         gboolean getcap_pending = FALSE;
2431
2432         if (session->version >= 0x0103)
2433                 getcap_cmd = AVDTP_GET_ALL_CAPABILITIES;
2434         else
2435                 getcap_cmd = AVDTP_GET_CAPABILITIES;
2436
2437         sep_count = size / sizeof(struct seid_info);
2438
2439         for (i = 0; i < sep_count; i++) {
2440                 struct avdtp_remote_sep *sep;
2441                 struct avdtp_stream *stream;
2442                 struct seid_req req;
2443
2444                 DBG("seid %d type %d media %d in use %d",
2445                                 resp->seps[i].seid, resp->seps[i].type,
2446                                 resp->seps[i].media_type, resp->seps[i].inuse);
2447
2448                 stream = find_stream_by_rseid(session, resp->seps[i].seid);
2449
2450                 sep = find_remote_sep(session->seps, resp->seps[i].seid);
2451                 if (!sep) {
2452                         if (resp->seps[i].inuse && !stream)
2453                                 continue;
2454                         sep = g_new0(struct avdtp_remote_sep, 1);
2455                         session->seps = g_slist_append(session->seps, sep);
2456                 }
2457
2458                 sep->stream = stream;
2459                 sep->seid = resp->seps[i].seid;
2460                 sep->type = resp->seps[i].type;
2461                 sep->media_type = resp->seps[i].media_type;
2462
2463                 memset(&req, 0, sizeof(req));
2464                 req.acp_seid = sep->seid;
2465
2466                 ret = send_request(session, TRUE, NULL, getcap_cmd,
2467                                                         &req, sizeof(req));
2468                 if (ret < 0)
2469                         break;
2470                 getcap_pending = TRUE;
2471         }
2472
2473         if (!getcap_pending)
2474                 finalize_discovery(session, -ret);
2475
2476         return TRUE;
2477 }
2478
2479 static gboolean avdtp_get_capabilities_resp(struct avdtp *session,
2480                                                 struct getcap_resp *resp,
2481                                                 unsigned int size)
2482 {
2483         struct avdtp_remote_sep *sep;
2484         uint8_t seid;
2485
2486         /* Check for minimum required packet size includes:
2487          *   1. getcap resp header
2488          *   2. media transport capability (2 bytes)
2489          *   3. media codec capability type + length (2 bytes)
2490          *   4. the actual media codec elements
2491          * */
2492         if (size < (sizeof(struct getcap_resp) + 4 +
2493                                 sizeof(struct avdtp_media_codec_capability))) {
2494                 error("Too short getcap resp packet");
2495                 return FALSE;
2496         }
2497
2498         seid = ((struct seid_req *) session->req->data)->acp_seid;
2499
2500         sep = find_remote_sep(session->seps, seid);
2501
2502         DBG("seid %d type %d media %d", sep->seid,
2503                                         sep->type, sep->media_type);
2504
2505         if (sep->caps) {
2506                 g_slist_free_full(sep->caps, g_free);
2507                 sep->caps = NULL;
2508                 sep->codec = NULL;
2509                 sep->delay_reporting = FALSE;
2510         }
2511
2512         sep->caps = caps_to_list(resp->caps, size - sizeof(struct getcap_resp),
2513                                         &sep->codec, &sep->delay_reporting);
2514
2515         return TRUE;
2516 }
2517
2518 static gboolean avdtp_set_configuration_resp(struct avdtp *session,
2519                                         struct avdtp_stream *stream,
2520                                         struct avdtp_single_header *resp,
2521                                         int size)
2522 {
2523         struct avdtp_local_sep *sep = stream->lsep;
2524
2525         avdtp_sep_set_state(session, sep, AVDTP_STATE_CONFIGURED);
2526
2527         if (sep->cfm && sep->cfm->set_configuration)
2528                 sep->cfm->set_configuration(session, sep, stream, NULL,
2529                                                 sep->user_data);
2530
2531         return TRUE;
2532 }
2533
2534 static gboolean avdtp_reconfigure_resp(struct avdtp *session,
2535                                         struct avdtp_stream *stream,
2536                                         struct avdtp_single_header *resp,
2537                                         int size)
2538 {
2539         return TRUE;
2540 }
2541
2542 static gboolean avdtp_open_resp(struct avdtp *session,
2543                                 struct avdtp_stream *stream,
2544                                 struct seid_rej *resp, int size)
2545 {
2546         struct avdtp_local_sep *sep = stream->lsep;
2547
2548         session->pending_open = stream;
2549
2550         if (!stream->open_acp && sep->cfm && sep->cfm->open)
2551                 sep->cfm->open(session, sep, stream, NULL, sep->user_data);
2552
2553         return TRUE;
2554 }
2555
2556 static gboolean avdtp_start_resp(struct avdtp *session,
2557                                         struct avdtp_stream *stream,
2558                                         struct seid_rej *resp, int size)
2559 {
2560         struct avdtp_local_sep *sep = stream->lsep;
2561
2562         /* We might be in STREAMING already if both sides send START_CMD at the
2563          * same time and the one in SNK role doesn't reject it as it should */
2564         if (sep->state != AVDTP_STATE_STREAMING)
2565                 avdtp_sep_set_state(session, sep, AVDTP_STATE_STREAMING);
2566
2567         if (sep->cfm && sep->cfm->start)
2568                 sep->cfm->start(session, sep, stream, NULL, sep->user_data);
2569
2570         return TRUE;
2571 }
2572
2573 static gboolean avdtp_close_resp(struct avdtp *session,
2574                                         struct avdtp_stream *stream,
2575                                         struct seid_rej *resp, int size)
2576 {
2577         struct avdtp_local_sep *sep = stream->lsep;
2578
2579         avdtp_sep_set_state(session, sep, AVDTP_STATE_CLOSING);
2580
2581         close_stream(stream);
2582
2583         return TRUE;
2584 }
2585
2586 static gboolean avdtp_suspend_resp(struct avdtp *session,
2587                                         struct avdtp_stream *stream,
2588                                         void *data, int size)
2589 {
2590         struct avdtp_local_sep *sep = stream->lsep;
2591
2592         avdtp_sep_set_state(session, sep, AVDTP_STATE_OPEN);
2593
2594         if (sep->cfm && sep->cfm->suspend)
2595                 sep->cfm->suspend(session, sep, stream, NULL, sep->user_data);
2596
2597         return TRUE;
2598 }
2599
2600 static gboolean avdtp_abort_resp(struct avdtp *session,
2601                                         struct avdtp_stream *stream,
2602                                         struct seid_rej *resp, int size)
2603 {
2604         struct avdtp_local_sep *sep = stream->lsep;
2605
2606         avdtp_sep_set_state(session, sep, AVDTP_STATE_ABORTING);
2607
2608         if (sep->cfm && sep->cfm->abort)
2609                 sep->cfm->abort(session, sep, stream, NULL, sep->user_data);
2610
2611         avdtp_sep_set_state(session, sep, AVDTP_STATE_IDLE);
2612
2613         return TRUE;
2614 }
2615
2616 static gboolean avdtp_delay_report_resp(struct avdtp *session,
2617                                         struct avdtp_stream *stream,
2618                                         void *data, int size)
2619 {
2620         struct avdtp_local_sep *sep = stream->lsep;
2621
2622         if (sep->cfm && sep->cfm->delay_report)
2623                 sep->cfm->delay_report(session, sep, stream, NULL,
2624                                                         sep->user_data);
2625
2626         return TRUE;
2627 }
2628
2629 static gboolean avdtp_parse_resp(struct avdtp *session,
2630                                         struct avdtp_stream *stream,
2631                                         uint8_t transaction, uint8_t signal_id,
2632                                         void *buf, int size)
2633 {
2634         struct pending_req *next;
2635         const char *get_all = "";
2636
2637         if (session->prio_queue)
2638                 next = session->prio_queue->data;
2639         else if (session->req_queue)
2640                 next = session->req_queue->data;
2641         else
2642                 next = NULL;
2643
2644         switch (signal_id) {
2645         case AVDTP_DISCOVER:
2646                 DBG("DISCOVER request succeeded");
2647                 return avdtp_discover_resp(session, buf, size);
2648         case AVDTP_GET_ALL_CAPABILITIES:
2649                 get_all = "ALL_";
2650         case AVDTP_GET_CAPABILITIES:
2651                 DBG("GET_%sCAPABILITIES request succeeded", get_all);
2652                 if (!avdtp_get_capabilities_resp(session, buf, size))
2653                         return FALSE;
2654                 if (!(next && (next->signal_id == AVDTP_GET_CAPABILITIES ||
2655                                 next->signal_id == AVDTP_GET_ALL_CAPABILITIES)))
2656                         finalize_discovery(session, 0);
2657                 return TRUE;
2658         }
2659
2660         /* The remaining commands require an existing stream so bail out
2661          * here if the stream got unexpectedly disconnected */
2662         if (!stream) {
2663                 DBG("AVDTP: stream was closed while waiting for reply");
2664                 return TRUE;
2665         }
2666
2667         switch (signal_id) {
2668         case AVDTP_SET_CONFIGURATION:
2669                 DBG("SET_CONFIGURATION request succeeded");
2670                 return avdtp_set_configuration_resp(session, stream,
2671                                                                 buf, size);
2672         case AVDTP_RECONFIGURE:
2673                 DBG("RECONFIGURE request succeeded");
2674                 return avdtp_reconfigure_resp(session, stream, buf, size);
2675         case AVDTP_OPEN:
2676                 DBG("OPEN request succeeded");
2677                 return avdtp_open_resp(session, stream, buf, size);
2678         case AVDTP_SUSPEND:
2679                 DBG("SUSPEND request succeeded");
2680                 return avdtp_suspend_resp(session, stream, buf, size);
2681         case AVDTP_START:
2682                 DBG("START request succeeded");
2683                 return avdtp_start_resp(session, stream, buf, size);
2684         case AVDTP_CLOSE:
2685                 DBG("CLOSE request succeeded");
2686                 return avdtp_close_resp(session, stream, buf, size);
2687         case AVDTP_ABORT:
2688                 DBG("ABORT request succeeded");
2689                 return avdtp_abort_resp(session, stream, buf, size);
2690         case AVDTP_DELAY_REPORT:
2691                 DBG("DELAY_REPORT request succeeded");
2692                 return avdtp_delay_report_resp(session, stream, buf, size);
2693         }
2694
2695         error("Unknown signal id in accept response: %u", signal_id);
2696         return TRUE;
2697 }
2698
2699 static gboolean seid_rej_to_err(struct seid_rej *rej, unsigned int size,
2700                                         struct avdtp_error *err)
2701 {
2702         if (size < sizeof(struct seid_rej)) {
2703                 error("Too small packet for seid_rej");
2704                 return FALSE;
2705         }
2706
2707         avdtp_error_init(err, 0x00, rej->error);
2708
2709         return TRUE;
2710 }
2711
2712 static gboolean conf_rej_to_err(struct conf_rej *rej, unsigned int size,
2713                                 struct avdtp_error *err)
2714 {
2715         if (size < sizeof(struct conf_rej)) {
2716                 error("Too small packet for conf_rej");
2717                 return FALSE;
2718         }
2719
2720         avdtp_error_init(err, rej->category, rej->error);
2721
2722         return TRUE;
2723 }
2724
2725 static gboolean stream_rej_to_err(struct stream_rej *rej, unsigned int size,
2726                                         struct avdtp_error *err,
2727                                         uint8_t *acp_seid)
2728 {
2729         if (size < sizeof(struct stream_rej)) {
2730                 error("Too small packet for stream_rej");
2731                 return FALSE;
2732         }
2733
2734         avdtp_error_init(err, 0x00, rej->error);
2735
2736         if (acp_seid)
2737                 *acp_seid = rej->acp_seid;
2738
2739         return TRUE;
2740 }
2741
2742 static gboolean avdtp_parse_rej(struct avdtp *session,
2743                                         struct avdtp_stream *stream,
2744                                         uint8_t transaction, uint8_t signal_id,
2745                                         void *buf, int size)
2746 {
2747         struct avdtp_error err;
2748         uint8_t acp_seid;
2749         struct avdtp_local_sep *sep = stream ? stream->lsep : NULL;
2750
2751         switch (signal_id) {
2752         case AVDTP_DISCOVER:
2753         case AVDTP_GET_CAPABILITIES:
2754         case AVDTP_GET_ALL_CAPABILITIES:
2755                 if (!seid_rej_to_err(buf, size, &err))
2756                         return FALSE;
2757                 error("%s request rejected: %s (%d)",
2758                         signal_id == AVDTP_DISCOVER ? "DISCOVER" :
2759                         signal_id == AVDTP_GET_CAPABILITIES ?
2760                         "GET_CAPABILITIES" : "GET_ALL_CAPABILITIES",
2761                         avdtp_strerror(&err), err.err.error_code);
2762                 if (session->discover) {
2763                         session->discover->cb(session, session->seps, &err,
2764                                                 session->discover->user_data);
2765                         g_free(session->discover);
2766                         session->discover = NULL;
2767                 }
2768                 return TRUE;
2769         case AVDTP_OPEN:
2770                 if (!seid_rej_to_err(buf, size, &err))
2771                         return FALSE;
2772                 error("OPEN request rejected: %s (%d)",
2773                                 avdtp_strerror(&err), err.err.error_code);
2774                 if (sep && sep->cfm && sep->cfm->open)
2775                         sep->cfm->open(session, sep, stream, &err,
2776                                         sep->user_data);
2777                 return TRUE;
2778         case AVDTP_SET_CONFIGURATION:
2779                 if (!conf_rej_to_err(buf, size, &err))
2780                         return FALSE;
2781                 error("SET_CONFIGURATION request rejected: %s (%d)",
2782                                 avdtp_strerror(&err), err.err.error_code);
2783                 if (sep && sep->cfm && sep->cfm->set_configuration)
2784                         sep->cfm->set_configuration(session, sep, stream,
2785                                                         &err, sep->user_data);
2786                 return TRUE;
2787         case AVDTP_GET_CONFIGURATION:
2788                 if (!seid_rej_to_err(buf, size, &err))
2789                         return FALSE;
2790                 error("GET_CONFIGURATION request rejected: %s (%d)",
2791                                 avdtp_strerror(&err), err.err.error_code);
2792                 if (sep && sep->cfm && sep->cfm->get_configuration)
2793                         sep->cfm->get_configuration(session, sep, stream, &err,
2794                                                                 sep->user_data);
2795                 return TRUE;
2796         case AVDTP_RECONFIGURE:
2797                 if (!conf_rej_to_err(buf, size, &err))
2798                         return FALSE;
2799                 error("RECONFIGURE request rejected: %s (%d)",
2800                                 avdtp_strerror(&err), err.err.error_code);
2801                 if (sep && sep->cfm && sep->cfm->reconfigure)
2802                         sep->cfm->reconfigure(session, sep, stream, &err,
2803                                                 sep->user_data);
2804                 return TRUE;
2805         case AVDTP_START:
2806                 if (!stream_rej_to_err(buf, size, &err, &acp_seid))
2807                         return FALSE;
2808                 error("START request rejected: %s (%d)",
2809                                 avdtp_strerror(&err), err.err.error_code);
2810                 if (sep && sep->cfm && sep->cfm->start) {
2811                         stream->starting = FALSE;
2812                         sep->cfm->start(session, sep, stream, &err,
2813                                         sep->user_data);
2814                 }
2815                 return TRUE;
2816         case AVDTP_SUSPEND:
2817                 if (!stream_rej_to_err(buf, size, &err, &acp_seid))
2818                         return FALSE;
2819                 error("SUSPEND request rejected: %s (%d)",
2820                                 avdtp_strerror(&err), err.err.error_code);
2821                 if (sep && sep->cfm && sep->cfm->suspend)
2822                         sep->cfm->suspend(session, sep, stream, &err,
2823                                                 sep->user_data);
2824                 return TRUE;
2825         case AVDTP_CLOSE:
2826                 if (!stream_rej_to_err(buf, size, &err, &acp_seid))
2827                         return FALSE;
2828                 error("CLOSE request rejected: %s (%d)",
2829                                 avdtp_strerror(&err), err.err.error_code);
2830                 if (sep && sep->cfm && sep->cfm->close) {
2831                         sep->cfm->close(session, sep, stream, &err,
2832                                         sep->user_data);
2833                         stream->close_int = FALSE;
2834                 }
2835                 return TRUE;
2836         case AVDTP_ABORT:
2837                 if (!stream_rej_to_err(buf, size, &err, &acp_seid))
2838                         return FALSE;
2839                 error("ABORT request rejected: %s (%d)",
2840                                 avdtp_strerror(&err), err.err.error_code);
2841                 if (sep && sep->cfm && sep->cfm->abort)
2842                         sep->cfm->abort(session, sep, stream, &err,
2843                                         sep->user_data);
2844                 return FALSE;
2845         case AVDTP_DELAY_REPORT:
2846                 if (!stream_rej_to_err(buf, size, &err, &acp_seid))
2847                         return FALSE;
2848                 error("DELAY_REPORT request rejected: %s (%d)",
2849                                 avdtp_strerror(&err), err.err.error_code);
2850                 if (sep && sep->cfm && sep->cfm->delay_report)
2851                         sep->cfm->delay_report(session, sep, stream, &err,
2852                                                         sep->user_data);
2853                 return TRUE;
2854         default:
2855                 error("Unknown reject response signal id: %u", signal_id);
2856                 return TRUE;
2857         }
2858 }
2859
2860 struct avdtp_service_capability *avdtp_stream_get_codec(
2861                                                 struct avdtp_stream *stream)
2862 {
2863         GSList *l;
2864
2865         for (l = stream->caps; l; l = l->next) {
2866                 struct avdtp_service_capability *cap = l->data;
2867
2868                 if (cap->category == AVDTP_MEDIA_CODEC)
2869                         return cap;
2870         }
2871
2872         return NULL;
2873 }
2874
2875 static gboolean avdtp_stream_has_capability(struct avdtp_stream *stream,
2876                                         struct avdtp_service_capability *cap)
2877 {
2878         GSList *l;
2879         struct avdtp_service_capability *stream_cap;
2880
2881         for (l = stream->caps; l; l = g_slist_next(l)) {
2882                 stream_cap = l->data;
2883
2884                 if (stream_cap->category != cap->category ||
2885                         stream_cap->length != cap->length)
2886                         continue;
2887
2888                 if (memcmp(stream_cap->data, cap->data, cap->length) == 0)
2889                         return TRUE;
2890         }
2891
2892         return FALSE;
2893 }
2894
2895 gboolean avdtp_stream_has_capabilities(struct avdtp_stream *stream,
2896                                         GSList *caps)
2897 {
2898         for (; caps; caps = g_slist_next(caps)) {
2899                 struct avdtp_service_capability *cap = caps->data;
2900
2901                 if (!avdtp_stream_has_capability(stream, cap))
2902                         return FALSE;
2903         }
2904
2905         return TRUE;
2906 }
2907
2908 struct avdtp_remote_sep *avdtp_stream_get_remote_sep(
2909                                                 struct avdtp_stream *stream)
2910 {
2911         GSList *l;
2912
2913         for (l = stream->session->seps; l; l = l->next) {
2914                 struct avdtp_remote_sep *sep = l->data;
2915
2916                 if (sep->seid == stream->rseid)
2917                         return sep;
2918         }
2919
2920         return NULL;
2921 }
2922
2923 gboolean avdtp_stream_set_transport(struct avdtp_stream *stream, int fd,
2924                                                 size_t imtu, size_t omtu)
2925 {
2926         GIOChannel *io;
2927
2928         if (stream != stream->session->pending_open)
2929                 return FALSE;
2930
2931         if (set_priority(fd, 5) < 0)
2932                 return FALSE;
2933
2934         io = g_io_channel_unix_new(fd);
2935
2936         handle_transport_connect(stream->session, io, imtu, omtu);
2937
2938         g_io_channel_unref(io);
2939
2940         return TRUE;
2941 }
2942
2943 gboolean avdtp_stream_get_transport(struct avdtp_stream *stream, int *sock,
2944                                         uint16_t *imtu, uint16_t *omtu,
2945                                         GSList **caps)
2946 {
2947         if (stream->io == NULL)
2948                 return FALSE;
2949
2950         if (sock)
2951                 *sock = g_io_channel_unix_get_fd(stream->io);
2952
2953         if (omtu)
2954                 *omtu = stream->omtu;
2955
2956         if (imtu)
2957                 *imtu = stream->imtu;
2958
2959         if (caps)
2960                 *caps = stream->caps;
2961
2962         return TRUE;
2963 }
2964
2965 static int process_queue(struct avdtp *session)
2966 {
2967         GSList **queue, *l;
2968         struct pending_req *req;
2969
2970         if (session->req)
2971                 return 0;
2972
2973         if (session->prio_queue)
2974                 queue = &session->prio_queue;
2975         else
2976                 queue = &session->req_queue;
2977
2978         if (!*queue)
2979                 return 0;
2980
2981         l = *queue;
2982         req = l->data;
2983
2984         *queue = g_slist_remove(*queue, req);
2985
2986         return send_req(session, FALSE, req);
2987 }
2988
2989 struct avdtp_service_capability *avdtp_get_codec(struct avdtp_remote_sep *sep)
2990 {
2991         return sep->codec;
2992 }
2993
2994 struct avdtp_service_capability *avdtp_service_cap_new(uint8_t category,
2995                                                         const void *data,
2996                                                         int length)
2997 {
2998         struct avdtp_service_capability *cap;
2999
3000         if (category < AVDTP_MEDIA_TRANSPORT ||
3001                                         category > AVDTP_DELAY_REPORTING)
3002                 return NULL;
3003
3004         if (length > 0 && !data)
3005                 return NULL;
3006
3007         cap = g_malloc(sizeof(struct avdtp_service_capability) + length);
3008         cap->category = category;
3009         cap->length = length;
3010
3011         if (length > 0)
3012                 memcpy(cap->data, data, length);
3013
3014         return cap;
3015 }
3016
3017 static gboolean process_discover(gpointer data)
3018 {
3019         struct avdtp *session = data;
3020
3021         session->discover->id = 0;
3022
3023         finalize_discovery(session, 0);
3024
3025         return FALSE;
3026 }
3027
3028 int avdtp_discover(struct avdtp *session, avdtp_discover_cb_t cb,
3029                         void *user_data)
3030 {
3031         int err;
3032
3033         if (session->discover)
3034                 return -EBUSY;
3035
3036         session->discover = g_new0(struct discover_callback, 1);
3037
3038         if (session->seps) {
3039                 session->discover->cb = cb;
3040                 session->discover->user_data = user_data;
3041                 session->discover->id = g_idle_add(process_discover, session);
3042                 return 0;
3043         }
3044
3045         err = send_request(session, FALSE, NULL, AVDTP_DISCOVER, NULL, 0);
3046         if (err == 0) {
3047                 session->discover->cb = cb;
3048                 session->discover->user_data = user_data;
3049         }
3050
3051         return err;
3052 }
3053
3054 gboolean avdtp_stream_remove_cb(struct avdtp *session,
3055                                 struct avdtp_stream *stream,
3056                                 unsigned int id)
3057 {
3058         GSList *l;
3059         struct stream_callback *cb;
3060
3061         if (!stream)
3062                 return FALSE;
3063
3064         for (cb = NULL, l = stream->callbacks; l != NULL; l = l->next) {
3065                 struct stream_callback *tmp = l->data;
3066                 if (tmp && tmp->id == id) {
3067                         cb = tmp;
3068                         break;
3069                 }
3070         }
3071
3072         if (!cb)
3073                 return FALSE;
3074
3075         stream->callbacks = g_slist_remove(stream->callbacks, cb);
3076         g_free(cb);
3077
3078         return TRUE;
3079 }
3080
3081 unsigned int avdtp_stream_add_cb(struct avdtp *session,
3082                                         struct avdtp_stream *stream,
3083                                         avdtp_stream_state_cb cb, void *data)
3084 {
3085         struct stream_callback *stream_cb;
3086         static unsigned int id = 0;
3087
3088         stream_cb = g_new(struct stream_callback, 1);
3089         stream_cb->cb = cb;
3090         stream_cb->user_data = data;
3091         stream_cb->id = ++id;
3092
3093         stream->callbacks = g_slist_append(stream->callbacks, stream_cb);
3094
3095         return stream_cb->id;
3096 }
3097
3098 int avdtp_get_configuration(struct avdtp *session, struct avdtp_stream *stream)
3099 {
3100         struct seid_req req;
3101
3102         memset(&req, 0, sizeof(req));
3103         req.acp_seid = stream->rseid;
3104
3105         return send_request(session, FALSE, stream, AVDTP_GET_CONFIGURATION,
3106                                                         &req, sizeof(req));
3107 }
3108
3109 static void copy_capabilities(gpointer data, gpointer user_data)
3110 {
3111         struct avdtp_service_capability *src_cap = data;
3112         struct avdtp_service_capability *dst_cap;
3113         GSList **l = user_data;
3114
3115         dst_cap = avdtp_service_cap_new(src_cap->category, src_cap->data,
3116                                         src_cap->length);
3117
3118         *l = g_slist_append(*l, dst_cap);
3119 }
3120
3121 int avdtp_set_configuration(struct avdtp *session,
3122                                 struct avdtp_remote_sep *rsep,
3123                                 struct avdtp_local_sep *lsep,
3124                                 GSList *caps,
3125                                 struct avdtp_stream **stream)
3126 {
3127         struct setconf_req *req;
3128         struct avdtp_stream *new_stream;
3129         unsigned char *ptr;
3130         int err, caps_len;
3131         struct avdtp_service_capability *cap;
3132         GSList *l;
3133
3134         if (!(lsep && rsep))
3135                 return -EINVAL;
3136
3137         DBG("%p: int_seid=%u, acp_seid=%u", session,
3138                         lsep->info.seid, rsep->seid);
3139
3140         new_stream = g_new0(struct avdtp_stream, 1);
3141         new_stream->session = session;
3142         new_stream->lsep = lsep;
3143         new_stream->rseid = rsep->seid;
3144
3145         if (rsep->delay_reporting && lsep->delay_reporting) {
3146                 struct avdtp_service_capability *delay_reporting;
3147
3148                 delay_reporting = avdtp_service_cap_new(AVDTP_DELAY_REPORTING,
3149                                                                 NULL, 0);
3150                 caps = g_slist_append(caps, delay_reporting);
3151                 new_stream->delay_reporting = TRUE;
3152         }
3153
3154         g_slist_foreach(caps, copy_capabilities, &new_stream->caps);
3155
3156         /* Calculate total size of request */
3157         for (l = caps, caps_len = 0; l != NULL; l = g_slist_next(l)) {
3158                 cap = l->data;
3159                 caps_len += cap->length + 2;
3160         }
3161
3162         req = g_malloc0(sizeof(struct setconf_req) + caps_len);
3163
3164         req->int_seid = lsep->info.seid;
3165         req->acp_seid = rsep->seid;
3166
3167         /* Copy the capabilities into the request */
3168         for (l = caps, ptr = req->caps; l != NULL; l = g_slist_next(l)) {
3169                 cap = l->data;
3170                 memcpy(ptr, cap, cap->length + 2);
3171                 ptr += cap->length + 2;
3172         }
3173
3174         err = send_request(session, FALSE, new_stream,
3175                                 AVDTP_SET_CONFIGURATION, req,
3176                                 sizeof(struct setconf_req) + caps_len);
3177         if (err < 0)
3178                 stream_free(new_stream);
3179         else {
3180                 lsep->info.inuse = 1;
3181                 lsep->stream = new_stream;
3182                 rsep->stream = new_stream;
3183                 session->streams = g_slist_append(session->streams, new_stream);
3184                 if (stream)
3185                         *stream = new_stream;
3186         }
3187
3188         g_free(req);
3189
3190         return err;
3191 }
3192
3193 int avdtp_open(struct avdtp *session, struct avdtp_stream *stream)
3194 {
3195         struct seid_req req;
3196
3197         if (!g_slist_find(session->streams, stream))
3198                 return -EINVAL;
3199
3200         if (stream->lsep->state > AVDTP_STATE_CONFIGURED)
3201                 return -EINVAL;
3202
3203         memset(&req, 0, sizeof(req));
3204         req.acp_seid = stream->rseid;
3205
3206         return send_request(session, FALSE, stream, AVDTP_OPEN,
3207                                                         &req, sizeof(req));
3208 }
3209
3210 static gboolean start_timeout(gpointer user_data)
3211 {
3212         struct avdtp_stream *stream = user_data;
3213         struct avdtp *session = stream->session;
3214
3215         stream->open_acp = FALSE;
3216
3217         if (avdtp_start(session, stream) < 0)
3218                 error("wait_timeout: avdtp_start failed");
3219
3220         stream->start_timer = 0;
3221
3222         return FALSE;
3223 }
3224
3225 int avdtp_start(struct avdtp *session, struct avdtp_stream *stream)
3226 {
3227         struct start_req req;
3228         int ret;
3229
3230         if (!g_slist_find(session->streams, stream))
3231                 return -EINVAL;
3232
3233         if (stream->lsep->state != AVDTP_STATE_OPEN)
3234                 return -EINVAL;
3235
3236         /* Recommendation 12:
3237          *  If the RD has configured and opened a stream it is also responsible
3238          *  to start the streaming via GAVDP_START.
3239          */
3240         if (stream->open_acp) {
3241                 /* If timer already active wait it */
3242                 if (stream->start_timer)
3243                         return 0;
3244
3245                 stream->start_timer = g_timeout_add_seconds(START_TIMEOUT,
3246                                                                 start_timeout,
3247                                                                 stream);
3248                 return 0;
3249         }
3250
3251         if (stream->close_int == TRUE) {
3252                 error("avdtp_start: rejecting start since close is initiated");
3253                 return -EINVAL;
3254         }
3255
3256         if (stream->starting == TRUE) {
3257                 DBG("stream already started");
3258                 return -EINPROGRESS;
3259         }
3260
3261         memset(&req, 0, sizeof(req));
3262         req.first_seid.seid = stream->rseid;
3263
3264         ret = send_request(session, FALSE, stream, AVDTP_START,
3265                                                         &req, sizeof(req));
3266         if (ret == 0)
3267                 stream->starting = TRUE;
3268
3269         return ret;
3270 }
3271
3272 int avdtp_close(struct avdtp *session, struct avdtp_stream *stream,
3273                 gboolean immediate)
3274 {
3275         struct seid_req req;
3276         int ret;
3277
3278         if (!g_slist_find(session->streams, stream))
3279                 return -EINVAL;
3280
3281         if (stream->close_int == TRUE) {
3282                 error("avdtp_close: rejecting since close is already initiated");
3283                 return -EINVAL;
3284         }
3285
3286         /* If stream is not yet in the OPEN state, let's use ABORT_CMD */
3287         if (stream->lsep->state < AVDTP_STATE_OPEN)
3288                 return avdtp_abort(session, stream);
3289
3290         if (immediate && session->req && stream == session->req->stream)
3291                 return avdtp_abort(session, stream);
3292
3293         memset(&req, 0, sizeof(req));
3294         req.acp_seid = stream->rseid;
3295
3296         ret = send_request(session, FALSE, stream, AVDTP_CLOSE,
3297                                                         &req, sizeof(req));
3298         if (ret == 0)
3299                 stream->close_int = TRUE;
3300
3301         return ret;
3302 }
3303
3304 int avdtp_suspend(struct avdtp *session, struct avdtp_stream *stream)
3305 {
3306         struct seid_req req;
3307
3308         if (!g_slist_find(session->streams, stream))
3309                 return -EINVAL;
3310
3311         if (stream->lsep->state <= AVDTP_STATE_OPEN || stream->close_int)
3312                 return -EINVAL;
3313
3314         memset(&req, 0, sizeof(req));
3315         req.acp_seid = stream->rseid;
3316
3317         return send_request(session, FALSE, stream, AVDTP_SUSPEND,
3318                                                         &req, sizeof(req));
3319 }
3320
3321 int avdtp_abort(struct avdtp *session, struct avdtp_stream *stream)
3322 {
3323         struct seid_req req;
3324         int ret;
3325
3326         if (!g_slist_find(session->streams, stream))
3327                 return -EINVAL;
3328
3329         if (stream->lsep->state == AVDTP_STATE_ABORTING)
3330                 return -EINVAL;
3331
3332         if (session->req && session->req->timeout > 0 &&
3333                                                 stream == session->req->stream)
3334                 return cancel_request(session, ECANCELED);
3335
3336         memset(&req, 0, sizeof(req));
3337         req.acp_seid = stream->rseid;
3338
3339         ret = send_request(session, TRUE, stream, AVDTP_ABORT,
3340                                                         &req, sizeof(req));
3341         if (ret == 0)
3342                 stream->abort_int = TRUE;
3343
3344         return ret;
3345 }
3346
3347 int avdtp_delay_report(struct avdtp *session, struct avdtp_stream *stream,
3348                                                         uint16_t delay)
3349 {
3350         struct delay_req req;
3351
3352         if (!g_slist_find(session->streams, stream))
3353                 return -EINVAL;
3354
3355         if (stream->lsep->state != AVDTP_STATE_CONFIGURED &&
3356                                 stream->lsep->state != AVDTP_STATE_STREAMING)
3357                 return -EINVAL;
3358
3359         if (!stream->delay_reporting || session->version < 0x0103)
3360                 return -EINVAL;
3361
3362         stream->delay = delay;
3363
3364         memset(&req, 0, sizeof(req));
3365         req.acp_seid = stream->rseid;
3366         req.delay = htons(delay);
3367
3368         return send_request(session, TRUE, stream, AVDTP_DELAY_REPORT,
3369                                                         &req, sizeof(req));
3370 }
3371
3372 struct avdtp_local_sep *avdtp_register_sep(struct queue *lseps, uint8_t type,
3373                                                 uint8_t media_type,
3374                                                 uint8_t codec_type,
3375                                                 gboolean delay_reporting,
3376                                                 struct avdtp_sep_ind *ind,
3377                                                 struct avdtp_sep_cfm *cfm,
3378                                                 void *user_data)
3379 {
3380         struct avdtp_local_sep *sep;
3381         uint8_t seid = util_get_uid(&seids, MAX_SEID);
3382
3383         if (!seid)
3384                 return NULL;
3385
3386         sep = g_new0(struct avdtp_local_sep, 1);
3387
3388         sep->state = AVDTP_STATE_IDLE;
3389         sep->info.seid = seid;
3390         sep->info.type = type;
3391         sep->info.media_type = media_type;
3392         sep->codec = codec_type;
3393         sep->ind = ind;
3394         sep->cfm = cfm;
3395         sep->user_data = user_data;
3396         sep->delay_reporting = delay_reporting;
3397
3398         DBG("SEP %p registered: type:%d codec:%d seid:%d", sep,
3399                         sep->info.type, sep->codec, sep->info.seid);
3400
3401         if (!queue_push_tail(lseps, sep)) {
3402                 g_free(sep);
3403                 return NULL;
3404         }
3405
3406         return sep;
3407 }
3408
3409 void avdtp_sep_set_vendor_codec(struct avdtp_local_sep *sep, uint32_t vendor_id,
3410                                                         uint16_t codec_id)
3411 {
3412         sep->vndcodec_vendor = vendor_id;
3413         sep->vndcodec_codec = codec_id;
3414 }
3415
3416 int avdtp_unregister_sep(struct queue *lseps, struct avdtp_local_sep *sep)
3417 {
3418         if (!sep)
3419                 return -EINVAL;
3420
3421         if (sep->stream)
3422                 release_stream(sep->stream, sep->stream->session);
3423
3424         DBG("SEP %p unregistered: type:%d codec:%d seid:%d", sep,
3425                         sep->info.type, sep->codec, sep->info.seid);
3426
3427         util_clear_uid(&seids, sep->info.seid);
3428         queue_remove(lseps, sep);
3429         g_free(sep);
3430
3431         return 0;
3432 }
3433
3434 const char *avdtp_strerror(struct avdtp_error *err)
3435 {
3436         if (err->category == AVDTP_ERRNO)
3437                 return strerror(err->err.posix_errno);
3438
3439         switch (err->err.error_code) {
3440         case AVDTP_BAD_HEADER_FORMAT:
3441                 return "Bad Header Format";
3442         case AVDTP_BAD_LENGTH:
3443                 return "Bad Packet Length";
3444         case AVDTP_BAD_ACP_SEID:
3445                 return "Bad Acceptor SEID";
3446         case AVDTP_SEP_IN_USE:
3447                 return "Stream End Point in Use";
3448         case AVDTP_SEP_NOT_IN_USE:
3449                 return "Stream End Point Not in Use";
3450         case AVDTP_BAD_SERV_CATEGORY:
3451                 return "Bad Service Category";
3452         case AVDTP_BAD_PAYLOAD_FORMAT:
3453                 return "Bad Payload format";
3454         case AVDTP_NOT_SUPPORTED_COMMAND:
3455                 return "Command Not Supported";
3456         case AVDTP_INVALID_CAPABILITIES:
3457                 return "Invalid Capabilities";
3458         case AVDTP_BAD_RECOVERY_TYPE:
3459                 return "Bad Recovery Type";
3460         case AVDTP_BAD_MEDIA_TRANSPORT_FORMAT:
3461                 return "Bad Media Transport Format";
3462         case AVDTP_BAD_RECOVERY_FORMAT:
3463                 return "Bad Recovery Format";
3464         case AVDTP_BAD_ROHC_FORMAT:
3465                 return "Bad Header Compression Format";
3466         case AVDTP_BAD_CP_FORMAT:
3467                 return "Bad Content Protection Format";
3468         case AVDTP_BAD_MULTIPLEXING_FORMAT:
3469                 return "Bad Multiplexing Format";
3470         case AVDTP_UNSUPPORTED_CONFIGURATION:
3471                 return "Configuration not supported";
3472         case AVDTP_BAD_STATE:
3473                 return "Bad State";
3474         default:
3475                 return "Unknown error";
3476         }
3477 }
3478
3479 avdtp_state_t avdtp_sep_get_state(struct avdtp_local_sep *sep)
3480 {
3481         return sep->state;
3482 }
3483
3484 gboolean avdtp_has_stream(struct avdtp *session, struct avdtp_stream *stream)
3485 {
3486         return g_slist_find(session->streams, stream) ? TRUE : FALSE;
3487 }