utils/audio/liba2dp.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2006-2007  Nokia Corporation
   6  *  Copyright (C) 2004-2008  Marcel Holtmann <marcel@holtmann.org>
   7  *
   8  *
   9  *  This library is free software; you can redistribute it and/or
  10  *  modify it under the terms of the GNU Lesser General Public
  11  *  License as published by the Free Software Foundation; either
  12  *  version 2.1 of the License, or (at your option) any later version.
  13  *
  14  *  This library 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 GNU
  17  *  Lesser General Public License for more details.
  18  *
  19  *  You should have received a copy of the GNU Lesser General Public
  20  *  License along with this library; 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 <stdint.h>
  30 #include <sys/socket.h>
  31 #include <sys/un.h>
  32 #include <signal.h>
  33 #include <limits.h>
  34 #include <fcntl.h>
  35 #include <unistd.h>
  36
  37 #include <netinet/in.h>
  38 #include <sys/poll.h>
  39
  40 #include "ipc.h"
  41 #include "sbc.h"
  42 #include "rtp.h"
  43 #include "liba2dp.h"
  44
  45 #define LOG_TAG "A2DP"
  46 #include <utils/Log.h>
  47
  48 /* #define ENABLE_DEBUG */
  49
  50 #define BUFFER_SIZE 2048
  51
  52 #ifdef ENABLE_DEBUG
  53 #define DBG LOGD
  54 #else
  55 #define DBG(fmt, arg...)
  56 #endif
  57
  58
  59 #ifndef MIN
  60 # define MIN(x, y) ((x) < (y) ? (x) : (y))
  61 #endif
  62
  63 #ifndef MAX
  64 # define MAX(x, y) ((x) > (y) ? (x) : (y))
  65 #endif
  66
  67 #define MAX_BITPOOL 64
  68 #define MIN_BITPOOL 2
  69
  70 #define SNDERR LOGE
  71
  72 /* Number of milliseconds worth of audio to buffer in our the data->stream.fd socket */
  73 #define SOCK_BUFFER_MS          100
  74
  75 struct bluetooth_data {
  76         int link_mtu;                                   /* MTU for selected transport channel */
  77         struct pollfd stream;                   /* Audio stream filedescriptor */
  78         struct pollfd server;                   /* Audio daemon filedescriptor */
  79
  80         sbc_capabilities_t sbc_capabilities;
  81         sbc_t sbc;                              /* Codec data */
  82         int sbc_initialized;                    /* Keep track if the encoder is initialized */
  83         int     frame_duration;                 /* length of an SBC frame in microseconds */
  84         int codesize;                           /* SBC codesize */
  85         int samples;                            /* Number of encoded samples */
  86         uint8_t buffer[BUFFER_SIZE];            /* Codec transfer buffer */
  87         int count;                              /* Codec transfer buffer counter */
  88
  89         int nsamples;                           /* Cumulative number of codec samples */
  90         uint16_t seq_num;                       /* Cumulative packet sequence */
  91         int frame_count;                        /* Current frames in buffer*/
  92
  93         int             started;
  94         char    address[20];
  95         int     rate;
  96         int     channels;
  97
  98         /* used for pacing our writes to the output socket */
  99         struct timeval  last_write;
 100         unsigned long   last_duration;
 101
 102         /* true if we already set the buffer size on the data->stream.fd socket */
 103         int adjusted_sock_buffer;
 104 };
 105
 106
 107 static int audioservice_send(int sk, const bt_audio_msg_header_t *msg);
 108 static int audioservice_expect(int sk, bt_audio_msg_header_t *outmsg,
 109                                 int expected_type);
 110 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data);
 111
 112
 113 static void bluetooth_exit(struct bluetooth_data *data)
 114 {
 115         if (data->server.fd >= 0)
 116                 bt_audio_service_close(data->server.fd);
 117
 118         if (data->stream.fd >= 0)
 119                 close(data->stream.fd);
 120
 121         if (data->sbc_initialized)
 122                 sbc_finish(&data->sbc);
 123 }
 124
 125 static int bluetooth_start(struct bluetooth_data *data)
 126 {
 127         char c = 'w';
 128         char buf[BT_AUDIO_IPC_PACKET_SIZE];
 129         struct bt_streamstart_req *start_req = (void*) buf;
 130         bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf;
 131         struct bt_streamfd_ind *streamfd_ind = (void*) buf;
 132         int opt_name, err;
 133         int retry = 0;
 134
 135 retry:
 136         /* send start */
 137         memset(start_req, 0, BT_AUDIO_IPC_PACKET_SIZE);
 138         start_req->h.msg_type = BT_STREAMSTART_REQ;
 139
 140         err = audioservice_send(data->server.fd, &start_req->h);
 141         if (err < 0)
 142                 return err;
 143
 144         err = audioservice_expect(data->server.fd, &rsp_hdr->msg_h,
 145                                         BT_STREAMSTART_RSP);
 146         if (err < 0)
 147                 return err;
 148
 149         if (rsp_hdr->posix_errno != 0) {
 150                 SNDERR("BT_START failed : %s(%d)",
 151                                         strerror(rsp_hdr->posix_errno),
 152                                         rsp_hdr->posix_errno);
 153
 154                 /* if the connection dropped, we may need to reset the configuration */
 155                 if (!retry) {
 156                         retry = 1;
 157                         if (bluetooth_a2dp_hw_params(data) == 0)
 158                                 goto retry;
 159                 }
 160
 161                 return -rsp_hdr->posix_errno;
 162         }
 163
 164         err = audioservice_expect(data->server.fd, &streamfd_ind->h,
 165                                         BT_STREAMFD_IND);
 166         if (err < 0)
 167                 return err;
 168
 169         if (data->stream.fd >= 0) {
 170                 close(data->stream.fd);
 171                 data->stream.fd = -1;
 172                 data->adjusted_sock_buffer = 0;
 173         }
 174
 175         data->stream.fd = bt_audio_service_get_data_fd(data->server.fd);
 176         if (data->stream.fd < 0) {
 177                 return -errno;
 178         }
 179         data->stream.events = POLLOUT;
 180
 181         return 0;
 182 }
 183
 184 static int bluetooth_stop(struct bluetooth_data *data)
 185 {
 186         char buf[BT_AUDIO_IPC_PACKET_SIZE];
 187         struct bt_streamstop_req *stop_req = (void*) buf;
 188         bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf;
 189         int err;
 190
 191         data->started = 0;
 192
 193         if (data->stream.fd >= 0) {
 194                 close(data->stream.fd);
 195                 data->stream.fd = 0;
 196         }
 197
 198         /* send stop request */
 199         memset(stop_req, 0, BT_AUDIO_IPC_PACKET_SIZE);
 200         stop_req->h.msg_type = BT_STREAMSTOP_REQ;
 201
 202         err = audioservice_send(data->server.fd, &stop_req->h);
 203         if (err < 0)
 204                 return err;
 205
 206         err = audioservice_expect(data->server.fd, &rsp_hdr->msg_h,
 207                                         BT_STREAMSTOP_RSP);
 208         if (err < 0)
 209                 return err;
 210
 211         if (rsp_hdr->posix_errno != 0) {
 212                 SNDERR("BT_STREAMSTOP failed : %s(%d)",
 213                                         strerror(rsp_hdr->posix_errno),
 214                                         rsp_hdr->posix_errno);
 215                 return -rsp_hdr->posix_errno;
 216         }
 217
 218         return 0;
 219 }
 220
 221 static uint8_t default_bitpool(uint8_t freq, uint8_t mode)
 222 {
 223         switch (freq) {
 224         case BT_SBC_SAMPLING_FREQ_16000:
 225         case BT_SBC_SAMPLING_FREQ_32000:
 226                 return 53;
 227         case BT_SBC_SAMPLING_FREQ_44100:
 228                 switch (mode) {
 229                 case BT_A2DP_CHANNEL_MODE_MONO:
 230                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 231                         return 31;
 232                 case BT_A2DP_CHANNEL_MODE_STEREO:
 233                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 234                         return 53;
 235                 default:
 236                         DBG("Invalid channel mode %u", mode);
 237                         return 53;
 238                 }
 239         case BT_SBC_SAMPLING_FREQ_48000:
 240                 switch (mode) {
 241                 case BT_A2DP_CHANNEL_MODE_MONO:
 242                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 243                         return 29;
 244                 case BT_A2DP_CHANNEL_MODE_STEREO:
 245                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 246                         return 51;
 247                 default:
 248                         DBG("Invalid channel mode %u", mode);
 249                         return 51;
 250                 }
 251         default:
 252                 DBG("Invalid sampling freq %u", freq);
 253                 return 53;
 254         }
 255 }
 256
 257 static int bluetooth_a2dp_init(struct bluetooth_data *data)
 258 {
 259         sbc_capabilities_t *cap = &data->sbc_capabilities;
 260         unsigned int max_bitpool, min_bitpool;
 261         int dir;
 262
 263         switch (data->rate) {
 264         case 48000:
 265                 cap->frequency = BT_SBC_SAMPLING_FREQ_48000;
 266                 break;
 267         case 44100:
 268                 cap->frequency = BT_SBC_SAMPLING_FREQ_44100;
 269                 break;
 270         case 32000:
 271                 cap->frequency = BT_SBC_SAMPLING_FREQ_32000;
 272                 break;
 273         case 16000:
 274                 cap->frequency = BT_SBC_SAMPLING_FREQ_16000;
 275                 break;
 276         default:
 277                 DBG("Rate %d not supported", data->rate);
 278                 return -1;
 279         }
 280
 281         if (data->channels == 2) {
 282                 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 283                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
 284                 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 285                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
 286                 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 287                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
 288         } else {
 289                 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
 290                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 291         }
 292
 293         if (!cap->channel_mode) {
 294                 DBG("No supported channel modes");
 295                 return -1;
 296         }
 297
 298         if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
 299                 cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
 300         else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
 301                 cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
 302         else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
 303                 cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
 304         else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
 305                 cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
 306         else {
 307                 DBG("No supported block lengths");
 308                 return -1;
 309         }
 310
 311         if (cap->subbands & BT_A2DP_SUBBANDS_8)
 312                 cap->subbands = BT_A2DP_SUBBANDS_8;
 313         else if (cap->subbands & BT_A2DP_SUBBANDS_4)
 314                 cap->subbands = BT_A2DP_SUBBANDS_4;
 315         else {
 316                 DBG("No supported subbands");
 317                 return -1;
 318         }
 319
 320         if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
 321                 cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
 322         else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
 323                 cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
 324
 325                 min_bitpool = MAX(MIN_BITPOOL, cap->min_bitpool);
 326                 max_bitpool = MIN(default_bitpool(cap->frequency,
 327                                         cap->channel_mode),
 328                                         cap->max_bitpool);
 329
 330         cap->min_bitpool = min_bitpool;
 331         cap->max_bitpool = max_bitpool;
 332
 333         DBG("bluetooth_a2dp_init bottom:\n  channel_mode: %d\n  frequency: %d\n  allocation_method: %d\n  subbands: %d\n  block_length: %d\n  min_bitpool: %d\n  max_bitpool: %d\n  ",
 334                 cap->channel_mode, cap->frequency, cap->allocation_method, cap->subbands,
 335                 cap->block_length, cap->min_bitpool, cap->max_bitpool);
 336
 337
 338         return 0;
 339 }
 340
 341 static void bluetooth_a2dp_setup(struct bluetooth_data *data)
 342 {
 343         sbc_capabilities_t active_capabilities = data->sbc_capabilities;
 344
 345         if (data->sbc_initialized)
 346                 sbc_reinit(&data->sbc, 0);
 347         else
 348                 sbc_init(&data->sbc, 0);
 349         data->sbc_initialized = 1;
 350
 351         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
 352                 data->sbc.frequency = SBC_FREQ_16000;
 353
 354         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
 355                 data->sbc.frequency = SBC_FREQ_32000;
 356
 357         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
 358                 data->sbc.frequency = SBC_FREQ_44100;
 359
 360         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
 361                 data->sbc.frequency = SBC_FREQ_48000;
 362
 363         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
 364                 data->sbc.mode = SBC_MODE_MONO;
 365
 366         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 367                 data->sbc.mode = SBC_MODE_DUAL_CHANNEL;
 368
 369         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 370                 data->sbc.mode = SBC_MODE_STEREO;
 371
 372         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 373                 data->sbc.mode = SBC_MODE_JOINT_STEREO;
 374
 375         data->sbc.allocation = active_capabilities.allocation_method
 376                                 == BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR
 377                                 : SBC_AM_LOUDNESS;
 378
 379         switch (active_capabilities.subbands) {
 380         case BT_A2DP_SUBBANDS_4:
 381                 data->sbc.subbands = SBC_SB_4;
 382                 break;
 383         case BT_A2DP_SUBBANDS_8:
 384                 data->sbc.subbands = SBC_SB_8;
 385                 break;
 386         }
 387
 388         switch (active_capabilities.block_length) {
 389         case BT_A2DP_BLOCK_LENGTH_4:
 390                 data->sbc.blocks = SBC_BLK_4;
 391                 break;
 392         case BT_A2DP_BLOCK_LENGTH_8:
 393                 data->sbc.blocks = SBC_BLK_8;
 394                 break;
 395         case BT_A2DP_BLOCK_LENGTH_12:
 396                 data->sbc.blocks = SBC_BLK_12;
 397                 break;
 398         case BT_A2DP_BLOCK_LENGTH_16:
 399                 data->sbc.blocks = SBC_BLK_16;
 400                 break;
 401         }
 402
 403         data->sbc.bitpool = active_capabilities.max_bitpool;
 404         data->codesize = sbc_get_codesize(&data->sbc);
 405         data->frame_duration = sbc_get_frame_duration(&data->sbc);
 406         data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
 407 }
 408
 409 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data)
 410 {
 411         char buf[BT_AUDIO_IPC_PACKET_SIZE];
 412         bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf;
 413         struct bt_setconfiguration_req *setconf_req = (void*) buf;
 414         struct bt_setconfiguration_rsp *setconf_rsp = (void*) buf;
 415         int err;
 416
 417         err = bluetooth_a2dp_init(data);
 418         if (err < 0)
 419                 return err;
 420
 421         memset(setconf_req, 0, BT_AUDIO_IPC_PACKET_SIZE);
 422         setconf_req->h.msg_type = BT_SETCONFIGURATION_REQ;
 423         strncpy(setconf_req->device, data->address, 18);
 424         setconf_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 425         setconf_req->sbc_capabilities = data->sbc_capabilities;
 426         setconf_req->access_mode = BT_CAPABILITIES_ACCESS_MODE_WRITE;
 427
 428         err = audioservice_send(data->server.fd, &setconf_req->h);
 429         if (err < 0)
 430                 return err;
 431
 432         err = audioservice_expect(data->server.fd, &rsp_hdr->msg_h,
 433                                         BT_SETCONFIGURATION_RSP);
 434         if (err < 0)
 435                 return err;
 436
 437         if (rsp_hdr->posix_errno != 0) {
 438                 SNDERR("BT_SETCONFIGURATION failed : %s(%d)",
 439                                         strerror(rsp_hdr->posix_errno),
 440                                         rsp_hdr->posix_errno);
 441                 return -rsp_hdr->posix_errno;
 442         }
 443
 444         data->link_mtu = setconf_rsp->link_mtu;
 445
 446         /* Setup SBC encoder now we agree on parameters */
 447         bluetooth_a2dp_setup(data);
 448
 449         DBG("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
 450                 data->sbc.allocation, data->sbc.subbands, data->sbc.blocks,
 451                 data->sbc.bitpool);
 452
 453         return 0;
 454 }
 455
 456 static int avdtp_write(struct bluetooth_data *data, unsigned long duration)
 457 {
 458         int ret = 0;
 459         struct rtp_header *header;
 460         struct rtp_payload *payload;
 461         unsigned long delta;
 462         struct timeval now;
 463         int microseconds, bytes;
 464
 465         header = (struct rtp_header *)data->buffer;
 466         payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
 467
 468         memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
 469
 470         payload->frame_count = data->frame_count;
 471         header->v = 2;
 472         header->pt = 1;
 473         header->sequence_number = htons(data->seq_num);
 474         header->timestamp = htonl(data->nsamples);
 475         header->ssrc = htonl(1);
 476
 477         data->stream.revents = 0;
 478         ret = poll(&data->stream, 1, -1);
 479         if (ret == 1 && data->stream.revents == POLLOUT) {
 480                 gettimeofday(&now, NULL);
 481                 if (data->last_write.tv_sec || data->last_write.tv_usec) {
 482                         if (now.tv_usec > data->last_write.tv_usec)
 483                                 delta = now.tv_usec - data->last_write.tv_usec;
 484                         else
 485                                 delta = (1000000 - data->last_write.tv_usec) + now.tv_usec;
 486
 487                         if (duration > delta) {
 488                                 DBG("duration: %ld delta: %ld, delay %ld us", duration, delta, duration - delta);
 489                                 usleep(duration - delta);
 490                         }
 491                 }
 492                 data->last_write = now;
 493
 494                 ret = send(data->stream.fd, data->buffer, data->count, 0);
 495                 if (ret < 0) {
 496                         DBG("send returned %d errno %s.", ret, strerror(errno));
 497                         ret = -errno;
 498                 }
 499         } else {
 500                 ret = -errno;
 501         }
 502
 503         if (!data->adjusted_sock_buffer) {
 504                 /* microseconds: number of microseconds of audio for this write */
 505                 microseconds = data->frame_duration * data->frame_count;
 506                 /* ret: number of bytes written */
 507                 /* bytes: number of bytes corresponding to SOCK_BUFFER_MS milliseconds of audio playback */
 508                 bytes = (ret * 1000 * SOCK_BUFFER_MS) / microseconds;
 509
 510                 DBG("microseconds: %d, ret: %d, bytes: %d\n", microseconds, ret, bytes);
 511                 setsockopt(data->stream.fd, SOL_SOCKET, SO_SNDBUF, &bytes, sizeof(bytes));
 512                 data->adjusted_sock_buffer = 1;
 513         }
 514
 515         /* Reset buffer of data to send */
 516         data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
 517         data->frame_count = 0;
 518         data->samples = 0;
 519         data->seq_num++;
 520
 521         return ret;
 522 }
 523
 524 static int audioservice_send(int sk, const bt_audio_msg_header_t *msg)
 525 {
 526         int err;
 527
 528         DBG("sending %s", bt_audio_strmsg(msg->msg_type));
 529         if (send(sk, msg, BT_AUDIO_IPC_PACKET_SIZE, 0) > 0)
 530                 err = 0;
 531         else {
 532                 err = -errno;
 533                 SNDERR("Error sending data to audio service: %s(%d)",
 534                         strerror(errno), errno);
 535         }
 536
 537         return err;
 538 }
 539
 540 static int audioservice_recv(int sk, bt_audio_msg_header_t *inmsg)
 541 {
 542         int err;
 543         const char *type;
 544
 545         DBG("trying to receive msg from audio service...");
 546         if (recv(sk, inmsg, BT_AUDIO_IPC_PACKET_SIZE, 0) > 0) {
 547                 type = bt_audio_strmsg(inmsg->msg_type);
 548                 if (type) {
 549                         DBG("Received %s", type);
 550                         err = 0;
 551                 } else {
 552                         err = -EINVAL;
 553                         SNDERR("Bogus message type %d "
 554                                         "received from audio service",
 555                                         inmsg->msg_type);
 556                 }
 557         } else {
 558                 err = -errno;
 559                 SNDERR("Error receiving data from audio service: %s(%d)",
 560                                         strerror(errno), errno);
 561         }
 562
 563         return err;
 564 }
 565
 566 static int audioservice_expect(int sk, bt_audio_msg_header_t *rsp_hdr,
 567                                 int expected_type)
 568 {
 569         int err = audioservice_recv(sk, rsp_hdr);
 570         if (err == 0) {
 571                 if (rsp_hdr->msg_type != expected_type) {
 572                         err = -EINVAL;
 573                         SNDERR("Bogus message %s received while "
 574                                         "%s was expected",
 575                                         bt_audio_strmsg(rsp_hdr->msg_type),
 576                                         bt_audio_strmsg(expected_type));
 577                 }
 578         }
 579         return err;
 580 }
 581
 582 static int bluetooth_init(struct bluetooth_data *data)
 583 {
 584         int sk, err;
 585         char buf[BT_AUDIO_IPC_PACKET_SIZE];
 586         bt_audio_rsp_msg_header_t *rsp_hdr = (void*) buf;
 587         struct bt_getcapabilities_req *getcaps_req = (void*) buf;
 588         struct bt_getcapabilities_rsp *getcaps_rsp = (void*) buf;
 589
 590         memset(data, 0, sizeof(struct bluetooth_data));
 591
 592         data->server.fd = -1;
 593         data->stream.fd = -1;
 594         data->adjusted_sock_buffer = 0;
 595
 596         sk = bt_audio_service_open();
 597         if (sk <= 0) {
 598                 SNDERR("bt_audio_service_open failed\n");
 599                 err = -errno;
 600                 goto failed;
 601         }
 602
 603         data->server.fd = sk;
 604         data->server.events = POLLIN;
 605
 606         memset(getcaps_req, 0, BT_AUDIO_IPC_PACKET_SIZE);
 607         getcaps_req->h.msg_type = BT_GETCAPABILITIES_REQ;
 608         getcaps_req->flags = 0;
 609         getcaps_req->flags |= BT_FLAG_AUTOCONNECT;
 610         strncpy(getcaps_req->device, data->address, 18);
 611         getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 612
 613         err = audioservice_send(data->server.fd, &getcaps_req->h);
 614         if (err < 0) {
 615                 SNDERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n");
 616                 goto failed;
 617         }
 618
 619         err = audioservice_expect(data->server.fd, &rsp_hdr->msg_h, BT_GETCAPABILITIES_RSP);
 620         if (err < 0) {
 621                 SNDERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n");
 622                 goto failed;
 623         }
 624         if (rsp_hdr->posix_errno != 0) {
 625                 SNDERR("BT_GETCAPABILITIES failed : %s(%d)",
 626                                         strerror(rsp_hdr->posix_errno),
 627                                         rsp_hdr->posix_errno);
 628                 err = -rsp_hdr->posix_errno;
 629                 goto failed;
 630         }
 631
 632         if (getcaps_rsp->transport == BT_CAPABILITIES_TRANSPORT_A2DP)
 633                 data->sbc_capabilities = getcaps_rsp->sbc_capabilities;
 634
 635         return 0;
 636
 637 failed:
 638         SNDERR("bluetooth_init failed, err: %d\n", err);
 639         bt_audio_service_close(sk);
 640         data->server.fd = -1;
 641         return err;
 642 }
 643
 644 int a2dp_init(const char* address, int rate, int channels, a2dpData* dataPtr)
 645 {
 646         int err;
 647
 648         DBG("a2dp_init");
 649         *dataPtr = NULL;
 650         struct bluetooth_data* data = malloc(sizeof(struct bluetooth_data));
 651         if (!data)
 652                 return -1;
 653
 654         strncpy(data->address, address, 18);
 655
 656         err = bluetooth_init(data);
 657         if (err < 0)
 658                 goto error;
 659
 660         data->rate = rate;
 661         data->channels = channels;
 662
 663         err = bluetooth_a2dp_hw_params(data);
 664         if (err < 0) {
 665                 SNDERR("bluetooth_a2dp_hw_params failed");
 666                 goto error;
 667         }
 668
 669         *dataPtr = data;
 670         return 0;
 671
 672 error:
 673         bluetooth_exit(data);
 674         free(data);
 675
 676         return err;
 677 }
 678
 679 int a2dp_write(a2dpData d, const void* buffer, int count)
 680 {
 681         struct bluetooth_data* data = (struct bluetooth_data*)d;
 682         const uint8_t* src = buffer;
 683         int codesize = data->codesize;
 684         long ret = 0;
 685         long frames_left = count;
 686         int encoded, written;
 687         const char *buff;
 688         unsigned long duration = 0;
 689
 690         if (!data->started) {
 691                 ret = bluetooth_start(data);
 692                 if (ret < 0) {
 693                         SNDERR("bluetooth_start failed");
 694                         return ret;
 695                 }
 696                 data->started = 1;
 697         }
 698
 699         while (frames_left >= codesize) {
 700                 /* Enough data to encode (sbc wants 1k blocks) */
 701                 encoded = sbc_encode(&(data->sbc), src, codesize,
 702                                         data->buffer + data->count,
 703                                         sizeof(data->buffer) - data->count,
 704                                         &written);
 705                 if (encoded <= 0) {
 706                         DBG("Encoding error %d", encoded);
 707                         goto done;
 708                 }
 709                 DBG("sbc_encode returned %d, codesize: %d, written: %d\n", encoded, codesize, written);
 710
 711                 src += encoded;
 712                 data->count += written;
 713                 data->frame_count++;
 714                 data->samples += encoded;
 715                 data->nsamples += encoded;
 716                 duration += data->frame_duration;
 717
 718                 /* No space left for another frame then send */
 719                 if (data->count + written >= data->link_mtu) {
 720                         DBG("sending packet %d, count %d, link_mtu %u",
 721                                         data->seq_num, data->count,
 722                                         data->link_mtu);
 723                         avdtp_write(data, data->last_duration);
 724                         data->last_duration = duration;
 725                         duration = 0;
 726                 }
 727
 728                 ret += encoded;
 729                 frames_left -= encoded;
 730         }
 731
 732         if (frames_left > 0)
 733                 SNDERR("%ld bytes left at end of a2dp_write\n", frames_left);
 734
 735 done:
 736         DBG("returning %ld", ret);
 737         return ret;
 738 }
 739
 740 int a2dp_stop(a2dpData d)
 741 {
 742         struct bluetooth_data* data = (struct bluetooth_data*)d;
 743         DBG("a2dp_stop\n");
 744         if (!data)
 745                 return 0;
 746
 747         return bluetooth_stop(data);
 748 }
 749
 750 void a2dp_cleanup(a2dpData d)
 751 {
 752         struct bluetooth_data* data = (struct bluetooth_data*)d;
 753         bluetooth_exit(data);
 754         free(data);
 755 }