audio/avdtp.c

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