src/modules/bluetooth/module-bluetooth-device.c

   1 /***
   2     This file is part of PulseAudio.
   3
   4     Copyright 2008 Joao Paulo Rechi Vita
   5
   6     PulseAudio is free software; you can redistribute it and/or modify
   7     it under the terms of the GNU Lesser General Public License as published
   8     by the Free Software Foundation; either version 2 of the License,
   9     or (at your option) any later version.
  10
  11     PulseAudio is distributed in the hope that it will be useful, but
  12     WITHOUT ANY WARRANTY; without even the implied warranty of
  13     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  14     General Public License for more details.
  15
  16     You should have received a copy of the GNU Lesser General Public License
  17     along with PulseAudio; if not, write to the Free Software
  18     Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307
  19     USA.
  20 ***/
  21
  22 #ifdef HAVE_CONFIG_H
  23 #include <config.h>
  24 #endif
  25
  26 #include <string.h>
  27 #include <errno.h>
  28 #include <poll.h>
  29 #include <sys/ioctl.h>
  30 #include <linux/sockios.h>
  31 #include <arpa/inet.h>
  32
  33 #include <pulse/xmalloc.h>
  34 #include <pulse/timeval.h>
  35 #include <pulse/sample.h>
  36 #include <pulsecore/module.h>
  37 #include <pulsecore/modargs.h>
  38 #include <pulsecore/core-util.h>
  39 #include <pulsecore/core-error.h>
  40 #include <pulsecore/socket-util.h>
  41 #include <pulsecore/thread.h>
  42 #include <pulsecore/thread-mq.h>
  43 #include <pulsecore/rtpoll.h>
  44 #include <pulsecore/time-smoother.h>
  45 #include <pulsecore/rtclock.h>
  46
  47 #include "../dbus-util.h"
  48 #include "module-bluetooth-device-symdef.h"
  49 #include "ipc.h"
  50 #include "sbc.h"
  51 #include "rtp.h"
  52
  53 #define DEFAULT_SINK_NAME "bluetooth_sink"
  54 #define BUFFER_SIZE 2048
  55 #define MAX_BITPOOL 64
  56 #define MIN_BITPOOL 2U
  57 #define SOL_SCO 17
  58 #define SCO_TXBUFS 0x03
  59 #define SCO_RXBUFS 0x04
  60
  61 PA_MODULE_AUTHOR("Joao Paulo Rechi Vita");
  62 PA_MODULE_DESCRIPTION("Bluetooth audio sink and source");
  63 PA_MODULE_VERSION(PACKAGE_VERSION);
  64 PA_MODULE_LOAD_ONCE(FALSE);
  65 PA_MODULE_USAGE(
  66         "sink_name=<name of the device> "
  67         "address=<address of the device> "
  68         "profile=<a2dp|hsp>");
  69
  70 struct bt_a2dp {
  71     sbc_capabilities_t sbc_capabilities;
  72     sbc_t sbc;                           /* Codec data */
  73     pa_bool_t sbc_initialized;           /* Keep track if the encoder is initialized */
  74     size_t codesize;                     /* SBC codesize */
  75     unsigned samples;                    /* Number of encoded samples */
  76     uint8_t buffer[BUFFER_SIZE];         /* Codec transfer buffer */
  77     size_t count;                        /* Codec transfer buffer counter */
  78
  79     unsigned total_samples;              /* Cumulative number of codec samples */
  80     uint16_t seq_num;                    /* Cumulative packet sequence */
  81     unsigned frame_count;                /* Current frames in buffer*/
  82 };
  83
  84 struct userdata {
  85     pa_core *core;
  86     pa_module *module;
  87     pa_sink *sink;
  88
  89     pa_thread_mq thread_mq;
  90     pa_rtpoll *rtpoll;
  91     pa_rtpoll_item *rtpoll_item;
  92     pa_thread *thread;
  93
  94     uint64_t offset;
  95     pa_smoother *smoother;
  96
  97     char *name;
  98     char *addr;
  99     char *profile;
 100     pa_sample_spec ss;
 101
 102     int audioservice_fd;
 103     int stream_fd;
 104
 105     uint8_t transport;
 106     char *strtransport;
 107     size_t link_mtu;
 108     size_t block_size;
 109     pa_usec_t latency;
 110
 111     struct bt_a2dp a2dp;
 112 };
 113
 114 static const char* const valid_modargs[] = {
 115     "sink_name",
 116     "address",
 117     "profile",
 118     "rate",
 119     "channels",
 120     NULL
 121 };
 122
 123 static int bt_audioservice_send(int sk, const bt_audio_msg_header_t *msg) {
 124     int e;
 125     const char *type, *name;
 126
 127     type = bt_audio_strtype(msg->type);
 128     name = bt_audio_strname(msg->name);
 129     pa_log_debug("sending: %s -> %s", type, name);
 130     if (send(sk, msg, BT_SUGGESTED_BUFFER_SIZE, 0) > 0)
 131         e = 0;
 132     else {
 133         e = -errno;
 134         pa_log_error("Error sending data to audio service: %s(%d)", pa_cstrerror(errno), errno);
 135     }
 136     return e;
 137 }
 138
 139 static int bt_audioservice_recv(int sk, bt_audio_msg_header_t *inmsg, uint16_t expected_length) {
 140     int e;
 141     const char *type, *name;
 142
 143     pa_log_debug("trying to receive msg from audio service...");
 144     if (recv(sk, inmsg, expected_length ? : BT_SUGGESTED_BUFFER_SIZE, 0) > 0) {
 145         type = bt_audio_strtype(inmsg->type);
 146         name = bt_audio_strname(inmsg->name);
 147         if (type && name) {
 148             pa_log_debug("Received: %s <- %s", type, name);
 149             e = 0;
 150         }
 151         else {
 152             e = -EINVAL;
 153             pa_log_error("Bogus message type %d name %d received from audio service",
 154                         inmsg->type, inmsg->name);
 155         }
 156     }
 157     else {
 158         e = -errno;
 159         pa_log_error("Error receiving data from audio service: %s(%d)", pa_cstrerror(errno), errno);
 160     }
 161
 162     return e;
 163 }
 164
 165 static int bt_audioservice_expect(int sk, bt_audio_msg_header_t *rsp, uint8_t expected_name, uint16_t expected_length) {
 166     int e = bt_audioservice_recv(sk, rsp, expected_length);
 167
 168     if (e < 0) {
 169         if (rsp->name != expected_name) {
 170             e = -EINVAL;
 171             pa_log_error("Bogus message %s received while %s was expected",
 172                     bt_audio_strname(rsp->name),
 173                     bt_audio_strname(expected_name));
 174         }
 175     }
 176
 177     if (rsp->type == BT_ERROR) {
 178         bt_audio_error_t *error = (void *) rsp;
 179         pa_log_error("%s failed : %s(%d)", bt_audio_strname(rsp->name), pa_cstrerror(error->posix_errno), error->posix_errno);
 180         return -error->posix_errno;
 181     }
 182
 183     return e;
 184 }
 185
 186 static int bt_parsecaps(struct userdata *u, struct bt_get_capabilities_rsp *rsp) {
 187     uint16_t bytes_left = rsp->h.length - sizeof(*rsp);
 188     codec_capabilities_t *codec = (void *) rsp->data;
 189
 190     u->transport = codec->transport;
 191
 192     if (codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP)
 193         return 0;
 194
 195     while (bytes_left > 0) {
 196         if (codec->type == BT_A2DP_CODEC_SBC)
 197             break;
 198
 199         bytes_left -= codec->length;
 200         codec = (void *) (codec + codec->length);
 201     }
 202
 203     if (bytes_left <= 0 || codec->length != sizeof(u->a2dp.sbc_capabilities))
 204         return -EINVAL;
 205
 206     memcpy(&u->a2dp.sbc_capabilities, codec, codec->length);
 207
 208     return 0;
 209 }
 210
 211 static int bt_getcaps(struct userdata *u) {
 212     int e;
 213     union {
 214         bt_audio_msg_header_t rsp;
 215         struct bt_get_capabilities_req getcaps_req;
 216         struct bt_get_capabilities_rsp getcaps_rsp;
 217         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 218     } msg;
 219
 220     memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 221     msg.getcaps_req.h.type = BT_REQUEST;
 222     msg.getcaps_req.h.name = BT_GET_CAPABILITIES;
 223     msg.getcaps_req.h.length = sizeof(msg.getcaps_req);
 224
 225     strncpy(msg.getcaps_req.device, u->addr, 18);
 226     if (strcasecmp(u->profile, "a2dp") == 0)
 227         msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 228     else if (strcasecmp(u->profile, "hsp") == 0)
 229         msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_SCO;
 230     else {
 231         pa_log_error("Invalid profile argument: %s", u->profile);
 232         return -1;
 233     }
 234     msg.getcaps_req.flags = BT_FLAG_AUTOCONNECT;
 235
 236     e = bt_audioservice_send(u->audioservice_fd, &msg.getcaps_req.h);
 237     if (e < 0) {
 238         pa_log_error("Failed to send GETCAPABILITIES_REQ");
 239         return e;
 240     }
 241
 242     e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp, BT_GET_CAPABILITIES, 0);
 243     if (e < 0) {
 244         pa_log_error("Failed to expect for GETCAPABILITIES_RSP");
 245         return e;
 246     }
 247
 248     return bt_parsecaps(u, &msg.getcaps_rsp);
 249 }
 250
 251 static uint8_t default_bitpool(uint8_t freq, uint8_t mode) {
 252     switch (freq) {
 253         case BT_SBC_SAMPLING_FREQ_16000:
 254         case BT_SBC_SAMPLING_FREQ_32000:
 255             return 53;
 256         case BT_SBC_SAMPLING_FREQ_44100:
 257             switch (mode) {
 258                 case BT_A2DP_CHANNEL_MODE_MONO:
 259                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 260                     return 31;
 261                 case BT_A2DP_CHANNEL_MODE_STEREO:
 262                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 263                     return 53;
 264                 default:
 265                     pa_log_warn("Invalid channel mode %u", mode);
 266                     return 53;
 267             }
 268         case BT_SBC_SAMPLING_FREQ_48000:
 269             switch (mode) {
 270                 case BT_A2DP_CHANNEL_MODE_MONO:
 271                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 272                     return 29;
 273                 case BT_A2DP_CHANNEL_MODE_STEREO:
 274                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 275                     return 51;
 276                 default:
 277                     pa_log_warn("Invalid channel mode %u", mode);
 278                     return 51;
 279             }
 280         default:
 281             pa_log_warn("Invalid sampling freq %u", freq);
 282             return 53;
 283     }
 284 }
 285
 286 static int bt_a2dp_init(struct userdata *u) {
 287     sbc_capabilities_t *cap = &u->a2dp.sbc_capabilities;
 288     uint8_t max_bitpool, min_bitpool;
 289     unsigned i;
 290
 291     static const struct {
 292         uint32_t rate;
 293         uint8_t cap;
 294     } freq_table[] = {
 295         { 16000U, BT_SBC_SAMPLING_FREQ_16000 },
 296         { 32000U, BT_SBC_SAMPLING_FREQ_32000 },
 297         { 44100U, BT_SBC_SAMPLING_FREQ_44100 },
 298         { 48000U, BT_SBC_SAMPLING_FREQ_48000 }
 299     };
 300
 301     /* Find the lowest freq that is at least as high as the requested
 302      * sampling rate */
 303     for (i = 0; i < PA_ELEMENTSOF(freq_table); i++)
 304         if (freq_table[i].rate >= u->ss.rate || i == PA_ELEMENTSOF(freq_table)-1 ) {
 305             u->ss.rate = freq_table[i].rate;
 306             cap->frequency = freq_table[i].cap;
 307             break;
 308         }
 309
 310     if (u->ss.channels >= 2) {
 311         if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 312             cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
 313         else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 314             cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
 315         else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 316             cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
 317
 318         u->ss.channels = 2;
 319     } else {
 320         if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
 321             cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 322     }
 323
 324     if (!cap->channel_mode) {
 325         pa_log_error("No supported channel modes");
 326         return -1;
 327     }
 328
 329     if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
 330         cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
 331     else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
 332         cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
 333     else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
 334         cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
 335     else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
 336         cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
 337     else {
 338         pa_log_error("No supported block lengths");
 339         return -1;
 340     }
 341
 342     if (cap->subbands & BT_A2DP_SUBBANDS_8)
 343         cap->subbands = BT_A2DP_SUBBANDS_8;
 344     else if (cap->subbands & BT_A2DP_SUBBANDS_4)
 345         cap->subbands = BT_A2DP_SUBBANDS_4;
 346     else {
 347         pa_log_error("No supported subbands");
 348         return -1;
 349     }
 350
 351     if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
 352         cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
 353     else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
 354         cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
 355
 356     min_bitpool = (uint8_t) PA_MAX(MIN_BITPOOL, cap->min_bitpool);
 357     max_bitpool = (uint8_t) PA_MIN(default_bitpool(cap->frequency, cap->channel_mode), cap->max_bitpool);
 358
 359     cap->min_bitpool = (uint8_t) min_bitpool;
 360     cap->max_bitpool = (uint8_t) max_bitpool;
 361
 362     return 0;
 363 }
 364
 365 static void bt_a2dp_setup(struct bt_a2dp *a2dp) {
 366     sbc_capabilities_t active_capabilities = a2dp->sbc_capabilities;
 367
 368     if (a2dp->sbc_initialized)
 369         sbc_reinit(&a2dp->sbc, 0);
 370     else
 371         sbc_init(&a2dp->sbc, 0);
 372     a2dp->sbc_initialized = TRUE;
 373
 374     if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_16000)
 375         a2dp->sbc.frequency = SBC_FREQ_16000;
 376
 377     if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_32000)
 378         a2dp->sbc.frequency = SBC_FREQ_32000;
 379
 380     if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_44100)
 381         a2dp->sbc.frequency = SBC_FREQ_44100;
 382
 383     if (active_capabilities.frequency & BT_SBC_SAMPLING_FREQ_48000)
 384         a2dp->sbc.frequency = SBC_FREQ_48000;
 385
 386     if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_MONO)
 387         a2dp->sbc.mode = SBC_MODE_MONO;
 388
 389     if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 390         a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
 391
 392     if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 393         a2dp->sbc.mode = SBC_MODE_STEREO;
 394
 395     if (active_capabilities.channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 396         a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
 397
 398     a2dp->sbc.allocation = (uint8_t) (active_capabilities.allocation_method == BT_A2DP_ALLOCATION_SNR ? SBC_AM_SNR : SBC_AM_LOUDNESS);
 399
 400     switch (active_capabilities.subbands) {
 401         case BT_A2DP_SUBBANDS_4:
 402             a2dp->sbc.subbands = SBC_SB_4;
 403             break;
 404         case BT_A2DP_SUBBANDS_8:
 405             a2dp->sbc.subbands = SBC_SB_8;
 406             break;
 407     }
 408
 409     switch (active_capabilities.block_length) {
 410         case BT_A2DP_BLOCK_LENGTH_4:
 411             a2dp->sbc.blocks = SBC_BLK_4;
 412             break;
 413         case BT_A2DP_BLOCK_LENGTH_8:
 414             a2dp->sbc.blocks = SBC_BLK_8;
 415             break;
 416         case BT_A2DP_BLOCK_LENGTH_12:
 417             a2dp->sbc.blocks = SBC_BLK_12;
 418             break;
 419         case BT_A2DP_BLOCK_LENGTH_16:
 420             a2dp->sbc.blocks = SBC_BLK_16;
 421             break;
 422     }
 423
 424     a2dp->sbc.bitpool = active_capabilities.max_bitpool;
 425     a2dp->codesize = (uint16_t) sbc_get_codesize(&a2dp->sbc);
 426     a2dp->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
 427 }
 428
 429 static int bt_setconf(struct userdata *u) {
 430     int e;
 431     union {
 432         bt_audio_msg_header_t rsp;
 433         struct bt_set_configuration_req setconf_req;
 434         struct bt_set_configuration_rsp setconf_rsp;
 435         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 436     } msg;
 437
 438     if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 439         e = bt_a2dp_init(u);
 440         if (e < 0) {
 441             pa_log_error("a2dp_init error");
 442             return e;
 443         }
 444         u->ss.format = PA_SAMPLE_S16LE;
 445     }
 446     else
 447         u->ss.format = PA_SAMPLE_U8;
 448
 449     memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 450     msg.setconf_req.h.type = BT_REQUEST;
 451     msg.setconf_req.h.name = BT_SET_CONFIGURATION;
 452     msg.setconf_req.h.length = sizeof(msg.setconf_req);
 453
 454     strncpy(msg.setconf_req.device, u->addr, 18);
 455     msg.setconf_req.codec.transport = u->transport;
 456     if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP)
 457         memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities,
 458                 sizeof(u->a2dp.sbc_capabilities));
 459     msg.setconf_req.access_mode = BT_CAPABILITIES_ACCESS_MODE_WRITE;
 460
 461     e = bt_audioservice_send(u->audioservice_fd, &msg.setconf_req.h);
 462     if (e < 0) {
 463         pa_log_error("Failed to send BT_SETCONFIGURATION_REQ");
 464         return e;
 465     }
 466
 467     e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp, BT_SET_CONFIGURATION, sizeof(msg.setconf_rsp));
 468     if (e < 0) {
 469         pa_log_error("Failed to expect BT_SETCONFIGURATION_RSP");
 470         return e;
 471     }
 472
 473     u->transport = msg.setconf_rsp.transport;
 474     u->strtransport = (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP ? pa_xstrdup("A2DP") : pa_xstrdup("SCO"));
 475     u->link_mtu = msg.setconf_rsp.link_mtu;
 476
 477     /* setup SBC encoder now we agree on parameters */
 478     if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 479         bt_a2dp_setup(&u->a2dp);
 480         u->block_size = u->a2dp.codesize;
 481         pa_log_info("sbc parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
 482                 u->a2dp.sbc.allocation, u->a2dp.sbc.subbands, u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool);
 483     }
 484     else
 485         u->block_size = u->link_mtu;
 486
 487     return 0;
 488 }
 489
 490 static int bt_getstreamfd(struct userdata *u) {
 491     int e;
 492 //    uint32_t period_count = io->buffer_size / io->period_size;
 493     union {
 494         bt_audio_msg_header_t rsp;
 495         struct bt_start_stream_req start_req;
 496         struct bt_start_stream_rsp start_rsp;
 497         struct bt_new_stream_ind streamfd_ind;
 498         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 499     } msg;
 500
 501     memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 502     msg.start_req.h.type = BT_REQUEST;
 503     msg.start_req.h.name = BT_START_STREAM;
 504     msg.start_req.h.length = sizeof(msg.start_req);
 505
 506     e = bt_audioservice_send(u->audioservice_fd, &msg.start_req.h);
 507     if (e < 0) {
 508         pa_log_error("Failed to send BT_STREAMSTART_REQ");
 509         return e;
 510     }
 511
 512     e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp, BT_START_STREAM, sizeof(msg.start_rsp));
 513     if (e < 0) {
 514         pa_log_error("Failed to expect BT_STREAMSTART_RSP");
 515         return e;
 516     }
 517
 518     e = bt_audioservice_expect(u->audioservice_fd, &msg.rsp, BT_NEW_STREAM, sizeof(msg.streamfd_ind));
 519     if (e < 0) {
 520         pa_log_error("Failed to expect BT_STREAMFD_IND");
 521         return e;
 522     }
 523
 524     if (u->stream_fd >= 0)
 525         pa_close(u->stream_fd);
 526
 527     u->stream_fd = bt_audio_service_get_data_fd(u->audioservice_fd);
 528     if (u->stream_fd < 0) {
 529         pa_log_error("Failed to get data fd: %s (%d)",pa_cstrerror(errno), errno);
 530         return -errno;
 531     }
 532
 533 //   if (setsockopt(u->stream_fd, SOL_SCO, SCO_TXBUFS, &period_count, sizeof(period_count)) == 0)
 534 //       return 0;
 535 //   if (setsockopt(u->stream_fd, SOL_SCO, SO_SNDBUF, &period_count, sizeof(period_count)) == 0)
 536 //       return 0;
 537 //   /* FIXME : handle error codes */
 538     pa_make_fd_nonblock(u->stream_fd);
 539 //    pa_make_socket_low_delay(u->stream_fd);
 540
 541     return 0;
 542 }
 543
 544 static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offset, pa_memchunk *chunk) {
 545     struct userdata *u = PA_SINK(o)->userdata;
 546
 547     pa_log_debug("got message: %d", code);
 548     switch (code) {
 549
 550         case PA_SINK_MESSAGE_SET_STATE:
 551             switch ((pa_sink_state_t) PA_PTR_TO_UINT(data)) {
 552                 case PA_SINK_SUSPENDED:
 553                     pa_assert(PA_SINK_IS_OPENED(u->sink->thread_info.state));
 554                     pa_smoother_pause(u->smoother, pa_rtclock_usec());
 555                     break;
 556                 case PA_SINK_IDLE:
 557                 case PA_SINK_RUNNING:
 558                     if (u->sink->thread_info.state == PA_SINK_SUSPENDED)
 559                         pa_smoother_resume(u->smoother, pa_rtclock_usec());
 560                     break;
 561                 case PA_SINK_UNLINKED:
 562                 case PA_SINK_INIT:
 563                     ;
 564             }
 565             break;
 566
 567         case PA_SINK_MESSAGE_GET_LATENCY: {
 568             pa_usec_t w, r;
 569 /*             r = pa_smoother_get(u->smoother, pa_rtclock_usec()); */
 570 /* /\*             w = pa_bytes_to_usec(u->offset + (uint64_t) u->memchunk.length, &u->sink->sample_spec); *\/ */
 571             *((pa_usec_t*) data) = /*w > r ? w - r :*/ 0;
 572             return 0;
 573         }
 574
 575     }
 576
 577     return pa_sink_process_msg(o, code, data, offset, chunk);
 578 }
 579
 580 static int sco_process_render(struct userdata *u) {
 581     void *p;
 582     int ret = 0;
 583     pa_memchunk memchunk;
 584
 585     pa_sink_render_full(u->sink, u->block_size, &memchunk);
 586
 587     p = pa_memblock_acquire(memchunk.memblock);
 588
 589     for (;;) {
 590         ssize_t l;
 591
 592         l = pa_loop_write(u->stream_fd, (uint8_t*) p, memchunk.length, NULL);
 593         pa_log_debug("Memblock written to socket: %li bytes", (long) l);
 594
 595         pa_assert(l != 0);
 596
 597         if (l > 0) {
 598             u->offset += (uint64_t) l;
 599             break;
 600         }
 601
 602         if (errno == EINTR)
 603             pa_log_debug("EINTR");
 604         else if (errno == EAGAIN)
 605             pa_log_debug("EAGAIN");
 606         else {
 607             pa_log_error("Failed to write data to FIFO: %s", pa_cstrerror(errno));
 608             ret = -1;
 609             break;
 610         }
 611     }
 612
 613     pa_memblock_release(memchunk.memblock);
 614     pa_memblock_unref(memchunk.memblock);
 615
 616     return ret;
 617 }
 618
 619 static int a2dp_process_render(struct userdata *u) {
 620     int written;
 621
 622     struct bt_a2dp *a2dp = &u->a2dp;
 623     struct rtp_header *header = (void *) a2dp->buffer;
 624     struct rtp_payload *payload = (void *) (a2dp->buffer + sizeof(*header));
 625
 626     pa_assert(u);
 627
 628     do {
 629         /* Render some data */
 630         int frame_size, encoded;
 631         void *p;
 632         pa_memchunk memchunk;
 633
 634         pa_sink_render_full(u->sink, u->block_size, &memchunk);
 635
 636         p = pa_memblock_acquire(memchunk.memblock);
 637
 638         frame_size = (uint16_t) sbc_get_frame_length(&a2dp->sbc);
 639         pa_log_debug("SBC frame_size: %d", frame_size);
 640
 641         encoded = sbc_encode(&a2dp->sbc, p, (int) a2dp->codesize, a2dp->buffer + a2dp->count,
 642                              (int) (sizeof(a2dp->buffer) - a2dp->count), &written);
 643         pa_log_debug("SBC: encoded: %d; written: %d", encoded, written);
 644
 645         pa_memblock_release(memchunk.memblock);
 646         pa_memblock_unref(memchunk.memblock);
 647
 648         if (encoded <= 0) {
 649             pa_log_error("SBC encoding error (%d)", encoded);
 650             return -1;
 651         }
 652
 653         a2dp->count += (size_t) written;
 654         a2dp->frame_count++;
 655         a2dp->samples += (unsigned) encoded / frame_size;
 656         a2dp->total_samples += (unsigned) encoded / frame_size;
 657
 658     } while (a2dp->count + (size_t) written <= u->link_mtu);
 659
 660     /* write it to the fifo */
 661     memset(a2dp->buffer, 0, sizeof(*header) + sizeof(*payload));
 662     payload->frame_count = a2dp->frame_count;
 663     header->v = 2;
 664     header->pt = 1;
 665     header->sequence_number = htons(a2dp->seq_num);
 666     header->timestamp = htonl(a2dp->total_samples);
 667     header->ssrc = htonl(1);
 668
 669     for (;;) {
 670         ssize_t l;
 671
 672         l = pa_loop_write(u->stream_fd, a2dp->buffer, a2dp->count, NULL);
 673         pa_log_debug("avdtp_write: requested %lu bytes; written %li bytes", (unsigned long) a2dp->count, (long) l);
 674
 675         pa_assert(l != 0);
 676
 677         if (l > 0)
 678             break;
 679
 680         if (errno == EINTR)
 681             pa_log_debug("EINTR");
 682         else if (errno == EAGAIN)
 683             pa_log_debug("EAGAIN");
 684         else {
 685             pa_log_error("Failed to write data to FIFO: %s", pa_cstrerror(errno));
 686             return -1;
 687         }
 688     }
 689
 690     u->offset += a2dp->codesize*a2dp->frame_count;
 691
 692     /* Reset buffer of data to send */
 693     a2dp->count = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
 694     a2dp->frame_count = 0;
 695     a2dp->samples = 0;
 696     a2dp->seq_num++;
 697
 698     return 0;
 699 }
 700
 701 static void thread_func(void *userdata) {
 702     struct userdata *u = userdata;
 703
 704     pa_assert(u);
 705
 706     pa_log_debug("IO Thread starting up");
 707
 708     if (u->core->realtime_scheduling)
 709         pa_make_realtime(u->core->realtime_priority);
 710
 711     pa_thread_mq_install(&u->thread_mq);
 712     pa_rtpoll_install(u->rtpoll);
 713
 714     pa_smoother_set_time_offset(u->smoother, pa_rtclock_usec());
 715
 716     for (;;) {
 717         int ret, l;
 718         struct pollfd *pollfd;
 719         uint64_t n;
 720         pa_usec_t usec;
 721
 722         if (PA_SINK_IS_OPENED(u->sink->thread_info.state))
 723             if (u->sink->thread_info.rewind_requested)
 724                 pa_sink_process_rewind(u->sink, 0);
 725
 726         pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL);
 727
 728         if (PA_SINK_IS_OPENED(u->sink->thread_info.state) && pollfd->revents) {
 729             if (u->transport == BT_CAPABILITIES_TRANSPORT_A2DP) {
 730                 if ((l = a2dp_process_render(u)) < 0)
 731                     goto fail;
 732             } else {
 733                 if ((l = sco_process_render(u)) < 0)
 734                     goto fail;
 735             }
 736             pollfd->revents = 0;
 737
 738             /* feed the time smoother */
 739             n = u->offset;
 740             if (ioctl(u->stream_fd, SIOCOUTQ, &l) >= 0 && l > 0)
 741                 n -= (uint64_t) l;
 742             usec = pa_bytes_to_usec(n, &u->sink->sample_spec);
 743             if (usec > u->latency)
 744                 usec -= u->latency;
 745             else
 746                 usec = 0;
 747             pa_smoother_put(u->smoother, pa_rtclock_usec(), usec);
 748         }
 749
 750         /* Hmm, nothing to do. Let's sleep */
 751         pa_log_debug("IO thread going to sleep");
 752         pollfd->events = (short) (PA_SINK_IS_OPENED(u->sink->thread_info.state) ? POLLOUT : 0);
 753         if ((ret = pa_rtpoll_run(u->rtpoll, TRUE)) < 0) {
 754             pa_log_error("rtpoll_run < 0");
 755             goto fail;
 756         }
 757         pa_log_debug("IO thread waking up");
 758
 759         if (ret == 0) {
 760             pa_log_debug("rtpoll_run == 0");
 761             goto finish;
 762         }
 763
 764         pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL);
 765         if (pollfd->revents & ~POLLOUT) {
 766             pa_log_error("FIFO shutdown.");
 767             goto fail;
 768         }
 769     }
 770
 771 fail:
 772     /* If this was no regular exit from the loop we have to continue processing messages until we receive PA_MESSAGE_SHUTDOWN */
 773     pa_log_debug("IO thread failed");
 774     pa_asyncmsgq_post(u->thread_mq.outq, PA_MSGOBJECT(u->core), PA_CORE_MESSAGE_UNLOAD_MODULE, u->module, 0, NULL, NULL);
 775     pa_asyncmsgq_wait_for(u->thread_mq.inq, PA_MESSAGE_SHUTDOWN);
 776
 777 finish:
 778     pa_log_debug("IO thread shutting down");
 779 }
 780
 781 int pa__init(pa_module* m) {
 782     int e;
 783     pa_modargs *ma;
 784     uint32_t channels;
 785     pa_sink_new_data data;
 786     struct pollfd *pollfd;
 787     struct userdata *u;
 788
 789     pa_assert(m);
 790     m->userdata = u = pa_xnew0(struct userdata, 1);
 791     u->module = m;
 792     u->core = m->core;
 793     u->audioservice_fd = -1;
 794     u->stream_fd = -1;
 795     u->transport = (uint8_t) -1;
 796     u->offset = 0;
 797     u->latency = 0;
 798     u->a2dp.sbc_initialized = FALSE;
 799     u->smoother = pa_smoother_new(PA_USEC_PER_SEC, PA_USEC_PER_SEC*2, TRUE, 10);
 800     u->rtpoll = pa_rtpoll_new();
 801     pa_thread_mq_init(&u->thread_mq, u->core->mainloop, u->rtpoll);
 802     u->rtpoll_item = NULL;
 803     u->ss = m->core->default_sample_spec;
 804
 805     if (!(ma = pa_modargs_new(m->argument, valid_modargs))) {
 806         pa_log_error("Failed to parse module arguments");
 807         goto fail;
 808     }
 809     if (!(u->name = pa_xstrdup(pa_modargs_get_value(ma, "sink_name", DEFAULT_SINK_NAME)))) {
 810         pa_log_error("Failed to get device name from module arguments");
 811         goto fail;
 812     }
 813     if (!(u->addr = pa_xstrdup(pa_modargs_get_value(ma, "address", NULL)))) {
 814         pa_log_error("Failed to get device address from module arguments");
 815         goto fail;
 816     }
 817     if (!(u->profile = pa_xstrdup(pa_modargs_get_value(ma, "profile", NULL)))) {
 818         pa_log_error("Failed to get profile from module arguments");
 819         goto fail;
 820     }
 821     if (pa_modargs_get_value_u32(ma, "rate", &u->ss.rate) < 0) {
 822         pa_log_error("Failed to get rate from module arguments");
 823         goto fail;
 824     }
 825
 826     channels = u->ss.channels;
 827     if (pa_modargs_get_value_u32(ma, "channels", &channels) < 0) {
 828         pa_log_error("Failed to get channels from module arguments");
 829         goto fail;
 830     }
 831     u->ss.channels = (uint8_t) channels;
 832
 833     /* connect to the bluez audio service */
 834     u->audioservice_fd = bt_audio_service_open();
 835     if (u->audioservice_fd <= 0) {
 836         pa_log_error("Couldn't connect to bluetooth audio service");
 837         goto fail;
 838     }
 839     pa_log_debug("Connected to the bluetooth audio service");
 840
 841     /* queries device capabilities */
 842     e = bt_getcaps(u);
 843     if (e < 0) {
 844         pa_log_error("Failed to get device capabilities");
 845         goto fail;
 846     }
 847     pa_log_debug("Got device capabilities");
 848
 849     /* configures the connection */
 850     e = bt_setconf(u);
 851     if (e < 0) {
 852         pa_log_error("Failed to set config");
 853         goto fail;
 854     }
 855     pa_log_debug("Connection to the device configured");
 856
 857     /* gets the device socket */
 858     e = bt_getstreamfd(u);
 859     if (e < 0) {
 860         pa_log_error("Failed to get stream fd (%d)", e);
 861         goto fail;
 862     }
 863     pa_log_debug("Got the device socket");
 864
 865     /* create sink */
 866     pa_sink_new_data_init(&data);
 867     data.driver = __FILE__;
 868     data.module = m;
 869     pa_sink_new_data_set_name(&data, u->name);
 870     pa_sink_new_data_set_sample_spec(&data, &u->ss);
 871     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_STRING, u->name);
 872     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_DESCRIPTION, "Bluetooth %s '%s' (%s)", u->strtransport, u->name, u->addr);
 873     pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile);
 874     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_API, "bluez");
 875     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_CLASS, "sound");
 876     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_CONNECTOR, "bluetooth");
 877 /*     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_FORM_FACTOR, "headset"); /\*FIXME*\/ */
 878 /*     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_VENDOR_PRODUCT_ID, "product_id"); /\*FIXME*\/ */
 879 /*     pa_proplist_setf(data.proplist, PA_PROP_DEVICE_SERIAL, "serial"); /\*FIXME*\/ */
 880     u->sink = pa_sink_new(m->core, &data, PA_SINK_HARDWARE|PA_SINK_LATENCY);
 881     pa_sink_new_data_done(&data);
 882     if (!u->sink) {
 883         pa_log_error("Failed to create sink");
 884         goto fail;
 885     }
 886     u->sink->userdata = u;
 887     u->sink->parent.process_msg = sink_process_msg;
 888     pa_sink_set_asyncmsgq(u->sink, u->thread_mq.inq);
 889     pa_sink_set_rtpoll(u->sink, u->rtpoll);
 890
 891     u->rtpoll_item = pa_rtpoll_item_new(u->rtpoll, PA_RTPOLL_NEVER, 1);
 892     pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL);
 893     pollfd->fd = u->stream_fd;
 894     pollfd->events = pollfd->revents = 0;
 895
 896     /* start rt thread */
 897     if (!(u->thread = pa_thread_new(thread_func, u))) {
 898         pa_log_error("Failed to create IO thread");
 899         goto fail;
 900     }
 901     pa_sink_put(u->sink);
 902
 903     pa_modargs_free(ma);
 904     return 0;
 905
 906 fail:
 907     if (ma)
 908         pa_modargs_free(ma);
 909
 910     pa__done(m);
 911     return -1;
 912 }
 913
 914 void pa__done(pa_module *m) {
 915     struct userdata *u;
 916     pa_assert(m);
 917
 918     if (!(u = m->userdata))
 919         return;
 920
 921     if (u->sink)
 922         pa_sink_unlink(u->sink);
 923
 924     if (u->thread) {
 925         pa_asyncmsgq_send(u->thread_mq.inq, NULL, PA_MESSAGE_SHUTDOWN, NULL, 0, NULL);
 926         pa_thread_free(u->thread);
 927     }
 928
 929     if (u->sink)
 930         pa_sink_unref(u->sink);
 931
 932     pa_thread_mq_done(&u->thread_mq);
 933
 934     if (u->rtpoll_item)
 935         pa_rtpoll_item_free(u->rtpoll_item);
 936
 937     if (u->rtpoll)
 938         pa_rtpoll_free(u->rtpoll);
 939
 940     if (u->smoother)
 941         pa_smoother_free(u->smoother);
 942
 943     pa_xfree(u->name);
 944     pa_xfree(u->addr);
 945     pa_xfree(u->profile);
 946     pa_xfree(u->strtransport);
 947
 948     if (u->stream_fd >= 0)
 949         pa_close(u->stream_fd);
 950
 951     if (u->audioservice_fd >= 0)
 952         pa_close(u->audioservice_fd);
 953
 954     pa_xfree(u);
 955 }