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