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 #include <pthread.h>
  37
  38 #include <netinet/in.h>
  39 #include <sys/poll.h>
  40 #include <sys/prctl.h>
  41
  42 #include "ipc.h"
  43 #include "sbc.h"
  44 #include "rtp.h"
  45 #include "liba2dp.h"
  46
  47 #define LOG_NDEBUG 0
  48 #define LOG_TAG "A2DP"
  49 #include <utils/Log.h>
  50
  51 #define ENABLE_DEBUG
  52 /* #define ENABLE_VERBOSE */
  53 /* #define ENABLE_TIMING */
  54
  55 #define BUFFER_SIZE 2048
  56
  57 #ifdef ENABLE_DEBUG
  58 #define DBG LOGD
  59 #else
  60 #define DBG(fmt, arg...)
  61 #endif
  62
  63 #ifdef ENABLE_VERBOSE
  64 #define VDBG LOGV
  65 #else
  66 #define VDBG(fmt, arg...)
  67 #endif
  68
  69 #ifndef MIN
  70 # define MIN(x, y) ((x) < (y) ? (x) : (y))
  71 #endif
  72
  73 #ifndef MAX
  74 # define MAX(x, y) ((x) > (y) ? (x) : (y))
  75 #endif
  76
  77 #define MAX_BITPOOL 64
  78 #define MIN_BITPOOL 2
  79
  80 #define ERR LOGE
  81
  82 /* Number of packets to buffer in the stream socket */
  83 #define PACKET_BUFFER_COUNT             10
  84
  85 /* timeout in milliseconds to prevent poll() from hanging indefinitely */
  86 #define POLL_TIMEOUT                    1000
  87
  88 /* timeout in milliseconds for a2dp_write */
  89 #define WRITE_TIMEOUT                   500
  90
  91
  92 typedef enum {
  93         A2DP_STATE_NONE = 0,
  94         A2DP_STATE_INITIALIZED,
  95         A2DP_STATE_CONFIGURING,
  96         A2DP_STATE_CONFIGURED,
  97         A2DP_STATE_STARTING,
  98         A2DP_STATE_STARTED,
  99         A2DP_STATE_STOPPING,
 100 } a2dp_state_t;
 101
 102 typedef enum {
 103         A2DP_CMD_NONE = 0,
 104         A2DP_CMD_INIT,
 105         A2DP_CMD_CONFIGURE,
 106         A2DP_CMD_START,
 107         A2DP_CMD_STOP,
 108         A2DP_CMD_QUIT,
 109 } a2dp_command_t;
 110
 111 struct bluetooth_data {
 112         int link_mtu;                                   /* MTU for transport channel */
 113         struct pollfd stream;                   /* Audio stream filedescriptor */
 114         struct pollfd server;                   /* Audio daemon filedescriptor */
 115         a2dp_state_t state;                             /* Current A2DP state */
 116         a2dp_command_t command;                 /* Current command for a2dp_thread */
 117         pthread_t thread;
 118         pthread_mutex_t mutex;
 119         int started;
 120         pthread_cond_t thread_start;
 121         pthread_cond_t thread_wait;
 122         pthread_cond_t client_wait;
 123
 124         sbc_capabilities_t sbc_capabilities;
 125         sbc_t sbc;                              /* Codec data */
 126         int     frame_duration;                 /* length of an SBC frame in microseconds */
 127         int codesize;                           /* SBC codesize */
 128         int samples;                            /* Number of encoded samples */
 129         uint8_t buffer[BUFFER_SIZE];            /* Codec transfer buffer */
 130         int count;                              /* Codec transfer buffer counter */
 131
 132         int nsamples;                           /* Cumulative number of codec samples */
 133         uint16_t seq_num;                       /* Cumulative packet sequence */
 134         int frame_count;                        /* Current frames in buffer*/
 135
 136         char    address[20];
 137         int     rate;
 138         int     channels;
 139
 140         /* used for pacing our writes to the output socket */
 141         uint64_t        next_write;
 142 };
 143
 144 static uint64_t get_microseconds()
 145 {
 146         struct timeval now;
 147         gettimeofday(&now, NULL);
 148         return (now.tv_sec * 1000000UL + now.tv_usec);
 149 }
 150
 151 #ifdef ENABLE_TIMING
 152 static void print_time(const char* message, uint64_t then, uint64_t now)
 153 {
 154         DBG("%s: %lld us", message, now - then);
 155 }
 156 #endif
 157
 158 static int audioservice_send(struct bluetooth_data *data, const bt_audio_msg_header_t *msg);
 159 static int audioservice_expect(struct bluetooth_data *data, bt_audio_msg_header_t *outmsg,
 160                                 int expected_type);
 161 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data);
 162 static void set_state(struct bluetooth_data *data, a2dp_state_t state);
 163
 164
 165 static void bluetooth_close(struct bluetooth_data *data)
 166 {
 167         DBG("bluetooth_close");
 168         if (data->server.fd >= 0) {
 169                 bt_audio_service_close(data->server.fd);
 170                 data->server.fd = -1;
 171         }
 172
 173         if (data->stream.fd >= 0) {
 174                 close(data->stream.fd);
 175                 data->stream.fd = -1;
 176         }
 177
 178         data->state = A2DP_STATE_NONE;
 179 }
 180
 181 static int bluetooth_start(struct bluetooth_data *data)
 182 {
 183         char c = 'w';
 184         char buf[BT_SUGGESTED_BUFFER_SIZE];
 185         struct bt_start_stream_req *start_req = (void*) buf;
 186         struct bt_start_stream_rsp *start_rsp = (void*) buf;
 187         struct bt_new_stream_ind *streamfd_ind = (void*) buf;
 188         int opt_name, err, bytes;
 189
 190         DBG("bluetooth_start");
 191         data->state = A2DP_STATE_STARTING;
 192         /* send start */
 193         memset(start_req, 0, BT_SUGGESTED_BUFFER_SIZE);
 194         start_req->h.type = BT_REQUEST;
 195         start_req->h.name = BT_START_STREAM;
 196         start_req->h.length = sizeof(*start_req);
 197
 198
 199         err = audioservice_send(data, &start_req->h);
 200         if (err < 0)
 201                 goto error;
 202
 203         start_rsp->h.length = sizeof(*start_rsp);
 204         err = audioservice_expect(data, &start_rsp->h, BT_START_STREAM);
 205         if (err < 0)
 206                 goto error;
 207
 208         streamfd_ind->h.length = sizeof(*streamfd_ind);
 209         err = audioservice_expect(data, &streamfd_ind->h, BT_NEW_STREAM);
 210         if (err < 0)
 211                 goto error;
 212
 213         data->stream.fd = bt_audio_service_get_data_fd(data->server.fd);
 214         if (data->stream.fd < 0) {
 215                 ERR("bt_audio_service_get_data_fd failed, errno: %d", errno);
 216                 err = -errno;
 217                 goto error;
 218         }
 219         data->stream.events = POLLOUT;
 220
 221         /* set our socket buffer to the size of PACKET_BUFFER_COUNT packets */
 222         bytes = data->link_mtu * PACKET_BUFFER_COUNT;
 223         setsockopt(data->stream.fd, SOL_SOCKET, SO_SNDBUF, &bytes,
 224                         sizeof(bytes));
 225
 226         data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
 227         data->frame_count = 0;
 228         data->samples = 0;
 229         data->nsamples = 0;
 230         data->seq_num = 0;
 231         data->frame_count = 0;
 232         data->next_write = 0;
 233
 234         set_state(data, A2DP_STATE_STARTED);
 235         return 0;
 236
 237 error:
 238         /* set state to A2DP_STATE_INITIALIZED to force reconfiguration */
 239         if (data->state == A2DP_STATE_STARTING)
 240                 set_state(data, A2DP_STATE_INITIALIZED);
 241         return err;
 242 }
 243
 244 static int bluetooth_stop(struct bluetooth_data *data)
 245 {
 246         char buf[BT_SUGGESTED_BUFFER_SIZE];
 247         struct bt_stop_stream_req *stop_req = (void*) buf;
 248         struct bt_stop_stream_rsp *stop_rsp = (void*) buf;
 249         int err;
 250
 251         DBG("bluetooth_stop");
 252
 253         data->state = A2DP_STATE_STOPPING;
 254         if (data->stream.fd >= 0) {
 255                 close(data->stream.fd);
 256                 data->stream.fd = -1;
 257         }
 258
 259         /* send stop request */
 260         memset(stop_req, 0, BT_SUGGESTED_BUFFER_SIZE);
 261         stop_req->h.type = BT_REQUEST;
 262         stop_req->h.name = BT_STOP_STREAM;
 263         stop_req->h.length = sizeof(*stop_req);
 264
 265         err = audioservice_send(data, &stop_req->h);
 266         if (err < 0)
 267                 goto error;
 268
 269         stop_rsp->h.length = sizeof(*stop_rsp);
 270         err = audioservice_expect(data, &stop_rsp->h, BT_STOP_STREAM);
 271         if (err < 0)
 272                 goto error;
 273
 274 error:
 275         if (data->state == A2DP_STATE_STOPPING)
 276                 set_state(data, A2DP_STATE_CONFIGURED);
 277         return err;
 278 }
 279
 280 static uint8_t default_bitpool(uint8_t freq, uint8_t mode)
 281 {
 282         switch (freq) {
 283         case BT_SBC_SAMPLING_FREQ_16000:
 284         case BT_SBC_SAMPLING_FREQ_32000:
 285                 return 53;
 286         case BT_SBC_SAMPLING_FREQ_44100:
 287                 switch (mode) {
 288                 case BT_A2DP_CHANNEL_MODE_MONO:
 289                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 290                         return 31;
 291                 case BT_A2DP_CHANNEL_MODE_STEREO:
 292                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 293                         return 53;
 294                 default:
 295                         ERR("Invalid channel mode %u", mode);
 296                         return 53;
 297                 }
 298         case BT_SBC_SAMPLING_FREQ_48000:
 299                 switch (mode) {
 300                 case BT_A2DP_CHANNEL_MODE_MONO:
 301                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 302                         return 29;
 303                 case BT_A2DP_CHANNEL_MODE_STEREO:
 304                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 305                         return 51;
 306                 default:
 307                         ERR("Invalid channel mode %u", mode);
 308                         return 51;
 309                 }
 310         default:
 311                 ERR("Invalid sampling freq %u", freq);
 312                 return 53;
 313         }
 314 }
 315
 316 static int bluetooth_a2dp_init(struct bluetooth_data *data)
 317 {
 318         sbc_capabilities_t *cap = &data->sbc_capabilities;
 319         unsigned int max_bitpool, min_bitpool;
 320         int dir;
 321
 322         switch (data->rate) {
 323         case 48000:
 324                 cap->frequency = BT_SBC_SAMPLING_FREQ_48000;
 325                 break;
 326         case 44100:
 327                 cap->frequency = BT_SBC_SAMPLING_FREQ_44100;
 328                 break;
 329         case 32000:
 330                 cap->frequency = BT_SBC_SAMPLING_FREQ_32000;
 331                 break;
 332         case 16000:
 333                 cap->frequency = BT_SBC_SAMPLING_FREQ_16000;
 334                 break;
 335         default:
 336                 ERR("Rate %d not supported", data->rate);
 337                 return -1;
 338         }
 339
 340         if (data->channels == 2) {
 341                 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 342                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
 343                 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 344                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
 345                 else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 346                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
 347         } else {
 348                 if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
 349                         cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 350         }
 351
 352         if (!cap->channel_mode) {
 353                 ERR("No supported channel modes");
 354                 return -1;
 355         }
 356
 357         if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
 358                 cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
 359         else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
 360                 cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
 361         else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
 362                 cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
 363         else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
 364                 cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
 365         else {
 366                 ERR("No supported block lengths");
 367                 return -1;
 368         }
 369
 370         if (cap->subbands & BT_A2DP_SUBBANDS_8)
 371                 cap->subbands = BT_A2DP_SUBBANDS_8;
 372         else if (cap->subbands & BT_A2DP_SUBBANDS_4)
 373                 cap->subbands = BT_A2DP_SUBBANDS_4;
 374         else {
 375                 ERR("No supported subbands");
 376                 return -1;
 377         }
 378
 379         if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
 380                 cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
 381         else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
 382                 cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
 383
 384                 min_bitpool = MAX(MIN_BITPOOL, cap->min_bitpool);
 385                 max_bitpool = MIN(default_bitpool(cap->frequency,
 386                                         cap->channel_mode),
 387                                         cap->max_bitpool);
 388
 389         cap->min_bitpool = min_bitpool;
 390         cap->max_bitpool = max_bitpool;
 391
 392         return 0;
 393 }
 394
 395 static void bluetooth_a2dp_setup(struct bluetooth_data *data)
 396 {
 397         sbc_capabilities_t active_capabilities = data->sbc_capabilities;
 398
 399         sbc_reinit(&data->sbc, 0);
 400
 401         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
 402                 data->sbc.frequency = SBC_FREQ_16000;
 403
 404         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
 405                 data->sbc.frequency = SBC_FREQ_32000;
 406
 407         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
 408                 data->sbc.frequency = SBC_FREQ_44100;
 409
 410         if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
 411                 data->sbc.frequency = SBC_FREQ_48000;
 412
 413         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
 414                 data->sbc.mode = SBC_MODE_MONO;
 415
 416         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 417                 data->sbc.mode = SBC_MODE_DUAL_CHANNEL;
 418
 419         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 420                 data->sbc.mode = SBC_MODE_STEREO;
 421
 422         if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 423                 data->sbc.mode = SBC_MODE_JOINT_STEREO;
 424
 425         data->sbc.allocation = active_capabilities.allocation_method
 426                                 == BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR
 427                                 : SBC_AM_LOUDNESS;
 428
 429         switch (active_capabilities.subbands) {
 430         case BT_A2DP_SUBBANDS_4:
 431                 data->sbc.subbands = SBC_SB_4;
 432                 break;
 433         case BT_A2DP_SUBBANDS_8:
 434                 data->sbc.subbands = SBC_SB_8;
 435                 break;
 436         }
 437
 438         switch (active_capabilities.block_length) {
 439         case BT_A2DP_BLOCK_LENGTH_4:
 440                 data->sbc.blocks = SBC_BLK_4;
 441                 break;
 442         case BT_A2DP_BLOCK_LENGTH_8:
 443                 data->sbc.blocks = SBC_BLK_8;
 444                 break;
 445         case BT_A2DP_BLOCK_LENGTH_12:
 446                 data->sbc.blocks = SBC_BLK_12;
 447                 break;
 448         case BT_A2DP_BLOCK_LENGTH_16:
 449                 data->sbc.blocks = SBC_BLK_16;
 450                 break;
 451         }
 452
 453         data->sbc.bitpool = active_capabilities.max_bitpool;
 454         data->codesize = sbc_get_codesize(&data->sbc);
 455         data->frame_duration = sbc_get_frame_duration(&data->sbc);
 456         DBG("frame_duration: %d us", data->frame_duration);
 457 }
 458
 459 static int bluetooth_a2dp_hw_params(struct bluetooth_data *data)
 460 {
 461         char buf[BT_SUGGESTED_BUFFER_SIZE];
 462         struct bt_open_req *open_req = (void *) buf;
 463         struct bt_open_rsp *open_rsp = (void *) buf;
 464         struct bt_set_configuration_req *setconf_req = (void*) buf;
 465         struct bt_set_configuration_rsp *setconf_rsp = (void*) buf;
 466         int err;
 467
 468         memset(open_req, 0, BT_SUGGESTED_BUFFER_SIZE);
 469         open_req->h.type = BT_REQUEST;
 470         open_req->h.name = BT_OPEN;
 471         open_req->h.length = sizeof(*open_req);
 472         strncpy(open_req->destination, data->address, 18);
 473         open_req->seid = data->sbc_capabilities.capability.seid;
 474         open_req->lock = BT_WRITE_LOCK;
 475
 476         err = audioservice_send(data, &open_req->h);
 477         if (err < 0)
 478                 return err;
 479
 480         open_rsp->h.length = sizeof(*open_rsp);
 481         err = audioservice_expect(data, &open_rsp->h, BT_OPEN);
 482         if (err < 0)
 483                 return err;
 484
 485         err = bluetooth_a2dp_init(data);
 486         if (err < 0)
 487                 return err;
 488
 489
 490         memset(setconf_req, 0, BT_SUGGESTED_BUFFER_SIZE);
 491         setconf_req->h.type = BT_REQUEST;
 492         setconf_req->h.name = BT_SET_CONFIGURATION;
 493         setconf_req->h.length = sizeof(*setconf_req);
 494         memcpy(&setconf_req->codec, &data->sbc_capabilities,
 495                                                 sizeof(data->sbc_capabilities));
 496
 497         setconf_req->codec.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 498         setconf_req->codec.length = sizeof(data->sbc_capabilities);
 499         setconf_req->h.length += setconf_req->codec.length - sizeof(setconf_req->codec);
 500
 501         DBG("bluetooth_a2dp_hw_params sending configuration:\n");
 502         switch (data->sbc_capabilities.channel_mode) {
 503                 case BT_A2DP_CHANNEL_MODE_MONO:
 504                         DBG("\tchannel_mode: MONO\n");
 505                         break;
 506                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 507                         DBG("\tchannel_mode: DUAL CHANNEL\n");
 508                         break;
 509                 case BT_A2DP_CHANNEL_MODE_STEREO:
 510                         DBG("\tchannel_mode: STEREO\n");
 511                         break;
 512                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 513                         DBG("\tchannel_mode: JOINT STEREO\n");
 514                         break;
 515                 default:
 516                         DBG("\tchannel_mode: UNKNOWN (%d)\n",
 517                                 data->sbc_capabilities.channel_mode);
 518         }
 519         switch (data->sbc_capabilities.frequency) {
 520                 case BT_SBC_SAMPLING_FREQ_16000:
 521                         DBG("\tfrequency: 16000\n");
 522                         break;
 523                 case BT_SBC_SAMPLING_FREQ_32000:
 524                         DBG("\tfrequency: 32000\n");
 525                         break;
 526                 case BT_SBC_SAMPLING_FREQ_44100:
 527                         DBG("\tfrequency: 44100\n");
 528                         break;
 529                 case BT_SBC_SAMPLING_FREQ_48000:
 530                         DBG("\tfrequency: 48000\n");
 531                         break;
 532                 default:
 533                         DBG("\tfrequency: UNKNOWN (%d)\n",
 534                                 data->sbc_capabilities.frequency);
 535         }
 536         switch (data->sbc_capabilities.allocation_method) {
 537                 case BT_A2DP_ALLOCATION_SNR:
 538                         DBG("\tallocation_method: SNR\n");
 539                         break;
 540                 case BT_A2DP_ALLOCATION_LOUDNESS:
 541                         DBG("\tallocation_method: LOUDNESS\n");
 542                         break;
 543                 default:
 544                         DBG("\tallocation_method: UNKNOWN (%d)\n",
 545                                 data->sbc_capabilities.allocation_method);
 546         }
 547         switch (data->sbc_capabilities.subbands) {
 548                 case BT_A2DP_SUBBANDS_4:
 549                         DBG("\tsubbands: 4\n");
 550                         break;
 551                 case BT_A2DP_SUBBANDS_8:
 552                         DBG("\tsubbands: 8\n");
 553                         break;
 554                 default:
 555                         DBG("\tsubbands: UNKNOWN (%d)\n",
 556                                 data->sbc_capabilities.subbands);
 557         }
 558         switch (data->sbc_capabilities.block_length) {
 559                 case BT_A2DP_BLOCK_LENGTH_4:
 560                         DBG("\tblock_length: 4\n");
 561                         break;
 562                 case BT_A2DP_BLOCK_LENGTH_8:
 563                         DBG("\tblock_length: 8\n");
 564                         break;
 565                 case BT_A2DP_BLOCK_LENGTH_12:
 566                         DBG("\tblock_length: 12\n");
 567                         break;
 568                 case BT_A2DP_BLOCK_LENGTH_16:
 569                         DBG("\tblock_length: 16\n");
 570                         break;
 571                 default:
 572                         DBG("\tblock_length: UNKNOWN (%d)\n",
 573                                 data->sbc_capabilities.block_length);
 574         }
 575         DBG("\tmin_bitpool: %d\n", data->sbc_capabilities.min_bitpool);
 576         DBG("\tmax_bitpool: %d\n", data->sbc_capabilities.max_bitpool);
 577
 578         err = audioservice_send(data, &setconf_req->h);
 579         if (err < 0)
 580                 return err;
 581
 582         err = audioservice_expect(data, &setconf_rsp->h, BT_SET_CONFIGURATION);
 583         if (err < 0)
 584                 return err;
 585
 586         data->link_mtu = setconf_rsp->link_mtu;
 587         DBG("MTU: %d", data->link_mtu);
 588
 589         /* Setup SBC encoder now we agree on parameters */
 590         bluetooth_a2dp_setup(data);
 591
 592         DBG("\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
 593                 data->sbc.allocation, data->sbc.subbands, data->sbc.blocks,
 594                 data->sbc.bitpool);
 595
 596         return 0;
 597 }
 598
 599 static int avdtp_write(struct bluetooth_data *data)
 600 {
 601         int ret = 0;
 602         struct rtp_header *header;
 603         struct rtp_payload *payload;
 604
 605         uint64_t now;
 606         long duration = data->frame_duration * data->frame_count;
 607 #ifdef ENABLE_TIMING
 608         uint64_t begin, end, begin2, end2;
 609         begin = get_microseconds();
 610 #endif
 611
 612         header = (struct rtp_header *)data->buffer;
 613         payload = (struct rtp_payload *)(data->buffer + sizeof(*header));
 614
 615         memset(data->buffer, 0, sizeof(*header) + sizeof(*payload));
 616
 617         payload->frame_count = data->frame_count;
 618         header->v = 2;
 619         header->pt = 1;
 620         header->sequence_number = htons(data->seq_num);
 621         header->timestamp = htonl(data->nsamples);
 622         header->ssrc = htonl(1);
 623
 624         data->stream.revents = 0;
 625 #ifdef ENABLE_TIMING
 626         begin2 = get_microseconds();
 627 #endif
 628         ret = poll(&data->stream, 1, POLL_TIMEOUT);
 629 #ifdef ENABLE_TIMING
 630         end2 = get_microseconds();
 631         print_time("poll", begin2, end2);
 632 #endif
 633         if (ret == 1 && data->stream.revents == POLLOUT) {
 634                 long ahead = 0;
 635                 now = get_microseconds();
 636
 637                 if (data->next_write) {
 638                         ahead = data->next_write - now;
 639 #ifdef ENABLE_TIMING
 640                         DBG("duration: %ld, ahead: %ld", duration, ahead);
 641 #endif
 642                         if (ahead > 0) {
 643                                 /* too fast, need to throttle */
 644                                 usleep(ahead);
 645                         }
 646                 } else {
 647                         data->next_write = now;
 648                 }
 649                 if (ahead < 0) {
 650                         /* fallen too far behind, don't try to catch up */
 651                         VDBG("ahead < 0, resetting next_write");
 652                         data->next_write = 0;
 653                 } else {
 654                         /* advance next_write by duration */
 655                         data->next_write += duration;
 656                 }
 657
 658 #ifdef ENABLE_TIMING
 659                 begin2 = get_microseconds();
 660 #endif
 661                 ret = send(data->stream.fd, data->buffer, data->count, MSG_NOSIGNAL);
 662 #ifdef ENABLE_TIMING
 663                 end2 = get_microseconds();
 664                 print_time("send", begin2, end2);
 665 #endif
 666                 if (ret < 0) {
 667                         /* can happen during normal remote disconnect */
 668                         VDBG("send() failed: %d (errno %s)", ret, strerror(errno));
 669                 }
 670                 if (ret == -EPIPE) {
 671                         bluetooth_close(data);
 672                 }
 673         } else {
 674                 /* can happen during normal remote disconnect */
 675                 VDBG("poll() failed: %d (revents = %d, errno %s)",
 676                                 ret, data->stream.revents, strerror(errno));
 677         }
 678
 679         /* Reset buffer of data to send */
 680         data->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
 681         data->frame_count = 0;
 682         data->samples = 0;
 683         data->seq_num++;
 684
 685 #ifdef ENABLE_TIMING
 686         end = get_microseconds();
 687         print_time("avdtp_write", begin, end);
 688 #endif
 689         return 0; /* always return success */
 690 }
 691
 692 static int audioservice_send(struct bluetooth_data *data,
 693                 const bt_audio_msg_header_t *msg)
 694 {
 695         int err;
 696         uint16_t length;
 697
 698         length = msg->length ? msg->length : BT_SUGGESTED_BUFFER_SIZE;
 699
 700         VDBG("sending %s", bt_audio_strmsg(msg->msg_type));
 701         if (send(data->server.fd, msg, length,
 702                         MSG_NOSIGNAL) > 0)
 703                 err = 0;
 704         else {
 705                 err = -errno;
 706                 ERR("Error sending data to audio service: %s(%d)",
 707                         strerror(errno), errno);
 708                 if (err == -EPIPE)
 709                         bluetooth_close(data);
 710         }
 711
 712         return err;
 713 }
 714
 715 static int audioservice_recv(struct bluetooth_data *data,
 716                 bt_audio_msg_header_t *inmsg)
 717 {
 718         int err, ret;
 719         const char *type, *name;
 720         uint16_t length;
 721
 722         length = inmsg->length ? inmsg->length : BT_SUGGESTED_BUFFER_SIZE;
 723
 724         ret = recv(data->server.fd, inmsg, length, 0);
 725         if (ret < 0) {
 726                 err = -errno;
 727                 ERR("Error receiving IPC data from bluetoothd: %s (%d)",
 728                                                 strerror(errno), errno);
 729                 if (err == -EPIPE)
 730                         bluetooth_close(data);
 731         } else if ((size_t) ret < sizeof(bt_audio_msg_header_t)) {
 732                 ERR("Too short (%d bytes) IPC packet from bluetoothd", ret);
 733                 err = -EINVAL;
 734         } else if (inmsg->type == BT_ERROR) {
 735                 bt_audio_error_t *error = (bt_audio_error_t *)inmsg;
 736                 ret = recv(data->server.fd, &error->posix_errno,
 737                                 sizeof(error->posix_errno), 0);
 738                 if (ret < 0) {
 739                         err = -errno;
 740                         ERR("Error receiving error code for BT_ERROR: %s (%d)",
 741                                                 strerror(errno), errno);
 742                         if (err == -EPIPE)
 743                                 bluetooth_close(data);
 744                 } else {
 745                         ERR("%s failed : %s(%d)",
 746                                         bt_audio_strname(error->h.name),
 747                                         strerror(error->posix_errno),
 748                                         error->posix_errno);
 749                         err = -error->posix_errno;
 750                 }
 751         } else {
 752                 type = bt_audio_strtype(inmsg->type);
 753                 name = bt_audio_strname(inmsg->name);
 754                 if (type && name) {
 755                         DBG("Received %s - %s", type, name);
 756                         err = 0;
 757                 } else {
 758                         err = -EINVAL;
 759                         ERR("Bogus message type %d - name %d"
 760                                         " received from audio service",
 761                                         inmsg->type, inmsg->name);
 762                 }
 763
 764         }
 765         return err;
 766 }
 767
 768 static int audioservice_expect(struct bluetooth_data *data,
 769                 bt_audio_msg_header_t *rsp_hdr, int expected_name)
 770 {
 771         int err = audioservice_recv(data, rsp_hdr);
 772
 773         if (err != 0)
 774                 return err;
 775
 776         if (rsp_hdr->name != expected_name) {
 777                 err = -EINVAL;
 778                 ERR("Bogus message %s received while %s was expected",
 779                                 bt_audio_strname(rsp_hdr->name),
 780                                 bt_audio_strname(expected_name));
 781         }
 782         return err;
 783
 784 }
 785
 786 static int bluetooth_init(struct bluetooth_data *data)
 787 {
 788         int sk, err;
 789
 790         DBG("bluetooth_init");
 791
 792         sk = bt_audio_service_open();
 793         if (sk <= 0) {
 794                 ERR("bt_audio_service_open failed\n");
 795                 return -errno;
 796         }
 797
 798         data->server.fd = sk;
 799         data->server.events = POLLIN;
 800         data->state = A2DP_STATE_INITIALIZED;
 801
 802         return 0;
 803 }
 804
 805 static int bluetooth_parse_capabilities(struct bluetooth_data *data,
 806                                         struct bt_get_capabilities_rsp *rsp)
 807 {
 808         int bytes_left = rsp->h.length - sizeof(*rsp);
 809         codec_capabilities_t *codec = (void *) rsp->data;
 810
 811         if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP)
 812                 return 0;
 813
 814         while (bytes_left > 0) {
 815                 if ((codec->type == BT_A2DP_SBC_SINK) &&
 816                                 !(codec->lock & BT_WRITE_LOCK))
 817                         break;
 818
 819                 bytes_left -= codec->length;
 820                 codec = (void *) codec + codec->length;
 821         }
 822
 823         if (bytes_left <= 0 ||
 824                         codec->length != sizeof(data->sbc_capabilities))
 825                 return -EINVAL;
 826
 827         memcpy(&data->sbc_capabilities, codec, codec->length);
 828         return 0;
 829 }
 830
 831
 832 static int bluetooth_configure(struct bluetooth_data *data)
 833 {
 834         char buf[BT_SUGGESTED_BUFFER_SIZE];
 835         struct bt_get_capabilities_req *getcaps_req = (void*) buf;
 836         struct bt_get_capabilities_rsp *getcaps_rsp = (void*) buf;
 837         int err;
 838
 839         DBG("bluetooth_configure");
 840
 841         data->state = A2DP_STATE_CONFIGURING;
 842         memset(getcaps_req, 0, BT_SUGGESTED_BUFFER_SIZE);
 843         getcaps_req->h.type = BT_REQUEST;
 844         getcaps_req->h.name = BT_GET_CAPABILITIES;
 845
 846         getcaps_req->flags = 0;
 847         getcaps_req->flags |= BT_FLAG_AUTOCONNECT;
 848         strncpy(getcaps_req->destination, data->address, 18);
 849         getcaps_req->transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 850         getcaps_req->h.length = sizeof(*getcaps_req);
 851
 852         err = audioservice_send(data, &getcaps_req->h);
 853         if (err < 0) {
 854                 ERR("audioservice_send failed for BT_GETCAPABILITIES_REQ\n");
 855                 goto error;
 856         }
 857
 858         getcaps_rsp->h.length = 0;
 859         err = audioservice_expect(data, &getcaps_rsp->h, BT_GET_CAPABILITIES);
 860         if (err < 0) {
 861                 ERR("audioservice_expect failed for BT_GETCAPABILITIES_RSP\n");
 862                 goto error;
 863         }
 864
 865         bluetooth_parse_capabilities(data, getcaps_rsp);
 866         err = bluetooth_a2dp_hw_params(data);
 867         if (err < 0) {
 868                 ERR("bluetooth_a2dp_hw_params failed err: %d", err);
 869                 goto error;
 870         }
 871
 872         set_state(data, A2DP_STATE_CONFIGURED);
 873         return 0;
 874
 875 error:
 876         if (data->state == A2DP_STATE_CONFIGURING)
 877                 set_state(data, A2DP_STATE_INITIALIZED);
 878         return err;
 879 }
 880
 881 static void set_state(struct bluetooth_data *data, a2dp_state_t state)
 882 {
 883         data->state = state;
 884         pthread_cond_signal(&data->client_wait);
 885 }
 886
 887 static void set_command(struct bluetooth_data *data, a2dp_command_t command)
 888 {
 889         VDBG("set_command %d\n", command);
 890         pthread_mutex_lock(&data->mutex);
 891         data->command = command;
 892         pthread_cond_signal(&data->thread_wait);
 893         pthread_mutex_unlock(&data->mutex);
 894 }
 895
 896 /* timeout is in milliseconds */
 897 static int wait_for_start(struct bluetooth_data *data, int timeout)
 898 {
 899         a2dp_state_t state = data->state;
 900         struct timeval tv;
 901         struct timespec ts;
 902         int err = 0;
 903
 904 #ifdef ENABLE_TIMING
 905         uint64_t begin, end;
 906         begin = get_microseconds();
 907 #endif
 908
 909         gettimeofday(&tv, (struct timezone *) NULL);
 910         ts.tv_sec = tv.tv_sec + (timeout / 1000);
 911         ts.tv_nsec = (tv.tv_usec + (timeout % 1000) * 1000L ) * 1000L;
 912
 913         while (state != A2DP_STATE_STARTED && !err) {
 914                 if (state == A2DP_STATE_NONE)
 915                         set_command(data, A2DP_CMD_INIT);
 916                 else if (state == A2DP_STATE_INITIALIZED)
 917                         set_command(data, A2DP_CMD_CONFIGURE);
 918                 else if (state == A2DP_STATE_CONFIGURED)
 919                         set_command(data, A2DP_CMD_START);
 920
 921                 pthread_mutex_lock(&data->mutex);
 922                 while ((err = pthread_cond_timedwait(&data->client_wait, &data->mutex, &ts))
 923                                 == EINTR) ;
 924                 state = data->state;
 925                 pthread_mutex_unlock(&data->mutex);
 926         }
 927
 928 #ifdef ENABLE_TIMING
 929         end = get_microseconds();
 930         print_time("wait_for_start", begin, end);
 931 #endif
 932
 933         /* pthread_cond_timedwait returns positive errors */
 934         return -err;
 935 }
 936
 937 static void* a2dp_thread(void *d)
 938 {
 939         struct bluetooth_data* data = (struct bluetooth_data*)d;
 940
 941         DBG("a2dp_thread started");
 942         prctl(PR_SET_NAME, "a2dp_thread", 0, 0, 0);
 943
 944         pthread_mutex_lock(&data->mutex);
 945
 946         data->started = 1;
 947         pthread_cond_signal(&data->thread_start);
 948
 949         while (1)
 950         {
 951                 a2dp_command_t command;
 952
 953                 pthread_cond_wait(&data->thread_wait, &data->mutex);
 954                 command = data->command;
 955
 956                 switch (command) {
 957                         case A2DP_CMD_INIT:
 958                                 if (data->state != A2DP_STATE_NONE)
 959                                         break;
 960                                 bluetooth_init(data);
 961                                 break;
 962                         case A2DP_CMD_CONFIGURE:
 963                                 if (data->state != A2DP_STATE_INITIALIZED)
 964                                         break;
 965                                 bluetooth_configure(data);
 966                                 break;
 967
 968                         case A2DP_CMD_START:
 969                                 if (data->state != A2DP_STATE_CONFIGURED)
 970                                         break;
 971                                 bluetooth_start(data);
 972                                 break;
 973
 974                         case A2DP_CMD_STOP:
 975                                 if (data->state != A2DP_STATE_STARTED)
 976                                         break;
 977                                 bluetooth_stop(data);
 978                                 break;
 979
 980                         case A2DP_CMD_QUIT:
 981                                 bluetooth_close(data);
 982                                 sbc_finish(&data->sbc);
 983                                 free(data);
 984                                 goto done;
 985
 986                         default:
 987                                 break;
 988                 }
 989         }
 990
 991 done:
 992         pthread_mutex_unlock(&data->mutex);
 993         DBG("a2dp_thread finished");
 994         return NULL;
 995 }
 996
 997 int a2dp_init(int rate, int channels, a2dpData* dataPtr)
 998 {
 999         struct bluetooth_data* data;
1000         pthread_attr_t attr;
1001         int err;
1002
1003         DBG("a2dp_init rate: %d channels: %d", rate, channels);
1004         *dataPtr = NULL;
1005         data = malloc(sizeof(struct bluetooth_data));
1006         if (!data)
1007                 return -1;
1008
1009         memset(data, 0, sizeof(struct bluetooth_data));
1010         data->server.fd = -1;
1011         data->stream.fd = -1;
1012         data->state = A2DP_STATE_NONE;
1013
1014         strncpy(data->address, "00:00:00:00:00:00", 18);
1015         data->rate = rate;
1016         data->channels = channels;
1017
1018         sbc_init(&data->sbc, 0);
1019
1020         pthread_mutex_init(&data->mutex, NULL);
1021         pthread_cond_init(&data->thread_start, NULL);
1022         pthread_cond_init(&data->thread_wait, NULL);
1023         pthread_cond_init(&data->client_wait, NULL);
1024
1025         pthread_mutex_lock(&data->mutex);
1026         data->started = 0;
1027
1028         pthread_attr_init(&attr);
1029         pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
1030
1031         err = pthread_create(&data->thread, &attr, a2dp_thread, data);
1032         if (err) {
1033                 /* If the thread create fails we must not wait */
1034                 pthread_mutex_unlock(&data->mutex);
1035                 err = -err;
1036                 goto error;
1037         }
1038
1039         /* Make sure the state machine is ready and waiting */
1040         while (!data->started) {
1041                 pthread_cond_wait(&data->thread_start, &data->mutex);
1042         }
1043
1044         /* Poke the state machine to get it going */
1045         pthread_cond_signal(&data->thread_wait);
1046
1047         pthread_mutex_unlock(&data->mutex);
1048
1049         *dataPtr = data;
1050         return 0;
1051 error:
1052         bluetooth_close(data);
1053         sbc_finish(&data->sbc);
1054         free(data);
1055
1056         return err;
1057 }
1058
1059 void a2dp_set_sink(a2dpData d, const char* address)
1060 {
1061         struct bluetooth_data* data = (struct bluetooth_data*)d;
1062         if (strncmp(data->address, address, 18)) {
1063                 strncpy(data->address, address, 18);
1064                 set_command(data, A2DP_CMD_INIT);
1065         }
1066 }
1067
1068 int a2dp_write(a2dpData d, const void* buffer, int count)
1069 {
1070         struct bluetooth_data* data = (struct bluetooth_data*)d;
1071         uint8_t* src = (uint8_t *)buffer;
1072         int codesize;
1073         int err, ret = 0;
1074         long frames_left = count;
1075         int encoded, written;
1076         const char *buff;
1077         int did_configure = 0;
1078 #ifdef ENABLE_TIMING
1079         uint64_t begin, end;
1080         DBG("********** a2dp_write **********");
1081         begin = get_microseconds();
1082 #endif
1083
1084         err = wait_for_start(data, WRITE_TIMEOUT);
1085         if (err < 0)
1086                 return err;
1087
1088         codesize = data->codesize;
1089
1090         while (frames_left >= codesize) {
1091                 /* Enough data to encode (sbc wants 512 byte blocks) */
1092                 encoded = sbc_encode(&(data->sbc), src, codesize,
1093                                         data->buffer + data->count,
1094                                         sizeof(data->buffer) - data->count,
1095                                         &written);
1096                 if (encoded <= 0) {
1097                         ERR("Encoding error %d", encoded);
1098                         goto done;
1099                 }
1100                 VDBG("sbc_encode returned %d, codesize: %d, written: %d\n",
1101                         encoded, codesize, written);
1102
1103                 src += encoded;
1104                 data->count += written;
1105                 data->frame_count++;
1106                 data->samples += encoded;
1107                 data->nsamples += encoded;
1108
1109                 /* No space left for another frame then send */
1110                 if (data->count + written >= data->link_mtu) {
1111                         VDBG("sending packet %d, count %d, link_mtu %u",
1112                                         data->seq_num, data->count,
1113                                         data->link_mtu);
1114                         err = avdtp_write(data);
1115                         if (err < 0)
1116                                 return err;
1117                 }
1118
1119                 ret += encoded;
1120                 frames_left -= encoded;
1121         }
1122
1123         if (frames_left > 0)
1124                 ERR("%ld bytes left at end of a2dp_write\n", frames_left);
1125
1126 done:
1127 #ifdef ENABLE_TIMING
1128         end = get_microseconds();
1129         print_time("a2dp_write total", begin, end);
1130 #endif
1131         return ret;
1132 }
1133
1134 int a2dp_stop(a2dpData d)
1135 {
1136         struct bluetooth_data* data = (struct bluetooth_data*)d;
1137         DBG("a2dp_stop\n");
1138         if (!data)
1139                 return 0;
1140
1141         set_command(data, A2DP_CMD_STOP);
1142         return 0;
1143 }
1144
1145 void a2dp_cleanup(a2dpData d)
1146 {
1147         struct bluetooth_data* data = (struct bluetooth_data*)d;
1148         DBG("a2dp_cleanup\n");
1149         set_command(data, A2DP_CMD_QUIT);
1150 }