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