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