bluetooth: Release transport when not available
[profile/ivi/pulseaudio-panda.git] / src / modules / bluetooth / module-bluetooth-device.c
index 5f119c7..ad68cfa 100644 (file)
@@ -1,7 +1,8 @@
 /***
   This file is part of PulseAudio.
 
-  Copyright 2008 Joao Paulo Rechi Vita
+  Copyright 2008-2009 Joao Paulo Rechi Vita
+  Copyright 2011-2012 BMW Car IT GmbH.
 
   PulseAudio is free software; you can redistribute it and/or modify
   it under the terms of the GNU Lesser General Public License as
 
 #include <string.h>
 #include <errno.h>
-#include <poll.h>
-#include <sys/ioctl.h>
 #include <linux/sockios.h>
 #include <arpa/inet.h>
 
-#include <pulse/i18n.h>
 #include <pulse/rtclock.h>
 #include <pulse/sample.h>
 #include <pulse/timeval.h>
 #include <pulse/xmalloc.h>
 
+#include <pulsecore/i18n.h>
 #include <pulsecore/module.h>
 #include <pulsecore/modargs.h>
 #include <pulsecore/core-rtclock.h>
 #include <pulsecore/core-util.h>
 #include <pulsecore/core-error.h>
+#include <pulsecore/shared.h>
 #include <pulsecore/socket-util.h>
 #include <pulsecore/thread.h>
 #include <pulsecore/thread-mq.h>
+#include <pulsecore/poll.h>
 #include <pulsecore/rtpoll.h>
 #include <pulsecore/time-smoother.h>
 #include <pulsecore/namereg.h>
 #include "module-bluetooth-device-symdef.h"
 #include "ipc.h"
 #include "sbc.h"
+#include "a2dp-codecs.h"
 #include "rtp.h"
 #include "bluetooth-util.h"
 
-#define MAX_BITPOOL 64
-#define MIN_BITPOOL 2U
+#define BITPOOL_DEC_LIMIT 32
+#define BITPOOL_DEC_STEP 5
+#define HSP_MAX_GAIN 15
 
 PA_MODULE_AUTHOR("Joao Paulo Rechi Vita");
 PA_MODULE_DESCRIPTION("Bluetooth audio sink and source");
@@ -71,17 +74,13 @@ PA_MODULE_USAGE(
         "source_name=<name for the source> "
         "source_properties=<properties for the source> "
         "address=<address of the device> "
-        "profile=<a2dp|hsp> "
+        "profile=<a2dp|hsp|hfgw> "
         "rate=<sample rate> "
         "channels=<number of channels> "
-        "path=<device object path>");
-
-/*
-#ifdef NOKIA
+        "path=<device object path> "
+        "auto_connect=<automatically connect?> "
         "sco_sink=<SCO over PCM sink name> "
-        "sco_source=<SCO over PCM source name>"
-#endif
-*/
+        "sco_source=<SCO over PCM source name>");
 
 /* TODO: not close fd when entering suspend mode in a2dp */
 
@@ -98,10 +97,9 @@ static const char* const valid_modargs[] = {
     "rate",
     "channels",
     "path",
-#ifdef NOKIA
+    "auto_connect",
     "sco_sink",
     "sco_source",
-#endif
     NULL
 };
 
@@ -115,31 +113,41 @@ struct a2dp_info {
     size_t buffer_size;                  /* Size of the buffer */
 
     uint16_t seq_num;                    /* Cumulative packet sequence */
+    uint8_t min_bitpool;
+    uint8_t max_bitpool;
 };
 
 struct hsp_info {
     pcm_capabilities_t pcm_capabilities;
-#ifdef NOKIA
     pa_sink *sco_sink;
+    void (*sco_sink_set_volume)(pa_sink *s);
     pa_source *sco_source;
-#endif
+    void (*sco_source_set_volume)(pa_source *s);
     pa_hook_slot *sink_state_changed_slot;
     pa_hook_slot *source_state_changed_slot;
+    pa_hook_slot *nrec_changed_slot;
 };
 
-enum profile {
-    PROFILE_A2DP,
-    PROFILE_HSP,
-    PROFILE_OFF
+struct bluetooth_msg {
+    pa_msgobject parent;
+    pa_card *card;
 };
 
+typedef struct bluetooth_msg bluetooth_msg;
+PA_DEFINE_PRIVATE_CLASS(bluetooth_msg, pa_msgobject);
+#define BLUETOOTH_MSG(o) (bluetooth_msg_cast(o))
+
 struct userdata {
     pa_core *core;
     pa_module *module;
 
     char *address;
     char *path;
+    char *transport;
+    char *accesstype;
+
     pa_bluetooth_discovery *discovery;
+    pa_bool_t auto_connect;
 
     pa_dbus_connection *connection;
 
@@ -151,6 +159,7 @@ struct userdata {
     pa_rtpoll *rtpoll;
     pa_rtpoll_item *rtpoll_item;
     pa_thread *thread;
+    bluetooth_msg *msg;
 
     uint64_t read_index, write_index;
     pa_usec_t started_at;
@@ -160,11 +169,13 @@ struct userdata {
 
     pa_sample_spec sample_spec, requested_sample_spec;
 
-    int service_fd;
     int stream_fd;
 
-    size_t link_mtu;
-    size_t block_size;
+    size_t read_link_mtu;
+    size_t read_block_size;
+
+    size_t write_link_mtu;
+    size_t write_block_size;
 
     struct a2dp_info a2dp;
     struct hsp_info hsp;
@@ -174,581 +185,104 @@ struct userdata {
     pa_modargs *modargs;
 
     int stream_write_type;
-    int service_write_type, service_read_type;
+
+    pa_bool_t filter_added;
+};
+
+enum {
+    BLUETOOTH_MESSAGE_IO_THREAD_FAILED,
+    BLUETOOTH_MESSAGE_MAX
 };
 
 #define FIXED_LATENCY_PLAYBACK_A2DP (25*PA_USEC_PER_MSEC)
+#define FIXED_LATENCY_RECORD_A2DP (25*PA_USEC_PER_MSEC)
 #define FIXED_LATENCY_PLAYBACK_HSP (125*PA_USEC_PER_MSEC)
 #define FIXED_LATENCY_RECORD_HSP (25*PA_USEC_PER_MSEC)
 
 #define MAX_PLAYBACK_CATCH_UP_USEC (100*PA_USEC_PER_MSEC)
 
-#ifdef NOKIA
 #define USE_SCO_OVER_PCM(u) (u->profile == PROFILE_HSP && (u->hsp.sco_sink && u->hsp.sco_source))
-#endif
 
-static int init_bt(struct userdata *u);
 static int init_profile(struct userdata *u);
 
-static int service_send(struct userdata *u, const bt_audio_msg_header_t *msg) {
-    ssize_t r;
-
-    pa_assert(u);
-    pa_assert(u->service_fd >= 0);
-    pa_assert(msg);
-    pa_assert(msg->length > 0);
-
-    pa_log_debug("Sending %s -> %s",
-                 pa_strnull(bt_audio_strtype(msg->type)),
-                 pa_strnull(bt_audio_strname(msg->name)));
-
-    if ((r = pa_loop_write(u->service_fd, msg, msg->length, &u->service_write_type)) == (ssize_t) msg->length)
-        return 0;
-
-    if (r < 0)
-        pa_log_error("Error sending data to audio service: %s", pa_cstrerror(errno));
-    else
-        pa_log_error("Short write()");
-
-    return -1;
-}
-
-static int service_recv(struct userdata *u, bt_audio_msg_header_t *msg, size_t room) {
-    ssize_t r;
-
-    pa_assert(u);
-    pa_assert(u->service_fd >= 0);
-    pa_assert(msg);
-
-    if (room <= 0)
-        room = BT_SUGGESTED_BUFFER_SIZE;
-
-    pa_log_debug("Trying to receive message from audio service...");
-
-    /* First, read the header */
-    if ((r = pa_loop_read(u->service_fd, msg, sizeof(*msg), &u->service_read_type)) != sizeof(*msg))
-        goto read_fail;
-
-    if (msg->length < sizeof(*msg)) {
-        pa_log_error("Invalid message size.");
-        return -1;
-    }
-
-    /* Secondly, read the payload */
-    if (msg->length > sizeof(*msg)) {
-
-        size_t remains = msg->length - sizeof(*msg);
-
-        if ((r = pa_loop_read(u->service_fd,
-                              (uint8_t*) msg + sizeof(*msg),
-                              remains,
-                              &u->service_read_type)) != (ssize_t) remains)
-            goto read_fail;
-    }
-
-    pa_log_debug("Received %s <- %s",
-                 pa_strnull(bt_audio_strtype(msg->type)),
-                 pa_strnull(bt_audio_strname(msg->name)));
-
-    return 0;
-
-read_fail:
-
-    if (r < 0)
-        pa_log_error("Error receiving data from audio service: %s", pa_cstrerror(errno));
-    else
-        pa_log_error("Short read()");
-
-    return -1;
-}
-
-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) {
-    int r;
-
-    pa_assert(u);
-    pa_assert(u->service_fd >= 0);
-    pa_assert(rsp);
-
-    if ((r = service_recv(u, rsp, room)) < 0)
-        return r;
-
-    if ((rsp->type != BT_INDICATION && rsp->type != BT_RESPONSE) ||
-        rsp->name != expected_name ||
-        (expected_size > 0 && rsp->length != expected_size)) {
-
-        if (rsp->type == BT_ERROR && rsp->length == sizeof(bt_audio_error_t))
-            pa_log_error("Received error condition: %s", pa_cstrerror(((bt_audio_error_t*) rsp)->posix_errno));
-        else
-            pa_log_error("Bogus message %s received while %s was expected",
-                         pa_strnull(bt_audio_strname(rsp->name)),
-                         pa_strnull(bt_audio_strname(expected_name)));
-        return -1;
-    }
-
-    return 0;
-}
-
-/* Run from main thread */
-static int parse_caps(struct userdata *u, uint8_t seid, const struct bt_get_capabilities_rsp *rsp) {
-    uint16_t bytes_left;
-    const codec_capabilities_t *codec;
-
-    pa_assert(u);
-    pa_assert(rsp);
-
-    bytes_left = rsp->h.length - sizeof(*rsp);
-
-    if (bytes_left < sizeof(codec_capabilities_t)) {
-        pa_log_error("Packet too small to store codec information.");
-        return -1;
-    }
-
-    codec = (codec_capabilities_t *) rsp->data; /** ALIGNMENT? **/
-
-    pa_log_debug("Payload size is %lu %lu", (unsigned long) bytes_left, (unsigned long) sizeof(*codec));
-
-    if ((u->profile == PROFILE_A2DP && codec->transport != BT_CAPABILITIES_TRANSPORT_A2DP) ||
-        (u->profile == PROFILE_HSP && codec->transport != BT_CAPABILITIES_TRANSPORT_SCO)) {
-        pa_log_error("Got capabilities for wrong codec.");
-        return -1;
-    }
-
-    if (u->profile == PROFILE_HSP) {
-
-        if (bytes_left <= 0 || codec->length != sizeof(u->hsp.pcm_capabilities))
-            return -1;
-
-        pa_assert(codec->type == BT_HFP_CODEC_PCM);
-
-        if (codec->configured && seid == 0)
-            return codec->seid;
-
-        memcpy(&u->hsp.pcm_capabilities, codec, sizeof(u->hsp.pcm_capabilities));
-
-    } else if (u->profile == PROFILE_A2DP) {
-
-        while (bytes_left > 0) {
-            if ((codec->type == BT_A2DP_SBC_SINK) && !codec->lock)
-                break;
-
-            bytes_left -= codec->length;
-            codec = (const codec_capabilities_t*) ((const uint8_t*) codec + codec->length);
-        }
-
-        if (bytes_left <= 0 || codec->length != sizeof(u->a2dp.sbc_capabilities))
-            return -1;
-
-        pa_assert(codec->type == BT_A2DP_SBC_SINK);
-
-        if (codec->configured && seid == 0)
-            return codec->seid;
-
-        memcpy(&u->a2dp.sbc_capabilities, codec, sizeof(u->a2dp.sbc_capabilities));
-    }
-
-    return 0;
-}
-
-/* Run from main thread */
-static int get_caps(struct userdata *u, uint8_t seid) {
-    union {
-        struct bt_get_capabilities_req getcaps_req;
-        struct bt_get_capabilities_rsp getcaps_rsp;
-        bt_audio_error_t error;
-        uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
-    } msg;
-    int ret;
-
-    pa_assert(u);
-
-    memset(&msg, 0, sizeof(msg));
-    msg.getcaps_req.h.type = BT_REQUEST;
-    msg.getcaps_req.h.name = BT_GET_CAPABILITIES;
-    msg.getcaps_req.h.length = sizeof(msg.getcaps_req);
-    msg.getcaps_req.seid = seid;
-
-    pa_strlcpy(msg.getcaps_req.object, u->path, sizeof(msg.getcaps_req.object));
-    if (u->profile == PROFILE_A2DP)
-        msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_A2DP;
-    else {
-        pa_assert(u->profile == PROFILE_HSP);
-        msg.getcaps_req.transport = BT_CAPABILITIES_TRANSPORT_SCO;
-    }
-    msg.getcaps_req.flags = BT_FLAG_AUTOCONNECT;
-
-    if (service_send(u, &msg.getcaps_req.h) < 0)
-        return -1;
-
-    if (service_expect(u, &msg.getcaps_rsp.h, sizeof(msg), BT_GET_CAPABILITIES, 0) < 0)
-        return -1;
-
-    ret = parse_caps(u, seid, &msg.getcaps_rsp);
-    if (ret <= 0)
-        return ret;
-
-    return get_caps(u, ret);
-}
-
-/* Run from main thread */
-static uint8_t a2dp_default_bitpool(uint8_t freq, uint8_t mode) {
-
-    switch (freq) {
-        case BT_SBC_SAMPLING_FREQ_16000:
-        case BT_SBC_SAMPLING_FREQ_32000:
-            return 53;
-
-        case BT_SBC_SAMPLING_FREQ_44100:
-
-            switch (mode) {
-                case BT_A2DP_CHANNEL_MODE_MONO:
-                case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
-                    return 31;
-
-                case BT_A2DP_CHANNEL_MODE_STEREO:
-                case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
-                    return 53;
-
-                default:
-                    pa_log_warn("Invalid channel mode %u", mode);
-                    return 53;
-            }
-
-        case BT_SBC_SAMPLING_FREQ_48000:
-
-            switch (mode) {
-                case BT_A2DP_CHANNEL_MODE_MONO:
-                case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
-                    return 29;
-
-                case BT_A2DP_CHANNEL_MODE_STEREO:
-                case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
-                    return 51;
-
-                default:
-                    pa_log_warn("Invalid channel mode %u", mode);
-                    return 51;
-            }
-
-        default:
-            pa_log_warn("Invalid sampling freq %u", freq);
-            return 53;
-    }
-}
-
-/* Run from main thread */
-static int setup_a2dp(struct userdata *u) {
-    sbc_capabilities_t *cap;
-    int i;
-
-    static const struct {
-        uint32_t rate;
-        uint8_t cap;
-    } freq_table[] = {
-        { 16000U, BT_SBC_SAMPLING_FREQ_16000 },
-        { 32000U, BT_SBC_SAMPLING_FREQ_32000 },
-        { 44100U, BT_SBC_SAMPLING_FREQ_44100 },
-        { 48000U, BT_SBC_SAMPLING_FREQ_48000 }
-    };
+/* from IO thread */
+static void a2dp_set_bitpool(struct userdata *u, uint8_t bitpool)
+{
+    struct a2dp_info *a2dp;
 
     pa_assert(u);
-    pa_assert(u->profile == PROFILE_A2DP);
-
-    cap = &u->a2dp.sbc_capabilities;
-
-    /* Find the lowest freq that is at least as high as the requested
-     * sampling rate */
-    for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
-        if (freq_table[i].rate >= u->sample_spec.rate && (cap->frequency & freq_table[i].cap)) {
-            u->sample_spec.rate = freq_table[i].rate;
-            cap->frequency = freq_table[i].cap;
-            break;
-        }
-
-    if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
-        for (--i; i >= 0; i--) {
-            if (cap->frequency & freq_table[i].cap) {
-                u->sample_spec.rate = freq_table[i].rate;
-                cap->frequency = freq_table[i].cap;
-                break;
-            }
-        }
-
-        if (i < 0) {
-            pa_log("Not suitable sample rate");
-            return -1;
-        }
-    }
-
-    pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
-
-    if (cap->capability.configured)
-        return 0;
-
-    if (u->sample_spec.channels <= 1) {
-        if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
-            cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
-            u->sample_spec.channels = 1;
-        } else
-            u->sample_spec.channels = 2;
-    }
-
-    if (u->sample_spec.channels >= 2) {
-        u->sample_spec.channels = 2;
-
-        if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_JOINT_STEREO)
-            cap->channel_mode = BT_A2DP_CHANNEL_MODE_JOINT_STEREO;
-        else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_STEREO)
-            cap->channel_mode = BT_A2DP_CHANNEL_MODE_STEREO;
-        else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL)
-            cap->channel_mode = BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL;
-        else if (cap->channel_mode & BT_A2DP_CHANNEL_MODE_MONO) {
-            cap->channel_mode = BT_A2DP_CHANNEL_MODE_MONO;
-            u->sample_spec.channels = 1;
-        } else {
-            pa_log("No supported channel modes");
-            return -1;
-        }
-    }
 
