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

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