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

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