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