-    if (cap->block_length & BT_A2DP_BLOCK_LENGTH_16)
-        cap->block_length = BT_A2DP_BLOCK_LENGTH_16;
-    else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_12)
-        cap->block_length = BT_A2DP_BLOCK_LENGTH_12;
-    else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_8)
-        cap->block_length = BT_A2DP_BLOCK_LENGTH_8;
-    else if (cap->block_length & BT_A2DP_BLOCK_LENGTH_4)
-        cap->block_length = BT_A2DP_BLOCK_LENGTH_4;
-    else {
-        pa_log_error("No supported block lengths");
-        return -1;
-    }
-
-    if (cap->subbands & BT_A2DP_SUBBANDS_8)
-        cap->subbands = BT_A2DP_SUBBANDS_8;
-    else if (cap->subbands & BT_A2DP_SUBBANDS_4)
-        cap->subbands = BT_A2DP_SUBBANDS_4;
-    else {
-        pa_log_error("No supported subbands");
-        return -1;
-    }
-
-    if (cap->allocation_method & BT_A2DP_ALLOCATION_LOUDNESS)
-        cap->allocation_method = BT_A2DP_ALLOCATION_LOUDNESS;
-    else if (cap->allocation_method & BT_A2DP_ALLOCATION_SNR)
-        cap->allocation_method = BT_A2DP_ALLOCATION_SNR;
-
-    cap->min_bitpool = (uint8_t) PA_MAX(MIN_BITPOOL, cap->min_bitpool);
-    cap->max_bitpool = (uint8_t) PA_MIN(a2dp_default_bitpool(cap->frequency, cap->channel_mode), cap->max_bitpool);
-
-    return 0;
-}
-
-/* Run from main thread */
-static void setup_sbc(struct a2dp_info *a2dp) {
-    sbc_capabilities_t *active_capabilities;
-
-    pa_assert(a2dp);
-
-    active_capabilities = &a2dp->sbc_capabilities;
-
-    if (a2dp->sbc_initialized)
-        sbc_reinit(&a2dp->sbc, 0);
-    else
-        sbc_init(&a2dp->sbc, 0);
-    a2dp->sbc_initialized = TRUE;
-
-    switch (active_capabilities->frequency) {
-        case BT_SBC_SAMPLING_FREQ_16000:
-            a2dp->sbc.frequency = SBC_FREQ_16000;
-            break;
-        case BT_SBC_SAMPLING_FREQ_32000:
-            a2dp->sbc.frequency = SBC_FREQ_32000;
-            break;
-        case BT_SBC_SAMPLING_FREQ_44100:
-            a2dp->sbc.frequency = SBC_FREQ_44100;
-            break;
-        case BT_SBC_SAMPLING_FREQ_48000:
-            a2dp->sbc.frequency = SBC_FREQ_48000;
-            break;
-        default:
-            pa_assert_not_reached();
-    }
-
-    switch (active_capabilities->channel_mode) {
-        case BT_A2DP_CHANNEL_MODE_MONO:
-            a2dp->sbc.mode = SBC_MODE_MONO;
-            break;
-        case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
-            a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
-            break;
-        case BT_A2DP_CHANNEL_MODE_STEREO:
-            a2dp->sbc.mode = SBC_MODE_STEREO;
-            break;
-        case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
-            a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
-            break;
-        default:
-            pa_assert_not_reached();
-    }
+    a2dp = &u->a2dp;
 
-    switch (active_capabilities->allocation_method) {
-        case BT_A2DP_ALLOCATION_SNR:
-            a2dp->sbc.allocation = SBC_AM_SNR;
-            break;
-        case BT_A2DP_ALLOCATION_LOUDNESS:
-            a2dp->sbc.allocation = SBC_AM_LOUDNESS;
-            break;
-        default:
-            pa_assert_not_reached();
-    }
+    if (a2dp->sbc.bitpool == bitpool)
+        return;
 
-    switch (active_capabilities->subbands) {
-        case BT_A2DP_SUBBANDS_4:
-            a2dp->sbc.subbands = SBC_SB_4;
-            break;
-        case BT_A2DP_SUBBANDS_8:
-            a2dp->sbc.subbands = SBC_SB_8;
-            break;
-        default:
-            pa_assert_not_reached();
-    }
+    if (bitpool > a2dp->max_bitpool)
+        bitpool = a2dp->max_bitpool;
+    else if (bitpool < a2dp->min_bitpool)
+        bitpool = a2dp->min_bitpool;
 
-    switch (active_capabilities->block_length) {
-        case BT_A2DP_BLOCK_LENGTH_4:
-            a2dp->sbc.blocks = SBC_BLK_4;
-            break;
-        case BT_A2DP_BLOCK_LENGTH_8:
-            a2dp->sbc.blocks = SBC_BLK_8;
-            break;
-        case BT_A2DP_BLOCK_LENGTH_12:
-            a2dp->sbc.blocks = SBC_BLK_12;
-            break;
-        case BT_A2DP_BLOCK_LENGTH_16:
-            a2dp->sbc.blocks = SBC_BLK_16;
-            break;
-        default:
-            pa_assert_not_reached();
-    }
+    a2dp->sbc.bitpool = bitpool;
 
-    a2dp->sbc.bitpool = active_capabilities->max_bitpool;
     a2dp->codesize = sbc_get_codesize(&a2dp->sbc);
     a2dp->frame_length = sbc_get_frame_length(&a2dp->sbc);
-}
 
