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             wi = pa_smoother_get(u->read_smoother, pa_rtclock_now());
 976             ri = pa_bytes_to_usec(u->read_index, &u->sample_spec);
 977
 978             *((pa_usec_t*) data) = (wi > ri ? wi - ri : 0) + u->source->thread_info.fixed_latency;
 979             return 0;
 980         }
 981
 982     }
 983
 984     r = pa_source_process_msg(o, code, data, offset, chunk);
 985
 986     return (r < 0 || !failed) ? r : -1;
 987 }
 988
 989 /* Run from IO thread */
 990 static int hsp_process_render(struct userdata *u) {
 991     int ret = 0;
 992
 993     pa_assert(u);
 994     pa_assert(u->profile == PROFILE_HSP);
 995     pa_assert(u->sink);
 996
 997     /* First, render some data */
 998     if (!u->write_memchunk.memblock)
 999         pa_sink_render_full(u->sink, u->block_size, &u->write_memchunk);
1000
1001     pa_assert(u->write_memchunk.length == u->block_size);
1002
1003     for (;;) {
1004         ssize_t l;
1005         const void *p;
1006
1007         /* Now write that data to the socket. The socket is of type
1008          * SEQPACKET, and we generated the data of the MTU size, so this
1009          * should just work. */
1010
1011         p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock) + u->write_memchunk.index;
1012         l = pa_write(u->stream_fd, p, u->write_memchunk.length, &u->stream_write_type);
1013         pa_memblock_release(u->write_memchunk.memblock);
1014
1015         pa_assert(l != 0);
1016
1017         if (l < 0) {
1018
1019             if (errno == EINTR)
1020                 /* Retry right away if we got interrupted */
1021                 continue;
1022
1023             else if (errno == EAGAIN)
1024                 /* Hmm, apparently the socket was not writable, give up for now */
1025                 break;
1026
1027             pa_log_error("Failed to write data to SCO socket: %s", pa_cstrerror(errno));
1028             ret = -1;
1029             break;
1030         }
1031
1032         pa_assert((size_t) l <= u->write_memchunk.length);
1033
1034         if ((size_t) l != u->write_memchunk.length) {
1035             pa_log_error("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
1036                         (unsigned long long) l,
1037                         (unsigned long long) u->write_memchunk.length);
1038             ret = -1;
1039             break;
1040         }
1041
1042         u->write_index += (uint64_t) u->write_memchunk.length;
1043         pa_memblock_unref(u->write_memchunk.memblock);
1044         pa_memchunk_reset(&u->write_memchunk);
1045
1046         ret = 1;
1047         break;
1048     }
1049
1050     return ret;
1051 }
1052
1053 /* Run from IO thread */
1054 static int hsp_process_push(struct userdata *u) {
1055     int ret = 0;
1056     pa_memchunk memchunk;
1057
1058     pa_assert(u);
1059     pa_assert(u->profile == PROFILE_HSP);
1060     pa_assert(u->source);
1061     pa_assert(u->read_smoother);
1062
1063     memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
1064     memchunk.index = memchunk.length = 0;
1065
1066     for (;;) {
1067         ssize_t l;
1068         void *p;
1069         struct msghdr m;
1070         struct cmsghdr *cm;
1071         uint8_t aux[1024];
1072         struct iovec iov;
1073         pa_bool_t found_tstamp = FALSE;
1074         pa_usec_t tstamp;
1075
1076         memset(&m, 0, sizeof(m));
1077         memset(&aux, 0, sizeof(aux));
1078         memset(&iov, 0, sizeof(iov));
1079
1080         m.msg_iov = &iov;
1081         m.msg_iovlen = 1;
1082         m.msg_control = aux;
1083         m.msg_controllen = sizeof(aux);
1084
1085         p = pa_memblock_acquire(memchunk.memblock);
1086         iov.iov_base = p;
1087         iov.iov_len = pa_memblock_get_length(memchunk.memblock);
1088         l = recvmsg(u->stream_fd, &m, 0);
1089         pa_memblock_release(memchunk.memblock);
1090
1091         if (l <= 0) {
1092
1093             if (l < 0 && errno == EINTR)
1094                 /* Retry right away if we got interrupted */
1095                 continue;
1096
1097             else if (l < 0 && errno == EAGAIN)
1098                 /* Hmm, apparently the socket was not readable, give up for now. */
1099                 break;
1100
1101             pa_log_error("Failed to read data from SCO socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
1102             ret = -1;
1103             break;
1104         }
1105
1106         pa_assert((size_t) l <= pa_memblock_get_length(memchunk.memblock));
1107
1108         memchunk.length = (size_t) l;
1109         u->read_index += (uint64_t) l;
1110
1111         for (cm = CMSG_FIRSTHDR(&m); cm; cm = CMSG_NXTHDR(&m, cm))
1112             if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMP) {
1113                 struct timeval *tv = (struct timeval*) CMSG_DATA(cm);
1114                 pa_rtclock_from_wallclock(tv);
1115                 tstamp = pa_timeval_load(tv);
1116                 found_tstamp = TRUE;
1117                 break;
1118             }
1119
1120         if (!found_tstamp) {
1121             pa_log_warn("Couldn't find SO_TIMESTAMP data in auxiliary recvmsg() data!");
1122             tstamp = pa_rtclock_now();
1123         }
1124
1125         pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->sample_spec));
1126         pa_smoother_resume(u->read_smoother, tstamp, TRUE);
1127
1128         pa_source_post(u->source, &memchunk);
1129
1130         ret = 1;
1131         break;
1132     }
1133
1134     pa_memblock_unref(memchunk.memblock);
1135
1136     return ret;
1137 }
1138
1139 /* Run from IO thread */
1140 static void a2dp_prepare_buffer(struct userdata *u) {
1141     pa_assert(u);
1142
1143     if (u->a2dp.buffer_size >= u->link_mtu)
1144         return;
1145
1146     u->a2dp.buffer_size = 2 * u->link_mtu;
1147     pa_xfree(u->a2dp.buffer);
1148     u->a2dp.buffer = pa_xmalloc(u->a2dp.buffer_size);
1149 }
1150
1151 /* Run from IO thread */
1152 static int a2dp_process_render(struct userdata *u) {
1153     struct a2dp_info *a2dp;
1154     struct rtp_header *header;
1155     struct rtp_payload *payload;
1156     size_t nbytes;
1157     void *d;
1158     const void *p;
1159     size_t to_write, to_encode;
1160     unsigned frame_count;
1161     int ret = 0;
1162
1163     pa_assert(u);
1164     pa_assert(u->profile == PROFILE_A2DP);
1165     pa_assert(u->sink);
1166
1167     /* First, render some data */
1168     if (!u->write_memchunk.memblock)
1169         pa_sink_render_full(u->sink, u->block_size, &u->write_memchunk);
1170
1171     pa_assert(u->write_memchunk.length == u->block_size);
1172
1173     a2dp_prepare_buffer(u);
1174
1175     a2dp = &u->a2dp;
1176     header = a2dp->buffer;
1177     payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
1178
1179     frame_count = 0;
1180
1181     /* Try to create a packet of the full MTU */
1182
1183     p = (const uint8_t*) pa_memblock_acquire(u->write_memchunk.memblock) + u->write_memchunk.index;
1184     to_encode = u->write_memchunk.length;
1185
1186     d = (uint8_t*) a2dp->buffer + sizeof(*header) + sizeof(*payload);
1187     to_write = a2dp->buffer_size - sizeof(*header) - sizeof(*payload);
1188
1189     while (PA_LIKELY(to_encode > 0 && to_write > 0)) {
1190         size_t written;
1191         ssize_t encoded;
1192
1193         encoded = sbc_encode(&a2dp->sbc,
1194                              p, to_encode,
1195                              d, to_write,
1196                              &written);
1197
1198         if (PA_UNLIKELY(encoded <= 0)) {
1199             pa_log_error("SBC encoding error (%li)", (long) encoded);
1200             pa_memblock_release(u->write_memchunk.memblock);
1201             return -1;
1202         }
1203
1204 /*         pa_log_debug("SBC: encoded: %lu; written: %lu", (unsigned long) encoded, (unsigned long) written); */
1205 /*         pa_log_debug("SBC: codesize: %lu; frame_length: %lu", (unsigned long) a2dp->codesize, (unsigned long) a2dp->frame_length); */
1206
1207         pa_assert_fp((size_t) encoded <= to_encode);
1208         pa_assert_fp((size_t) encoded == a2dp->codesize);
1209
1210         pa_assert_fp((size_t) written <= to_write);
1211         pa_assert_fp((size_t) written == a2dp->frame_length);
1212
1213         p = (const uint8_t*) p + encoded;
1214         to_encode -= encoded;
1215
1216         d = (uint8_t*) d + written;
1217         to_write -= written;
1218
1219         frame_count++;
1220     }
1221
1222     pa_memblock_release(u->write_memchunk.memblock);
1223
1224     pa_assert(to_encode == 0);
1225
1226     PA_ONCE_BEGIN {
1227         pa_log_debug("Using SBC encoder implementation: %s", pa_strnull(sbc_get_implementation_info(&a2dp->sbc)));
1228     } PA_ONCE_END;
1229
1230     /* write it to the fifo */
1231     memset(a2dp->buffer, 0, sizeof(*header) + sizeof(*payload));
1232     header->v = 2;
1233     header->pt = 1;
1234     header->sequence_number = htons(a2dp->seq_num++);
1235     header->timestamp = htonl(u->write_index / pa_frame_size(&u->sample_spec));
1236     header->ssrc = htonl(1);
1237     payload->frame_count = frame_count;
1238
1239     nbytes = (uint8_t*) d - (uint8_t*) a2dp->buffer;
1240
1241     for (;;) {
1242         ssize_t l;
1243
1244         l = pa_write(u->stream_fd, a2dp->buffer, nbytes, &u->stream_write_type);
1245
1246         pa_assert(l != 0);
1247
1248         if (l < 0) {
1249
1250             if (errno == EINTR)
1251                 /* Retry right away if we got interrupted */
1252                 continue;
1253
1254             else if (errno == EAGAIN)
1255                 /* Hmm, apparently the socket was not writable, give up for now */
1256                 break;
1257
1258             pa_log_error("Failed to write data to socket: %s", pa_cstrerror(errno));
1259             ret  = -1;
1260             break;
1261         }
1262
1263         pa_assert((size_t) l <= nbytes);
1264
1265         if ((size_t) l != nbytes) {
1266             pa_log_warn("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
1267                         (unsigned long long) l,
1268                         (unsigned long long) nbytes);
1269             ret = -1;
1270             break;
1271         }
1272
1273         u->write_index += (uint64_t) u->write_memchunk.length;
1274         pa_memblock_unref(u->write_memchunk.memblock);
1275         pa_memchunk_reset(&u->write_memchunk);
1276
1277         ret = 1;
1278
1279         break;
1280     }
1281
1282     return ret;
1283 }
1284
1285 static int a2dp_process_push(struct userdata *u) {
1286     int ret = 0;
1287     pa_memchunk memchunk;
1288
1289     pa_assert(u);
1290     pa_assert(u->profile == PROFILE_A2DP_SOURCE);
1291     pa_assert(u->source);
1292     pa_assert(u->read_smoother);
1293
1294     memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
1295     memchunk.index = memchunk.length = 0;
1296
1297     for (;;) {
1298         pa_bool_t found_tstamp = FALSE;
1299         pa_usec_t tstamp;
1300         struct a2dp_info *a2dp;
1301         struct rtp_header *header;
1302         struct rtp_payload *payload;
1303         const void *p;
1304         void *d;
1305         ssize_t l;
1306         size_t to_write, to_decode;
1307         unsigned frame_count;
1308
1309         a2dp_prepare_buffer(u);
1310
1311         a2dp = &u->a2dp;
1312         header = a2dp->buffer;
1313         payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
1314
1315         l = pa_read(u->stream_fd, a2dp->buffer, a2dp->buffer_size, &u->stream_write_type);
1316
1317         if (l <= 0) {
1318
1319             if (l < 0 && errno == EINTR)
1320                 /* Retry right away if we got interrupted */
1321                 continue;
1322
1323             else if (l < 0 && errno == EAGAIN)
1324                 /* Hmm, apparently the socket was not readable, give up for now. */
1325                 break;
1326
1327             pa_log_error("Failed to read data from socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
1328             ret = -1;
1329             break;
1330         }
1331
1332         pa_assert((size_t) l <= a2dp->buffer_size);
1333
1334         u->read_index += (uint64_t) l;
1335
1336         /* TODO: get timestamp from rtp */
1337         if (!found_tstamp) {
1338             /* pa_log_warn("Couldn't find SO_TIMESTAMP data in auxiliary recvmsg() data!"); */
1339             tstamp = pa_rtclock_now();
1340         }
1341
1342         pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->sample_spec));
1343         pa_smoother_resume(u->read_smoother, tstamp, TRUE);
1344
1345         p = (uint8_t*) a2dp->buffer + sizeof(*header) + sizeof(*payload);
1346         to_decode = l - sizeof(*header) - sizeof(*payload);
1347
1348         d = pa_memblock_acquire(memchunk.memblock);
1349         to_write = memchunk.length = pa_memblock_get_length(memchunk.memblock);
1350
1351         while (PA_LIKELY(to_decode > 0 && to_write > 0)) {
1352             size_t written;
1353             ssize_t decoded;
1354
1355             decoded = sbc_decode(&a2dp->sbc,
1356                                  p, to_decode,
1357                                  d, to_write,
1358                                  &written);
1359
1360             if (PA_UNLIKELY(decoded <= 0)) {
1361                 pa_log_error("SBC decoding error (%li)", (long) decoded);
1362                 pa_memblock_release(memchunk.memblock);
1363                 pa_memblock_unref(memchunk.memblock);
1364                 return -1;
1365             }
1366
1367 /*             pa_log_debug("SBC: decoded: %lu; written: %lu", (unsigned long) decoded, (unsigned long) written); */
1368 /*             pa_log_debug("SBC: frame_length: %lu; codesize: %lu", (unsigned long) a2dp->frame_length, (unsigned long) a2dp->codesize); */
1369
1370             pa_assert_fp((size_t) decoded <= to_decode);
1371             pa_assert_fp((size_t) decoded == a2dp->frame_length);
1372
1373             pa_assert_fp((size_t) written <= to_write);
1374             pa_assert_fp((size_t) written == a2dp->codesize);
1375
1376             p = (const uint8_t*) p + decoded;
1377             to_decode -= decoded;
1378
1379             d = (uint8_t*) d + written;
1380             to_write -= written;
1381
1382             frame_count++;
1383         }
1384
1385         pa_memblock_release(memchunk.memblock);
1386
1387         pa_source_post(u->source, &memchunk);
1388
1389         ret = 1;
1390         break;
1391     }
1392
1393     pa_memblock_unref(memchunk.memblock);
1394
1395     return ret;
1396 }
1397
1398 static void thread_func(void *userdata) {
1399     struct userdata *u = userdata;
1400     unsigned do_write = 0;
1401     pa_bool_t writable = FALSE;
1402
1403     pa_assert(u);
1404
1405     pa_log_debug("IO Thread starting up");
1406
1407     if (u->core->realtime_scheduling)
1408         pa_make_realtime(u->core->realtime_priority);
1409
1410     pa_thread_mq_install(&u->thread_mq);
1411
1412     if (start_stream_fd(u) < 0)
1413         goto fail;
1414
1415     for (;;) {
1416         struct pollfd *pollfd;
1417         int ret;
1418         pa_bool_t disable_timer = TRUE;
1419
1420         pollfd = u->rtpoll_item ? pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL) : NULL;
1421
1422         if (u->source && PA_SOURCE_IS_LINKED(u->source->thread_info.state)) {
1423
1424             /* We should send two blocks to the device before we expect
1425              * a response. */
1426
1427             if (u->write_index == 0 && u->read_index <= 0)
1428                 do_write = 2;
1429
1430             if (pollfd && (pollfd->revents & POLLIN)) {
1431                 int n_read;
1432
1433                 if (u->profile == PROFILE_HSP)
1434                     n_read = hsp_process_push(u);
1435                 else
1436                     n_read = a2dp_process_push(u);
1437
1438                 if (n_read < 0)
1439                     goto fail;
1440
1441                 /* We just read something, so we are supposed to write something, too */
1442                 do_write += n_read;
1443             }
1444         }
1445
1446         if (u->sink && PA_SINK_IS_LINKED(u->sink->thread_info.state)) {
1447
1448             if (u->sink->thread_info.rewind_requested)
1449                 pa_sink_process_rewind(u->sink, 0);
1450
1451             if (pollfd) {
1452                 if (pollfd->revents & POLLOUT)
1453                     writable = TRUE;
1454
1455                 if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable) {
1456                     pa_usec_t time_passed;
1457                     pa_usec_t audio_sent;
1458
1459                     /* Hmm, there is no input stream we could synchronize
1460                      * to. So let's do things by time */
1461
1462                     time_passed = pa_rtclock_now() - u->started_at;
1463                     audio_sent = pa_bytes_to_usec(u->write_index, &u->sample_spec);
1464
1465                     if (audio_sent <= time_passed) {
1466                         pa_usec_t audio_to_send = time_passed - audio_sent;
1467
1468                         /* Never try to catch up for more than 100ms */
1469                         if (u->write_index > 0 && audio_to_send > MAX_PLAYBACK_CATCH_UP_USEC) {
1470                             pa_usec_t skip_usec;
1471                             uint64_t skip_bytes;
1472
1473                             skip_usec = audio_to_send - MAX_PLAYBACK_CATCH_UP_USEC;
1474                             skip_bytes = pa_usec_to_bytes(skip_usec, &u->sample_spec);
1475
1476                             if (skip_bytes > 0) {
1477                                 pa_memchunk tmp;
1478
1479                                 pa_log_warn("Skipping %llu us (= %llu bytes) in audio stream",
1480                                             (unsigned long long) skip_usec,
1481                                             (unsigned long long) skip_bytes);
1482
1483                                 pa_sink_render_full(u->sink, skip_bytes, &tmp);
1484                                 pa_memblock_unref(tmp.memblock);
1485                                 u->write_index += skip_bytes;
1486                             }
1487                         }
1488
1489                         do_write = 1;
1490                     }
1491                 }
1492
1493                 if (writable && do_write > 0) {
1494                     int n_written;
1495
1496                     if (u->write_index <= 0)
1497                         u->started_at = pa_rtclock_now();
1498
1499                     if (u->profile == PROFILE_A2DP) {
1500                         if ((n_written = a2dp_process_render(u)) < 0)
1501                             goto fail;
1502                     } else {
1503                         if ((n_written = hsp_process_render(u)) < 0)
1504                             goto fail;
1505                     }
1506
1507                     if (n_written == 0)
1508                         pa_log("Broken kernel: we got EAGAIN on write() after POLLOUT!");
1509
1510                     do_write -= n_written;
1511                     writable = FALSE;
1512                 }
1513
1514                 if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable) {
1515                     pa_usec_t time_passed, next_write_at, sleep_for;
1516
1517                     /* Hmm, there is no input stream we could synchronize
1518                      * to. So let's estimate when we need to wake up the latest */
1519
1520                     time_passed = pa_rtclock_now() - u->started_at;
1521                     next_write_at = pa_bytes_to_usec(u->write_index, &u->sample_spec);
1522                     sleep_for = time_passed < next_write_at ? next_write_at - time_passed : 0;
1523
1524 /*                 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); */
1525
1526                     pa_rtpoll_set_timer_relative(u->rtpoll, sleep_for);
1527                     disable_timer = FALSE;
1528                 }
1529             }
1530         }
1531
1532         if (disable_timer)
1533             pa_rtpoll_set_timer_disabled(u->rtpoll);
1534
1535         /* Hmm, nothing to do. Let's sleep */
1536         if (pollfd)
1537             pollfd->events = (short) (((u->sink && PA_SINK_IS_LINKED(u->sink->thread_info.state) && !writable) ? POLLOUT : 0) |
1538                                       (u->source && PA_SOURCE_IS_LINKED(u->source->thread_info.state) ? POLLIN : 0));
1539
1540         if ((ret = pa_rtpoll_run(u->rtpoll, TRUE)) < 0)
1541             goto fail;
1542
1543         if (ret == 0)
1544             goto finish;
1545
1546         pollfd = u->rtpoll_item ? pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL) : NULL;
1547
1548         if (pollfd && (pollfd->revents & ~(POLLOUT|POLLIN))) {
1549             pa_log_info("FD error: %s%s%s%s",
1550                         pollfd->revents & POLLERR ? "POLLERR " :"",
1551                         pollfd->revents & POLLHUP ? "POLLHUP " :"",
1552                         pollfd->revents & POLLPRI ? "POLLPRI " :"",
1553                         pollfd->revents & POLLNVAL ? "POLLNVAL " :"");
1554             goto fail;
1555         }
1556     }
1557
1558 fail:
1559     /* If this was no regular exit from the loop we have to continue processing messages until we receive PA_MESSAGE_SHUTDOWN */
1560     pa_log_debug("IO thread failed");
1561     pa_asyncmsgq_post(u->thread_mq.outq, PA_MSGOBJECT(u->core), PA_CORE_MESSAGE_UNLOAD_MODULE, u->module, 0, NULL, NULL);
1562     pa_asyncmsgq_wait_for(u->thread_mq.inq, PA_MESSAGE_SHUTDOWN);
1563
1564 finish:
1565     pa_log_debug("IO thread shutting down");
1566 }
1567
1568 /* Run from main thread */
1569 static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *m, void *userdata) {
1570     DBusError err;
1571     struct userdata *u;
1572
1573     pa_assert(bus);
1574     pa_assert(m);
1575     pa_assert_se(u = userdata);
1576
1577     dbus_error_init(&err);
1578
1579     pa_log_debug("dbus: interface=%s, path=%s, member=%s\n",
1580                  dbus_message_get_interface(m),
1581                  dbus_message_get_path(m),
1582                  dbus_message_get_member(m));
1583
1584    if (!dbus_message_has_path(m, u->path))
1585        goto fail;
1586
1587     if (dbus_message_is_signal(m, "org.bluez.Headset", "SpeakerGainChanged") ||
1588         dbus_message_is_signal(m, "org.bluez.Headset", "MicrophoneGainChanged")) {
1589
1590         dbus_uint16_t gain;
1591         pa_cvolume v;
1592
1593         if (!dbus_message_get_args(m, &err, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID) || gain > 15) {
1594             pa_log("Failed to parse org.bluez.Headset.{Speaker|Microphone}GainChanged: %s", err.message);
1595             goto fail;
1596         }
1597
1598         if (u->profile == PROFILE_HSP) {
1599             if (u->sink && dbus_message_is_signal(m, "org.bluez.Headset", "SpeakerGainChanged")) {
1600
1601                 pa_cvolume_set(&v, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1602                 pa_sink_volume_changed(u->sink, &v);
1603
1604             } else if (u->source && dbus_message_is_signal(m, "org.bluez.Headset", "MicrophoneGainChanged")) {
1605
1606                 pa_cvolume_set(&v, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1607                 pa_source_volume_changed(u->source, &v);
1608             }
1609         }
1610     }
1611
1612 fail:
1613     dbus_error_free(&err);
1614
1615     return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
1616 }
1617
1618 /* Run from main thread */
1619 static void sink_set_volume_cb(pa_sink *s) {
1620     struct userdata *u = s->userdata;
1621     DBusMessage *m;
1622     dbus_uint16_t gain;
1623
1624     pa_assert(u);
1625
1626     if (u->profile != PROFILE_HSP)
1627         return;
1628
1629     gain = (pa_cvolume_max(&s->real_volume) * 15) / PA_VOLUME_NORM;
1630
1631     if (gain > 15)
1632         gain = 15;
1633
1634     pa_cvolume_set(&s->real_volume, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1635
1636     pa_assert_se(m = dbus_message_new_method_call("org.bluez", u->path, "org.bluez.Headset", "SetSpeakerGain"));
1637     pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID));
1638     pa_assert_se(dbus_connection_send(pa_dbus_connection_get(u->connection), m, NULL));
1639     dbus_message_unref(m);
1640 }
1641
1642 /* Run from main thread */
1643 static void source_set_volume_cb(pa_source *s) {
1644     struct userdata *u = s->userdata;
1645     DBusMessage *m;
1646     dbus_uint16_t gain;
1647
1648     pa_assert(u);
1649
1650     if (u->profile != PROFILE_HSP)
1651         return;
1652
1653     gain = (pa_cvolume_max(&s->volume) * 15) / PA_VOLUME_NORM;
1654
1655     if (gain > 15)
1656         gain = 15;
1657
1658     pa_cvolume_set(&s->volume, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
1659
1660     pa_assert_se(m = dbus_message_new_method_call("org.bluez", u->path, "org.bluez.Headset", "SetMicrophoneGain"));
1661     pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID));
1662     pa_assert_se(dbus_connection_send(pa_dbus_connection_get(u->connection), m, NULL));
1663     dbus_message_unref(m);
1664 }
1665
1666 /* Run from main thread */
1667 static char *get_name(const char *type, pa_modargs *ma, const char *device_id, pa_bool_t *namereg_fail) {
1668     char *t;
1669     const char *n;
1670
1671     pa_assert(type);
1672     pa_assert(ma);
1673     pa_assert(device_id);
1674     pa_assert(namereg_fail);
1675
1676     t = pa_sprintf_malloc("%s_name", type);
1677     n = pa_modargs_get_value(ma, t, NULL);
1678     pa_xfree(t);
1679
1680     if (n) {
1681         *namereg_fail = TRUE;
1682         return pa_xstrdup(n);
1683     }
1684
1685     if ((n = pa_modargs_get_value(ma, "name", NULL)))
1686         *namereg_fail = TRUE;
1687     else {
1688         n = device_id;
1689         *namereg_fail = FALSE;
1690     }
1691
1692     return pa_sprintf_malloc("bluez_%s.%s", type, n);
1693 }
1694
1695 #ifdef NOKIA
1696
1697 static void sco_over_pcm_state_update(struct userdata *u) {
1698     pa_assert(u);
1699     pa_assert(USE_SCO_OVER_PCM(u));
1700
1701     if (PA_SINK_IS_OPENED(pa_sink_get_state(u->hsp.sco_sink)) ||
1702         PA_SOURCE_IS_OPENED(pa_source_get_state(u->hsp.sco_source))) {
1703
1704         if (u->service_fd >= 0)
1705             return;
1706
1707         pa_log_debug("Resuming SCO over PCM");
1708         if ((init_bt(u) < 0) || (init_profile(u) < 0))
1709             pa_log("Can't resume SCO over PCM");
1710
1711         start_stream_fd(u);
1712     } else {
1713
1714         if (u->service_fd < 0)
1715             return;
1716
1717         stop_stream_fd(u);
1718
1719         pa_log_debug("Closing SCO over PCM");
1720         pa_close(u->service_fd);
1721         u->service_fd = -1;
1722     }
1723 }
1724
1725 static pa_hook_result_t sink_state_changed_cb(pa_core *c, pa_sink *s, struct userdata *u) {
1726     pa_assert(c);
1727     pa_sink_assert_ref(s);
1728     pa_assert(u);
1729
1730     if (s != u->hsp.sco_sink)
1731         return PA_HOOK_OK;
1732
1733     sco_over_pcm_state_update(u);
1734
1735     return PA_HOOK_OK;
1736 }
1737
1738 static pa_hook_result_t source_state_changed_cb(pa_core *c, pa_source *s, struct userdata *u) {
1739     pa_assert(c);
1740     pa_source_assert_ref(s);
1741     pa_assert(u);
1742
1743     if (s != u->hsp.sco_source)
1744         return PA_HOOK_OK;
1745
1746     sco_over_pcm_state_update(u);
1747
1748     return PA_HOOK_OK;
1749 }
1750
1751 #endif
1752
1753 /* Run from main thread */
1754 static int add_sink(struct userdata *u) {
1755
1756 #ifdef NOKIA
1757     if (USE_SCO_OVER_PCM(u)) {
1758         pa_proplist *p;
1759
1760         u->sink = u->hsp.sco_sink;
1761         p = pa_proplist_new();
1762         pa_proplist_sets(p, "bluetooth.protocol", "sco");
1763         pa_proplist_update(u->sink->proplist, PA_UPDATE_MERGE, p);
1764         pa_proplist_free(p);
1765
1766         if (!u->hsp.sink_state_changed_slot)
1767             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);
1768
1769     } else
1770 #endif
1771
1772     {
1773         pa_sink_new_data data;
1774         pa_bool_t b;
1775
1776         pa_sink_new_data_init(&data);
1777         data.driver = __FILE__;
1778         data.module = u->module;
1779         pa_sink_new_data_set_sample_spec(&data, &u->sample_spec);
1780         pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP ? "a2dp" : "sco");
1781         if (u->profile == PROFILE_HSP)
1782             pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
1783         data.card = u->card;
1784         data.name = get_name("sink", u->modargs, u->address, &b);
1785         data.namereg_fail = b;
1786
1787         if (pa_modargs_get_proplist(u->modargs, "sink_properties", data.proplist, PA_UPDATE_REPLACE) < 0) {
1788             pa_log("Invalid properties");
1789             pa_sink_new_data_done(&data);
1790             return -1;
1791         }
1792
1793         u->sink = pa_sink_new(u->core, &data, PA_SINK_HARDWARE|PA_SINK_LATENCY | (u->profile == PROFILE_HSP ? PA_SINK_HW_VOLUME_CTRL : 0));
1794         pa_sink_new_data_done(&data);
1795
1796         if (!u->sink) {
1797             pa_log_error("Failed to create sink");
1798             return -1;
1799         }
1800
1801         u->sink->userdata = u;
1802         u->sink->parent.process_msg = sink_process_msg;
1803
1804         pa_sink_set_max_request(u->sink, u->block_size);
1805         pa_sink_set_fixed_latency(u->sink,
1806                                   (u->profile == PROFILE_A2DP ? FIXED_LATENCY_PLAYBACK_A2DP : FIXED_LATENCY_PLAYBACK_HSP) +
1807                                   pa_bytes_to_usec(u->block_size, &u->sample_spec));
1808     }
1809
1810     if (u->profile == PROFILE_HSP) {
1811         u->sink->set_volume = sink_set_volume_cb;
1812         u->sink->n_volume_steps = 16;
1813     }
1814
1815     return 0;
1816 }
1817
1818 /* Run from main thread */
1819 static int add_source(struct userdata *u) {
1820
1821 #ifdef NOKIA
1822     if (USE_SCO_OVER_PCM(u)) {
1823         u->source = u->hsp.sco_source;
1824         pa_proplist_sets(u->source->proplist, "bluetooth.protocol", "hsp");
1825
1826         if (!u->hsp.source_state_changed_slot)
1827             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);
1828
1829     } else
1830 #endif
1831
1832     {
1833         pa_source_new_data data;
1834         pa_bool_t b;
1835
1836         pa_source_new_data_init(&data);
1837         data.driver = __FILE__;
1838         data.module = u->module;
1839         pa_source_new_data_set_sample_spec(&data, &u->sample_spec);
1840         pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP_SOURCE ? "a2dp_source" : "hsp");
1841         if (u->profile == PROFILE_HSP)
1842             pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
1843         data.card = u->card;
1844         data.name = get_name("source", u->modargs, u->address, &b);
1845         data.namereg_fail = b;
1846
1847         if (pa_modargs_get_proplist(u->modargs, "source_properties", data.proplist, PA_UPDATE_REPLACE) < 0) {
1848             pa_log("Invalid properties");
1849             pa_source_new_data_done(&data);
1850             return -1;
1851         }
1852
1853         u->source = pa_source_new(u->core, &data, PA_SOURCE_HARDWARE|PA_SOURCE_LATENCY | (u->profile == PROFILE_HSP ? PA_SOURCE_HW_VOLUME_CTRL : 0));
1854         pa_source_new_data_done(&data);
1855
1856         if (!u->source) {
1857             pa_log_error("Failed to create source");
1858             return -1;
1859         }
1860
1861         u->source->userdata = u;
1862         u->source->parent.process_msg = source_process_msg;
1863
1864         pa_source_set_fixed_latency(u->source,
1865                                     (u->profile == PROFILE_A2DP_SOURCE ? FIXED_LATENCY_RECORD_A2DP : FIXED_LATENCY_RECORD_HSP) +
1866                                     pa_bytes_to_usec(u->block_size, &u->sample_spec));
1867     }
1868
1869     if (u->profile == PROFILE_HSP) {
1870         pa_proplist_sets(u->source->proplist, "bluetooth.nrec", (u->hsp.pcm_capabilities.flags & BT_PCM_FLAG_NREC) ? "1" : "0");
1871         u->source->set_volume = source_set_volume_cb;
1872         u->source->n_volume_steps = 16;
1873     }
1874
1875     return 0;
1876 }
1877
1878 /* Run from main thread */
1879 static void shutdown_bt(struct userdata *u) {
1880     pa_assert(u);
1881
1882     if (u->stream_fd >= 0) {
1883         pa_close(u->stream_fd);
1884         u->stream_fd = -1;
1885
1886         u->stream_write_type = 0;
1887     }
1888
1889     if (u->service_fd >= 0) {
1890         pa_close(u->service_fd);
1891         u->service_fd = -1;
1892         u->service_write_type = 0;
1893         u->service_read_type = 0;
1894     }
1895
1896     if (u->write_memchunk.memblock) {
1897         pa_memblock_unref(u->write_memchunk.memblock);
1898         pa_memchunk_reset(&u->write_memchunk);
1899     }
1900 }
1901
1902 /* Run from main thread */
1903 static int init_bt(struct userdata *u) {
1904     pa_assert(u);
1905
1906     shutdown_bt(u);
1907
1908     u->stream_write_type = 0;
1909     u->service_write_type = 0;
1910     u->service_read_type = 0;
1911
1912     if ((u->service_fd = bt_audio_service_open()) < 0) {
1913         pa_log_error("Couldn't connect to bluetooth audio service");
1914         return -1;
1915     }
1916
1917     pa_log_debug("Connected to the bluetooth audio service");
1918
1919     return 0;
1920 }
1921
1922 /* Run from main thread */
1923 static int setup_bt(struct userdata *u) {
1924     pa_assert(u);
1925
1926     if (get_caps(u, 0) < 0)
1927         return -1;
1928
1929     pa_log_debug("Got device capabilities");
1930
1931     if (set_conf(u) < 0)
1932         return -1;
1933
1934     pa_log_debug("Connection to the device configured");
1935
1936 #ifdef NOKIA
1937     if (USE_SCO_OVER_PCM(u)) {
1938         pa_log_debug("Configured to use SCO over PCM");
1939         return 0;
1940     }
1941 #endif
1942
1943     pa_log_debug("Got the stream socket");
1944
1945     return 0;
1946 }
1947
1948 /* Run from main thread */
1949 static int init_profile(struct userdata *u) {
1950     int r = 0;
1951     pa_assert(u);
1952     pa_assert(u->profile != PROFILE_OFF);
1953
1954     if (setup_bt(u) < 0)
1955         return -1;
1956
1957     if (u->profile == PROFILE_A2DP ||
1958         u->profile == PROFILE_HSP)
1959         if (add_sink(u) < 0)
1960             r = -1;
1961
1962     if (u->profile == PROFILE_HSP ||
1963         u->profile == PROFILE_A2DP_SOURCE)
1964         if (add_source(u) < 0)
1965             r = -1;
1966
1967     return r;
1968 }
1969
1970 /* Run from main thread */
1971 static void stop_thread(struct userdata *u) {
1972     pa_assert(u);
1973
1974     if (u->thread) {
1975         pa_asyncmsgq_send(u->thread_mq.inq, NULL, PA_MESSAGE_SHUTDOWN, NULL, 0, NULL);
1976         pa_thread_free(u->thread);
1977         u->thread = NULL;
1978     }
1979
1980     if (u->rtpoll_item) {
1981         pa_rtpoll_item_free(u->rtpoll_item);
1982         u->rtpoll_item = NULL;
1983     }
1984
1985     if (u->hsp.sink_state_changed_slot) {
1986         pa_hook_slot_free(u->hsp.sink_state_changed_slot);
1987         u->hsp.sink_state_changed_slot = NULL;
1988     }
1989
1990     if (u->hsp.source_state_changed_slot) {
1991         pa_hook_slot_free(u->hsp.source_state_changed_slot);
1992         u->hsp.source_state_changed_slot = NULL;
1993     }
1994
1995     if (u->sink) {
1996         pa_sink_unref(u->sink);
1997         u->sink = NULL;
1998     }
1999
2000     if (u->source) {
2001         pa_source_unref(u->source);
2002         u->source = NULL;
2003     }
2004
2005     if (u->rtpoll) {
2006         pa_thread_mq_done(&u->thread_mq);
2007
2008         pa_rtpoll_free(u->rtpoll);
2009         u->rtpoll = NULL;
2010     }
2011
2012     if (u->read_smoother) {
2013         pa_smoother_free(u->read_smoother);
2014         u->read_smoother = NULL;
2015     }
2016 }
2017
2018 /* Run from main thread */
2019 static int start_thread(struct userdata *u) {
2020     pa_assert(u);
2021     pa_assert(!u->thread);
2022     pa_assert(!u->rtpoll);
2023     pa_assert(!u->rtpoll_item);
2024
2025     u->rtpoll = pa_rtpoll_new();
2026     pa_thread_mq_init(&u->thread_mq, u->core->mainloop, u->rtpoll);
2027
2028 #ifdef NOKIA
2029     if (USE_SCO_OVER_PCM(u)) {
2030         if (start_stream_fd(u) < 0)
2031             return -1;
2032
2033         pa_sink_ref(u->sink);
2034         pa_source_ref(u->source);
2035         /* FIXME: monitor stream_fd error */
2036         return 0;
2037     }
2038 #endif
2039
2040     if (!(u->thread = pa_thread_new(thread_func, u))) {
2041         pa_log_error("Failed to create IO thread");
2042         stop_thread(u);
2043         return -1;
2044     }
2045
2046     if (u->sink) {
2047         pa_sink_set_asyncmsgq(u->sink, u->thread_mq.inq);
2048         pa_sink_set_rtpoll(u->sink, u->rtpoll);
2049         pa_sink_put(u->sink);
2050
2051         if (u->sink->set_volume)
2052             u->sink->set_volume(u->sink);
2053     }
2054
2055     if (u->source) {
2056         pa_source_set_asyncmsgq(u->source, u->thread_mq.inq);
2057         pa_source_set_rtpoll(u->source, u->rtpoll);
2058         pa_source_put(u->source);
2059
2060         if (u->source->set_volume)
2061             u->source->set_volume(u->source);
2062     }
2063
2064     return 0;
2065 }
2066
2067 /* Run from main thread */
2068 static int card_set_profile(pa_card *c, pa_card_profile *new_profile) {
2069     struct userdata *u;
2070     enum profile *d;
2071     pa_queue *inputs = NULL, *outputs = NULL;
2072     const pa_bluetooth_device *device;
2073
2074     pa_assert(c);
2075     pa_assert(new_profile);
2076     pa_assert_se(u = c->userdata);
2077
2078     d = PA_CARD_PROFILE_DATA(new_profile);
2079
2080     if (!(device = pa_bluetooth_discovery_get_by_path(u->discovery, u->path))) {
2081         pa_log_error("Failed to get device object.");
2082         return -PA_ERR_IO;
2083     }
2084
2085     /* The state signal is sent by bluez, so it is racy to check
2086        strictly for CONNECTED, we should also accept STREAMING state
2087        as being good enough. However, if the profile is used
2088        concurrently (which is unlikely), ipc will fail later on, and
2089        module will be unloaded. */
2090     if (device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) {
2091         pa_log_warn("HSP is not connected, refused to switch profile");
2092         return -PA_ERR_IO;
2093     }
2094     else if (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP) {
2095         pa_log_warn("A2DP is not connected, refused to switch profile");
2096         return -PA_ERR_IO;
2097     }
2098
2099     if (u->sink) {
2100         inputs = pa_sink_move_all_start(u->sink, NULL);
2101 #ifdef NOKIA
2102         if (!USE_SCO_OVER_PCM(u))
2103 #endif
2104             pa_sink_unlink(u->sink);
2105     }
2106
2107     if (u->source) {
2108         outputs = pa_source_move_all_start(u->source, NULL);
2109 #ifdef NOKIA
2110         if (!USE_SCO_OVER_PCM(u))
2111 #endif
2112             pa_source_unlink(u->source);
2113     }
2114
2115     stop_thread(u);
2116     shutdown_bt(u);
2117
2118     u->profile = *d;
2119     u->sample_spec = u->requested_sample_spec;
2120
2121     init_bt(u);
2122
2123     if (u->profile != PROFILE_OFF)
2124         init_profile(u);
2125
2126     if (u->sink || u->source)
2127         start_thread(u);
2128
2129     if (inputs) {
2130         if (u->sink)
2131             pa_sink_move_all_finish(u->sink, inputs, FALSE);
2132         else
2133             pa_sink_move_all_fail(inputs);
2134     }
2135
2136     if (outputs) {
2137         if (u->source)
2138             pa_source_move_all_finish(u->source, outputs, FALSE);
2139         else
2140             pa_source_move_all_fail(outputs);
2141     }
2142
2143     return 0;
2144 }
2145
2146 /* Run from main thread */
2147 static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
2148     pa_card_new_data data;
2149     pa_bool_t b;
2150     pa_card_profile *p;
2151     enum profile *d;
2152     const char *ff;
2153     char *n;
2154     const char *default_profile;
2155
2156     pa_assert(u);
2157     pa_assert(device);
2158
2159     pa_card_new_data_init(&data);
2160     data.driver = __FILE__;
2161     data.module = u->module;
2162
2163     n = pa_bluetooth_cleanup_name(device->name);
2164     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_DESCRIPTION, n);
2165     pa_xfree(n);
2166     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_STRING, device->address);
2167     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_API, "bluez");
2168     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_CLASS, "sound");
2169     pa_proplist_sets(data.proplist, PA_PROP_DEVICE_BUS, "bluetooth");
2170     if ((ff = pa_bluetooth_get_form_factor(device->class)))
2171         pa_proplist_sets(data.proplist, PA_PROP_DEVICE_FORM_FACTOR, ff);
2172     pa_proplist_sets(data.proplist, "bluez.path", device->path);
2173     pa_proplist_setf(data.proplist, "bluez.class", "0x%06x", (unsigned) device->class);
2174     pa_proplist_sets(data.proplist, "bluez.name", device->name);
2175     data.name = get_name("card", u->modargs, device->address, &b);
2176     data.namereg_fail = b;
2177
2178     if (pa_modargs_get_proplist(u->modargs, "card_properties", data.proplist, PA_UPDATE_REPLACE) < 0) {
2179         pa_log("Invalid properties");
2180         pa_card_new_data_done(&data);
2181         return -1;
2182     }
2183
2184     data.profiles = pa_hashmap_new(pa_idxset_string_hash_func, pa_idxset_string_compare_func);
2185
2186     /* we base hsp/a2dp availability on UUIDs.
2187        Ideally, it would be based on "Connected" state, but
2188        we can't afford to wait for this information when
2189        we are loaded with profile="hsp", for instance */
2190     if (pa_bluetooth_uuid_has(device->uuids, A2DP_SINK_UUID)) {
2191         p = pa_card_profile_new("a2dp", _("High Fidelity Playback (A2DP)"), sizeof(enum profile));
2192         p->priority = 10;
2193         p->n_sinks = 1;
2194         p->n_sources = 0;
2195         p->max_sink_channels = 2;
2196         p->max_source_channels = 0;
2197
2198         d = PA_CARD_PROFILE_DATA(p);
2199         *d = PROFILE_A2DP;
2200
2201         pa_hashmap_put(data.profiles, p->name, p);
2202     }
2203
2204     if (pa_bluetooth_uuid_has(device->uuids, A2DP_SOURCE_UUID)) {
2205         p = pa_card_profile_new("a2dp_source", _("High Fidelity Capture (A2DP)"), sizeof(enum profile));
2206         p->priority = 10;
2207         p->n_sinks = 0;
2208         p->n_sources = 1;
2209         p->max_sink_channels = 0;
2210         p->max_source_channels = 2;
2211
2212         d = PA_CARD_PROFILE_DATA(p);
2213         *d = PROFILE_A2DP_SOURCE;
2214
2215         pa_hashmap_put(data.profiles, p->name, p);
2216     }
2217
2218     if (pa_bluetooth_uuid_has(device->uuids, HSP_HS_UUID) ||
2219         pa_bluetooth_uuid_has(device->uuids, HFP_HS_UUID)) {
2220         p = pa_card_profile_new("hsp", _("Telephony Duplex (HSP/HFP)"), sizeof(enum profile));
2221         p->priority = 20;
2222         p->n_sinks = 1;
2223         p->n_sources = 1;
2224         p->max_sink_channels = 1;
2225         p->max_source_channels = 1;
2226
2227         d = PA_CARD_PROFILE_DATA(p);
2228         *d = PROFILE_HSP;
2229
2230         pa_hashmap_put(data.profiles, p->name, p);
2231     }
2232
2233     pa_assert(!pa_hashmap_isempty(data.profiles));
2234
2235     p = pa_card_profile_new("off", _("Off"), sizeof(enum profile));
2236     d = PA_CARD_PROFILE_DATA(p);
2237     *d = PROFILE_OFF;
2238     pa_hashmap_put(data.profiles, p->name, p);
2239
2240     if ((default_profile = pa_modargs_get_value(u->modargs, "profile", NULL))) {
2241         if (pa_hashmap_get(data.profiles, default_profile))
2242             pa_card_new_data_set_profile(&data, default_profile);
2243         else
2244             pa_log_warn("Profile '%s' not valid or not supported by device.", default_profile);
2245     }
2246
2247     u->card = pa_card_new(u->core, &data);
2248     pa_card_new_data_done(&data);
2249
2250     if (!u->card) {
2251         pa_log("Failed to allocate card.");
2252         return -1;
2253     }
2254
2255     u->card->userdata = u;
2256     u->card->set_profile = card_set_profile;
2257
2258     d = PA_CARD_PROFILE_DATA(u->card->active_profile);
2259
2260     if ((device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) ||
2261         (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP)) {
2262         pa_log_warn("Default profile not connected, selecting off profile");
2263         u->card->active_profile = pa_hashmap_get(u->card->profiles, "off");
2264         u->card->save_profile = FALSE;
2265     }
2266
2267     d = PA_CARD_PROFILE_DATA(u->card->active_profile);
2268     u->profile = *d;
2269
2270     return 0;
2271 }
2272
2273 /* Run from main thread */
2274 static const pa_bluetooth_device* find_device(struct userdata *u, const char *address, const char *path) {
2275     const pa_bluetooth_device *d = NULL;
2276
2277     pa_assert(u);
2278
2279     if (!address && !path) {
2280         pa_log_error("Failed to get device address/path from module arguments.");
2281         return NULL;
2282     }
2283
2284     if (path) {
2285         if (!(d = pa_bluetooth_discovery_get_by_path(u->discovery, path))) {
2286             pa_log_error("%s is not a valid BlueZ audio device.", path);
2287             return NULL;
2288         }
2289
2290         if (address && !(pa_streq(d->address, address))) {
2291             pa_log_error("Passed path %s and address %s don't match.", path, address);
2292             return NULL;
2293         }
2294
2295     } else {
2296         if (!(d = pa_bluetooth_discovery_get_by_address(u->discovery, address))) {
2297             pa_log_error("%s is not known.", address);
2298             return NULL;
2299         }
2300     }
2301
2302     if (d) {
2303         u->address = pa_xstrdup(d->address);
2304         u->path = pa_xstrdup(d->path);
2305     }
2306
2307     return d;
2308 }
2309
2310 /* Run from main thread */
2311 static int setup_dbus(struct userdata *u) {
2312     DBusError err;
2313
2314     dbus_error_init(&err);
2315
2316     u->connection = pa_dbus_bus_get(u->core, DBUS_BUS_SYSTEM, &err);
2317
2318     if (dbus_error_is_set(&err) || !u->connection) {
2319         pa_log("Failed to get D-Bus connection: %s", err.message);
2320         dbus_error_free(&err);
2321         return -1;
2322     }
2323
2324     return 0;
2325 }
2326
2327 int pa__init(pa_module* m) {
2328     pa_modargs *ma;
2329     uint32_t channels;
2330     struct userdata *u;
2331     const char *address, *path;
2332     DBusError err;
2333     char *mike, *speaker;
2334     const pa_bluetooth_device *device;
2335
2336     pa_assert(m);
2337
2338     dbus_error_init(&err);
2339
2340     if (!(ma = pa_modargs_new(m->argument, valid_modargs))) {
2341         pa_log_error("Failed to parse module arguments");
2342         goto fail;
2343     }
2344
2345     m->userdata = u = pa_xnew0(struct userdata, 1);
2346     u->module = m;
2347     u->core = m->core;
2348     u->service_fd = -1;
2349     u->stream_fd = -1;
2350     u->sample_spec = m->core->default_sample_spec;
2351     u->modargs = ma;
2352
2353 #ifdef NOKIA
2354     if (pa_modargs_get_value(ma, "sco_sink", NULL) &&
2355         !(u->hsp.sco_sink = pa_namereg_get(m->core, pa_modargs_get_value(ma, "sco_sink", NULL), PA_NAMEREG_SINK))) {
2356         pa_log("SCO sink not found");
2357         goto fail;
2358     }
2359
2360     if (pa_modargs_get_value(ma, "sco_source", NULL) &&
2361         !(u->hsp.sco_source = pa_namereg_get(m->core, pa_modargs_get_value(ma, "sco_source", NULL), PA_NAMEREG_SOURCE))) {
2362         pa_log("SCO source not found");
2363         goto fail;
2364     }
2365 #endif
2366
2367     if (pa_modargs_get_value_u32(ma, "rate", &u->sample_spec.rate) < 0 ||
2368         u->sample_spec.rate <= 0 || u->sample_spec.rate > PA_RATE_MAX) {
2369         pa_log_error("Failed to get rate from module arguments");
2370         goto fail;
2371     }
2372
2373     u->auto_connect = TRUE;
2374     if (pa_modargs_get_value_boolean(ma, "auto_connect", &u->auto_connect)) {
2375         pa_log("Failed to parse auto_connect= argument");
2376         goto fail;
2377     }
2378
2379     channels = u->sample_spec.channels;
2380     if (pa_modargs_get_value_u32(ma, "channels", &channels) < 0 ||
2381         channels <= 0 || channels > PA_CHANNELS_MAX) {
2382         pa_log_error("Failed to get channels from module arguments");
2383         goto fail;
2384     }
2385     u->sample_spec.channels = (uint8_t) channels;
2386     u->requested_sample_spec = u->sample_spec;
2387
2388     address = pa_modargs_get_value(ma, "address", NULL);
2389     path = pa_modargs_get_value(ma, "path", NULL);
2390
2391     if (setup_dbus(u) < 0)
2392         goto fail;
2393
2394     if (!(u->discovery = pa_bluetooth_discovery_get(m->core)))
2395         goto fail;
2396
2397     if (!(device = find_device(u, address, path)))
2398         goto fail;
2399
2400     /* Add the card structure. This will also initialize the default profile */
2401     if (add_card(u, device) < 0)
2402         goto fail;
2403
2404     /* Connect to the BT service and query capabilities */
2405     if (init_bt(u) < 0)
2406         goto fail;
2407
2408     if (!dbus_connection_add_filter(pa_dbus_connection_get(u->connection), filter_cb, u, NULL)) {
2409         pa_log_error("Failed to add filter function");
2410         goto fail;
2411     }
2412     u->filter_added = TRUE;
2413
2414     speaker = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='SpeakerGainChanged',path='%s'", u->path);
2415     mike = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='MicrophoneGainChanged',path='%s'", u->path);
2416
2417     if (pa_dbus_add_matches(
2418                 pa_dbus_connection_get(u->connection), &err,
2419                 speaker,
2420                 mike,
2421                 NULL) < 0) {
2422
2423         pa_xfree(speaker);
2424         pa_xfree(mike);
2425
2426         pa_log("Failed to add D-Bus matches: %s", err.message);
2427         goto fail;
2428     }
2429
2430     pa_xfree(speaker);
2431     pa_xfree(mike);
2432
2433     if (u->profile != PROFILE_OFF)
2434         if (init_profile(u) < 0)
2435             goto fail;
2436
2437     if (u->sink || u->source)
2438         if (start_thread(u) < 0)
2439             goto fail;
2440
2441     return 0;
2442
2443 fail:
2444
2445     pa__done(m);
2446
2447     dbus_error_free(&err);
2448
2449     return -1;
2450 }
2451
2452 int pa__get_n_used(pa_module *m) {
2453     struct userdata *u;
2454
2455     pa_assert(m);
2456     pa_assert_se(u = m->userdata);
2457
2458     return
2459         (u->sink ? pa_sink_linked_by(u->sink) : 0) +
2460         (u->source ? pa_source_linked_by(u->source) : 0);
2461 }
2462
2463 void pa__done(pa_module *m) {
2464     struct userdata *u;
2465     pa_assert(m);
2466
2467     if (!(u = m->userdata))
2468         return;
2469
2470     if (u->sink
2471 #ifdef NOKIA
2472         && !USE_SCO_OVER_PCM(u)
2473 #endif
2474     )
2475         pa_sink_unlink(u->sink);
2476
2477     if (u->source
2478 #ifdef NOKIA
2479         && !USE_SCO_OVER_PCM(u)
2480 #endif
2481     )
2482         pa_source_unlink(u->source);
2483
2484     stop_thread(u);
2485
2486     if (u->connection) {
2487
2488         if (u->path) {
2489             char *speaker, *mike;
2490             speaker = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='SpeakerGainChanged',path='%s'", u->path);
2491             mike = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='MicrophoneGainChanged',path='%s'", u->path);
2492
2493             pa_dbus_remove_matches(pa_dbus_connection_get(u->connection),
2494                                    speaker,
2495                                    mike,
2496                                    NULL);
2497
2498             pa_xfree(speaker);
2499             pa_xfree(mike);
2500         }
2501
2502         if (u->filter_added)
2503             dbus_connection_remove_filter(pa_dbus_connection_get(u->connection), filter_cb, u);
2504
2505         pa_dbus_connection_unref(u->connection);
2506     }
2507
2508     if (u->card)
2509         pa_card_free(u->card);
2510
2511     if (u->read_smoother)
2512         pa_smoother_free(u->read_smoother);
2513
2514     shutdown_bt(u);
2515
2516     if (u->a2dp.buffer)
2517         pa_xfree(u->a2dp.buffer);
2518
2519     sbc_finish(&u->a2dp.sbc);
2520
2521     if (u->modargs)
2522         pa_modargs_free(u->modargs);
2523
2524     pa_xfree(u->address);
2525     pa_xfree(u->path);
2526
2527     if (u->discovery)
2528         pa_bluetooth_discovery_unref(u->discovery);
2529
2530     pa_xfree(u);
2531 }