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