code.delx.au - pulseaudio/blob - src/modules/bluetooth/module-bluetooth-device.c

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