-/* Run from main thread */
-static int set_conf(struct userdata *u) {
-    union {
-        struct bt_open_req open_req;
-        struct bt_open_rsp open_rsp;
-        struct bt_set_configuration_req setconf_req;
-        struct bt_set_configuration_rsp setconf_rsp;
-        bt_audio_error_t error;
-        uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
-    } msg;
-
-    memset(&msg, 0, sizeof(msg));
-    msg.open_req.h.type = BT_REQUEST;
-    msg.open_req.h.name = BT_OPEN;
-    msg.open_req.h.length = sizeof(msg.open_req);
-
-    pa_strlcpy(msg.open_req.object, u->path, sizeof(msg.open_req.object));
-    msg.open_req.seid = u->profile == PROFILE_A2DP ? u->a2dp.sbc_capabilities.capability.seid : BT_A2DP_SEID_RANGE + 1;
-    msg.open_req.lock = u->profile == PROFILE_A2DP ? BT_WRITE_LOCK : BT_READ_LOCK | BT_WRITE_LOCK;
-
-    if (service_send(u, &msg.open_req.h) < 0)
-        return -1;
+    pa_log_debug("Bitpool has changed to %u", a2dp->sbc.bitpool);
 
-    if (service_expect(u, &msg.open_rsp.h, sizeof(msg), BT_OPEN, sizeof(msg.open_rsp)) < 0)
-        return -1;
+    u->read_block_size =
+        (u->read_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+        / a2dp->frame_length * a2dp->codesize;
 
-    if (u->profile == PROFILE_A2DP ) {
-        u->sample_spec.format = PA_SAMPLE_S16LE;
+    u->write_block_size =
+        (u->write_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+        / a2dp->frame_length * a2dp->codesize;
 
-        if (setup_a2dp(u) < 0)
-            return -1;
+    pa_sink_set_max_request_within_thread(u->sink, u->write_block_size);
+    pa_sink_set_fixed_latency_within_thread(u->sink,
+            FIXED_LATENCY_PLAYBACK_A2DP + pa_bytes_to_usec(u->write_block_size, &u->sample_spec));
+}
+
+/* from IO thread, except in SCO over PCM */
+static void bt_transport_config_mtu(struct userdata *u) {
+    /* Calculate block sizes */
+    if (u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW) {
+        u->read_block_size = u->read_link_mtu;
+        u->write_block_size = u->write_link_mtu;
     } else {
-        pa_assert(u->profile == PROFILE_HSP);
+        u->read_block_size =
+            (u->read_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+            / u->a2dp.frame_length * u->a2dp.codesize;
 
-        u->sample_spec.format = PA_SAMPLE_S16LE;
-        u->sample_spec.channels = 1;
-        u->sample_spec.rate = 8000;
+        u->write_block_size =
+            (u->write_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+            / u->a2dp.frame_length * u->a2dp.codesize;
     }
 
-    memset(&msg, 0, sizeof(msg));
-    msg.setconf_req.h.type = BT_REQUEST;
-    msg.setconf_req.h.name = BT_SET_CONFIGURATION;
-    msg.setconf_req.h.length = sizeof(msg.setconf_req);
+    if (USE_SCO_OVER_PCM(u))
+        return;
 
-    if (u->profile == PROFILE_A2DP) {
-        memcpy(&msg.setconf_req.codec, &u->a2dp.sbc_capabilities, sizeof(u->a2dp.sbc_capabilities));
-    } else {
-        msg.setconf_req.codec.transport = BT_CAPABILITIES_TRANSPORT_SCO;
-        msg.setconf_req.codec.seid = BT_A2DP_SEID_RANGE + 1;
-        msg.setconf_req.codec.length = sizeof(pcm_capabilities_t);
+    if (u->sink) {
+        pa_sink_set_max_request_within_thread(u->sink, u->write_block_size);
+        pa_sink_set_fixed_latency_within_thread(u->sink,
+                                                (u->profile == PROFILE_A2DP ?
+                                                 FIXED_LATENCY_PLAYBACK_A2DP : FIXED_LATENCY_PLAYBACK_HSP) +
+                                                pa_bytes_to_usec(u->write_block_size, &u->sample_spec));
     }
-    msg.setconf_req.h.length += msg.setconf_req.codec.length - sizeof(msg.setconf_req.codec);
-
-    if (service_send(u, &msg.setconf_req.h) < 0)
-        return -1;
-
-    if (service_expect(u, &msg.setconf_rsp.h, sizeof(msg), BT_SET_CONFIGURATION, sizeof(msg.setconf_rsp)) < 0)
-        return -1;
-
-    u->link_mtu = msg.setconf_rsp.link_mtu;
-
-    /* setup SBC encoder now we agree on parameters */
-    if (u->profile == PROFILE_A2DP) {
-        setup_sbc(&u->a2dp);
 
-        u->block_size =
-            ((u->link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
-            / u->a2dp.frame_length
-            * u->a2dp.codesize);
-
-        pa_log_info("SBC parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
-                    u->a2dp.sbc.allocation, u->a2dp.sbc.subbands, u->a2dp.sbc.blocks, u->a2dp.sbc.bitpool);
-    } else
-        u->block_size = u->link_mtu;
-
-    return 0;
+    if (u->source)
+        pa_source_set_fixed_latency_within_thread(u->source,
+                                                  (u->profile == PROFILE_A2DP_SOURCE ?
+                                                   FIXED_LATENCY_RECORD_A2DP : FIXED_LATENCY_RECORD_HSP) +
+                                                  pa_bytes_to_usec(u->read_block_size, &u->sample_spec));
 }
 
 /* from IO thread, except in SCO over PCM */
-static int start_stream_fd(struct userdata *u) {
-    union {
-        bt_audio_msg_header_t rsp;
-        struct bt_start_stream_req start_req;
-        struct bt_start_stream_rsp start_rsp;
-        struct bt_new_stream_ind streamfd_ind;
-        bt_audio_error_t error;
-        uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
-    } msg;
+
+static void setup_stream(struct userdata *u) {
     struct pollfd *pollfd;
     int one;
 
-    pa_assert(u);
-    pa_assert(u->rtpoll);
-    pa_assert(!u->rtpoll_item);
-    pa_assert(u->stream_fd < 0);
-
-    memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
-    msg.start_req.h.type = BT_REQUEST;
-    msg.start_req.h.name = BT_START_STREAM;
-    msg.start_req.h.length = sizeof(msg.start_req);
-
-    if (service_send(u, &msg.start_req.h) < 0)
-        return -1;
-
-    if (service_expect(u, &msg.rsp, sizeof(msg), BT_START_STREAM, sizeof(msg.start_rsp)) < 0)
-        return -1;
-
-    if (service_expect(u, &msg.rsp, sizeof(msg), BT_NEW_STREAM, sizeof(msg.streamfd_ind)) < 0)
-        return -1;
-
-    if ((u->stream_fd = bt_audio_service_get_data_fd(u->service_fd)) < 0) {
-        pa_log("Failed to get stream fd from audio service.");
-        return -1;
-    }
+    bt_transport_config_mtu(u);
 
     pa_make_fd_nonblock(u->stream_fd);
     pa_make_socket_low_delay(u->stream_fd);
@@ -759,6 +293,9 @@ static int start_stream_fd(struct userdata *u) {
 
     pa_log_debug("Stream properly set up, we're ready to roll!");
 
+    if (u->profile == PROFILE_A2DP)
+        a2dp_set_bitpool(u, u->a2dp.max_bitpool);
+
     u->rtpoll_item = pa_rtpoll_item_new(u->rtpoll, PA_RTPOLL_NEVER, 1);
     pollfd = pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL);
     pollfd->fd = u->stream_fd;
@@ -776,47 +313,91 @@ static int start_stream_fd(struct userdata *u) {
                 10,
                 pa_rtclock_now(),
                 TRUE);
+}
 
-    return 0;
+static bool bt_transport_is_acquired(struct userdata *u) {
+    if (u->accesstype == NULL) {
+        pa_assert(u->stream_fd < 0);
+        return FALSE;
+    } else {
+        pa_assert(u->stream_fd >= 0);
+        return TRUE;
+    }
 }
 
-/* from IO thread */
-static int stop_stream_fd(struct userdata *u) {
-    union {
-        bt_audio_msg_header_t rsp;
-        struct bt_stop_stream_req start_req;
-        struct bt_stop_stream_rsp start_rsp;
-        bt_audio_error_t error;
-        uint8_t buf[BT_SUGGESTED_BUFFER_SIZE];
-    } msg;
-    int r = 0;
+static void bt_transport_release(struct userdata *u) {
+    const char *accesstype = "rw";
+    const pa_bluetooth_transport *t;
 
-    pa_assert(u);
-    pa_assert(u->rtpoll);
-    pa_assert(u->rtpoll_item);
-    pa_assert(u->stream_fd >= 0);
+    /* Ignore if already released */
+    if (!bt_transport_is_acquired(u))
+        return;
 
-    pa_rtpoll_item_free(u->rtpoll_item);
-    u->rtpoll_item = NULL;
+    pa_log_debug("Releasing transport %s", u->transport);
 
-    memset(msg.buf, 0, BT_SUGGESTED_BUFFER_SIZE);
-    msg.start_req.h.type = BT_REQUEST;
-    msg.start_req.h.name = BT_STOP_STREAM;
-    msg.start_req.h.length = sizeof(msg.start_req);
+    t = pa_bluetooth_discovery_get_transport(u->discovery, u->transport);
+    if (t)
+        pa_bluetooth_transport_release(t, accesstype);
 
-    if (service_send(u, &msg.start_req.h) < 0 ||
-        service_expect(u, &msg.rsp, sizeof(msg), BT_STOP_STREAM, sizeof(msg.start_rsp)) < 0)
-        r = -1;
+    pa_xfree(u->accesstype);
+    u->accesstype = NULL;
 
-    pa_close(u->stream_fd);
-    u->stream_fd = -1;
+    if (u->rtpoll_item) {
+        pa_rtpoll_item_free(u->rtpoll_item);
+        u->rtpoll_item = NULL;
+    }
+
+    if (u->stream_fd >= 0) {
+        pa_close(u->stream_fd);
+        u->stream_fd = -1;
+    }
 
     if (u->read_smoother) {
         pa_smoother_free(u->read_smoother);
         u->read_smoother = NULL;
     }
+}
 
-    return r;
+static int bt_transport_acquire(struct userdata *u, pa_bool_t start) {
+    const char *accesstype = "rw";
+    const pa_bluetooth_transport *t;
+
+    if (u->transport == NULL) {
+        pa_log("Transport no longer available.");
+        return -1;
+    }
+
+    if (bt_transport_is_acquired(u)) {
+        if (start)
+            goto done;
+        return 0;
+    }
+
+    pa_log_debug("Acquiring transport %s", u->transport);
+
+    t = pa_bluetooth_discovery_get_transport(u->discovery, u->transport);
+    if (!t) {
+        pa_log("Transport %s no longer available", u->transport);
+        pa_xfree(u->transport);
+        u->transport = NULL;
+        return -1;
+    }
+
+    u->stream_fd = pa_bluetooth_transport_acquire(t, accesstype, &u->read_link_mtu, &u->write_link_mtu);
+    if (u->stream_fd < 0)
+        return -1;
+
+    u->accesstype = pa_xstrdup(accesstype);
+    pa_log_info("Transport %s acquired: fd %d", u->transport, u->stream_fd);
+
+    if (!start)
+        return 0;
+
+done:
+    pa_log_info("Transport %s resuming", u->transport);
+    setup_stream(u);
+
+    return 0;
 }
 
 /* Run from IO thread */
@@ -826,6 +407,7 @@ static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offse
     int r;
 
     pa_assert(u->sink == PA_SINK(o));
+    pa_assert(u->transport);
 
     switch (code) {
 
@@ -834,14 +416,16 @@ static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offse
             switch ((pa_sink_state_t) PA_PTR_TO_UINT(data)) {
 
                 case PA_SINK_SUSPENDED:
-                    pa_assert(PA_SINK_IS_OPENED(u->sink->thread_info.state));
+                    /* Ignore if transition is PA_SINK_INIT->PA_SINK_SUSPENDED */
+                    if (!PA_SINK_IS_OPENED(u->sink->thread_info.state))
+                        break;
 
                     /* Stop the device if the source is suspended as well */
                     if (!u->source || u->source->state == PA_SOURCE_SUSPENDED)
                         /* We deliberately ignore whether stopping
                          * actually worked. Since the stream_fd is
                          * closed it doesn't really matter */
-                        stop_stream_fd(u);
+                        bt_transport_release(u);
 
                     break;
 
@@ -851,9 +435,10 @@ static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offse
                         break;
 
                     /* Resume the device if the source was suspended as well */
-                    if (!u->source || u->source->state == PA_SOURCE_SUSPENDED)
-                        if (start_stream_fd(u) < 0)
+                    if (!u->source || u->source->state == PA_SOURCE_SUSPENDED) {
+                        if (bt_transport_acquire(u, TRUE) < 0)
                             failed = TRUE;
+                    }
                     break;
 
                 case PA_SINK_UNLINKED:
@@ -869,7 +454,7 @@ static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offse
                 pa_usec_t wi, ri;
 
                 ri = pa_smoother_get(u->read_smoother, pa_rtclock_now());
-                wi = pa_bytes_to_usec(u->write_index + u->block_size, &u->sample_spec);
+                wi = pa_bytes_to_usec(u->write_index + u->write_block_size, &u->sample_spec);
 
                 *((pa_usec_t*) data) = wi > ri ? wi - ri : 0;
             } else {
@@ -898,6 +483,7 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
     int r;
 
     pa_assert(u->source == PA_SOURCE(o));
+    pa_assert(u->transport);
 
     switch (code) {
 
@@ -906,11 +492,13 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
             switch ((pa_source_state_t) PA_PTR_TO_UINT(data)) {
 
                 case PA_SOURCE_SUSPENDED:
-                    pa_assert(PA_SOURCE_IS_OPENED(u->source->thread_info.state));
+                    /* Ignore if transition is PA_SOURCE_INIT->PA_SOURCE_SUSPENDED */
+                    if (!PA_SOURCE_IS_OPENED(u->source->thread_info.state))
+                        break;
 
                     /* Stop the device if the sink is suspended as well */
                     if (!u->sink || u->sink->state == PA_SINK_SUSPENDED)
-                        stop_stream_fd(u);
+                        bt_transport_release(u);
 
                     if (u->read_smoother)
                         pa_smoother_pause(u->read_smoother, pa_rtclock_now());
@@ -922,10 +510,10 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
                         break;
 
                     /* Resume the device if the sink was suspended as well */
-                    if (!u->sink || u->sink->thread_info.state == PA_SINK_SUSPENDED)
-                        if (start_stream_fd(u) < 0)
+                    if (!u->sink || u->sink->thread_info.state == PA_SINK_SUSPENDED) {
+                        if (bt_transport_acquire(u, TRUE) < 0)
                             failed = TRUE;
-
+                    }
                     /* We don't resume the smoother here. Instead we
                      * wait until the first packet arrives */
                     break;
@@ -940,10 +528,14 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
         case PA_SOURCE_MESSAGE_GET_LATENCY: {
             pa_usec_t wi, ri;
 
-            wi = pa_smoother_get(u->read_smoother, pa_rtclock_now());
-            ri = pa_bytes_to_usec(u->read_index, &u->sample_spec);
+            if (u->read_smoother) {
+                wi = pa_smoother_get(u->read_smoother, pa_rtclock_now());
+                ri = pa_bytes_to_usec(u->read_index, &u->sample_spec);
+
+                *((pa_usec_t*) data) = (wi > ri ? wi - ri : 0) + u->source->thread_info.fixed_latency;
+            } else
+                *((pa_usec_t*) data) = 0;
 
-            *((pa_usec_t*) data) = (wi > ri ? wi - ri : 0) + u->source->thread_info.fixed_latency;
             return 0;
         }
 
@@ -954,19 +546,38 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
     return (r < 0 || !failed) ? r : -1;
 }
 
+/* Called from main thread context */
+static int device_process_msg(pa_msgobject *obj, int code, void *data, int64_t offset, pa_memchunk *chunk) {
+    struct bluetooth_msg *u = BLUETOOTH_MSG(obj);
+
+    switch (code) {
+        case BLUETOOTH_MESSAGE_IO_THREAD_FAILED: {
+            if (u->card->module->unload_requested)
+                break;
+
+            pa_log_debug("Switching the profile to off due to IO thread failure.");
+
+            if (pa_card_set_profile(u->card, "off", FALSE) < 0)
+                pa_log_debug("Failed to switch profile to off");
+            break;
+        }
+    }
+    return 0;
+}
+
 /* Run from IO thread */
 static int hsp_process_render(struct userdata *u) {
     int ret = 0;
 
     pa_assert(u);
-    pa_assert(u->profile == PROFILE_HSP);
+    pa_assert(u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW);
     pa_assert(u->sink);
 
     /* First, render some data */
     if (!u->write_memchunk.memblock)
-        pa_sink_render_full(u->sink, u->block_size, &u->write_memchunk);
+        pa_sink_render_full(u->sink, u->write_block_size, &u->write_memchunk);
 
-    pa_assert(u->write_memchunk.length == u->block_size);
+    pa_assert(u->write_memchunk.length == u->write_block_size);
 
     for (;;) {
         ssize_t l;
@@ -1024,11 +635,11 @@ static int hsp_process_push(struct userdata *u) {
     pa_memchunk memchunk;
 
     pa_assert(u);
-    pa_assert(u->profile == PROFILE_HSP);
+    pa_assert(u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW);
     pa_assert(u->source);
     pa_assert(u->read_smoother);
 
-    memchunk.memblock = pa_memblock_new(u->core->mempool, u->block_size);
+    memchunk.memblock = pa_memblock_new(u->core->mempool, u->read_block_size);
     memchunk.index = memchunk.length = 0;
 
     for (;;) {
@@ -1095,7 +706,7 @@ static int hsp_process_push(struct userdata *u) {
 
         pa_source_post(u->source, &memchunk);
 
-        ret = 1;
+        ret = l;
         break;
     }
 
@@ -1106,12 +717,14 @@ static int hsp_process_push(struct userdata *u) {
 
 /* Run from IO thread */
 static void a2dp_prepare_buffer(struct userdata *u) {
+    size_t min_buffer_size = PA_MAX(u->read_link_mtu, u->write_link_mtu);
+
     pa_assert(u);
 
-    if (u->a2dp.buffer_size >= u->link_mtu)
+    if (u->a2dp.buffer_size >= min_buffer_size)
         return;
 
-    u->a2dp.buffer_size = 2 * u->link_mtu;
+    u->a2dp.buffer_size = 2 * min_buffer_size;
     pa_xfree(u->a2dp.buffer);
     u->a2dp.buffer = pa_xmalloc(u->a2dp.buffer_size);
 }
@@ -1134,9 +747,9 @@ static int a2dp_process_render(struct userdata *u) {
 
     /* First, render some data */
     if (!u->write_memchunk.memblock)
-        pa_sink_render_full(u->sink, u->block_size, &u->write_memchunk);
+        pa_sink_render_full(u->sink, u->write_block_size, &u->write_memchunk);
 
-    pa_assert(u->write_memchunk.length == u->block_size);
+    pa_assert(u->write_memchunk.length == u->write_block_size);
 
     a2dp_prepare_buffer(u);
 
@@ -1155,7 +768,7 @@ static int a2dp_process_render(struct userdata *u) {
     to_write = a2dp->buffer_size - sizeof(*header) - sizeof(*payload);
 
     while (PA_LIKELY(to_encode > 0 && to_write > 0)) {
-        size_t written;
+        ssize_t written;
         ssize_t encoded;
 
         encoded = sbc_encode(&a2dp->sbc,
@@ -1224,7 +837,7 @@ static int a2dp_process_render(struct userdata *u) {
                 break;
 
             pa_log_error("Failed to write data to socket: %s", pa_cstrerror(errno));
-            ret  = -1;
+            ret = -1;
             break;
         }
 
@@ -1250,23 +863,160 @@ static int a2dp_process_render(struct userdata *u) {
     return ret;
 }
 
+static int a2dp_process_push(struct userdata *u) {
+    int ret = 0;
+    pa_memchunk memchunk;
+
+    pa_assert(u);
+    pa_assert(u->profile == PROFILE_A2DP_SOURCE);
+    pa_assert(u->source);
+    pa_assert(u->read_smoother);
+
+    memchunk.memblock = pa_memblock_new(u->core->mempool, u->read_block_size);
+    memchunk.index = memchunk.length = 0;
+
+    for (;;) {
+        pa_bool_t found_tstamp = FALSE;
+        pa_usec_t tstamp;
+        struct a2dp_info *a2dp;
+        struct rtp_header *header;
+        struct rtp_payload *payload;
+        const void *p;
+        void *d;
+        ssize_t l;
+        size_t to_write, to_decode;
+
+        a2dp_prepare_buffer(u);
+
+        a2dp = &u->a2dp;
+        header = a2dp->buffer;
+        payload = (struct rtp_payload*) ((uint8_t*) a2dp->buffer + sizeof(*header));
+
+        l = pa_read(u->stream_fd, a2dp->buffer, a2dp->buffer_size, &u->stream_write_type);
+
+        if (l <= 0) {
+
+            if (l < 0 && errno == EINTR)
+                /* Retry right away if we got interrupted */
+                continue;
+
+            else if (l < 0 && errno == EAGAIN)
+                /* Hmm, apparently the socket was not readable, give up for now. */
+                break;
+
+            pa_log_error("Failed to read data from socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
+            ret = -1;
+            break;
+        }
+
+        pa_assert((size_t) l <= a2dp->buffer_size);
+
+        u->read_index += (uint64_t) l;
+
+        /* TODO: get timestamp from rtp */
+        if (!found_tstamp) {
+            /* pa_log_warn("Couldn't find SO_TIMESTAMP data in auxiliary recvmsg() data!"); */
+            tstamp = pa_rtclock_now();
+        }
+
+        pa_smoother_put(u->read_smoother, tstamp, pa_bytes_to_usec(u->read_index, &u->sample_spec));
+        pa_smoother_resume(u->read_smoother, tstamp, TRUE);
+
+        p = (uint8_t*) a2dp->buffer + sizeof(*header) + sizeof(*payload);
+        to_decode = l - sizeof(*header) - sizeof(*payload);
+
+        d = pa_memblock_acquire(memchunk.memblock);
+        to_write = memchunk.length = pa_memblock_get_length(memchunk.memblock);
+
+        while (PA_LIKELY(to_decode > 0)) {
+            size_t written;
+            ssize_t decoded;
+
+            decoded = sbc_decode(&a2dp->sbc,
+                                 p, to_decode,
+                                 d, to_write,
+                                 &written);
+
+            if (PA_UNLIKELY(decoded <= 0)) {
+                pa_log_error("SBC decoding error (%li)", (long) decoded);
+                pa_memblock_release(memchunk.memblock);
+                pa_memblock_unref(memchunk.memblock);
+                return -1;
+            }
+
+/*             pa_log_debug("SBC: decoded: %lu; written: %lu", (unsigned long) decoded, (unsigned long) written); */
+/*             pa_log_debug("SBC: frame_length: %lu; codesize: %lu", (unsigned long) a2dp->frame_length, (unsigned long) a2dp->codesize); */
+
+            /* Reset frame length, it can be changed due to bitpool change */
+            a2dp->frame_length = sbc_get_frame_length(&a2dp->sbc);
+
+            pa_assert_fp((size_t) decoded <= to_decode);
+            pa_assert_fp((size_t) decoded == a2dp->frame_length);
+
+            pa_assert_fp((size_t) written == a2dp->codesize);
+
+            p = (const uint8_t*) p + decoded;
+            to_decode -= decoded;
+
+            d = (uint8_t*) d + written;
+            to_write -= written;
+        }
+
+        memchunk.length -= to_write;
+
+        pa_memblock_release(memchunk.memblock);
+
+        pa_source_post(u->source, &memchunk);
+
+        ret = l;
+        break;
+    }
+
+    pa_memblock_unref(memchunk.memblock);
+
+    return ret;
+}
+
+static void a2dp_reduce_bitpool(struct userdata *u)
+{
+    struct a2dp_info *a2dp;
+    uint8_t bitpool;
+
+    pa_assert(u);
+
+    a2dp = &u->a2dp;
+
+    /* Check if bitpool is already at its limit */
+    if (a2dp->sbc.bitpool <= BITPOOL_DEC_LIMIT)
+        return;
+
+    bitpool = a2dp->sbc.bitpool - BITPOOL_DEC_STEP;
+
+    if (bitpool < BITPOOL_DEC_LIMIT)
+        bitpool = BITPOOL_DEC_LIMIT;
+
+    a2dp_set_bitpool(u, bitpool);
+}
+
 static void thread_func(void *userdata) {
     struct userdata *u = userdata;
     unsigned do_write = 0;
+    unsigned pending_read_bytes = 0;
     pa_bool_t writable = FALSE;
 
     pa_assert(u);
+    pa_assert(u->transport);
 
     pa_log_debug("IO Thread starting up");
 
     if (u->core->realtime_scheduling)
         pa_make_realtime(u->core->realtime_priority);
 
-    if (start_stream_fd(u) < 0)
-        goto fail;
-
     pa_thread_mq_install(&u->thread_mq);
 
+    if (bt_transport_acquire(u, TRUE) < 0)
+        goto fail;
+
     for (;;) {
         struct pollfd *pollfd;
         int ret;
@@ -1285,11 +1035,18 @@ static void thread_func(void *userdata) {
             if (pollfd && (pollfd->revents & POLLIN)) {
                 int n_read;
 
-                if ((n_read = hsp_process_push(u)) < 0)
+                if (u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW)
+                    n_read = hsp_process_push(u);
+                else
+                    n_read = a2dp_process_push(u);
+
+                if (n_read < 0)
                     goto fail;
 
                 /* We just read something, so we are supposed to write something, too */
-                do_write += n_read;
+                pending_read_bytes += n_read;
+                do_write += pending_read_bytes / u->write_block_size;
+                pending_read_bytes = pending_read_bytes % u->write_block_size;
             }
         }
 
@@ -1333,10 +1090,14 @@ static void thread_func(void *userdata) {
                                 pa_sink_render_full(u->sink, skip_bytes, &tmp);
                                 pa_memblock_unref(tmp.memblock);
                                 u->write_index += skip_bytes;
+
+                                if (u->profile == PROFILE_A2DP)
+                                    a2dp_reduce_bitpool(u);
                             }
                         }
 
                         do_write = 1;
+                        pending_read_bytes = 0;
                     }
                 }
 
@@ -1361,17 +1122,20 @@ static void thread_func(void *userdata) {
                     writable = FALSE;
                 }
 
-                if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0 && writable) {
-                    pa_usec_t time_passed, next_write_at, sleep_for;
-
-                    /* Hmm, there is no input stream we could synchronize
-                     * to. So let's estimate when we need to wake up the latest */
-
-                    time_passed = pa_rtclock_now() - u->started_at;
-                    next_write_at = pa_bytes_to_usec(u->write_index, &u->sample_spec);
-                    sleep_for = time_passed < next_write_at ? next_write_at - time_passed : 0;
-
-/*                 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); */
+                if ((!u->source || !PA_SOURCE_IS_LINKED(u->source->thread_info.state)) && do_write <= 0) {
+                    pa_usec_t sleep_for;
+                    pa_usec_t time_passed, next_write_at;
+
+                    if (writable) {
+                        /* Hmm, there is no input stream we could synchronize
+                         * to. So let's estimate when we need to wake up the latest */
+                        time_passed = pa_rtclock_now() - u->started_at;
+                        next_write_at = pa_bytes_to_usec(u->write_index, &u->sample_spec);
+                        sleep_for = time_passed < next_write_at ? next_write_at - time_passed : 0;
+                        /* 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); */
+                    } else
+                        /* drop stream every 500 ms */
+                        sleep_for = PA_USEC_PER_MSEC * 500;
 
                     pa_rtpoll_set_timer_relative(u->rtpoll, sleep_for);
                     disable_timer = FALSE;
@@ -1387,11 +1151,15 @@ static void thread_func(void *userdata) {
             pollfd->events = (short) (((u->sink && PA_SINK_IS_LINKED(u->sink->thread_info.state) && !writable) ? POLLOUT : 0) |
                                       (u->source && PA_SOURCE_IS_LINKED(u->source->thread_info.state) ? POLLIN : 0));
 
-        if ((ret = pa_rtpoll_run(u->rtpoll, TRUE)) < 0)
+        if ((ret = pa_rtpoll_run(u->rtpoll, TRUE)) < 0) {
+            pa_log_debug("pa_rtpoll_run failed with: %d", ret);
             goto fail;
-
-        if (ret == 0)
+        }
+        if (ret == 0) {
+            pa_log_debug("IO thread shutdown requested, stopping cleanly");
+            bt_transport_release(u);
             goto finish;
+        }
 
         pollfd = u->rtpoll_item ? pa_rtpoll_item_get_pollfd(u->rtpoll_item, NULL) : NULL;
 
@@ -1408,17 +1176,74 @@ static void thread_func(void *userdata) {
 fail:
     /* If this was no regular exit from the loop we have to continue processing messages until we receive PA_MESSAGE_SHUTDOWN */
     pa_log_debug("IO thread failed");
-    pa_asyncmsgq_post(u->thread_mq.outq, PA_MSGOBJECT(u->core), PA_CORE_MESSAGE_UNLOAD_MODULE, u->module, 0, NULL, NULL);
+    pa_asyncmsgq_post(pa_thread_mq_get()->outq, PA_MSGOBJECT(u->msg), BLUETOOTH_MESSAGE_IO_THREAD_FAILED, NULL, 0, NULL, NULL);
     pa_asyncmsgq_wait_for(u->thread_mq.inq, PA_MESSAGE_SHUTDOWN);
 
 finish:
     pa_log_debug("IO thread shutting down");
 }
 
+static pa_bt_audio_state_t parse_state_property_change(DBusMessage *m) {
+    DBusMessageIter iter;
+    DBusMessageIter variant;
+    const char *key;
+    const char *value;
+    pa_bt_audio_state_t state;
+
+    if (!dbus_message_iter_init(m, &iter)) {
+        pa_log("Failed to parse PropertyChanged");
+        return PA_BT_AUDIO_STATE_INVALID;
+    }
+
+    if (dbus_message_iter_get_arg_type(&iter) != DBUS_TYPE_STRING) {
+        pa_log("Property name not a string");
+        return PA_BT_AUDIO_STATE_INVALID;
+    }
+
+    dbus_message_iter_get_basic(&iter, &key);
+
+    if (!pa_streq(key, "State"))
+        return PA_BT_AUDIO_STATE_INVALID;
+
+    if (!dbus_message_iter_next(&iter)) {
+        pa_log("Property value missing");
+        return PA_BT_AUDIO_STATE_INVALID;
+    }
+
+    dbus_message_iter_recurse(&iter, &variant);
+
+    if (dbus_message_iter_get_arg_type(&variant) != DBUS_TYPE_STRING) {
+        pa_log("Property value not a string");
+        return PA_BT_AUDIO_STATE_INVALID;
+    }
+
+    dbus_message_iter_get_basic(&variant, &value);
+
+    pa_log_debug("dbus: %s property 'State' changed to value '%s'", dbus_message_get_interface(m), value);
+
+    state = pa_bt_audio_state_from_string(value);
+
+    if (state == PA_BT_AUDIO_STATE_INVALID)
+        pa_log("Unexpected value for property 'State': '%s'", value);
+
+    return state;
+}
+
+static pa_port_available_t audio_state_to_availability(pa_bt_audio_state_t state) {
+    if (state < PA_BT_AUDIO_STATE_CONNECTED)
+        return PA_PORT_AVAILABLE_NO;
+    else if (state >= PA_BT_AUDIO_STATE_PLAYING)
+        return PA_PORT_AVAILABLE_YES;
+    else
+        return PA_PORT_AVAILABLE_UNKNOWN;
+}
+
 /* Run from main thread */
 static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *m, void *userdata) {
     DBusError err;
     struct userdata *u;
+    bool acquire = FALSE;
+    bool release = FALSE;
 
     pa_assert(bus);
     pa_assert(m);
@@ -1431,8 +1256,8 @@ static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *m, void *us
                  dbus_message_get_path(m),
                  dbus_message_get_member(m));
 
-   if (!dbus_message_has_path(m, u->path))
-       goto fail;
+    if (!dbus_message_has_path(m, u->path) && !dbus_message_has_path(m, u->transport))
+        goto fail;
 
     if (dbus_message_is_signal(m, "org.bluez.Headset", "SpeakerGainChanged") ||
         dbus_message_is_signal(m, "org.bluez.Headset", "MicrophoneGainChanged")) {
@@ -1440,23 +1265,130 @@ static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *m, void *us
         dbus_uint16_t gain;
         pa_cvolume v;
 
-        if (!dbus_message_get_args(m, &err, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID) || gain > 15) {
+        if (!dbus_message_get_args(m, &err, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID) || gain > HSP_MAX_GAIN) {
             pa_log("Failed to parse org.bluez.Headset.{Speaker|Microphone}GainChanged: %s", err.message);
             goto fail;
         }
 
         if (u->profile == PROFILE_HSP) {
             if (u->sink && dbus_message_is_signal(m, "org.bluez.Headset", "SpeakerGainChanged")) {
+                pa_volume_t volume = (pa_volume_t) (gain * PA_VOLUME_NORM / HSP_MAX_GAIN);
 
-                pa_cvolume_set(&v, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
+                /* increment volume by one to correct rounding errors */
+                if (volume < PA_VOLUME_NORM)
+                    volume++;
+
+                pa_cvolume_set(&v, u->sample_spec.channels, volume);
                 pa_sink_volume_changed(u->sink, &v);
 
             } else if (u->source && dbus_message_is_signal(m, "org.bluez.Headset", "MicrophoneGainChanged")) {
+                pa_volume_t volume = (pa_volume_t) (gain * PA_VOLUME_NORM / HSP_MAX_GAIN);
+
+                /* increment volume by one to correct rounding errors */
+                if (volume < PA_VOLUME_NORM)
+                    volume++;
 
-                pa_cvolume_set(&v, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
+                pa_cvolume_set(&v, u->sample_spec.channels, volume);
                 pa_source_volume_changed(u->source, &v);
             }
         }
+    } else if (dbus_message_is_signal(m, "org.bluez.HandsfreeGateway", "PropertyChanged")) {
+        pa_bt_audio_state_t state = parse_state_property_change(m);
+
+        switch(state) {
+            case PA_BT_AUDIO_STATE_INVALID:
+            case PA_BT_AUDIO_STATE_DISCONNECTED:
+            case PA_BT_AUDIO_STATE_CONNECTED:
+            case PA_BT_AUDIO_STATE_CONNECTING:
+                goto fail;
+
+            case PA_BT_AUDIO_STATE_PLAYING:
+                if (u->card) {
+                    pa_log_debug("Changing profile to hfgw");
+                    if (pa_card_set_profile(u->card, "hfgw", FALSE) < 0)
+                        pa_log("Failed to change profile to hfgw");
+                }
+                break;
+        }
+
+        if (state != PA_BT_AUDIO_STATE_INVALID) {
+            pa_device_port *port;
+            pa_port_available_t available = audio_state_to_availability(state);
+
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "hfgw-output"));
+            pa_device_port_set_available(port, available);
+
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "hfgw-input"));
+            pa_device_port_set_available(port, available);
+
+            acquire = (available == PA_PORT_AVAILABLE_YES && u->profile == PROFILE_HFGW);
+            release = (available != PA_PORT_AVAILABLE_YES && u->profile == PROFILE_HFGW);
+        }
+    } else if (dbus_message_is_signal(m, "org.bluez.Headset", "PropertyChanged")) {
+        pa_bt_audio_state_t state = parse_state_property_change(m);
+
+        if (state != PA_BT_AUDIO_STATE_INVALID) {
+            pa_device_port *port;
+            pa_port_available_t available = audio_state_to_availability(state);
+
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "hsp-output"));
+            pa_device_port_set_available(port, available);
+
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "hsp-input"));
+            pa_device_port_set_available(port, available);
+
+            acquire = (available == PA_PORT_AVAILABLE_YES && u->profile == PROFILE_HSP);
+            release = (available != PA_PORT_AVAILABLE_YES && u->profile == PROFILE_HSP);
+        }
+    } else if (dbus_message_is_signal(m, "org.bluez.AudioSource", "PropertyChanged")) {
+        pa_bt_audio_state_t state = parse_state_property_change(m);
+
+        if (state != PA_BT_AUDIO_STATE_INVALID) {
+            pa_device_port *port;
+            pa_port_available_t available = audio_state_to_availability(state);
+
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "a2dp-input"));
+            pa_device_port_set_available(port, available);
+
+            acquire = (available == PA_PORT_AVAILABLE_YES && u->profile == PROFILE_A2DP_SOURCE);
+            release = (available != PA_PORT_AVAILABLE_YES && u->profile == PROFILE_A2DP_SOURCE);
+        }
+    } else if (dbus_message_is_signal(m, "org.bluez.AudioSink", "PropertyChanged")) {
+        pa_bt_audio_state_t state = parse_state_property_change(m);
+
+        if (state != PA_BT_AUDIO_STATE_INVALID) {
+            pa_device_port *port;
+            pa_port_available_t available = audio_state_to_availability(state);
+
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "a2dp-output"));
+            pa_device_port_set_available(port, available);
+
+            acquire = (available == PA_PORT_AVAILABLE_YES && u->profile == PROFILE_A2DP);
+            release = (available != PA_PORT_AVAILABLE_YES && u->profile == PROFILE_A2DP);
+        }
+    }
+
+    if (acquire)
+        if (bt_transport_acquire(u, FALSE) >= 0) {
+            if (u->source)
+                pa_source_suspend(u->source, FALSE, PA_SUSPEND_IDLE|PA_SUSPEND_USER);
+
+            if (u->sink)
+                pa_sink_suspend(u->sink, FALSE, PA_SUSPEND_IDLE|PA_SUSPEND_USER);
+        }
+
+    if (release && bt_transport_is_acquired(u)) {
+        /* FIXME: this release is racy, since the audio stream might have
+           been set up again in the meantime (but not processed yet by PA).
+           BlueZ should probably release the transport automatically, and
+           in that case we would just mark the transport as released */
+
+        /* Remote side closed the stream so we consider it PA_SUSPEND_USER */
+        if (u->source)
+            pa_source_suspend(u->source, TRUE, PA_SUSPEND_USER);
+
+        if (u->sink)
+            pa_sink_suspend(u->sink, TRUE, PA_SUSPEND_USER);
     }
 
 fail:
@@ -1467,21 +1399,35 @@ fail:
 
 /* Run from main thread */
 static void sink_set_volume_cb(pa_sink *s) {
-    struct userdata *u = s->userdata;
     DBusMessage *m;
     dbus_uint16_t gain;
+    pa_volume_t volume;
+    struct userdata *u;
+    char *k;
+
+    pa_assert(s);
+    pa_assert(s->core);
+
+    k = pa_sprintf_malloc("bluetooth-device@%p", (void*) s);
+    u = pa_shared_get(s->core, k);
+    pa_xfree(k);
 
     pa_assert(u);
+    pa_assert(u->sink == s);
+    pa_assert(u->profile == PROFILE_HSP);
+
+    gain = (pa_cvolume_max(&s->real_volume) * HSP_MAX_GAIN) / PA_VOLUME_NORM;
 
-    if (u->profile != PROFILE_HSP)
-        return;
+    if (gain > HSP_MAX_GAIN)
+        gain = HSP_MAX_GAIN;
 
-    gain = (pa_cvolume_max(&s->virtual_volume) * 15) / PA_VOLUME_NORM;
+    volume = (pa_volume_t) (gain * PA_VOLUME_NORM / HSP_MAX_GAIN);
 
-    if (gain > 15)
-        gain = 15;
+    /* increment volume by one to correct rounding errors */
+    if (volume < PA_VOLUME_NORM)
+        volume++;
 
-    pa_cvolume_set(&s->virtual_volume, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
+    pa_cvolume_set(&s->real_volume, u->sample_spec.channels, volume);
 
     pa_assert_se(m = dbus_message_new_method_call("org.bluez", u->path, "org.bluez.Headset", "SetSpeakerGain"));
     pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID));
@@ -1491,21 +1437,35 @@ static void sink_set_volume_cb(pa_sink *s) {
 
 /* Run from main thread */
 static void source_set_volume_cb(pa_source *s) {
-    struct userdata *u = s->userdata;
     DBusMessage *m;
     dbus_uint16_t gain;
+    pa_volume_t volume;
+    struct userdata *u;
+    char *k;
+
+    pa_assert(s);
+    pa_assert(s->core);
+
+    k = pa_sprintf_malloc("bluetooth-device@%p", (void*) s);
+    u = pa_shared_get(s->core, k);
+    pa_xfree(k);
 
     pa_assert(u);
+    pa_assert(u->source == s);
+    pa_assert(u->profile == PROFILE_HSP);
 
-    if (u->profile != PROFILE_HSP)
-        return;
+    gain = (pa_cvolume_max(&s->real_volume) * HSP_MAX_GAIN) / PA_VOLUME_NORM;
 
-    gain = (pa_cvolume_max(&s->virtual_volume) * 15) / PA_VOLUME_NORM;
+    if (gain > HSP_MAX_GAIN)
+        gain = HSP_MAX_GAIN;
 
-    if (gain > 15)
-        gain = 15;
+    volume = (pa_volume_t) (gain * PA_VOLUME_NORM / HSP_MAX_GAIN);
 
-    pa_cvolume_set(&s->virtual_volume, u->sample_spec.channels, (pa_volume_t) (gain * PA_VOLUME_NORM / 15));
+    /* increment volume by one to correct rounding errors */
+    if (volume < PA_VOLUME_NORM)
+        volume++;
+
+    pa_cvolume_set(&s->real_volume, u->sample_spec.channels, volume);
 
     pa_assert_se(m = dbus_message_new_method_call("org.bluez", u->path, "org.bluez.Headset", "SetMicrophoneGain"));
     pa_assert_se(dbus_message_append_args(m, DBUS_TYPE_UINT16, &gain, DBUS_TYPE_INVALID));
@@ -1542,34 +1502,35 @@ static char *get_name(const char *type, pa_modargs *ma, const char *device_id, p
     return pa_sprintf_malloc("bluez_%s.%s", type, n);
 }
 
-#ifdef NOKIA
-
-static void sco_over_pcm_state_update(struct userdata *u) {
+static int sco_over_pcm_state_update(struct userdata *u, pa_bool_t changed) {
     pa_assert(u);
     pa_assert(USE_SCO_OVER_PCM(u));
 
     if (PA_SINK_IS_OPENED(pa_sink_get_state(u->hsp.sco_sink)) ||
         PA_SOURCE_IS_OPENED(pa_source_get_state(u->hsp.sco_source))) {
 
-        if (u->service_fd >= 0)
-            return;
+        if (u->stream_fd >= 0)
+            return 0;
 
         pa_log_debug("Resuming SCO over PCM");
-        if ((init_bt(u) < 0) || (init_profile(u) < 0))
+        if (init_profile(u) < 0) {
             pa_log("Can't resume SCO over PCM");
+            return -1;
+        }
 
-        start_stream_fd(u);
-    } else {
-
-        if (u->service_fd < 0)
-            return;
+        return bt_transport_acquire(u, TRUE);
+    }
 
-        stop_stream_fd(u);
+    if (changed) {
+        if (u->stream_fd < 0)
+            return 0;
 
         pa_log_debug("Closing SCO over PCM");
-        pa_close(u->service_fd);
-        u->service_fd = -1;
+
+        bt_transport_release(u);
     }
+
+    return 0;
 }
 
 static pa_hook_result_t sink_state_changed_cb(pa_core *c, pa_sink *s, struct userdata *u) {
@@ -1580,7 +1541,7 @@ static pa_hook_result_t sink_state_changed_cb(pa_core *c, pa_sink *s, struct use
     if (s != u->hsp.sco_sink)
         return PA_HOOK_OK;
 
-    sco_over_pcm_state_update(u);
+    sco_over_pcm_state_update(u, TRUE);
 
     return PA_HOOK_OK;
 }
@@ -1593,33 +1554,120 @@ static pa_hook_result_t source_state_changed_cb(pa_core *c, pa_source *s, struct
     if (s != u->hsp.sco_source)
         return PA_HOOK_OK;
 
-    sco_over_pcm_state_update(u);
+    sco_over_pcm_state_update(u, TRUE);
 
     return PA_HOOK_OK;
 }
 
-#endif
+static pa_hook_result_t nrec_changed_cb(pa_bluetooth_transport *t, void *call_data, struct userdata *u) {
+    pa_proplist *p;
+
+    pa_assert(t);
+    pa_assert(u);
+
+    p = pa_proplist_new();
+    pa_proplist_sets(p, "bluetooth.nrec", t->nrec ? "1" : "0");
+    pa_source_update_proplist(u->source, PA_UPDATE_REPLACE, p);
+    pa_proplist_free(p);
+
+    return PA_HOOK_OK;
+}
+
+static void connect_ports(struct userdata *u, void *sink_or_source_new_data, pa_direction_t direction) {
+    union {
+        pa_sink_new_data *sink_new_data;
+        pa_source_new_data *source_new_data;
+    } data;
+    pa_device_port *port;
+
+    if (direction == PA_DIRECTION_OUTPUT) {
+        data.sink_new_data = sink_or_source_new_data;
+        data.sink_new_data->ports = pa_hashmap_new(pa_idxset_string_hash_func, pa_idxset_string_compare_func);
+    } else {
+        data.source_new_data = sink_or_source_new_data;
+        data.source_new_data->ports = pa_hashmap_new(pa_idxset_string_hash_func, pa_idxset_string_compare_func);
+    }
+
+    switch (u->profile) {
+        case PROFILE_A2DP:
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "a2dp-output"));
+            pa_assert_se(pa_hashmap_put(data.sink_new_data->ports, port->name, port) >= 0);
+            pa_device_port_ref(port);
+            break;
+
+        case PROFILE_A2DP_SOURCE:
+            pa_assert_se(port = pa_hashmap_get(u->card->ports, "a2dp-input"));
+            pa_assert_se(pa_hashmap_put(data.source_new_data->ports, port->name, port) >= 0);
+            pa_device_port_ref(port);
+            break;
+
+        case PROFILE_HSP:
+            if (direction == PA_DIRECTION_OUTPUT) {
+                pa_assert_se(port = pa_hashmap_get(u->card->ports, "hsp-output"));
+                pa_assert_se(pa_hashmap_put(data.sink_new_data->ports, port->name, port) >= 0);
+            } else {
+                pa_assert_se(port = pa_hashmap_get(u->card->ports, "hsp-input"));
+                pa_assert_se(pa_hashmap_put(data.source_new_data->ports, port->name, port) >= 0);
+            }
+            pa_device_port_ref(port);
+            break;
+
+        case PROFILE_HFGW:
+            if (direction == PA_DIRECTION_OUTPUT) {
+                pa_assert_se(port = pa_hashmap_get(u->card->ports, "hfgw-output"));
+                pa_assert_se(pa_hashmap_put(data.sink_new_data->ports, port->name, port) >= 0);
+            } else {
+                pa_assert_se(port = pa_hashmap_get(u->card->ports, "hfgw-input"));
+                pa_assert_se(pa_hashmap_put(data.source_new_data->ports, port->name, port) >= 0);
+            }
+            pa_device_port_ref(port);
+            break;
+
+        default:
+            pa_assert_not_reached();
+        }
+}
+
+static const char *profile_to_string(enum profile profile) {
+    switch(profile) {
+        case PROFILE_A2DP:
+            return "a2dp";
+        case PROFILE_A2DP_SOURCE:
+            return "a2dp_source";
+        case PROFILE_HSP:
+            return "hsp";
+        case PROFILE_HFGW:
+            return "hfgw";
+        default:
+            pa_assert_not_reached();
+    }
+}
+
+static int sink_set_port_cb(pa_sink *s, pa_device_port *p) {
+    return 0;
+}
+
+static int source_set_port_cb(pa_source *s, pa_device_port *p) {
+    return 0;
+}
 
 /* Run from main thread */
 static int add_sink(struct userdata *u) {
+    char *k;
 
-#ifdef NOKIA
     if (USE_SCO_OVER_PCM(u)) {
         pa_proplist *p;
 
         u->sink = u->hsp.sco_sink;
         p = pa_proplist_new();
-        pa_proplist_sets(p, "bluetooth.protocol", "sco");
+        pa_proplist_sets(p, "bluetooth.protocol", profile_to_string(u->profile));
         pa_proplist_update(u->sink->proplist, PA_UPDATE_MERGE, p);
         pa_proplist_free(p);
 
         if (!u->hsp.sink_state_changed_slot)
             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);
 
-    } else
-#endif
-
-    {
+    } else {
         pa_sink_new_data data;
         pa_bool_t b;
 
@@ -1627,7 +1675,7 @@ static int add_sink(struct userdata *u) {
         data.driver = __FILE__;
         data.module = u->module;
         pa_sink_new_data_set_sample_spec(&data, &u->sample_spec);
-        pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP ? "a2dp" : "sco");
+        pa_proplist_sets(data.proplist, "bluetooth.protocol", profile_to_string(u->profile));
         if (u->profile == PROFILE_HSP)
             pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
         data.card = u->card;
@@ -1639,8 +1687,9 @@ static int add_sink(struct userdata *u) {
             pa_sink_new_data_done(&data);
             return -1;
         }
+        connect_ports(u, &data, PA_DIRECTION_OUTPUT);
 
-        u->sink = pa_sink_new(u->core, &data, PA_SINK_HARDWARE|PA_SINK_LATENCY | (u->profile == PROFILE_HSP ? PA_SINK_HW_VOLUME_CTRL : 0));
+        u->sink = pa_sink_new(u->core, &data, PA_SINK_HARDWARE|PA_SINK_LATENCY);
         pa_sink_new_data_done(&data);
 
         if (!u->sink) {
@@ -1650,16 +1699,16 @@ static int add_sink(struct userdata *u) {
 
         u->sink->userdata = u;
         u->sink->parent.process_msg = sink_process_msg;
-
-        pa_sink_set_max_request(u->sink, u->block_size);
-        pa_sink_set_fixed_latency(u->sink,
-                                  (u->profile == PROFILE_A2DP ? FIXED_LATENCY_PLAYBACK_A2DP : FIXED_LATENCY_PLAYBACK_HSP) +
-                                  pa_bytes_to_usec(u->block_size, &u->sample_spec));
+        u->sink->set_port = sink_set_port_cb;
     }
 
     if (u->profile == PROFILE_HSP) {
-        u->sink->set_volume = sink_set_volume_cb;
+        pa_sink_set_set_volume_callback(u->sink, sink_set_volume_cb);
         u->sink->n_volume_steps = 16;
+
+        k = pa_sprintf_malloc("bluetooth-device@%p", (void*) u->sink);
+        pa_shared_set(u->core, k, u);
+        pa_xfree(k);
     }
 
     return 0;
@@ -1667,19 +1716,16 @@ static int add_sink(struct userdata *u) {
 
 /* Run from main thread */
 static int add_source(struct userdata *u) {
+    char *k;
 
-#ifdef NOKIA
     if (USE_SCO_OVER_PCM(u)) {
         u->source = u->hsp.sco_source;
-        pa_proplist_sets(u->source->proplist, "bluetooth.protocol", "hsp");
+        pa_proplist_sets(u->source->proplist, "bluetooth.protocol", profile_to_string(u->profile));
 
         if (!u->hsp.source_state_changed_slot)
             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);
 
-    } else
-#endif
-
-    {
+    } else {
         pa_source_new_data data;
         pa_bool_t b;
 
@@ -1687,9 +1733,10 @@ static int add_source(struct userdata *u) {
         data.driver = __FILE__;
         data.module = u->module;
         pa_source_new_data_set_sample_spec(&data, &u->sample_spec);
-        pa_proplist_sets(data.proplist, "bluetooth.protocol", u->profile == PROFILE_A2DP ? "a2dp" : "hsp");
+        pa_proplist_sets(data.proplist, "bluetooth.protocol", profile_to_string(u->profile));
         if (u->profile == PROFILE_HSP)
             pa_proplist_sets(data.proplist, PA_PROP_DEVICE_INTENDED_ROLES, "phone");
+
         data.card = u->card;
         data.name = get_name("source", u->modargs, u->address, &b);
         data.namereg_fail = b;
@@ -1700,7 +1747,8 @@ static int add_source(struct userdata *u) {
             return -1;
         }
 
-        u->source = pa_source_new(u->core, &data, PA_SOURCE_HARDWARE|PA_SOURCE_LATENCY | (u->profile == PROFILE_HSP ? PA_SOURCE_HW_VOLUME_CTRL : 0));
+        connect_ports(u, &data, PA_DIRECTION_INPUT);
+        u->source = pa_source_new(u->core, &data, PA_SOURCE_HARDWARE|PA_SOURCE_LATENCY);
         pa_source_new_data_done(&data);
 
         if (!u->source) {
@@ -1710,87 +1758,183 @@ static int add_source(struct userdata *u) {
 
         u->source->userdata = u;
         u->source->parent.process_msg = source_process_msg;
+        u->source->set_port = source_set_port_cb;
+    }
+
+    if ((u->profile == PROFILE_HSP) || (u->profile == PROFILE_HFGW)) {
+        pa_bluetooth_transport *t;
+        t = pa_bluetooth_discovery_get_transport(u->discovery, u->transport);
+        pa_assert(t);
+        pa_proplist_sets(u->source->proplist, "bluetooth.nrec", t->nrec ? "1" : "0");
 
-        pa_source_set_fixed_latency(u->source,
-                                    (/* u->profile == PROFILE_A2DP ? FIXED_LATENCY_RECORD_A2DP : */ FIXED_LATENCY_RECORD_HSP) +
-                                    pa_bytes_to_usec(u->block_size, &u->sample_spec));
+        if (!u->hsp.nrec_changed_slot)
+            u->hsp.nrec_changed_slot = pa_hook_connect(&t->hooks[PA_BLUETOOTH_TRANSPORT_HOOK_NREC_CHANGED], PA_HOOK_NORMAL, (pa_hook_cb_t) nrec_changed_cb, u);
     }
 
     if (u->profile == PROFILE_HSP) {
-        pa_proplist_sets(u->source->proplist, "bluetooth.nrec", (u->hsp.pcm_capabilities.flags & BT_PCM_FLAG_NREC) ? "1" : "0");
-        u->source->set_volume = source_set_volume_cb;
+        pa_source_set_set_volume_callback(u->source, source_set_volume_cb);
         u->source->n_volume_steps = 16;
+
+        k = pa_sprintf_malloc("bluetooth-device@%p", (void*) u->source);
+        pa_shared_set(u->core, k, u);
+        pa_xfree(k);
     }
 
     return 0;
 }
 
-/* Run from main thread */
-static void shutdown_bt(struct userdata *u) {
-    pa_assert(u);
+static void bt_transport_config_a2dp(struct userdata *u) {
+    const pa_bluetooth_transport *t;
+    struct a2dp_info *a2dp = &u->a2dp;
+    a2dp_sbc_t *config;
 
-    if (u->stream_fd >= 0) {
-        pa_close(u->stream_fd);
-        u->stream_fd = -1;
+    t = pa_bluetooth_discovery_get_transport(u->discovery, u->transport);
+    pa_assert(t);
+
+    config = (a2dp_sbc_t *) t->config;
+
+    u->sample_spec.format = PA_SAMPLE_S16LE;
+
+    if (a2dp->sbc_initialized)
+        sbc_reinit(&a2dp->sbc, 0);
+    else
+        sbc_init(&a2dp->sbc, 0);
+    a2dp->sbc_initialized = TRUE;
 
-        u->stream_write_type = 0;
+    switch (config->frequency) {
+        case BT_SBC_SAMPLING_FREQ_16000:
+            a2dp->sbc.frequency = SBC_FREQ_16000;
+            u->sample_spec.rate = 16000U;
+            break;
+        case BT_SBC_SAMPLING_FREQ_32000:
+            a2dp->sbc.frequency = SBC_FREQ_32000;
+            u->sample_spec.rate = 32000U;
+            break;
+        case BT_SBC_SAMPLING_FREQ_44100:
+            a2dp->sbc.frequency = SBC_FREQ_44100;
+            u->sample_spec.rate = 44100U;
+            break;
+        case BT_SBC_SAMPLING_FREQ_48000:
+            a2dp->sbc.frequency = SBC_FREQ_48000;
+            u->sample_spec.rate = 48000U;
+            break;
+        default:
+            pa_assert_not_reached();
     }
 
-    if (u->service_fd >= 0) {
-        pa_close(u->service_fd);
-        u->service_fd = -1;
-        u->service_write_type = 0;
-        u->service_read_type = 0;
+    switch (config->channel_mode) {
+        case BT_A2DP_CHANNEL_MODE_MONO:
+            a2dp->sbc.mode = SBC_MODE_MONO;
+            u->sample_spec.channels = 1;
+            break;
+        case BT_A2DP_CHANNEL_MODE_DUAL_CHANNEL:
+            a2dp->sbc.mode = SBC_MODE_DUAL_CHANNEL;
+            u->sample_spec.channels = 2;
+            break;
+        case BT_A2DP_CHANNEL_MODE_STEREO:
+            a2dp->sbc.mode = SBC_MODE_STEREO;
+            u->sample_spec.channels = 2;
+            break;
+        case BT_A2DP_CHANNEL_MODE_JOINT_STEREO:
+            a2dp->sbc.mode = SBC_MODE_JOINT_STEREO;
+            u->sample_spec.channels = 2;
+            break;
+        default:
+            pa_assert_not_reached();
     }
 
-    if (u->write_memchunk.memblock) {
-        pa_memblock_unref(u->write_memchunk.memblock);
-        pa_memchunk_reset(&u->write_memchunk);
+    switch (config->allocation_method) {
+        case BT_A2DP_ALLOCATION_SNR:
+            a2dp->sbc.allocation = SBC_AM_SNR;
+            break;
+        case BT_A2DP_ALLOCATION_LOUDNESS:
+            a2dp->sbc.allocation = SBC_AM_LOUDNESS;
+            break;
+        default:
+            pa_assert_not_reached();
     }
-}
 
-/* Run from main thread */
-static int init_bt(struct userdata *u) {
-    pa_assert(u);
+    switch (config->subbands) {
+        case BT_A2DP_SUBBANDS_4:
+            a2dp->sbc.subbands = SBC_SB_4;
+            break;
+        case BT_A2DP_SUBBANDS_8:
+            a2dp->sbc.subbands = SBC_SB_8;
+            break;
+        default:
+            pa_assert_not_reached();
+    }
 
-    shutdown_bt(u);
+    switch (config->block_length) {
+        case BT_A2DP_BLOCK_LENGTH_4:
+            a2dp->sbc.blocks = SBC_BLK_4;
+            break;
+        case BT_A2DP_BLOCK_LENGTH_8:
+            a2dp->sbc.blocks = SBC_BLK_8;
+            break;
+        case BT_A2DP_BLOCK_LENGTH_12:
+            a2dp->sbc.blocks = SBC_BLK_12;
+            break;
+        case BT_A2DP_BLOCK_LENGTH_16:
+            a2dp->sbc.blocks = SBC_BLK_16;
+            break;
+        default:
+            pa_assert_not_reached();
+    }
 
-    u->stream_write_type = 0;
-    u->service_write_type = 0;
-    u->service_read_type = 0;
+    a2dp->min_bitpool = config->min_bitpool;
+    a2dp->max_bitpool = config->max_bitpool;
 
-    if ((u->service_fd = bt_audio_service_open()) < 0) {
-        pa_log_error("Couldn't connect to bluetooth audio service");
-        return -1;
-    }
+    /* Set minimum bitpool for source to get the maximum possible block_size */
+    a2dp->sbc.bitpool = u->profile == PROFILE_A2DP ? a2dp->max_bitpool : a2dp->min_bitpool;
+    a2dp->codesize = sbc_get_codesize(&a2dp->sbc);
+    a2dp->frame_length = sbc_get_frame_length(&a2dp->sbc);
 
-    pa_log_debug("Connected to the bluetooth audio service");
+    pa_log_info("SBC parameters:\n\tallocation=%u\n\tsubbands=%u\n\tblocks=%u\n\tbitpool=%u\n",
+                a2dp->sbc.allocation, a2dp->sbc.subbands, a2dp->sbc.blocks, a2dp->sbc.bitpool);
+}
 
-    return 0;
+static void bt_transport_config(struct userdata *u) {
+    if (u->profile == PROFILE_HSP || u->profile == PROFILE_HFGW) {
+        u->sample_spec.format = PA_SAMPLE_S16LE;
+        u->sample_spec.channels = 1;
+        u->sample_spec.rate = 8000;
+    } else
+        bt_transport_config_a2dp(u);
 }
 
 /* Run from main thread */
 static int setup_bt(struct userdata *u) {
+    const pa_bluetooth_device *d;
+    const pa_bluetooth_transport *t;
+
     pa_assert(u);
 
-    if (get_caps(u, 0) < 0)
+    if (!(d = pa_bluetooth_discovery_get_by_path(u->discovery, u->path))) {
+        pa_log_error("Failed to get device object.");
         return -1;
+    }
 
-    pa_log_debug("Got device capabilities");
+    /* release transport if exist */
+    if (u->transport) {
+        bt_transport_release(u);
+        pa_xfree(u->transport);
+        u->transport = NULL;
+    }
 
-    if (set_conf(u) < 0)
+    /* check if profile has a transport */
+    t = pa_bluetooth_device_get_transport(d, u->profile);
+    if (t == NULL) {
+        pa_log_warn("Profile has no transport");
         return -1;
+    }
 
-    pa_log_debug("Connection to the device configured");
+    u->transport = pa_xstrdup(t->path);
 
-#ifdef NOKIA
-    if (USE_SCO_OVER_PCM(u)) {
-        pa_log_debug("Configured to use SCO over PCM");
-        return 0;
-    }
-#endif
+    if (bt_transport_acquire(u, FALSE) < 0)
+        return -1;
 
-    pa_log_debug("Got the stream socket");
+    bt_transport_config(u);
 
     return 0;
 }
@@ -1805,11 +1949,14 @@ static int init_profile(struct userdata *u) {
         return -1;
 
     if (u->profile == PROFILE_A2DP ||
-        u->profile == PROFILE_HSP)
+        u->profile == PROFILE_HSP ||
+        u->profile == PROFILE_HFGW)
         if (add_sink(u) < 0)
             r = -1;
 
-    if (u->profile == PROFILE_HSP)
+    if (u->profile == PROFILE_HSP ||
+        u->profile == PROFILE_A2DP_SOURCE ||
+        u->profile == PROFILE_HFGW)
         if (add_source(u) < 0)
             r = -1;
 
@@ -1818,6 +1965,8 @@ static int init_profile(struct userdata *u) {
 
 /* Run from main thread */
 static void stop_thread(struct userdata *u) {
+    char *k;
+
     pa_assert(u);
 
     if (u->thread) {
@@ -1841,12 +1990,29 @@ static void stop_thread(struct userdata *u) {
         u->hsp.source_state_changed_slot = NULL;
     }
 
+    if (u->hsp.nrec_changed_slot) {
+        pa_hook_slot_free(u->hsp.nrec_changed_slot);
+        u->hsp.nrec_changed_slot = NULL;
+    }
+
     if (u->sink) {
+        if (u->profile == PROFILE_HSP) {
+            k = pa_sprintf_malloc("bluetooth-device@%p", (void*) u->sink);
+            pa_shared_remove(u->core, k);
+            pa_xfree(k);
+        }
+
         pa_sink_unref(u->sink);
         u->sink = NULL;
     }
 
     if (u->source) {
+        if (u->profile == PROFILE_HSP) {
+            k = pa_sprintf_malloc("bluetooth-device@%p", (void*) u->source);
+            pa_shared_remove(u->core, k);
+            pa_xfree(k);
+        }
+
         pa_source_unref(u->source);
         u->source = NULL;
     }
@@ -1874,19 +2040,32 @@ static int start_thread(struct userdata *u) {
     u->rtpoll = pa_rtpoll_new();
     pa_thread_mq_init(&u->thread_mq, u->core->mainloop, u->rtpoll);
 
-#ifdef NOKIA
     if (USE_SCO_OVER_PCM(u)) {
-        if (start_stream_fd(u) < 0)
+        if (sco_over_pcm_state_update(u, FALSE) < 0) {
+            char *k;
+
+            if (u->sink) {
+                k = pa_sprintf_malloc("bluetooth-device@%p", (void*) u->sink);
+                pa_shared_remove(u->core, k);
+                pa_xfree(k);
+                u->sink = NULL;
+            }
+            if (u->source) {
+                k = pa_sprintf_malloc("bluetooth-device@%p", (void*) u->source);
+                pa_shared_remove(u->core, k);
+                pa_xfree(k);
+                u->source = NULL;
+            }
             return -1;
+        }
 
         pa_sink_ref(u->sink);
         pa_source_ref(u->source);
         /* FIXME: monitor stream_fd error */
         return 0;
     }
-#endif
 
-    if (!(u->thread = pa_thread_new(thread_func, u))) {
+    if (!(u->thread = pa_thread_new("bluetooth", thread_func, u))) {
         pa_log_error("Failed to create IO thread");
         stop_thread(u);
         return -1;
@@ -1913,6 +2092,22 @@ static int start_thread(struct userdata *u) {
     return 0;
 }
 
+static void save_sco_volume_callbacks(struct userdata *u) {
+    pa_assert(u);
+    pa_assert(USE_SCO_OVER_PCM(u));
+
+    u->hsp.sco_sink_set_volume = u->hsp.sco_sink->set_volume;
+    u->hsp.sco_source_set_volume = u->hsp.sco_source->set_volume;
+}
+
+static void restore_sco_volume_callbacks(struct userdata *u) {
+    pa_assert(u);
+    pa_assert(USE_SCO_OVER_PCM(u));
+
+    pa_sink_set_set_volume_callback(u->hsp.sco_sink, u->hsp.sco_sink_set_volume);
+    pa_source_set_set_volume_callback(u->hsp.sco_source, u->hsp.sco_source_set_volume);
+}
+
 /* Run from main thread */
 static int card_set_profile(pa_card *c, pa_card_profile *new_profile) {
     struct userdata *u;
@@ -1939,35 +2134,47 @@ static int card_set_profile(pa_card *c, pa_card_profile *new_profile) {
     if (device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) {
         pa_log_warn("HSP is not connected, refused to switch profile");
         return -PA_ERR_IO;
-    }
-    else if (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP) {
-        pa_log_warn("A2DP is not connected, refused to switch profile");
+    } else if (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP) {
+        pa_log_warn("A2DP Sink is not connected, refused to switch profile");
+        return -PA_ERR_IO;
+    } else if (device->audio_source_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP_SOURCE) {
+        pa_log_warn("A2DP Source is not connected, refused to switch profile");
+        return -PA_ERR_IO;
+    } else if (device->hfgw_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HFGW) {
+        pa_log_warn("HandsfreeGateway is not connected, refused to switch profile");
         return -PA_ERR_IO;
     }
 
     if (u->sink) {
         inputs = pa_sink_move_all_start(u->sink, NULL);
-#ifdef NOKIA
+
         if (!USE_SCO_OVER_PCM(u))
-#endif
             pa_sink_unlink(u->sink);
     }
 
     if (u->source) {
         outputs = pa_source_move_all_start(u->source, NULL);
-#ifdef NOKIA
+
         if (!USE_SCO_OVER_PCM(u))
-#endif
             pa_source_unlink(u->source);
     }
 
     stop_thread(u);
-    shutdown_bt(u);
+
+    if (u->profile != PROFILE_OFF && u->transport) {
+        bt_transport_release(u);
+        pa_xfree(u->transport);
+        u->transport = NULL;
+    }
+
+    if (USE_SCO_OVER_PCM(u))
+        restore_sco_volume_callbacks(u);
 
     u->profile = *d;
     u->sample_spec = u->requested_sample_spec;
 
-    init_bt(u);
+    if (USE_SCO_OVER_PCM(u))
+        save_sco_volume_callbacks(u);
 
     if (u->profile != PROFILE_OFF)
         init_profile(u);
@@ -1992,6 +2199,75 @@ static int card_set_profile(pa_card *c, pa_card_profile *new_profile) {
     return 0;
 }
 
+static void create_ports_for_profile(struct userdata *u, const pa_bluetooth_device *device, pa_card_new_data *card_new_data, pa_card_profile *profile) {
+    pa_device_port *port;
+    enum profile *d;
+
+    d = PA_CARD_PROFILE_DATA(profile);
+
+    switch (*d) {
+        case PROFILE_A2DP:
+            pa_assert_se(port = pa_device_port_new(u->core, "a2dp-output", _("Bluetooth High Quality (A2DP)"), 0));
+            pa_assert_se(pa_hashmap_put(card_new_data->ports, port->name, port) >= 0);
+            port->is_output = 1;
+            port->is_input = 0;
+            port->priority = profile->priority * 100;
+            port->available = audio_state_to_availability(device->audio_sink_state);
+            pa_hashmap_put(port->profiles, profile->name, profile);
+            break;
+
+        case PROFILE_A2DP_SOURCE:
+            pa_assert_se(port = pa_device_port_new(u->core, "a2dp-input", _("Bluetooth High Quality (A2DP)"), 0));
+            pa_assert_se(pa_hashmap_put(card_new_data->ports, port->name, port) >= 0);
+            port->is_output = 0;
+            port->is_input = 1;
+            port->priority = profile->priority * 100;
+            port->available = audio_state_to_availability(device->audio_source_state);
+            pa_hashmap_put(port->profiles, profile->name, profile);
+            break;
+
+        case PROFILE_HSP:
+            pa_assert_se(port = pa_device_port_new(u->core, "hsp-output", _("Bluetooth Telephony (HSP/HFP)"), 0));
+            pa_assert_se(pa_hashmap_put(card_new_data->ports, port->name, port) >= 0);
+            port->is_output = 1;
+            port->is_input = 0;
+            port->priority = profile->priority * 100;
+            port->available = audio_state_to_availability(device->headset_state);
+            pa_hashmap_put(port->profiles, profile->name, profile);
+
+            pa_assert_se(port = pa_device_port_new(u->core, "hsp-input", _("Bluetooth Telephony (HSP/HFP)"), 0));
+            pa_assert_se(pa_hashmap_put(card_new_data->ports, port->name, port) >= 0);
+            port->is_output = 0;
+            port->is_input = 1;
+            port->priority = profile->priority * 100;
+            port->available = audio_state_to_availability(device->headset_state);
+            pa_hashmap_put(port->profiles, profile->name, profile);
+            break;
+
+        case PROFILE_HFGW:
+            pa_assert_se(port = pa_device_port_new(u->core, "hfgw-output", _("Bluetooth Handsfree Gateway"), 0));
+            pa_assert_se(pa_hashmap_put(card_new_data->ports, port->name, port) >= 0);
+            port->is_output = 1;
+            port->is_input = 0;
+            port->priority = profile->priority * 100;
+            port->available = audio_state_to_availability(device->hfgw_state);
+            pa_hashmap_put(port->profiles, profile->name, profile);
+
+            pa_assert_se(port = pa_device_port_new(u->core, "hfgw-input", _("Bluetooth Handsfree Gateway"), 0));
+            pa_assert_se(pa_hashmap_put(card_new_data->ports, port->name, port) >= 0);
+            port->is_output = 0;
+            port->is_input = 1;
+            port->priority = profile->priority * 100;
+            port->available = audio_state_to_availability(device->hfgw_state);
+            pa_hashmap_put(port->profiles, profile->name, profile);
+            break;
+
+        default:
+            pa_assert_not_reached();
+    }
+
+}
+
 /* Run from main thread */
 static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
     pa_card_new_data data;
@@ -2046,6 +2322,22 @@ static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
 
         d = PA_CARD_PROFILE_DATA(p);
         *d = PROFILE_A2DP;
+        create_ports_for_profile(u, device, &data, p);
+
+        pa_hashmap_put(data.profiles, p->name, p);
+    }
+
+    if (pa_bluetooth_uuid_has(device->uuids, A2DP_SOURCE_UUID)) {
+        p = pa_card_profile_new("a2dp_source", _("High Fidelity Capture (A2DP)"), sizeof(enum profile));
+        p->priority = 10;
+        p->n_sinks = 0;
+        p->n_sources = 1;
+        p->max_sink_channels = 0;
+        p->max_source_channels = 2;
+
+        d = PA_CARD_PROFILE_DATA(p);
+        *d = PROFILE_A2DP_SOURCE;
+        create_ports_for_profile(u, device, &data, p);
 
         pa_hashmap_put(data.profiles, p->name, p);
     }
@@ -2061,6 +2353,22 @@ static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
 
         d = PA_CARD_PROFILE_DATA(p);
         *d = PROFILE_HSP;
+        create_ports_for_profile(u, device, &data, p);
+
+        pa_hashmap_put(data.profiles, p->name, p);
+    }
+
+    if (pa_bluetooth_uuid_has(device->uuids, HFP_AG_UUID)) {
+        p = pa_card_profile_new("hfgw", _("Handsfree Gateway"), sizeof(enum profile));
+        p->priority = 20;
+        p->n_sinks = 1;
+        p->n_sources = 1;
+        p->max_sink_channels = 1;
+        p->max_source_channels = 1;
+
+        d = PA_CARD_PROFILE_DATA(p);
+        *d = PROFILE_HFGW;
+        create_ports_for_profile(u, device, &data, p);
 
         pa_hashmap_put(data.profiles, p->name, p);
     }
@@ -2093,7 +2401,9 @@ static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
     d = PA_CARD_PROFILE_DATA(u->card->active_profile);
 
     if ((device->headset_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HSP) ||
-        (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP)) {
+        (device->audio_sink_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP) ||
+        (device->audio_source_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_A2DP_SOURCE) ||
+        (device->hfgw_state < PA_BT_AUDIO_STATE_CONNECTED && *d == PROFILE_HFGW)) {
         pa_log_warn("Default profile not connected, selecting off profile");
         u->card->active_profile = pa_hashmap_get(u->card->profiles, "off");
         u->card->save_profile = FALSE;
@@ -2102,6 +2412,9 @@ static int add_card(struct userdata *u, const pa_bluetooth_device *device) {
     d = PA_CARD_PROFILE_DATA(u->card->active_profile);
     u->profile = *d;
 
+    if (USE_SCO_OVER_PCM(u))
+        save_sco_volume_callbacks(u);
+
     return 0;
 }
 
@@ -2123,7 +2436,7 @@ static const pa_bluetooth_device* find_device(struct userdata *u, const char *ad
         }
 
         if (address && !(pa_streq(d->address, address))) {
-            pa_log_error("Passed path %s and address %s don't match.", path, address);
+            pa_log_error("Passed path %s address %s != %s don't match.", path, d->address, address);
             return NULL;
         }
 
@@ -2180,12 +2493,10 @@ int pa__init(pa_module* m) {
     m->userdata = u = pa_xnew0(struct userdata, 1);
     u->module = m;
     u->core = m->core;
-    u->service_fd = -1;
     u->stream_fd = -1;
     u->sample_spec = m->core->default_sample_spec;
     u->modargs = ma;
 
-#ifdef NOKIA
     if (pa_modargs_get_value(ma, "sco_sink", NULL) &&
         !(u->hsp.sco_sink = pa_namereg_get(m->core, pa_modargs_get_value(ma, "sco_sink", NULL), PA_NAMEREG_SINK))) {
         pa_log("SCO sink not found");
@@ -2197,7 +2508,6 @@ int pa__init(pa_module* m) {
         pa_log("SCO source not found");
         goto fail;
     }
-#endif
 
     if (pa_modargs_get_value_u32(ma, "rate", &u->sample_spec.rate) < 0 ||
         u->sample_spec.rate <= 0 || u->sample_spec.rate > PA_RATE_MAX) {
@@ -2205,6 +2515,12 @@ int pa__init(pa_module* m) {
         goto fail;
     }
 
+    u->auto_connect = TRUE;
+    if (pa_modargs_get_value_boolean(ma, "auto_connect", &u->auto_connect)) {
+        pa_log("Failed to parse auto_connect= argument");
+        goto fail;
+    }
+
     channels = u->sample_spec.channels;
     if (pa_modargs_get_value_u32(ma, "channels", &channels) < 0 ||
         channels <= 0 || channels > PA_CHANNELS_MAX) {
@@ -2230,14 +2546,17 @@ int pa__init(pa_module* m) {
     if (add_card(u, device) < 0)
         goto fail;
 
-    /* Connect to the BT service and query capabilities */
-    if (init_bt(u) < 0)
+    if (!(u->msg = pa_msgobject_new(bluetooth_msg)))
         goto fail;
 
+    u->msg->parent.process_msg = device_process_msg;
+    u->msg->card = u->card;
+
     if (!dbus_connection_add_filter(pa_dbus_connection_get(u->connection), filter_cb, u, NULL)) {
         pa_log_error("Failed to add filter function");
         goto fail;
     }
+    u->filter_added = TRUE;
 
     speaker = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='SpeakerGainChanged',path='%s'", u->path);
     mike = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='MicrophoneGainChanged',path='%s'", u->path);
@@ -2246,6 +2565,11 @@ int pa__init(pa_module* m) {
                 pa_dbus_connection_get(u->connection), &err,
                 speaker,
                 mike,
+                "type='signal',sender='org.bluez',interface='org.bluez.MediaTransport',member='PropertyChanged'",
+                "type='signal',sender='org.bluez',interface='org.bluez.HandsfreeGateway',member='PropertyChanged'",
+                "type='signal',sender='org.bluez',interface='org.bluez.Headset',member='PropertyChanged'",
+                "type='signal',sender='org.bluez',interface='org.bluez.AudioSource',member='PropertyChanged'",
+                "type='signal',sender='org.bluez',interface='org.bluez.AudioSink',member='PropertyChanged'",
                 NULL) < 0) {
 
         pa_xfree(speaker);
@@ -2290,27 +2614,23 @@ int pa__get_n_used(pa_module *m) {
 
 void pa__done(pa_module *m) {
     struct userdata *u;
+
     pa_assert(m);
 
     if (!(u = m->userdata))
         return;
 
-    if (u->sink
-#ifdef NOKIA
-        && !USE_SCO_OVER_PCM(u)
-#endif
-    )
+    if (u->sink && !USE_SCO_OVER_PCM(u))
         pa_sink_unlink(u->sink);
 
-    if (u->source
-#ifdef NOKIA
-        && !USE_SCO_OVER_PCM(u)
-#endif
-    )
+    if (u->source && !USE_SCO_OVER_PCM(u))
         pa_source_unlink(u->source);
 
     stop_thread(u);
 
+    if (USE_SCO_OVER_PCM(u))
+        restore_sco_volume_callbacks(u);
+
     if (u->connection) {
 
         if (u->path) {
@@ -2318,27 +2638,30 @@ void pa__done(pa_module *m) {
             speaker = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='SpeakerGainChanged',path='%s'", u->path);
             mike = pa_sprintf_malloc("type='signal',sender='org.bluez',interface='org.bluez.Headset',member='MicrophoneGainChanged',path='%s'", u->path);
 
-            pa_dbus_remove_matches(pa_dbus_connection_get(u->connection),
-                                   speaker,
-                                   mike,
-                                   NULL);
+            pa_dbus_remove_matches(pa_dbus_connection_get(u->connection), speaker, mike,
+                "type='signal',sender='org.bluez',interface='org.bluez.MediaTransport',member='PropertyChanged'",
+                "type='signal',sender='org.bluez',interface='org.bluez.HandsfreeGateway',member='PropertyChanged'",
+                NULL);
 
             pa_xfree(speaker);
             pa_xfree(mike);
         }
 
-        dbus_connection_remove_filter(pa_dbus_connection_get(u->connection), filter_cb, u);
+        if (u->filter_added)
+            dbus_connection_remove_filter(pa_dbus_connection_get(u->connection), filter_cb, u);
+
         pa_dbus_connection_unref(u->connection);
     }
 
+    if (u->msg)
+        pa_xfree(u->msg);
+
     if (u->card)
         pa_card_free(u->card);
 
     if (u->read_smoother)
         pa_smoother_free(u->read_smoother);
 
-    shutdown_bt(u);
-
     if (u->a2dp.buffer)
         pa_xfree(u->a2dp.buffer);
 
@@ -2350,6 +2673,11 @@ void pa__done(pa_module *m) {
     pa_xfree(u->address);
     pa_xfree(u->path);
 
+    if (u->transport) {
+        bt_transport_release(u);
+        pa_xfree(u->transport);
+    }
+
     if (u->discovery)
         pa_bluetooth_discovery_unref(u->discovery);