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