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

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