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

   1 /***
   2   This file is part of PulseAudio.
   3
   4   Copyright 2008-2009 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
   8   published by the Free Software Foundation; either version 2.1 of the
   9   License, 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
  17   License 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/i18n.h>
  34 #include <pulse/rtclock.h>
  35 #include <pulse/sample.h>
  36 #include <pulse/timeval.h>
  37 #include <pulse/xmalloc.h>
  38
  39 #include <pulsecore/module.h>
  40 #include <pulsecore/modargs.h>
  41 #include <pulsecore/core-rtclock.h>
  42 #include <pulsecore/core-util.h>
  43 #include <pulsecore/core-error.h>
  44 #include <pulsecore/socket-util.h>
  45 #include <pulsecore/thread.h>
  46 #include <pulsecore/thread-mq.h>
  47 #include <pulsecore/rtpoll.h>
  48 #include <pulsecore/time-smoother.h>
  49 #include <pulsecore/namereg.h>
  50 #include <pulsecore/dbus-shared.h>
  51
  52 #include "module-bluetooth-device-symdef.h"
  53 #include "ipc.h"
  54 #include "sbc.h"
  55 #include "rtp.h"
  56 #include "bluetooth-util.h"
  57
  58 #define MAX_BITPOOL 64
  59 #define MIN_BITPOOL 2U
  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         "name=<name for the card/sink/source, to be prefixed> "
  67         "card_name=<name for the card> "
  68         "card_properties=<properties for the card> "
  69         "sink_name=<name for the sink> "
  70         "sink_properties=<properties for the sink> "
  71         "source_name=<name for the source> "
  72         "source_properties=<properties for the source> "
  73         "address=<address of the device> "
  74         "profile=<a2dp|hsp> "
  75         "rate=<sample rate> "
  76         "channels=<number of channels> "
  77         "path=<device object path> "
  78         "auto_connect=<automatically connect?>");
  79
  80 /*
  81 #ifdef NOKIA
  82         "sco_sink=<SCO over PCM sink name> "
  83         "sco_source=<SCO over PCM source name>"
  84 #endif
  85 */
  86
  87 /* TODO: not close fd when entering suspend mode in a2dp */
  88
  89 static const char* const valid_modargs[] = {
  90     "name",
  91     "card_name",
  92     "card_properties",
  93     "sink_name",
  94     "sink_properties",
  95     "source_name",
  96     "source_properties",
  97     "address",
  98     "profile",
  99     "rate",
 100     "channels",
 101     "path",
 102     "auto_connect",
 103 #ifdef NOKIA
 104     "sco_sink",
 105     "sco_source",
 106 #endif
 107     NULL
 108 };
 109
 110 struct a2dp_info {
 111     sbc_capabilities_t sbc_capabilities;
 112     sbc_t sbc;                           /* Codec data */
 113     pa_bool_t sbc_initialized;           /* Keep track if the encoder is initialized */
 114     size_t codesize, frame_length;       /* SBC Codesize, frame_length. We simply cache those values here */
 115
 116     void* buffer;                        /* Codec transfer buffer */
 117     size_t buffer_size;                  /* Size of the buffer */
 118
 119     uint16_t seq_num;                    /* Cumulative packet sequence */
 120 };
 121
 122 struct hsp_info {
 123     pcm_capabilities_t pcm_capabilities;
 124 #ifdef NOKIA
 125     pa_sink *sco_sink;
 126     pa_source *sco_source;
 127 #endif
 128     pa_hook_slot *sink_state_changed_slot;
 129     pa_hook_slot *source_state_changed_slot;
 130 };
 131
 132 enum profile {
 133     PROFILE_A2DP,
 134     PROFILE_A2DP_SOURCE,
 135     PROFILE_HSP,
 136     PROFILE_OFF
 137 };
 138
 139 struct userdata {
 140     pa_core *core;
 141     pa_module *module;
 142
 143     char *address;
 144     char *path;
 145     pa_bluetooth_discovery *discovery;
 146     pa_bool_t auto_connect;
 147
 148     pa_dbus_connection *connection;
 149
 150     pa_card *card;
 151     pa_sink *sink;
 152     pa_source *source;
 153
 154     pa_thread_mq thread_mq;
 155     pa_rtpoll *rtpoll;
 156     pa_rtpoll_item *rtpoll_item;
 157     pa_thread *thread;
 158
 159     uint64_t read_index, write_index;
 160     pa_usec_t started_at;
 161     pa_smoother *read_smoother;
 162
 163     pa_memchunk write_memchunk;
 164
 165     pa_sample_spec sample_spec, requested_sample_spec;
 166
 167     int service_fd;
 168     int stream_fd;
 169
 170     size_t link_mtu;
 171     size_t block_size;
 172
 173     struct a2dp_info a2dp;
 174     struct hsp_info hsp;
 175
 176     enum profile profile;
 177
 178     pa_modargs *modargs;
 179
 180     int stream_write_type;
 181     int service_write_type, service_read_type;
 182
 183     pa_bool_t filter_added;
 184 };
 185
 186 #define FIXED_LATENCY_PLAYBACK_A2DP (25*PA_USEC_PER_MSEC)
 187 #define FIXED_LATENCY_RECORD_A2DP (25*PA_USEC_PER_MSEC)
 188 #define FIXED_LATENCY_PLAYBACK_HSP (125*PA_USEC_PER_MSEC)
 189 #define FIXED_LATENCY_RECORD_HSP (25*PA_USEC_PER_MSEC)
 190
 191 #define MAX_PLAYBACK_CATCH_UP_USEC (100*PA_USEC_PER_MSEC)
 192
 193 #ifdef NOKIA
 194 #define USE_SCO_OVER_PCM(u) (u->profile == PROFILE_HSP && (u->hsp.sco_sink && u->hsp.sco_source))
 195 #endif
 196
 197 static int init_bt(struct userdata *u);
 198 static int init_profile(struct userdata *u);
 199
 200 static int service_send(struct userdata *u, const bt_audio_msg_header_t *msg) {
 201     ssize_t r;
 202
 203     pa_assert(u);
 204     pa_assert(u->service_fd >= 0);
 205     pa_assert(msg);
 206     pa_assert(msg->length > 0);
 207
 208     pa_log_debug("Sending %s -> %s",
 209                  pa_strnull(bt_audio_strtype(msg->type)),
 210                  pa_strnull(bt_audio_strname(msg->name)));
 211
 212     if ((r = pa_loop_write(u->service_fd, msg, msg->length, &u->service_write_type)) == (ssize_t) msg->length)
 213         return 0;
 214
 215     if (r < 0)
 216         pa_log_error("Error sending data to audio service: %s", pa_cstrerror(errno));
 217     else
 218         pa_log_error("Short write()");
 219
 220     return -1;
 221 }
 222
 223 static int service_recv(struct userdata *u, bt_audio_msg_header_t *msg, size_t room) {
 224     ssize_t r;
 225
 226     pa_assert(u);
 227     pa_assert(u->service_fd >= 0);
 228     pa_assert(msg);
 229     pa_assert(room >= sizeof(*msg));
 230
 231     pa_log_debug("Trying to receive message from audio service...");
 232
 233     /* First, read the header */
 234     if ((r = pa_loop_read(u->service_fd, msg, sizeof(*msg), &u->service_read_type)) != sizeof(*msg))
 235         goto read_fail;
 236
 237     if (msg->length < sizeof(*msg)) {
 238         pa_log_error("Invalid message size.");
 239         return -1;
 240     }
 241
 242     if (msg->length > room) {
 243         pa_log_error("Not enough room.");
 244         return -1;
 245     }
 246
 247     /* Secondly, read the payload */
 248     if (msg->length > sizeof(*msg)) {
 249
 250         size_t remains = msg->length - sizeof(*msg);
 251
 252         if ((r = pa_loop_read(u->service_fd,
 253                               (uint8_t*) msg + sizeof(*msg),
 254                               remains,
 255                               &u->service_read_type)) != (ssize_t) remains)
 256             goto read_fail;
 257     }
 258
 259     pa_log_debug("Received %s <- %s",
 260                  pa_strnull(bt_audio_strtype(msg->type)),
 261                  pa_strnull(bt_audio_strname(msg->name)));
 262
 263     return 0;
 264
 265 read_fail:
 266
 267     if (r < 0)
 268         pa_log_error("Error receiving data from audio service: %s", pa_cstrerror(errno));
 269     else
 270         pa_log_error("Short read()");
 271
 272     return -1;
 273 }
 274
 275 static ssize_t service_expect(struct userdata*u, bt_audio_msg_header_t *rsp, size_t room, uint8_t expected_name, size_t expected_size) {
 276     int r;
 277
 278     pa_assert(u);
 279     pa_assert(u->service_fd >= 0);
 280     pa_assert(rsp);
 281
 282     if ((r = service_recv(u, rsp, room)) < 0)
 283         return r;
 284
 285     if ((rsp->type != BT_INDICATION && rsp->type != BT_RESPONSE) ||
 286         rsp->name != expected_name ||
 287         (expected_size > 0 && rsp->length != expected_size)) {
 288
 289         if (rsp->type == BT_ERROR && rsp->length == sizeof(bt_audio_error_t))
 290             pa_log_error("Received error condition: %s", pa_cstrerror(((bt_audio_error_t*) rsp)->posix_errno));
 291         else
 292             pa_log_error("Bogus message %s received while %s was expected",
 293                          pa_strnull(bt_audio_strname(rsp->name)),
 294                          pa_strnull(bt_audio_strname(expected_name)));
 295         return -1;
 296     }
 297
 298     return 0;
 299 }
 300
 301 /* Run from main thread */
 302 static int parse_caps(struct userdata *u, uint8_t seid, const struct bt_get_capabilities_rsp *rsp) {
 303     uint16_t bytes_left;
 304     const codec_capabilities_t *codec;
 305
 306     pa_assert(u);
 307     pa_assert(rsp);
 308
 309     bytes_left = rsp->h.length - sizeof(*rsp);
 310
 311     if (bytes_left < sizeof(codec_capabilities_t)) {
 312         pa_log_error("Packet too small to store codec information.");
 313         return -1;
 314     }
 315
 316     codec = (codec_capabilities_t *) rsp->data; /** ALIGNMENT? **/
 317
 318     pa_log_debug("Payload size is %lu %lu", (unsigned long) bytes_left, (unsigned long) sizeof(*codec));
 319
 320     if (((u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) && codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP) ||
 321         (u->profile == PROFILE_HSP && codec->transport != BT_CAPABILITIES_TRANSPORT_SCO)) {
 322         pa_log_error("Got capabilities for wrong codec.");
 323         return -1;
 324     }
 325
 326     if (u->profile == PROFILE_HSP) {
 327
 328         if (bytes_left <= 0 || codec->length != sizeof(u->hsp.pcm_capabilities))
 329             return -1;
 330
 331         pa_assert(codec->type == BT_HFP_CODEC_PCM);
 332
 333         if (codec->configured && seid == 0)
 334             return codec->seid;
 335
 336         memcpy(&u->hsp.pcm_capabilities, codec, sizeof(u->hsp.pcm_capabilities));
 337
 338     } else if (u->profile == PROFILE_A2DP) {
 339
 340         while (bytes_left > 0) {
 341             if ((codec->type == BT_A2DP_SBC_SINK) && !codec->lock)
 342                 break;
 343
 344             bytes_left -= codec->length;
 345             codec = (const codec_capabilities_t*) ((const uint8_t*) codec + codec->length);
 346         }
 347
 348         if (bytes_left <= 0 || codec->length != sizeof(u->a2dp.sbc_capabilities))
 349             return -1;
 350
 351         pa_assert(codec->type == BT_A2DP_SBC_SINK);
 352
 353         if (codec->configured && seid == 0)
 354             return codec->seid;
 355
 356         memcpy(&u->a2dp.sbc_capabilities, codec, sizeof(u->a2dp.sbc_capabilities));
 357
 358     } else if (u->profile == PROFILE_A2DP_SOURCE) {
 359
 360         while (bytes_left > 0) {
 361             if ((codec->type == BT_A2DP_SBC_SOURCE) && !codec->lock)
 362                 break;
 363
 364             bytes_left -= codec->length;
 365             codec = (const codec_capabilities_t*) ((const uint8_t*) codec + codec->length);
 366         }
 367
 368         if (bytes_left <= 0 || codec->length != sizeof(u->a2dp.sbc_capabilities))
 369             return -1;
 370
 371         pa_assert(codec->type == BT_A2DP_SBC_SOURCE);
 372
 373         if (codec->configured && seid == 0)
 374             return codec->seid;
 375
 376         memcpy(&u->a2dp.sbc_capabilities, codec, sizeof(u->a2dp.sbc_capabilities));
 377     }
 378
 379     return 0;
 380 }
 381
 382 /* Run from main thread */
 383 static int get_caps(struct userdata *u, uint8_t seid) {
 384     union {
 385         struct bt_get_capabilities_req getcaps_req;
 386         struct bt_get_capabilities_rsp getcaps_rsp;
 387         bt_audio_error_t error;
 388         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 389     } msg;
 390     int ret;
 391
 392     pa_assert(u);
 393
 394     memset(&msg, 0, sizeof(msg));
 395     msg.getcaps_req.h.type = BT_REQUEST;
 396     msg.getcaps_req.h.name = BT_GET_CAPABILITIES;
 397     msg.getcaps_req.h.length = sizeof(msg.getcaps_req);
 398     msg.getcaps_req.seid = seid;
 399
 400     pa_strlcpy(msg.getcaps_req.object, u->path, sizeof(msg.getcaps_req.object));
 401     if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE)
 402         msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
 403     else {
 404         pa_assert(u->profile == PROFILE_HSP);
 405         msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_SCO;
 406     }
 407     msg.getcaps_req.flags = u->auto_connect ? BT_FLAG_AUTOCONNECT : 0;
 408
 409     if (service_send(u, &msg.getcaps_req.h) < 0)
 410         return -1;
 411
 412     if (service_expect(u, &msg.getcaps_rsp.h, sizeof(msg), BT_GET_CAPABILITIES, 0) < 0)
 413         return -1;
 414
 415     ret = parse_caps(u, seid, &msg.getcaps_rsp);
 416     if (ret <= 0)
 417         return ret;
 418
 419     return get_caps(u, ret);
 420 }
 421
 422 /* Run from main thread */
 423 static uint8_t a2dp_default_bitpool(uint8_t freq, uint8_t mode) {
 424
 425     switch (freq) {
 426         case BT_SBC_SAMPLING_FREQ_16000:
 427         case BT_SBC_SAMPLING_FREQ_32000:
 428             return 53;
 429
 430         case BT_SBC_SAMPLING_FREQ_44100:
 431
 432             switch (mode) {
 433                 case BT_A2DP_CHANNEL_MODE_MONO:
 434                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 435                     return 31;
 436
 437                 case BT_A2DP_CHANNEL_MODE_STEREO:
 438                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 439                     return 53;
 440
 441                 default:
 442                     pa_log_warn("Invalid channel mode %u", mode);
 443                     return 53;
 444             }
 445
 446         case BT_SBC_SAMPLING_FREQ_48000:
 447
 448             switch (mode) {
 449                 case BT_A2DP_CHANNEL_MODE_MONO:
 450                 case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 451                     return 29;
 452
 453                 case BT_A2DP_CHANNEL_MODE_STEREO:
 454                 case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 455                     return 51;
 456
 457                 default:
 458                     pa_log_warn("Invalid channel mode %u", mode);
 459                     return 51;
 460             }
 461
 462         default:
 463             pa_log_warn("Invalid sampling freq %u", freq);
 464             return 53;
 465     }
 466 }
 467
 468 /* Run from main thread */
 469 static int setup_a2dp(struct userdata *u) {
 470     sbc_capabilities_t *cap;
 471     int i;
 472
 473     static const struct {
 474         uint32_t rate;
 475         uint8_t cap;
 476     } freq_table[] = {
 477         { 16000U, BT_SBC_SAMPLING_FREQ_16000 },
 478         { 32000U, BT_SBC_SAMPLING_FREQ_32000 },
 479         { 44100U, BT_SBC_SAMPLING_FREQ_44100 },
 480         { 48000U, BT_SBC_SAMPLING_FREQ_48000 }
 481     };
 482
 483     pa_assert(u);
 484     pa_assert(u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE);
 485
 486     cap = &u->a2dp.sbc_capabilities;
 487
 488     /* Find the lowest freq that is at least as high as the requested
 489      * sampling rate */
 490     for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
 491         if (freq_table[i].rate >= u->sample_spec.rate && (cap->frequency & freq_table[i].cap)) {
 492             u->sample_spec.rate = freq_table[i].rate;
 493             cap->frequency = freq_table[i].cap;
 494             break;
 495         }
 496
 497     if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
 498         for (--i; i >= 0; i--) {
 499             if (cap->frequency & freq_table[i].cap) {
 500                 u->sample_spec.rate = freq_table[i].rate;
 501                 cap->frequency = freq_table[i].cap;
 502                 break;
 503             }
 504         }
 505
 506         if (i < 0) {
 507             pa_log("Not suitable sample rate");
 508             return -1;
 509         }
 510     }
 511
 512     pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
 513
 514     if (cap->capability.configured)
 515         return 0;
 516
 517     if (u->sample_spec.channels <= 1) {
 518         if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
 519             cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 520             u->sample_spec.channels = 1;
 521         } else
 522             u->sample_spec.channels = 2;
 523     }
 524
 525     if (u->sample_spec.channels >= 2) {
 526         u->sample_spec.channels = 2;
 527
 528         if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
 529             cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
 530         else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
 531             cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
 532         else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
 533             cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
 534         else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
 535             cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
 536             u->sample_spec.channels = 1;
 537         } else {
 538             pa_log("No supported channel modes");
 539             return -1;
 540         }
 541     }
 542
 543     if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
 544         cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
 545     else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
 546         cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
 547     else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
 548         cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
 549     else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
 550         cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
 551     else {
 552         pa_log_error("No supported block lengths");
 553         return -1;
 554     }
 555
 556     if (cap->subbands & BT_A2DP_SUBBANDS_8)
 557         cap->subbands = BT_A2DP_SUBBANDS_8;
 558     else if (cap->subbands & BT_A2DP_SUBBANDS_4)
 559         cap->subbands = BT_A2DP_SUBBANDS_4;
 560     else {
 561         pa_log_error("No supported subbands");
 562         return -1;
 563     }
 564
 565     if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
 566         cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
 567     else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
 568         cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
 569
 570     cap->min_bitpool = (uint8_t) PA_MAX(MIN_BITPOOL, cap->min_bitpool);
 571     cap->max_bitpool = (uint8_t) PA_MIN(a2dp_default_bitpool(cap->frequency, cap->channel_mode), cap->max_bitpool);
 572
 573     return 0;
 574 }
 575
 576 /* Run from main thread */
 577 static void setup_sbc(struct a2dp_info *a2dp) {
 578     sbc_capabilities_t *active_capabilities;
 579
 580     pa_assert(a2dp);
 581
 582     active_capabilities = &a2dp->sbc_capabilities;
 583
 584     if (a2dp->sbc_initialized)
 585         sbc_reinit(&a2dp->sbc, 0);
 586     else
 587         sbc_init(&a2dp->sbc, 0);
 588     a2dp->sbc_initialized = TRUE;
 589
 590     switch (active_capabilities->frequency) {
 591         case BT_SBC_SAMPLING_FREQ_16000:
 592             a2dp->sbc.frequency = SBC_FREQ_16000;
 593             break;
 594         case BT_SBC_SAMPLING_FREQ_32000:
 595             a2dp->sbc.frequency = SBC_FREQ_32000;
 596             break;
 597         case BT_SBC_SAMPLING_FREQ_44100:
 598             a2dp->sbc.frequency = SBC_FREQ_44100;
 599             break;
 600         case BT_SBC_SAMPLING_FREQ_48000:
 601             a2dp->sbc.frequency = SBC_FREQ_48000;
 602             break;
 603         default:
 604             pa_assert_not_reached();
 605     }
 606
 607     switch (active_capabilities->channel_mode) {
 608         case BT_A2DP_CHANNEL_MODE_MONO:
 609             a2dp->sbc.mode = SBC_MODE_MONO;
 610             break;
 611         case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
 612             a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
 613             break;
 614         case BT_A2DP_CHANNEL_MODE_STEREO:
 615             a2dp->sbc.mode = SBC_MODE_STEREO;
 616             break;
 617         case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
 618             a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
 619             break;
 620         default:
 621             pa_assert_not_reached();
 622     }
 623
 624     switch (active_capabilities->allocation_method) {
 625         case BT_A2DP_ALLOCATION_SNR:
 626             a2dp->sbc.allocation = SBC_AM_SNR;
 627             break;
 628         case BT_A2DP_ALLOCATION_LOUDNESS:
 629             a2dp->sbc.allocation = SBC_AM_LOUDNESS;
 630             break;
 631         default:
 632             pa_assert_not_reached();
 633     }
 634
 635     switch (active_capabilities->subbands) {
 636         case BT_A2DP_SUBBANDS_4:
 637             a2dp->sbc.subbands = SBC_SB_4;
 638             break;
 639         case BT_A2DP_SUBBANDS_8:
 640             a2dp->sbc.subbands = SBC_SB_8;
 641             break;
 642         default:
 643             pa_assert_not_reached();
 644     }
 645
 646     switch (active_capabilities->block_length) {
 647         case BT_A2DP_BLOCK_LENGTH_4:
 648             a2dp->sbc.blocks = SBC_BLK_4;
 649             break;
 650         case BT_A2DP_BLOCK_LENGTH_8:
 651             a2dp->sbc.blocks = SBC_BLK_8;
 652             break;
 653         case BT_A2DP_BLOCK_LENGTH_12:
 654             a2dp->sbc.blocks = SBC_BLK_12;
 655             break;
 656         case BT_A2DP_BLOCK_LENGTH_16:
 657             a2dp->sbc.blocks = SBC_BLK_16;
 658             break;
 659         default:
 660             pa_assert_not_reached();
 661     }
 662
 663     a2dp->sbc.bitpool = active_capabilities->max_bitpool;
 664     a2dp->codesize = sbc_get_codesize(&a2dp->sbc);
 665     a2dp->frame_length = sbc_get_frame_length(&a2dp->sbc);
 666 }
 667
 668 /* Run from main thread */
 669 static int set_conf(struct userdata *u) {
 670     union {
 671         struct bt_open_req open_req;
 672         struct bt_open_rsp open_rsp;
 673         struct bt_set_configuration_req setconf_req;
 674         struct bt_set_configuration_rsp setconf_rsp;
 675         bt_audio_error_t error;
 676         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 677     } msg;
 678
 679     memset(&msg, 0, sizeof(msg));
 680     msg.open_req.h.type = BT_REQUEST;
 681     msg.open_req.h.name = BT_OPEN;
 682     msg.open_req.h.length = sizeof(msg.open_req);
 683
 684     pa_strlcpy(msg.open_req.object, u->path, sizeof(msg.open_req.object));
 685     msg.open_req.seid = (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) ? u->a2dp.sbc_capabilities.capability.seid : BT_A2DP_SEID_RANGE + 1;
 686     msg.open_req.lock = (u->profile == PROFILE_A2DP) ? BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK;
 687
 688     if (service_send(u, &msg.open_req.h) < 0)
 689         return -1;
 690
 691     if (service_expect(u, &msg.open_rsp.h, sizeof(msg), BT_OPEN, sizeof(msg.open_rsp)) < 0)
 692         return -1;
 693
 694     if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) {
 695         u->sample_spec.format = PA_SAMPLE_S16LE;
 696
 697         if (setup_a2dp(u) < 0)
 698             return -1;
 699     } else {
 700         pa_assert(u->profile == PROFILE_HSP);
 701
 702         u->sample_spec.format = PA_SAMPLE_S16LE;
 703         u->sample_spec.channels = 1;
 704         u->sample_spec.rate = 8000;
 705     }
 706
 707     memset(&msg, 0, sizeof(msg));
 708     msg.setconf_req.h.type = BT_REQUEST;
 709     msg.setconf_req.h.name = BT_SET_CONFIGURATION;
 710     msg.setconf_req.h.length = sizeof(msg.setconf_req);
 711
 712     if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) {
 713         memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities, sizeof(u->a2dp.sbc_capabilities));
 714     } else {
 715         msg.setconf_req.codec.transport = BT_CAPABILITIES_TRANSPORT_SCO;
 716         msg.setconf_req.codec.seid = BT_A2DP_SEID_RANGE + 1;
 717         msg.setconf_req.codec.length = sizeof(pcm_capabilities_t);
 718     }
 719     msg.setconf_req.h.length += msg.setconf_req.codec.length - sizeof(msg.setconf_req.codec);
 720
 721     if (service_send(u, &msg.setconf_req.h) < 0)
 722         return -1;
 723
 724     if (service_expect(u, &msg.setconf_rsp.h, sizeof(msg), BT_SET_CONFIGURATION, sizeof(msg.setconf_rsp)) < 0)
 725         return -1;
 726
 727     u->link_mtu = msg.setconf_rsp.link_mtu;
 728
 729     /* setup SBC encoder now we agree on parameters */
 730     if (u->profile == PROFILE_A2DP || u->profile == PROFILE_A2DP_SOURCE) {
 731         setup_sbc(&u->a2dp);
 732
 733         u->block_size =
 734             ((u->link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
 735             / u->a2dp.frame_length
 736             * u->a2dp.codesize);
 737
 738         pa_log_info("SBC parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
 739                     u->a2dp.sbc.allocation, u->a2dp.sbc.subbands, u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool);
 740     } else
 741         u->block_size = u->link_mtu;
 742
 743     return 0;
 744 }
 745
 746 /* from IO thread, except in SCO over PCM */
 747 static int start_stream_fd(struct userdata *u) {
 748     union {
 749         bt_audio_msg_header_t rsp;
 750         struct bt_start_stream_req start_req;
 751         struct bt_start_stream_rsp start_rsp;
 752         struct bt_new_stream_ind streamfd_ind;
 753         bt_audio_error_t error;
 754         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 755     } msg;
 756     struct pollfd *pollfd;
 757     int one;
 758
 759     pa_assert(u);
 760     pa_assert(u->rtpoll);
 761     pa_assert(!u->rtpoll_item);
 762     pa_assert(u->stream_fd < 0);
 763
 764     memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 765     msg.start_req.h.type = BT_REQUEST;
 766     msg.start_req.h.name = BT_START_STREAM;
 767     msg.start_req.h.length = sizeof(msg.start_req);
 768
 769     if (service_send(u, &msg.start_req.h) < 0)
 770         return -1;
 771
 772     if (service_expect(u, &msg.rsp, sizeof(msg), BT_START_STREAM, sizeof(msg.start_rsp)) < 0)
 773         return -1;
 774
 775     if (service_expect(u, &msg.rsp, sizeof(msg), BT_NEW_STREAM, sizeof(msg.streamfd_ind)) < 0)
 776         return -1;
 777
 778     if ((u->stream_fd = bt_audio_service_get_data_fd(u->service_fd)) < 0) {
 779         pa_log("Failed to get stream fd from audio service.");
 780         return -1;
 781     }
 782
 783     pa_make_fd_nonblock(u->stream_fd);
 784     pa_make_socket_low_delay(u->stream_fd);
 785
 786     one = 1;
 787     if (setsockopt(u->stream_fd, SOL_SOCKET, SO_TIMESTAMP, &one, sizeof(one)) < 0)
 788         pa_log_warn("Failed to enable SO_TIMESTAMP: %s", pa_cstrerror(errno));
 789
 790     pa_log_debug("Stream properly set up, we're ready to roll!");
 791
 792     u->rtpoll_item = pa_rtpoll_item_new(u->rtpoll, PA_RTPOLL_NEVER, 1);
 793     pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL);
 794     pollfd->fd = u->stream_fd;
 795     pollfd->events = pollfd->revents = 0;
 796
 797     u->read_index = u->write_index = 0;
 798     u->started_at = 0;
 799
 800     if (u->source)
 801         u->read_smoother = pa_smoother_new(
 802                 PA_USEC_PER_SEC,
 803                 PA_USEC_PER_SEC*2,
 804                 TRUE,
 805                 TRUE,
 806                 10,
 807                 pa_rtclock_now(),
 808                 TRUE);
 809
 810     return 0;
 811 }
 812
 813 /* from IO thread */
 814 static int stop_stream_fd(struct userdata *u) {
 815     union {
 816         bt_audio_msg_header_t rsp;
 817         struct bt_stop_stream_req start_req;
 818         struct bt_stop_stream_rsp start_rsp;
 819         bt_audio_error_t error;
 820         uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
 821     } msg;
 822     int r = 0;
 823
 824     pa_assert(u);
 825     pa_assert(u->rtpoll);
 826
 827     if (u->rtpoll_item) {
 828         pa_rtpoll_item_free(u->rtpoll_item);
 829         u->rtpoll_item = NULL;
 830     }
 831
 832     if (u->stream_fd >= 0) {
 833         memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
 834         msg.start_req.h.type = BT_REQUEST;
 835         msg.start_req.h.name = BT_STOP_STREAM;
 836         msg.start_req.h.length = sizeof(msg.start_req);
 837
 838         if (service_send(u, &msg.start_req.h) < 0 ||
 839             service_expect(u, &msg.rsp, sizeof(msg), BT_STOP_STREAM, sizeof(msg.start_rsp)) < 0)
 840             r = -1;
 841
 842         pa_close(u->stream_fd);
 843         u->stream_fd = -1;
 844     }
 845
 846     if (u->read_smoother) {
 847         pa_smoother_free(u->read_smoother);
 848         u->read_smoother = NULL;
 849     }
 850
 851     return r;
 852 }
 853
 854 /* Run from IO thread */
 855 static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offset, pa_memchunk *chunk) {
 856     struct userdata *u = PA_SINK(o)->userdata;
 857     pa_bool_t failed = FALSE;
 858     int r;
 859
 860     pa_assert(u->sink == PA_SINK(o));
 861
 862     switch (code) {
 863
 864         case PA_SINK_MESSAGE_SET_STATE:
 865
 866             switch ((pa_sink_state_t) PA_PTR_TO_UINT(data)) {
 867
 868                 case PA_SINK_SUSPENDED:
 869                     pa_assert(PA_SINK_IS_OPENED(u->sink->thread_info.state));
 870
 871                     /* Stop the device if the source is suspended as well */
 872                     if (!u->source || u->source->state == PA_SOURCE_SUSPENDED)
 873                         /* We deliberately ignore whether stopping
 874                          * actually worked. Since the stream_fd is
 875                          * closed it doesn't really matter */
 876                         stop_stream_fd(u);
 877
 878                     break;
 879
 880                 case PA_SINK_IDLE:
 881                 case PA_SINK_RUNNING:
 882                     if (u->sink->thread_info.state != PA_SINK_SUSPENDED)
 883                         break;
 884
 885                     /* Resume the device if the source was suspended as well */
 886                     if (!u->source || u->source->state == PA_SOURCE_SUSPENDED)
 887                         if (start_stream_fd(u) < 0)
 888                             failed = TRUE;
 889                     break;
 890
 891                 case PA_SINK_UNLINKED:
 892                 case PA_SINK_INIT:
 893                 case PA_SINK_INVALID_STATE:
 894                     ;
 895             }
 896             break;
 897
 898         case PA_SINK_MESSAGE_GET_LATENCY: {
 899
 900             if (u->read_smoother) {
 901                 pa_usec_t wi, ri;
 902
 903                 ri = pa_smoother_get(u->read_smoother, pa_rtclock_now());
 904                 wi = pa_bytes_to_usec(u->write_index + u->block_size, &u->sample_spec);
 905
 906                 *((pa_usec_t*) data) = wi > ri ? wi - ri : 0;
 907             } else {
 908                 pa_usec_t ri, wi;
 909
 910                 ri = pa_rtclock_now() - u->started_at;
 911                 wi = pa_bytes_to_usec(u->write_index, &u->sample_spec);
 912
 913                 *((pa_usec_t*) data) = wi > ri ? wi - ri : 0;
 914             }
 915
 916             *((pa_usec_t*) data) += u->sink->thread_info.fixed_latency;
 917             return 0;
 918         }
 919     }
 920
 921     r = pa_sink_process_msg(o, code, data, offset, chunk);
 922
 923     return (r < 0 || !failed) ? r : -1;
 924 }
 925
 926 /* Run from IO thread */
 927 static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t offset, pa_memchunk *chunk) {
 928     struct userdata *u = PA_SOURCE(o)->userdata;
 929     pa_bool_t failed = FALSE;
 930     int r;
 931
 932     pa_assert(u->source == PA_SOURCE(o));
 933
 934     switch (code) {
 935
 936         case PA_SOURCE_MESSAGE_SET_STATE:
 937
 938             switch ((pa_source_state_t) PA_PTR_TO_UINT(data)) {
 939
 940                 case PA_SOURCE_SUSPENDED:
 941                     pa_assert(PA_SOURCE_IS_OPENED(u->source->thread_info.state));
 942
 943                     /* Stop the device if the sink is suspended as well */
 944                     if (!u->sink || u->sink->state == PA_SINK_SUSPENDED)
 945                         stop_stream_fd(u);
 946
 947                     if (u->read_smoother)
 948                         pa_smoother_pause(u->read_smoother, pa_rtclock_now());
 949                     break;
 950
 951                 case PA_SOURCE_IDLE:
 952                 case PA_SOURCE_RUNNING:
 953                     if (u->source->thread_info.state != PA_SOURCE_SUSPENDED)
 954                         break;
 955
 956                     /* Resume the device if the sink was suspended as well */
 957                     if (!u->sink || u->sink->thread_info.state == PA_SINK_SUSPENDED)
 958                         if (start_stream_fd(u) < 0)
 959                             failed = TRUE;
 960
 961                     /* We don't resume the smoother here. Instead we
 962                      * wait until the first packet arrives */
 963                     break;
 964
 965                 case PA_SOURCE_UNLINKED:
 966                 case PA_SOURCE_INIT:
 967                 case PA_SOURCE_INVALID_STATE:
 968                     ;
 969             }
 970             break;
 971
 972         case PA_SOURCE_MESSAGE_GET_LATENCY: {
 973             pa_usec_t wi, ri;
 974
 975             if (u->read_smoother) {
 976                 wi = pa_smoother_get(u->read_smoother, pa_rtclock_now());
 977                 ri = pa_bytes_to_usec(u->read_index, &u->sample_spec);
 978
 979                 *((pa_usec_t*) data) = (wi > ri ? wi - ri : 0) + u->source->thread_info.fixed_latency;
 980             } else
 981                 *((pa_usec_t*) data) = 0;
 982
 983             return 0;
 984         }
 985
 986     }
 987
 988     r = pa_source_process_msg(o, code, data, offset, chunk);
 989
 990     return (r < 0 || !failed) ? r : -1;
 991 }
 992
 993 /* Run from IO thread */
 994 static int hsp_process_render(struct userdata *u) {
 995     int ret = 0;
 996
 997     pa_assert(u);
 998     pa_assert(u->profile == PROFILE_HSP);
 999     pa_assert(u->sink);
1000
1001     /* First, render some data */
1002     if (!u->write_memchunk.memblock)
1003         pa_sink_render_full(u->sink, u->block_size, &u->write_memchunk);
1004
1005     pa_assert(u->write_memchunk.length == u->block_size);
1006
1007     for (;;) {
1008         ssize_t l;
1009         const void *p;
1010
1011         /* Now write that data to the socket. The socket is of type
1012          * SEQPACKET, and we generated the data of the MTU size, so this
1013          * should just work. */
1014
1015         p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock) + u->write_memchunk.index;
1016         l = pa_write(u->stream_fd, p, u->write_memchunk.length, &u->stream_write_type);
1017         pa_memblock_release(u->write_memchunk.memblock);
1018
1019         pa_assert(l != 0);
1020
1021         if (l < 0) {
1022
1023             if (errno == EINTR)
1024                 /* Retry right away if we got interrupted */
1025                 continue;
1026
1027             else if (errno == EAGAIN)
1028                 /* Hmm, apparently the socket was not writable, give up for now */
1029                 break;
1030
1031             pa_log_error("Failed to write data to SCO socket: %s", pa_cstrerror(errno));
1032             ret = -1;
1033             break;
1034         }
1035
1036         pa_assert((size_t) l <= u->write_memchunk.length);
1037
1038         if ((size_t) l != u->write_memchunk.length) {
1039             pa_log_error("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
1040                         (unsigned long long) l,
1041                         (unsigned long long) u->write_memchunk.length);
1042             ret = -1;
1043             break;
1044         }
1045
1046         u->write_index += (uint64_t) u->write_memchunk.length;
1047         pa_memblock_unref(u->write_memchunk.memblock);
1048         pa_memchunk_reset(&u->write_memchunk);
1049
1050         ret = 1;
1051         break;
1052     }
1053
1054     return ret;
1055 }
1056
1057 /* Run from IO thread */
1058 static int hsp_process_push(struct userdata *u) {
1059     int ret = 0;
1060     pa_memchunk memchunk;
1061
1062     pa_assert(u);
1063     pa_assert(u->profile == PROFILE_HSP);
1064     pa_assert(u->source);
1065     pa_assert(u->read_smoother);
1066
1067     memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
1068     memchunk.index = memchunk.length = 0;
1069
1070     for (;;) {
1071         ssize_t l;
1072         void *p;
1073         struct msghdr m;
1074         struct cmsghdr *cm;
1075         uint8_t aux[1024];
1076         struct iovec iov;
1077         pa_bool_t found_tstamp = FALSE;
1078         pa_usec_t tstamp;
1079
1080         memset(&m, 0, sizeof(m));
1081         memset(&aux, 0, sizeof(aux));
1082         memset(&iov, 0, sizeof(iov));
1083
1084         m.msg_iov = &iov;
1085         m.msg_iovlen = 1;
1086         m.msg_control = aux;
1087         m.msg_controllen = sizeof(aux);
1088
1089         p = pa_memblock_acquire(memchunk.memblock);
1090         iov.iov_base = p;
1091         iov.iov_len = pa_memblock_get_length(memchunk.memblock);
1092         l = recvmsg(u->stream_fd, &m, 0);
1093         pa_memblock_release(memchunk.memblock);
1094
1095         if (l <= 0) {
1096
1097             if (l < 0 && errno == EINTR)
1098                 /* Retry right away if we got interrupted */
1099                 continue;
1100
1101             else if (l < 0 && errno == EAGAIN)
1102                 /* Hmm, apparently the socket was not readable, give up for now. */
1103                 break;
1104
1105             pa_log_error("Failed to read data from SCO socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
1106             ret = -1;
1107             break;
1108         }
1109
1110         pa_assert((size_t) l <= pa_memblock_get_length(memchunk.memblock));
1111
1112         memchunk.length = (size_t) l;
1113         u->read_index += (uint64_t) l;
1114
1115         for (cm = CMSG_FIRSTHDR(&m); cm; cm = CMSG_NXTHDR(&m, cm))
1116             if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMP) {
1117                 struct timeval *tv = (struct timeval*) CMSG_DATA(cm);
1118                 pa_rtclock_from_wallclock(tv);
1119                 tstamp = pa_timeval_load(tv);
1120                 found_tstamp = TRUE;
1121                 break;
1122             }
1123
1124         if (!found_tstamp) {
1125             pa_log_warn("Couldn't find SO_TIMESTAMP data in auxiliary recvmsg() data!");
1126             tstamp = pa_rtclock_now();
1127         }
1128
1129         pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->sample_spec));
1130         pa_smoother_resume(u->read_smoother, tstamp, TRUE);
1131
1132         pa_source_post(u->source, &memchunk);
1133
1134         ret = 1;
1135         break;
1136     }
1137
1138     pa_memblock_unref(memchunk.memblock);
1139
1140     return ret;
1141 }
1142
1143 /* Run from IO thread */
1144 static void a2dp_prepare_buffer(struct userdata *u) {
1145     pa_assert(u);
1146
1147     if (u->a2dp.buffer_size >= u->link_mtu)
1148         return;
1149
1150     u->a2dp.buffer_size = 2 * u->link_mtu;
1151     pa_xfree(u->a2dp.buffer);
1152     u->a2dp.buffer = pa_xmalloc(u->a2dp.buffer_size);
1153 }
1154
1155 /* Run from IO thread */
1156 static int a2dp_process_render(struct userdata *u) {
1157     struct a2dp_info *a2dp;
1158     struct rtp_header *header;
1159     struct rtp_payload *payload;
1160     size_t nbytes;
1161     void *d;
1162     const void *p;
1163     size_t to_write, to_encode;
1164     unsigned frame_count;
1165     int ret = 0;
1166
1167     pa_assert(u);
1168     pa_assert(u->profile == PROFILE_A2DP);
1169     pa_assert(u->sink);
1170
1171     /* First, render some data */
1172     if (!u->write_memchunk.memblock)
1173         pa_sink_render_full(u->sink, u->block_size, &u->write_memchunk);
1174
1175     pa_assert(u->write_memchunk.length == u->block_size);
1176
1177     a2dp_prepare_buffer(u);
1178
1179     a2dp = &u->a2dp;
1180     header = a2dp->buffer;
1181     payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
1182
1183     frame_count = 0;
1184
1185     /* Try to create a packet of the full MTU */
1186
1187     p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock) + u->write_memchunk.index;
1188     to_encode = u->write_memchunk.length;
1189
1190     d = (uint8_t*) a2dp->buffer + sizeof(*header) + sizeof(*payload);
1191     to_write = a2dp->buffer_size - sizeof(*header) - sizeof(*payload);
1192
1193     while (PA_LIKELY(to_encode > 0 && to_write > 0)) {
1194         size_t written;
1195         ssize_t encoded;
1196
1197         encoded = sbc_encode(&a2dp->sbc,
1198                              p, to_encode,
1199                              d, to_write,
1200                              &written);
1201
1202         if (PA_UNLIKELY(encoded <= 0)) {
1203             pa_log_error("SBC encoding error (%li)", (long) encoded);
1204             pa_memblock_release(u->write_memchunk.memblock);
1205             return -1;
1206         }
1207
1208 /*         pa_log_debug("SBC: encoded: %lu; written: %lu", (unsigned long) encoded, (unsigned long) written); */
1209 /*         pa_log_debug("SBC: codesize: %lu; frame_length: %lu", (unsigned long) a2dp->codesize, (unsigned long) a2dp->frame_length); */
1210
1211         pa_assert_fp((size_t) encoded <= to_encode);
1212         pa_assert_fp((size_t) encoded == a2dp->codesize);
1213
1214         pa_assert_fp((size_t) written <= to_write);
1215         pa_assert_fp((size_t) written == a2dp->frame_length);
1216
1217         p = (const uint8_t*) p + encoded;
1218         to_encode -= encoded;
1219
1220         d = (uint8_t*) d + written;
1221         to_write -= written;
1222
1223         frame_count++;
1224     }
1225
1226     pa_memblock_release(u->write_memchunk.memblock);
1227
1228     pa_assert(to_encode == 0);
1229
1230     PA_ONCE_BEGIN {
1231         pa_log_debug("Using SBC encoder implementation: %s", pa_strnull(sbc_get_implementation_info(&a2dp->sbc)));
1232     } PA_ONCE_END;
1233
1234     /* write it to the fifo */
1235     memset(a2dp->buffer, 0, sizeof(*header) + sizeof(*payload));
1236     header->v = 2;
1237     header->pt = 1;
1238     header->sequence_number = htons(a2dp->seq_num++);
1239     header->timestamp = htonl(u->write_index / pa_frame_size(&u->sample_spec));
1240     header->ssrc = htonl(1);
1241     payload->frame_count = frame_count;
1242
1243     nbytes = (uint8_t*) d - (uint8_t*) a2dp->buffer;
1244
1245     for (;;) {
1246         ssize_t l;
1247
1248         l = pa_write(u->stream_fd, a2dp->buffer, nbytes, &u->stream_write_type);
1249
1250         pa_assert(l != 0);
1251
1252         if (l < 0) {
1253
1254             if (errno == EINTR)
1255                 /* Retry right away if we got interrupted */
1256                 continue;
1257
1258             else if (errno == EAGAIN)
1259                 /* Hmm, apparently the socket was not writable, give up for now */
1260                 break;
1261
1262             pa_log_error("Failed to write data to socket: %s", pa_cstrerror(errno));
1263             ret  = -1;
1264             break;
1265         }
1266
1267         pa_assert((size_t) l <= nbytes);
1268
1269         if ((size_t) l != nbytes) {
1270             pa_log_warn("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
1271                         (unsigned long long) l,
1272                         (unsigned long long) nbytes);
1273             ret = -1;
1274             break;
1275         }
1276
1277         u->write_index += (uint64_t) u->write_memchunk.length;
1278         pa_memblock_unref(u->write_memchunk.memblock);
1279         pa_memchunk_reset(&u->write_memchunk);
1280
1281         ret = 1;
1282
1283         break;
1284     }
1285
1286     return ret;
1287 }
1288
1289 static int a2dp_process_push(struct userdata *u) {
1290     int ret = 0;
1291     pa_memchunk memchunk;
1292
1293     pa_assert(u);
1294     pa_assert(u->profile == PROFILE_A2DP_SOURCE);
1295     pa_assert(u->source);
1296     pa_assert(u->read_smoother);
1297
1298     memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
1299     memchunk.index = memchunk.length = 0;
1300
1301     for (;;) {
1302         pa_bool_t found_tstamp = FALSE;
1303         pa_usec_t tstamp;
1304         struct a2dp_info *a2dp;
1305         struct rtp_header *header;
1306         struct rtp_payload *payload;
1307         const void *p;
1308         void *d;
1309         ssize_t l;
1310         size_t to_write, to_decode;
1311         unsigned frame_count;
1312
1313         a2dp_prepare_buffer(u);
1314
1315         a2dp = &u->a2dp;
1316         header = a2dp->buffer;
1317         payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
1318
1319         l = pa_read(u->stream_fd, a2dp->buffer, a2dp->buffer_size, &u->stream_write_type);
1320
1321         if (l <= 0) {
1322
1323             if (l < 0 && errno == EINTR)
1324                 /* Retry right away if we got interrupted */
1325                 continue;
1326
1327             else if (l < 0 && errno == EAGAIN)
1328                 /* Hmm, apparently the socket was not readable, give up for now. */
1329                 break;
1330
1331             pa_log_error("Failed to read data from socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
1332             ret = -1;
1333             break;
1334         }
1335
1336         pa_assert((size_t) l <= a2dp->buffer_size);
1337
1338         u->read_index += (uint64_t) l;
1339
1340         /* TODO: get timestamp from rtp */
1341         if (!found_tstamp) {
1342             /* pa_log_warn("Couldn't find SO_TIMESTAMP data in auxiliary recvmsg() data!"); */
1343             tstamp = pa_rtclock_now();
1344         }
1345
1346         pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->sample_spec));
1347         pa_smoother_resume(u->read_smoother, tstamp, TRUE);
1348
1349         p = (uint8_t*) a2dp->buffer + sizeof(*header) + sizeof(*payload);
1350         to_decode = l - sizeof(*header) - sizeof(*payload);
1351
1352         d = pa_memblock_acquire(memchunk.memblock);
1353         to_write = memchunk.length = pa_memblock_get_length(memchunk.memblock);
1354
1355         while (PA_LIKELY(to_decode > 0 && to_write > 0)) {
1356             size_t written;
1357             ssize_t decoded;
1358
1359             decoded = sbc_decode(&a2dp->sbc,
1360                                  p, to_decode,
1361                                  d, to_write,
1362                                  &written);
1363
1364             if (PA_UNLIKELY(decoded <= 0)) {
1365                 pa_log_error("SBC decoding error (%li)", (long) decoded);
1366                 pa_memblock_release(memchunk.memblock);
1367                 pa_memblock_unref(memchunk.memblock);
1368                 return -1;
1369             }
1370
1371 /*             pa_log_debug("SBC: decoded: %lu; written: %lu", (unsigned long) decoded, (unsigned long) written); */
1372 /*             pa_log_debug("SBC: frame_length: %lu; codesize: %lu", (unsigned long) a2dp->frame_length, (unsigned long) a2dp->codesize); */
1373
1374             pa_assert_fp((size_t) decoded <= to_decode);
1375             pa_assert_fp((size_t) decoded == a2dp->frame_length);
1376
1377             pa_assert_fp((size_t) written <= to_write);
1378             pa_assert_fp((size_t) written == a2dp->codesize);
1379
1380             p = (const uint8_t*) p + decoded;
1381             to_decode -= decoded;
1382
1383             d = (uint8_t*) d + written;
1384             to_write -= written;
1385
1386             frame_count++;
1387         }
1388
1389         pa_memblock_release(memchunk.memblock);
1390
1391         pa_source_post(u->source, &memchunk);
1392
1393         ret = 1;
1394         break;
1395     }
1396
1397     pa_memblock_unref(memchunk.memblock);
1398
1399     return ret;
1400 }
1401
1402 static void thread_func(void *userdata) {
1403     struct userdata *u = userdata;
1404     unsigned do_write = 0;
1405     pa_bool_t writable = FALSE;
1406
1407     pa_assert(u);
1408
1409     pa_log_debug("IO Thread starting up");
1410
1411     if (u->core->realtime_scheduling)
1412         pa_make_realtime(u->core->realtime_priority);
1413
1414     pa_thread_mq_install(&u->thread_mq);
1415
1416     if (start_stream_fd(u) < 0)
1417         goto fail;
1418
1419     for (;;) {
1420         struct pollfd *pollfd;
1421         int ret;
1422         pa_bool_t disable_timer = TRUE;
1423
1424         pollfd = u->rtpoll_item ? pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL) : NULL;
1425
1426         if (u->source && PA_SOURCE_IS_LINKED(u->source->thread_info.state)) {
1427
1428             /* We should send two blocks to the device before we expect
1429              * a response. */
1430
1431             if (u->write_index == 0 && u->read_index <= 0)
1432                 do_write = 2;
1433
1434             if (pollfd && (pollfd->revents & POLLIN)) {
1435                 int n_read;
1436
1437                 if (u->profile == PROFILE_HSP)
1438                     n_read = hsp_process_push(u);
1439                 else
1440                     n_read = a2dp_process_push(u);
1441
1442                 if (n_read < 0)
1443                     goto fail;
1444
1445                 /* We just read something, so we are supposed to write something, too */
1446                 do_write += n_read;
1447             }
1448         }
1449
1450         if (u->sink && PA_SINK_IS_LINKED(u->sink->thread_info.state)) {
1451
1452             if (u->sink->thread_info.rewind_requested)
1453                 pa_sink_process_rewind(u->sink, 0);
1454
1455             if (pollfd) {
1456                 if (pollfd->revents & POLLOUT)
1457                     writable = TRUE;
1458
1459                 if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable) {
1460                     pa_usec_t time_passed;
1461                     pa_usec_t audio_sent;
1462
1463                     /* Hmm, there is no input stream we could synchronize
1464                      * to. So let's do things by time */
1465
1466                     time_passed = pa_rtclock_now() - u->started_at;
1467                     audio_sent = pa_bytes_to_usec(u->write_index, &u->sample_spec);
1468
1469                     if (audio_sent <= time_passed) {
1470                         pa_usec_t audio_to_send = time_passed - audio_sent;
1471
1472                         /* Never try to catch up for more than 100ms */
1473                         if (u->write_index > 0 && audio_to_send > MAX_PLAYBACK_CATCH_UP_USEC) {
1474                             pa_usec_t skip_usec;
1475                             uint64_t skip_bytes;
1476
1477                             skip_usec = audio_to_send - MAX_PLAYBACK_CATCH_UP_USEC;
1478                             skip_bytes = pa_usec_to_bytes(skip_usec, &u->sample_spec);
1479
1480                             if (skip_bytes > 0) {
1481                                 pa_memchunk tmp;
1482
1483                                 pa_log_warn("Skipping %llu us (= %llu bytes) in audio stream",
1484                                             (unsigned long long) skip_usec,
1485                                             (unsigned long long) skip_bytes);
1486
1487                                 pa_sink_render_full(u->sink, skip_bytes, &tmp);
1488                                 pa_memblock_unref(tmp.memblock);
1489                                 u->write_index += skip_bytes;
1490                             }
1491                         }
1492
1493                         do_write = 1;
1494                     }
1495                 }
1496
1497                 if (writable && do_write > 0) {
1498                     int n_written;
1499
1500                     if (u->write_index <= 0)
1501                         u->started_at = pa_rtclock_now();
1502
1503                     if (u->profile == PROFILE_A2DP) {
1504                         if ((n_written = a2dp_process_render(u)) < 0)
1505                             goto fail;
1506                     } else {
1507                         if ((n_written = hsp_process_render(u)) < 0)
1508                             goto fail;
1509                     }
1510
1511                     if (n_written == 0)
1512                         pa_log("Broken kernel: we got EAGAIN on write() after POLLOUT!");
1513
1514                     do_write -= n_written;
1515                     writable = FALSE;
1516                 }
1517
1518                 if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable) {
1519                     pa_usec_t time_passed, next_write_at, sleep_for;
1520
1521                     /* Hmm, there is no input stream we could synchronize
1522                      * to. So let's estimate when we need to wake up the latest */
1523
1524                     time_passed = pa_rtclock_now() - u->started_at;
1525                     next_write_at = pa_bytes_to_usec(u->write_index, &u->sample_spec);
1526                     sleep_for = time_passed < next_write_at ? next_write_at - time_passed : 0;
1527
1528 /*                 pa_log("Sleeping for %lu; time passed %lu, next write at %lu", (unsigned long) sleep_for, (unsigned long) time_passed, (unsigned long)next_write_at); */
1529
1530                     pa_rtpoll_set_timer_relative(u->rtpoll, sleep_for);
1531                     disable_timer = FALSE;
1532                 }
1533             }
1534         }
1535
1536         if (disable_timer)
1537             pa_rtpoll_set_timer_disabled(u->rtpoll);
1538
1539         /* Hmm, nothing to do. Let's sleep */
1540         if (pollfd)
1541             pollfd->events = (short) (((u->sink && PA_SINK_IS_LINKED(u->sink->thread_info.state) && !writable) ? POLLOUT : 0) |
1542                                       (u->source && PA_SOURCE_IS_LINKED(u->source->thread_info.state) ? POLLIN : 0));
1543
1544         if ((ret = pa_rtpoll_run(u->rtpoll, TRUE)) < 0)
1545             goto fail;
1546
1547         if (ret == 0)
1548             goto finish;
1549
1550         pollfd = u->rtpoll_item ? pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL) : NULL;
1551
1552         if (pollfd && (pollfd->revents & ~(POLLOUT|POLLIN))) {
1553             pa_log_info("FD error: %s%s%s%s",
1554                         pollfd->revents & POLLERR ? "POLLERR " :"",
1555                         pollfd->revents & POLLHUP ? "POLLHUP " :"",
1556                         pollfd->revents & POLLPRI ? "POLLPRI " :"",
1557                         pollfd->revents & POLLNVAL ? "POLLNVAL " :"");
1558             goto fail;
1559         }
1560     }
1561
1562 fail:
1563     /* If this was no regular exit from the loop we have to continue processing messages until we receive PA_MESSAGE_SHUTDOWN */
1564     pa_log_debug("IO thread failed");
1565     pa_asyncmsgq_post(u->thread_mq.outq, PA_MSGOBJECT(u->core), PA_CORE_MESSAGE_UNLOAD_MODULE, u->module, 0, NULL, NULL);
1566     pa_asyncmsgq_wait_for(u->thread_mq.inq, PA_MESSAGE_SHUTDOWN);
1567
1568 finish:
1569     pa_log_debug("IO thread shutting down");
1570 }
1571
1572 /* Run from main thread */
1573 static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *m, void *userdata) {
1574     DBusError err;
1575     struct userdata *u;
1576
1577     pa_assert(bus);
1578     pa_assert(m);
1579     pa_assert_se(u = userdata);
1580
1581     dbus_error_init(&err);
1582
1583     pa_log_debug("dbus: interface=%s, path=%s, member=%s\n",
1584                  dbus_message_get_interface(m),
1585                  dbus_message_get_path(m),
1586                  dbus_message_get_member(m));
1587
1588    if (!dbus_message_has_path(m, u->path))
1589        goto fail;
1590
1591     if (dbus_message_is_signal(m, "org.bluez.Headset", "SpeakerGainChanged") ||
1592         dbus_message_is_signal(m, "org.bluez.Headset", "MicrophoneGainChanged")) {
1593
1594         dbus_uint16_t gain;
1595         pa_cvolume v;
1596
1597         if (!dbus_message_get_args(m, &err, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID) || gain > 15) {
1598             pa_log("Failed to parse org.bluez.Headset.{Speaker|Microphone}GainChanged: %s", err.message);
1599             goto fail;
1600         }
1601
1602         if (u->profile == PROFILE_HSP) {
1603             if (u->sink && dbus_message_is_signal(m, "org.bluez.Headset", "SpeakerGainChanged")) {
1604
1605                 pa_cvolume_set(&v, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1606                 pa_sink_volume_changed(u->sink, &v);
1607
1608             } else if (u->source && dbus_message_is_signal(m, "org.bluez.Headset", "MicrophoneGainChanged")) {
1609
1610                 pa_cvolume_set(&v, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1611                 pa_source_volume_changed(u->source, &v);
1612             }
1613         }
1614     }
1615
1616 fail:
1617     dbus_error_free(&err);
1618
1619     return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
1620 }
1621
1622 /* Run from main thread */
1623 static void sink_set_volume_cb(pa_sink *s) {
1624     struct userdata *u = s->userdata;
1625     DBusMessage *m;
1626     dbus_uint16_t gain;
1627
1628     pa_assert(u);
1629
1630     if (u->profile != PROFILE_HSP)
1631         return;
1632
1633     gain = (pa_cvolume_max(&s->real_volume) * 15) / PA_VOLUME_NORM;
1634
1635     if (gain > 15)
1636         gain = 15;
1637
1638     pa_cvolume_set(&s->real_volume, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1639
1640     pa_assert_se(m = dbus_message_new_method_call("org.bluez", u->path, "org.bluez.Headset", "SetSpeakerGain"));
1641     pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID));
1642     pa_assert_se(dbus_connection_send(pa_dbus_connection_get(u->connection), m, NULL));
1643     dbus_message_unref(m);
1644 }
1645
1646 /* Run from main thread */
1647 static void source_set_volume_cb(pa_source *s) {
1648     struct userdata *u = s->userdata;
1649     DBusMessage *m;
1650     dbus_uint16_t gain;
1651
1652     pa_assert(u);
1653
1654     if (u->profile != PROFILE_HSP)
1655         return;
1656
1657     gain = (pa_cvolume_max(&s->volume) * 15) / PA_VOLUME_NORM;
1658
1659     if (gain > 15)
1660         gain = 15;
1661
1662     pa_cvolume_set(&s->volume, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1663
1664     pa_assert_se(m = dbus_message_new_method_call("org.bluez", u->path, "org.bluez.Headset", "SetMicrophoneGain"));
1665     pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID));
1666     pa_assert_se(dbus_connection_send(pa_dbus_connection_get(u->connection), m, NULL));
1667     dbus_message_unref(m);
1668 }
1669
1670 /* Run from main thread */
1671 static char *get_name(const char *type, pa_modargs *ma, const char *device_id, pa_bool_t *namereg_fail) {
1672     char *t;
1673     const char *n;
1674
1675     pa_assert(type);
1676     pa_assert(ma);
1677     pa_assert(device_id);
1678     pa_assert(namereg_fail);
1679
1680     t = pa_sprintf_malloc("%s_name", type);
1681     n = pa_modargs_get_value(ma, t, NULL);
1682     pa_xfree(t);
1683
1684     if (n) {
1685         *namereg_fail = TRUE;
1686         return pa_xstrdup(n);
1687     }
1688
1689     if ((n = pa_modargs_get_value(ma, "name", NULL)))
1690         *namereg_fail = TRUE;
1691     else {
1692         n = device_id;
1693         *namereg_fail = FALSE;
1694     }
1695
1696     return pa_sprintf_malloc("bluez_%s.%s", type, n);
1697 }
1698
1699 #ifdef NOKIA
1700
1701 static void sco_over_pcm_state_update(struct userdata *u) {
1702     pa_assert(u);
1703     pa_assert(USE_SCO_OVER_PCM(u));
1704
1705     if (PA_SINK_IS_OPENED(pa_sink_get_state(u->hsp.sco_sink)) ||
1706         PA_SOURCE_IS_OPENED(pa_source_get_state(u->hsp.sco_source))) {
1707
1708         if (u->service_fd >= 0)
1709             return;
1710
1711         pa_log_debug("Resuming SCO over PCM");
1712         if ((init_bt(u) < 0) || (init_profile(u) < 0))
1713             pa_log("Can't resume SCO over PCM");
1714
1715         start_stream_fd(u);
1716     } else {
1717
1718         if (u->service_fd < 0)
1719             return;
1720
1721         stop_stream_fd(u);
1722
1723         pa_log_debug("Closing SCO over PCM");
1724         pa_close(u->service_fd);
1725         u->service_fd = -1;
1726     }
1727 }
1728
1729 static pa_hook_result_t sink_state_changed_cb(pa_core *c, pa_sink *s, struct userdata *u) {
1730     pa_assert(c);
1731     pa_sink_assert_ref(s);
1732     pa_assert(u);
1733
1734     if (s != u->hsp.sco_sink)
1735         return PA_HOOK_OK;
1736
1737     sco_over_pcm_state_update(u);
1738
1739     return PA_HOOK_OK;
1740 }
1741
1742 static pa_hook_result_t source_state_changed_cb(pa_core *c, pa_source *s, struct userdata *u) {
1743     pa_assert(c);
1744     pa_source_assert_ref(s);
1745     pa_assert(u);
1746
1747     if (s != u->hsp.sco_source)
1748         return PA_HOOK_OK;
1749
1750     sco_over_pcm_state_update(u);
1751
1752     return PA_HOOK_OK;
1753 }
1754
1755 #endif
1756
1757 /* Run from main thread */
1758 static int add_sink(struct userdata *u) {
1759
1760 #ifdef NOKIA
1761     if (USE_SCO_OVER_PCM(u)) {
1762         pa_proplist *p;
1763
1764         u->sink = u->hsp.sco_sink;
1765         p = pa_proplist_new();
1766         pa_proplist_sets(p, "bluetooth.protocol", "sco");
1767         pa_proplist_update(u->sink->proplist, PA_UPDATE_MERGE, p);
1768         pa_proplist_free(p);
1769
1770         if (!u->hsp.sink_state_changed_slot)
1771             u->hsp.sink_state_changed_slot = pa_hook_connect(&u->core->hooks[PA_CORE_HOOK_SINK_STATE_CHANGED], PA_HOOK_NORMAL, (pa_hook_cb_t) sink_state_changed_cb, u);
1772
1773     } else
1774 #endif
1775
1776     {
1777         pa_sink_new_data data;
1778         pa_bool_t b;
1779
1780         pa_sink_new_data_init(&data);
1781         data.driver = __FILE__;
1782         data.module = u->module;
1783         pa_sink_new_data_set_sample_spec(&data, &u->sample_spec);
1784         pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP ? "a2dp" : "sco");
1785         if (u->profile == PROFILE_HSP)
1786             pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
1787         data.card = u->card;
1788         data.name = get_name("sink", u->modargs, u->address, &b);
1789         data.namereg_fail = b;
1790
1791         if (pa_modargs_get_proplist(u->modargs, "sink_properties", data.proplist, PA_UPDATE_REPLACE) < 0) {
1792             pa_log("Invalid properties");
1793             pa_sink_new_data_done(&data);
1794             return -1;
1795         }
1796
1797         u->sink = pa_sink_new(u->core, &data, PA_SINK_HARDWARE|PA_SINK_LATENCY | (u->profile == PROFILE_HSP ? PA_SINK_HW_VOLUME_CTRL : 0));
1798         pa_sink_new_data_done(&data);
1799
1800         if (!u->sink) {
1801             pa_log_error("Failed to create sink");
1802             return -1;
1803         }
1804
1805         u->sink->userdata = u;
1806         u->sink->parent.process_msg = sink_process_msg;
1807
1808         pa_sink_set_max_request(u->sink, u->block_size);
1809         pa_sink_set_fixed_latency(u->sink,
1810                                   (u->profile == PROFILE_A2DP ? FIXED_LATENCY_PLAYBACK_A2DP : FIXED_LATENCY_PLAYBACK_HSP) +
1811                                   pa_bytes_to_usec(u->block_size, &u->sample_spec));
1812     }
1813
1814     if (u->profile == PROFILE_HSP) {
1815         u->sink->set_volume = sink_set_volume_cb;
1816         u->sink->n_volume_steps = 16;
1817     }
1818
1819     return 0;
1820 }
1821
1822 /* Run from main thread */
1823 static int add_source(struct userdata *u) {
1824
1825 #ifdef NOKIA
1826     if (USE_SCO_OVER_PCM(u)) {
1827         u->source = u->hsp.sco_source;
1828         pa_proplist_sets(u->source->proplist, "bluetooth.protocol", "hsp");
1829
1830         if (!u->hsp.source_state_changed_slot)
1831             u->hsp.source_state_changed_slot = pa_hook_connect(&u->core->hooks[PA_CORE_HOOK_SOURCE_STATE_CHANGED], PA_HOOK_NORMAL, (pa_hook_cb_t) source_state_changed_cb, u);
1832
1833     } else
1834 #endif
1835
1836     {
1837         pa_source_new_data data;
1838         pa_bool_t b;
1839
1840         pa_source_new_data_init(&data);
1841         data.driver = __FILE__;
1842         data.module = u->module;
1843         pa_source_new_data_set_sample_spec(&data, &u->sample_spec);
1844         pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP_SOURCE ? "a2dp_source" : "hsp");
1845         if (u->profile == PROFILE_HSP)
1846             pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
1847         data.card = u->card;
1848         data.name = get_name("source", u->modargs, u->address, &b);
1849         data.namereg_fail = b;
1850
1851         if (pa_modargs_get_proplist(u->modargs, "source_properties", data.proplist, PA_UPDATE_REPLACE) < 0) {
1852             pa_log("Invalid properties");
1853             pa_source_new_data_done(&data);
1854             return -1;
1855         }
1856
1857         u->source = pa_source_new(u->core, &data, PA_SOURCE_HARDWARE|PA_SOURCE_LATENCY | (u->profile == PROFILE_HSP ? PA_SOURCE_HW_VOLUME_CTRL : 0));
1858         pa_source_new_data_done(&data);
1859
1860         if (!u->source) {
1861             pa_log_error("Failed to create source");
1862             return -1;
1863         }
1864
1865         u->source->userdata = u;
1866         u->source->parent.process_msg = source_process_msg;
1867
1868         pa_source_set_fixed_latency(u->source,
1869                                     (u->profile == PROFILE_A2DP_SOURCE ? FIXED_LATENCY_RECORD_A2DP : FIXED_LATENCY_RECORD_HSP) +
1870                                     pa_bytes_to_usec(u->block_size, &u->sample_spec));
1871     }
1872
1873     if (u->profile == PROFILE_HSP) {
1874         pa_proplist_sets(u->source->proplist, "bluetooth.nrec", (u->hsp.pcm_capabilities.flags & BT_PCM_FLAG_NREC) ? "1" : "0");
1875         u->source->set_volume = source_set_volume_cb;
1876         u->source->n_volume_steps = 16;
1877     }
1878
1879     return 0;
1880 }
1881
1882 /* Run from main thread */
1883 static void shutdown_bt(struct userdata *u) {
1884     pa_assert(u);
1885
1886     if (u->stream_fd >= 0) {
1887         pa_close(u->stream_fd);
1888         u->stream_fd = -1;
1889
1890         u->stream_write_type = 0;
1891     }
1892
1893     if (u->service_fd >= 0) {
1894         pa_close(u->service_fd);
1895         u->service_fd = -1;
1896         u->service_write_type = 0;
1897         u->service_read_type = 0;
1898     }
1899
1900     if (u->write_memchunk.memblock) {
1901         pa_memblock_unref(u->write_memchunk.memblock);
1902         pa_memchunk_reset(&u->write_memchunk);
1903     }
1904 }
1905
1906 /* Run from main thread */
1907 static int init_bt(struct userdata *u) {
1908     pa_assert(u);
1909
1910     shutdown_bt(u);
1911
1912     u->stream_write_type = 0;
1913     u->service_write_type = 0;
1914     u->service_read_type = 0;
1915
1916     if ((u->service_fd = bt_audio_service_open()) < 0) {
1917         pa_log_error("Couldn't connect to bluetooth audio service");
1918         return -1;
1919     }
1920
1921     pa_log_debug("Connected to the bluetooth audio service");
1922
1923     return 0;
1924 }
1925
1926 /* Run from main thread */
1927 static int setup_bt(struct userdata *u) {
1928     pa_assert(u);
1929
1930     if (get_caps(u, 0) < 0)
1931         return -1;
1932
1933     pa_log_debug("Got device capabilities");
1934
1935     if (set_conf(u) < 0)
1936         return -1;
1937
1938     pa_log_debug("Connection to the device configured");
1939
1940 #ifdef NOKIA
1941     if (USE_SCO_OVER_PCM(u)) {
1942         pa_log_debug("Configured to use SCO over PCM");
1943         return 0;
1944     }
1945 #endif
1946
1947     pa_log_debug("Got the stream socket");
1948
1949     return 0;
1950 }
1951
1952 /* Run from main thread */
1953 static int init_profile(struct userdata *u) {
1954     int r = 0;
1955     pa_assert(u);
1956     pa_assert(u->profile != PROFILE_OFF);
1957
1958     if (setup_bt(u) < 0)
1959         return -1;
1960
1961     if (u->profile == PROFILE_A2DP ||
1962         u->profile == PROFILE_HSP)
1963         if (add_sink(u) < 0)
1964             r = -1;
1965
1966     if (u->profile == PROFILE_HSP ||
1967         u->profile == PROFILE_A2DP_SOURCE)
1968         if (add_source(u) < 0)
1969             r = -1;
1970
1971     return r;
1972 }
1973
1974 /* Run from main thread */
1975 static void stop_thread(struct userdata *u) {
1976     pa_assert(u);
1977
1978     if (u->thread) {
1979         pa_asyncmsgq_send(u->thread_mq.inq, NULL, PA_MESSAGE_SHUTDOWN, NULL, 0, NULL);
1980         pa_thread_free(u->thread);
1981         u->thread = NULL;
1982     }
1983
1984     if (u->rtpoll_item) {
1985         pa_rtpoll_item_free(u->rtpoll_item);
1986         u->rtpoll_item = NULL;
1987     }
1988
1989     if (u->hsp.sink_state_changed_slot) {
1990         pa_hook_slot_free(u->hsp.sink_state_changed_slot);
1991         u->hsp.sink_state_changed_slot = NULL;
1992     }
1993
1994     if (u->hsp.source_state_changed_slot) {
1995         pa_hook_slot_free(u->hsp.source_state_changed_slot);
1996         u->hsp.source_state_changed_slot = NULL;
1997     }
1998
1999     if (u->sink) {
2000         pa_sink_unref(u->sink);
2001         u->sink = NULL;
2002     }
2003
2004     if (u->source) {
2005         pa_source_unref(u->source);
2006         u->source = NULL;
2007     }
2008
2009     if (u->rtpoll) {
2010         pa_thread_mq_done(&u->thread_mq);
2011
2012         pa_rtpoll_free(u->rtpoll);
2013         u->rtpoll = NULL;
2014     }
2015
2016     if (u->read_smoother) {
2017         pa_smoother_free(u->read_smoother);
2018         u->read_smoother = NULL;
2019     }
2020 }
2021
2022 /* Run from main thread */
2023 static int start_thread(struct userdata *u) {
2024     pa_assert(u);
2025     pa_assert(!u->thread);
2026     pa_assert(!u->rtpoll);
2027     pa_assert(!u->rtpoll_item);
2028
2029     u->rtpoll = pa_rtpoll_new();
2030     pa_thread_mq_init(&u->thread_mq, u->core->mainloop, u->rtpoll);
2031
2032 #ifdef NOKIA
2033     if (USE_SCO_OVER_PCM(u)) {
2034         if (start_stream_fd(u) < 0)
2035             return -1;
2036
2037         pa_sink_ref(u->sink);
2038         pa_source_ref(u->source);
2039         /* FIXME: monitor stream_fd error */
2040         return 0;
2041     }
2042 #endif
2043
2044     if (!(u->thread = pa_thread_new(thread_func, u))) {
2045         pa_log_error("Failed to create IO thread");
2046         stop_thread(u);
2047         return -1;
2048     }
2049
2050     if (u->sink) {
2051         pa_sink_set_asyncmsgq(u->sink, u->thread_mq.inq);
2052         pa_sink_set_rtpoll(u->sink, u->rtpoll);
2053         pa_sink_put(u->sink);
2054
2055         if (u->sink->set_volume)
2056             u->sink->set_volume(u->sink);
2057     }
2058
2059     if (u->source) {
2060         pa_source_set_asyncmsgq(u->source, u->thread_mq.inq);
2061         pa_source_set_rtpoll(u->source, u->rtpoll);
2062         pa_source_put(u->source);
2063
2064         if (u->source->set_volume)
2065             u->source->set_volume(u->source);
2066     }
2067
2068     return 0;
2069 }
2070
2071 /* Run from main thread */
2072 static int card_set_profile(pa_card *c, pa_card_profile *new_profile) {
2073     struct userdata *u;
2074     enum profile *d;
2075     pa_queue *inputs = NULL, *outputs = NULL;
2076     const pa_bluetooth_device *device;
2077
2078     pa_assert(c);
2079     pa_assert(new_profile);
2080     pa_assert_se(u = c->userdata);
2081
2082     d = PA_CARD_PROFILE_DATA(new_profile);
2083
2084     if (!(device = pa_bluetooth_discovery_get_by_path(u->discovery, u->path))) {
2085         pa_log_error("Failed to get device object.");
2086         return -PA_ERR_IO;
2087     }
2088
2089     /* The state signal is sent by bluez, so it is racy to check
2090        strictly for CONNECTED, we should also accept STREAMING state
2091        as being good enough. However, if the profile is used
2092        concurrently (which is unlikely), ipc will fail later on, and
2093        module will be unloaded. */
2094     if (device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) {
2095         pa_log_warn("HSP is not connected, refused to switch profile");
2096         return -PA_ERR_IO;
2097     }
2098     else if (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP) {
2099         pa_log_warn("A2DP is not connected, refused to switch profile");
2100         return -PA_ERR_IO;
2101     }
2102
2103     if (u->sink) {
2104         inputs = pa_sink_move_all_start(u->sink, NULL);
2105 #ifdef NOKIA
2106         if (!USE_SCO_OVER_PCM(u))
2107 #endif
2108             pa_sink_unlink(u->sink);
2109     }
2110
2111     if (u->source) {
2112         outputs = pa_source_move_all_start(u->source, NULL);
2113 #ifdef NOKIA
2114         if (!USE_SCO_OVER_PCM(u))
2115 #endif
2116             pa_source_unlink(u->source);
2117     }
2118
2119     stop_thread(u);
2120     shutdown_bt(u);
2121
2122     u->profile = *d;
2123     u->sample_spec = u->requested_sample_spec;
2124
2125     init_bt(u);
2126
2127     if (u->profile != PROFILE_OFF)
2128         init_profile(u);
2129
2130     if (u->sink || u->source)
2131         start_thread(u);
2132
2133     if (inputs) {
2134         if (u->sink)
2135             pa_sink_move_all_finish(u->sink, inputs, FALSE);
2136         else
2137             pa_sink_move_all_fail(inputs);
2138     }
2139
2140     if (outputs) {
2141         if (u->source)
2142             pa_source_move_all_finish(u->source, outputs, FALSE);
2143         else
2144             pa_source_move_all_fail(outputs);
2145     }
2146
2147     return 0;
2148 }
2149
2150 /* Run from main thread */
2151 static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
2152     pa_card_new_data data;
2153     pa_bool_t b;
2154     pa_card_profile *p;
2155     enum profile *d;
2156     const char *ff;
2157     char *n;
2158     const char *default_profile;
2159
2160     pa_assert(u);
2161     pa_assert(device);
2162
2163     pa_card_new_data_init(&data);
2164     data.driver = __FILE__;
2165     data.module = u->module;
2166
2167     n = pa_bluetooth_cleanup_name(device->name);
2168     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_DESCRIPTION, n);
2169     pa_xfree(n);
2170     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_STRING, device->address);
2171     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_API, "bluez");
2172     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_CLASS, "sound");
2173     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_BUS, "bluetooth");
2174     if ((ff = pa_bluetooth_get_form_factor(device->class)))
2175         pa_proplist_sets(data.proplist, PA_PROP_DEVICE_FORM_FACTOR, ff);
2176     pa_proplist_sets(data.proplist, "bluez.path", device->path);
2177     pa_proplist_setf(data.proplist, "bluez.class", "0x%06x", (unsigned) device->class);
2178     pa_proplist_sets(data.proplist, "bluez.name", device->name);
2179     data.name = get_name("card", u->modargs, device->address, &b);
2180     data.namereg_fail = b;
2181
2182     if (pa_modargs_get_proplist(u->modargs, "card_properties", data.proplist, PA_UPDATE_REPLACE) < 0) {
2183         pa_log("Invalid properties");
2184         pa_card_new_data_done(&data);
2185         return -1;
2186     }
2187
2188     data.profiles = pa_hashmap_new(pa_idxset_string_hash_func, pa_idxset_string_compare_func);
2189
2190     /* we base hsp/a2dp availability on UUIDs.
2191        Ideally, it would be based on "Connected" state, but
2192        we can't afford to wait for this information when
2193        we are loaded with profile="hsp", for instance */
2194     if (pa_bluetooth_uuid_has(device->uuids, A2DP_SINK_UUID)) {
2195         p = pa_card_profile_new("a2dp", _("High Fidelity Playback (A2DP)"), sizeof(enum profile));
2196         p->priority = 10;
2197         p->n_sinks = 1;
2198         p->n_sources = 0;
2199         p->max_sink_channels = 2;
2200         p->max_source_channels = 0;
2201
2202         d = PA_CARD_PROFILE_DATA(p);
2203         *d = PROFILE_A2DP;
2204
2205         pa_hashmap_put(data.profiles, p->name, p);
2206     }
2207
2208     if (pa_bluetooth_uuid_has(device->uuids, A2DP_SOURCE_UUID)) {
2209         p = pa_card_profile_new("a2dp_source", _("High Fidelity Capture (A2DP)"), sizeof(enum profile));
2210         p->priority = 10;
2211         p->n_sinks = 0;
2212         p->n_sources = 1;
2213         p->max_sink_channels = 0;
2214         p->max_source_channels = 2;
2215
2216         d = PA_CARD_PROFILE_DATA(p);
2217         *d = PROFILE_A2DP_SOURCE;
2218
2219         pa_hashmap_put(data.profiles, p->name, p);
2220     }
2221
2222     if (pa_bluetooth_uuid_has(device->uuids, HSP_HS_UUID) ||
2223         pa_bluetooth_uuid_has(device->uuids, HFP_HS_UUID)) {
2224         p = pa_card_profile_new("hsp", _("Telephony Duplex (HSP/HFP)"), sizeof(enum profile));
2225         p->priority = 20;
2226         p->n_sinks = 1;
2227         p->n_sources = 1;
2228         p->max_sink_channels = 1;
2229         p->max_source_channels = 1;
2230
2231         d = PA_CARD_PROFILE_DATA(p);
2232         *d = PROFILE_HSP;
2233
2234         pa_hashmap_put(data.profiles, p->name, p);
2235     }
2236
2237     pa_assert(!pa_hashmap_isempty(data.profiles));
2238
2239     p = pa_card_profile_new("off", _("Off"), sizeof(enum profile));
2240     d = PA_CARD_PROFILE_DATA(p);
2241     *d = PROFILE_OFF;
2242     pa_hashmap_put(data.profiles, p->name, p);
2243
2244     if ((default_profile = pa_modargs_get_value(u->modargs, "profile", NULL))) {
2245         if (pa_hashmap_get(data.profiles, default_profile))
2246             pa_card_new_data_set_profile(&data, default_profile);
2247         else
2248             pa_log_warn("Profile '%s' not valid or not supported by device.", default_profile);
2249     }
2250
2251     u->card = pa_card_new(u->core, &data);
2252     pa_card_new_data_done(&data);
2253
2254     if (!u->card) {
2255         pa_log("Failed to allocate card.");
2256         return -1;
2257     }
2258
2259     u->card->userdata = u;
2260     u->card->set_profile = card_set_profile;
2261
2262     d = PA_CARD_PROFILE_DATA(u->card->active_profile);
2263
2264     if ((device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) ||
2265         (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP)) {
2266         pa_log_warn("Default profile not connected, selecting off profile");
2267         u->card->active_profile = pa_hashmap_get(u->card->profiles, "off");
2268         u->card->save_profile = FALSE;
2269     }
2270
2271     d = PA_CARD_PROFILE_DATA(u->card->active_profile);
2272     u->profile = *d;
2273
2274     return 0;
2275 }
2276
2277 /* Run from main thread */
2278 static const pa_bluetooth_device* find_device(struct userdata *u, const char *address, const char *path) {
2279     const pa_bluetooth_device *d = NULL;
2280
2281     pa_assert(u);
2282
2283     if (!address && !path) {
2284         pa_log_error("Failed to get device address/path from module arguments.");
2285         return NULL;
2286     }
2287
2288     if (path) {
2289         if (!(d = pa_bluetooth_discovery_get_by_path(u->discovery, path))) {
2290             pa_log_error("%s is not a valid BlueZ audio device.", path);
2291             return NULL;
2292         }
2293
2294         if (address && !(pa_streq(d->address, address))) {
2295             pa_log_error("Passed path %s and address %s don't match.", path, address);
2296             return NULL;
2297         }
2298
2299     } else {
2300         if (!(d = pa_bluetooth_discovery_get_by_address(u->discovery, address))) {
2301             pa_log_error("%s is not known.", address);
2302             return NULL;
2303         }
2304     }
2305
2306     if (d) {
2307         u->address = pa_xstrdup(d->address);
2308         u->path = pa_xstrdup(d->path);
2309     }
2310
2311     return d;
2312 }
2313
2314 /* Run from main thread */
2315 static int setup_dbus(struct userdata *u) {
2316     DBusError err;
2317
2318     dbus_error_init(&err);
2319
2320     u->connection = pa_dbus_bus_get(u->core, DBUS_BUS_SYSTEM, &err);
2321
2322     if (dbus_error_is_set(&err) || !u->connection) {
2323         pa_log("Failed to get D-Bus connection: %s", err.message);
2324         dbus_error_free(&err);
2325         return -1;
2326     }
2327
2328     return 0;
2329 }
2330
2331 int pa__init(pa_module* m) {
2332     pa_modargs *ma;
2333     uint32_t channels;
2334     struct userdata *u;
2335     const char *address, *path;
2336     DBusError err;
2337     char *mike, *speaker;
2338     const pa_bluetooth_device *device;
2339
2340     pa_assert(m);
2341
2342     dbus_error_init(&err);
2343
2344     if (!(ma = pa_modargs_new(m->argument, valid_modargs))) {
2345         pa_log_error("Failed to parse module arguments");
2346         goto fail;
2347     }
2348
2349     m->userdata = u = pa_xnew0(struct userdata, 1);
2350     u->module = m;
2351     u->core = m->core;
2352     u->service_fd = -1;
2353     u->stream_fd = -1;
2354     u->sample_spec = m->core->default_sample_spec;
2355     u->modargs = ma;
2356
2357 #ifdef NOKIA
2358     if (pa_modargs_get_value(ma, "sco_sink", NULL) &&
2359         !(u->hsp.sco_sink = pa_namereg_get(m->core, pa_modargs_get_value(ma, "sco_sink", NULL), PA_NAMEREG_SINK))) {
2360         pa_log("SCO sink not found");
2361         goto fail;
2362     }
2363
2364     if (pa_modargs_get_value(ma, "sco_source", NULL) &&
2365         !(u->hsp.sco_source = pa_namereg_get(m->core, pa_modargs_get_value(ma, "sco_source", NULL), PA_NAMEREG_SOURCE))) {
2366         pa_log("SCO source not found");
2367         goto fail;
2368     }
2369 #endif
2370
2371     if (pa_modargs_get_value_u32(ma, "rate", &u->sample_spec.rate) < 0 ||
2372         u->sample_spec.rate <= 0 || u->sample_spec.rate > PA_RATE_MAX) {
2373         pa_log_error("Failed to get rate from module arguments");
2374         goto fail;
2375     }
2376
2377     u->auto_connect = TRUE;
2378     if (pa_modargs_get_value_boolean(ma, "auto_connect", &u->auto_connect)) {
2379         pa_log("Failed to parse auto_connect= argument");
2380         goto fail;
2381     }
2382
2383     channels = u->sample_spec.channels;
2384     if (pa_modargs_get_value_u32(ma, "channels", &channels) < 0 ||
2385         channels <= 0 || channels > PA_CHANNELS_MAX) {
2386         pa_log_error("Failed to get channels from module arguments");
2387         goto fail;
2388     }
2389     u->sample_spec.channels = (uint8_t) channels;
2390     u->requested_sample_spec = u->sample_spec;
2391
2392     address = pa_modargs_get_value(ma, "address", NULL);
2393     path = pa_modargs_get_value(ma, "path", NULL);
2394
2395     if (setup_dbus(u) < 0)
2396         goto fail;
2397
2398     if (!(u->discovery = pa_bluetooth_discovery_get(m->core)))
2399         goto fail;
2400
2401     if (!(device = find_device(u, address, path)))
2402         goto fail;
2403
2404     /* Add the card structure. This will also initialize the default profile */
2405     if (add_card(u, device) < 0)
2406         goto fail;
2407
2408     /* Connect to the BT service and query capabilities */
2409     if (init_bt(u) < 0)
2410         goto fail;
2411
2412     if (!dbus_connection_add_filter(pa_dbus_connection_get(u->connection), filter_cb, u, NULL)) {
2413         pa_log_error("Failed to add filter function");
2414         goto fail;
2415     }
2416     u->filter_added = TRUE;
2417
2418     speaker = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='SpeakerGainChanged',path='%s'", u->path);
2419     mike = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='MicrophoneGainChanged',path='%s'", u->path);
2420
2421     if (pa_dbus_add_matches(
2422                 pa_dbus_connection_get(u->connection), &err,
2423                 speaker,
2424                 mike,
2425                 NULL) < 0) {
2426
2427         pa_xfree(speaker);
2428         pa_xfree(mike);
2429
2430         pa_log("Failed to add D-Bus matches: %s", err.message);
2431         goto fail;
2432     }
2433
2434     pa_xfree(speaker);
2435     pa_xfree(mike);
2436
2437     if (u->profile != PROFILE_OFF)
2438         if (init_profile(u) < 0)
2439             goto fail;
2440
2441     if (u->sink || u->source)
2442         if (start_thread(u) < 0)
2443             goto fail;
2444
2445     return 0;
2446
2447 fail:
2448
2449     pa__done(m);
2450
2451     dbus_error_free(&err);
2452
2453     return -1;
2454 }
2455
2456 int pa__get_n_used(pa_module *m) {
2457     struct userdata *u;
2458
2459     pa_assert(m);
2460     pa_assert_se(u = m->userdata);
2461
2462     return
2463         (u->sink ? pa_sink_linked_by(u->sink) : 0) +
2464         (u->source ? pa_source_linked_by(u->source) : 0);
2465 }
2466
2467 void pa__done(pa_module *m) {
2468     struct userdata *u;
2469     pa_assert(m);
2470
2471     if (!(u = m->userdata))
2472         return;
2473
2474     if (u->sink
2475 #ifdef NOKIA
2476         && !USE_SCO_OVER_PCM(u)
2477 #endif
2478     )
2479         pa_sink_unlink(u->sink);
2480
2481     if (u->source
2482 #ifdef NOKIA
2483         && !USE_SCO_OVER_PCM(u)
2484 #endif
2485     )
2486         pa_source_unlink(u->source);
2487
2488     stop_thread(u);
2489
2490     if (u->connection) {
2491
2492         if (u->path) {
2493             char *speaker, *mike;
2494             speaker = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='SpeakerGainChanged',path='%s'", u->path);
2495             mike = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='MicrophoneGainChanged',path='%s'", u->path);
2496
2497             pa_dbus_remove_matches(pa_dbus_connection_get(u->connection),
2498                                    speaker,
2499                                    mike,
2500                                    NULL);
2501
2502             pa_xfree(speaker);
2503             pa_xfree(mike);
2504         }
2505
2506         if (u->filter_added)
2507             dbus_connection_remove_filter(pa_dbus_connection_get(u->connection), filter_cb, u);
2508
2509         pa_dbus_connection_unref(u->connection);
2510     }
2511
2512     if (u->card)
2513         pa_card_free(u->card);
2514
2515     if (u->read_smoother)
2516         pa_smoother_free(u->read_smoother);
2517
2518     shutdown_bt(u);
2519
2520     if (u->a2dp.buffer)
2521         pa_xfree(u->a2dp.buffer);
2522
2523     sbc_finish(&u->a2dp.sbc);
2524
2525     if (u->modargs)
2526         pa_modargs_free(u->modargs);
2527
2528     pa_xfree(u->address);
2529     pa_xfree(u->path);
2530
2531     if (u->discovery)
2532         pa_bluetooth_discovery_unref(u->discovery);
2533
2534     pa_xfree(u);
2535 }