Add HSP profileHSP-profile

3 files changed, 592 insertions, 13 deletions

diff --git a/src/modules/bluetooth/bluez5-util.c b/src/modules/bluetooth/bluez5-util.c
index 5b6b372f9..c11645a5b 100644
--- a/src/modules/bluetooth/bluez5-util.c
+++ b/src/modules/bluetooth/bluez5-util.c
@@ -26,6 +26,7 @@
 #include <pulse/xmalloc.h>
 
 #include <pulsecore/core.h>
+#include <pulsecore/core-error.h>
 #include <pulsecore/core-util.h>
 #include <pulsecore/dbus-shared.h>
 #include <pulsecore/log.h>
@@ -33,6 +34,13 @@
 #include <pulsecore/refcnt.h>
 #include <pulsecore/shared.h>
 
+#include <errno.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+
+#include <bluetooth/bluetooth.h>
+#include <bluetooth/sco.h>
+
 #include "a2dp-codecs.h"
 
 #include "bluez5-util.h"
@@ -43,12 +51,16 @@
 #define BLUEZ_MEDIA_INTERFACE BLUEZ_SERVICE ".Media1"
 #define BLUEZ_MEDIA_ENDPOINT_INTERFACE BLUEZ_SERVICE ".MediaEndpoint1"
 #define BLUEZ_MEDIA_TRANSPORT_INTERFACE BLUEZ_SERVICE ".MediaTransport1"
+#define BLUEZ_PROFILE_MANAGER_INTERFACE BLUEZ_SERVICE ".ProfileManager1"
+#define BLUEZ_PROFILE_INTERFACE BLUEZ_SERVICE ".Profile1"
 
 #define BLUEZ_ERROR_NOT_SUPPORTED "org.bluez.Error.NotSupported"
 
 #define A2DP_SOURCE_ENDPOINT "/MediaEndpoint/A2DPSource"
 #define A2DP_SINK_ENDPOINT "/MediaEndpoint/A2DPSink"
 
+#define HSP_AG_PROFILE "/Profile/HSPAGProfile"
+
 #define ENDPOINT_INTROSPECT_XML                                         \
     DBUS_INTROSPECT_1_0_XML_DOCTYPE_DECL_NODE                           \
     "<node>"                                                            \
@@ -74,6 +86,30 @@
     " </interface>"                                                     \
     "</node>"
 
+#define PROFILE_INTROSPECT_XML                                          \
+    DBUS_INTROSPECT_1_0_XML_DOCTYPE_DECL_NODE                           \
+    "<node>"                                                            \
+    " <interface name=\"" BLUEZ_PROFILE_INTERFACE "\">"                 \
+    "  <method name=\"Release\">"                                       \
+    "  </method>"                                                       \
+    "  <method name=\"Cancel\">"                                        \
+    "  </method>"                                                       \
+    "  <method name=\"RequestDisconnection\">"                          \
+    "   <arg name=\"device\" direction=\"in\" type=\"o\"/>"             \
+    "  </method>"                                                       \
+    "  <method name=\"NewConnection\">"                                 \
+    "   <arg name=\"device\" direction=\"in\" type=\"o\"/>"             \
+    "   <arg name=\"fd\" direction=\"in\" type=\"h\"/>"                 \
+    "   <arg name=\"opts\" direction=\"in\" type=\"a{sv}\"/>"           \
+    "  </method>"                                                       \
+    " </interface>"                                                     \
+    " <interface name=\"org.freedesktop.DBus.Introspectable\">"         \
+    "  <method name=\"Introspect\">"                                    \
+    "   <arg name=\"data\" type=\"s\" direction=\"out\"/>"              \
+    "  </method>"                                                       \
+    " </interface>"                                                     \
+    "</node>"
+
 struct pa_bluetooth_discovery {
     PA_REFCNT_DECLARE;
 
@@ -197,12 +233,90 @@ void pa_bluetooth_transport_free(pa_bluetooth_transport *t) {
     pa_assert(t);
 
     pa_hashmap_remove(t->device->discovery->transports, t->path);
+    if (t->rfcomm_io) {
+        t->device->discovery->core->mainloop->io_free(t->rfcomm_io);
+        close (t->rfcomm_fd);
+    }
     pa_xfree(t->owner);
     pa_xfree(t->path);
     pa_xfree(t->config);
     pa_xfree(t);
 }
 
+
+static int bluez5_sco_acquire_cb(pa_bluetooth_transport *t, bool optional, size_t *imtu, size_t *omtu) {
+    pa_bluetooth_device *d = t->device;
+    struct sockaddr_sco addr;
+    int err, i;
+    int sock;
+    bdaddr_t src;
+    bdaddr_t dst;
+    int voice = 0x60;
+    const char *src_addr, *dst_addr;
+
+    src_addr = d->adapter->address;
+    dst_addr = d->address;
+
+    for (i = 5; i >= 0; i--, src_addr += 3)
+        src.b[i] = strtol(src_addr, NULL, 16);
+    for (i = 5; i >= 0; i--, dst_addr += 3)
+        dst.b[i] = strtol(dst_addr, NULL, 16);
+
+    sock = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
+    if (sock < 0) {
+        pa_log_error("socket(SEQPACKET, SCO) %s", pa_cstrerror(errno));
+        return -1;
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.sco_family = AF_BLUETOOTH;
+    bacpy(&addr.sco_bdaddr, &src);
+
+    if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+        pa_log_error("bind(): %s", pa_cstrerror(errno));
+        goto fail_close;
+    }
+
+    if (voice) {
+        struct bt_voice opts;
+
+        /* SCO voice setting */
+        memset(&opts, 0, sizeof(opts));
+        opts.setting = voice;
+        if (setsockopt(sock, SOL_BLUETOOTH, BT_VOICE, &opts, sizeof(opts)) < 0) {
+          pa_log_error("setsockopt(): %s", pa_cstrerror(errno));
+          goto fail_close;
+        }
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.sco_family = AF_BLUETOOTH;
+    bacpy(&addr.sco_bdaddr, &dst);
+
+    pa_log_info ("doing connect\n");
+    err = connect(sock, (struct sockaddr *) &addr, sizeof(addr));
+    if (err < 0 && !(errno == EAGAIN || errno == EINPROGRESS)) {
+        pa_log_error("connect(): %s", pa_cstrerror(errno));
+        goto fail_close;
+    }
+
+    if (imtu)
+        *imtu = 48;
+
+    if (omtu)
+        *omtu = 48;
+
+    return sock;
+
+fail_close:
+    close(sock);
+    return -1;
+}
+
+static void bluez5_sco_release_cb(pa_bluetooth_transport *t) {
+    pa_log_info("Transport %s released", t->path);
+}
+
 static int bluez5_transport_acquire_cb(pa_bluetooth_transport *t, bool optional, size_t *imtu, size_t *omtu) {
     DBusMessage *m, *r;
     DBusError err;
@@ -767,6 +881,56 @@ static void register_endpoint(pa_bluetooth_discovery *y, const char *path, const
     send_and_add_to_pending(y, m, register_endpoint_reply, pa_xstrdup(endpoint));
 }
 
+static void register_profile_reply(DBusPendingCall *pending, void *userdata) {
+    DBusMessage *r;
+    pa_dbus_pending *p;
+    pa_bluetooth_discovery *y;
+    char *profile;
+
+    pa_assert(pending);
+    pa_assert_se(p = userdata);
+    pa_assert_se(y = p->context_data);
+    pa_assert_se(profile = p->call_data);
+    pa_assert_se(r = dbus_pending_call_steal_reply(pending));
+
+    if (dbus_message_is_error(r, BLUEZ_ERROR_NOT_SUPPORTED)) {
+        pa_log_info("Couldn't register profile %s because it is disabled in BlueZ", profile);
+        goto finish;
+    }
+
+    if (dbus_message_get_type(r) == DBUS_MESSAGE_TYPE_ERROR) {
+        pa_log_error(BLUEZ_PROFILE_MANAGER_INTERFACE ".RegisterProfile() failed: %s: %s", dbus_message_get_error_name(r),
+                     pa_dbus_get_error_message(r));
+        goto finish;
+    }
+
+finish:
+    dbus_message_unref(r);
+
+    PA_LLIST_REMOVE(pa_dbus_pending, y->pending, p);
+    pa_dbus_pending_free(p);
+
+    pa_xfree(profile);
+}
+
+static void register_profile(pa_bluetooth_discovery *y, const char *profile, const char *uuid) {
+    DBusMessage *m;
+    DBusMessageIter i, d;
+
+    pa_log_debug("Registering Profile %s", profile);
+
+    pa_assert_se(m = dbus_message_new_method_call(BLUEZ_SERVICE, "/org/bluez", BLUEZ_PROFILE_MANAGER_INTERFACE, "RegisterProfile"));
+
+    dbus_message_iter_init_append(m, &i);
+    dbus_message_iter_append_basic(&i, DBUS_TYPE_OBJECT_PATH, &profile);
+    dbus_message_iter_append_basic(&i, DBUS_TYPE_STRING, &uuid);
+    dbus_message_iter_open_container(&i, DBUS_TYPE_ARRAY, DBUS_DICT_ENTRY_BEGIN_CHAR_AS_STRING DBUS_TYPE_STRING_AS_STRING
+                                         DBUS_TYPE_VARIANT_AS_STRING DBUS_DICT_ENTRY_END_CHAR_AS_STRING, &d);
+    dbus_message_iter_close_container(&i, &d);
+
+    send_and_add_to_pending(y, m, register_profile_reply, pa_xstrdup(profile));
+}
+
 static void parse_interfaces_and_properties(pa_bluetooth_discovery *y, DBusMessageIter *dict_i) {
     DBusMessageIter element_i;
     const char *path;
@@ -1050,6 +1214,8 @@ static DBusHandlerResult filter_cb(DBusConnection *bus, DBusMessage *m, void *us
                 return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
 
             parse_transport_properties(t, &arg_i);
+        } else {
+            pa_log_debug("Properties changed in %s", iface);
         }
 
         return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
@@ -1109,6 +1275,8 @@ const char *pa_bluetooth_profile_to_string(pa_bluetooth_profile_t profile) {
             return "a2dp_sink";
         case PA_BLUETOOTH_PROFILE_A2DP_SOURCE:
             return "a2dp_source";
+        case PA_BLUETOOTH_PROFILE_HSP_AG:
+            return "hsp_ag";
         case PA_BLUETOOTH_PROFILE_OFF:
             return "off";
     }
@@ -1529,6 +1697,212 @@ static void endpoint_done(pa_bluetooth_discovery *y, pa_bluetooth_profile_t prof
     }
 }
 
+static void rfcomm_io_callback(pa_mainloop_api *io, pa_io_event *e, int fd, pa_io_event_flags_t events, void*userdata) {
+    pa_bluetooth_transport *t = userdata;
+
+    pa_assert(io);
+    pa_assert(t);
+
+    if (events & (PA_IO_EVENT_HANGUP|PA_IO_EVENT_ERROR)) {
+        pa_log("Lost RFCOMM connection.");
+        goto fail;
+    }
+
+    if (events & PA_IO_EVENT_INPUT) {
+        char buf[512];
+        ssize_t len;
+
+        len = read (fd, buf, 511);
+        buf[len] = 0;
+        pa_log("RFCOMM << %s", buf);
+
+        pa_log("RFCOMM >> OK");
+        len = write (fd, "\r\nOK\r\n", 5);
+        if (len < 0)
+            pa_log_error("RFCOMM write error: %s", pa_cstrerror(errno));
+    }
+    return;
+
+fail:
+    transport_state_changed(t, PA_BLUETOOTH_TRANSPORT_STATE_DISCONNECTED);
+    pa_bluetooth_transport_free(t);
+}
+
+
+static DBusMessage *profile_new_connection(DBusConnection *conn, DBusMessage *m, void *userdata) {
+    pa_bluetooth_discovery *y = userdata;
+    pa_bluetooth_device *d;
+    pa_bluetooth_transport *t;
+    pa_bluetooth_profile_t p;
+    DBusMessage *r;
+    int fd;
+    const char *sender, *path, *handler;
+    DBusMessageIter arg_i, element_i;
+    char *pathfd;
+
+    if (!dbus_message_iter_init(m, &arg_i) || !pa_streq(dbus_message_get_signature(m), "oha{sv}")) {
+        pa_log_error("Invalid signature found in NewConnection");
+        goto fail;
+    }
+
+    handler = dbus_message_get_path(m);
+
+    pa_assert(dbus_message_iter_get_arg_type(&arg_i) == DBUS_TYPE_OBJECT_PATH);
+    dbus_message_iter_get_basic(&arg_i, &path);
+
+    if ((d = pa_hashmap_get(y->devices, path))) {
+        if (!d->valid) {
+            pa_log_error("Information about device %s is invalid", path);
+            goto fail;
+        }
+    } else {
+        /* InterfacesAdded signal is probably on its way, device_info_valid is kept as 0. */
+        pa_log_warn("received for unknown device %s", path);
+        d = device_create(y, path);
+    }
+
+    if (pa_streq(handler, HSP_AG_PROFILE)) {
+        p = PA_BLUETOOTH_PROFILE_HSP_AG;
+    } else
+        goto refused;
+
+    pa_assert_se(dbus_message_iter_next(&arg_i));
+
+    pa_assert(dbus_message_iter_get_arg_type(&arg_i) == DBUS_TYPE_UNIX_FD);
+    dbus_message_iter_get_basic(&arg_i, &fd);
+
+    pa_log_debug("dbus: NewConnection path=%s, fd=%d", path, fd);
+
+    pa_assert_se(dbus_message_iter_next(&arg_i));
+    pa_assert(dbus_message_iter_get_arg_type(&arg_i) == DBUS_TYPE_ARRAY);
+
+    dbus_message_iter_recurse(&arg_i, &element_i);
+
+    while (dbus_message_iter_get_arg_type(&element_i) == DBUS_TYPE_DICT_ENTRY) {
+        DBusMessageIter dict_i;
+        const char *key;
+
+        dbus_message_iter_recurse(&element_i, &dict_i);
+
+        key = check_variant_property(&dict_i);
+        if (key == NULL)
+            break;
+
+        pa_log_debug("key=%s", key);
+
+        dbus_message_iter_next(&element_i);
+    }
+
+    sender = dbus_message_get_sender(m);
+
+    pathfd = pa_sprintf_malloc ("%s/fd%d", path, fd);
+    d->transports[p] = t = pa_bluetooth_transport_new(d, sender, pathfd, p, NULL, 0);
+    pa_xfree(pathfd);
+    t->rfcomm_fd = fd;
+    t->rfcomm_io = y->core->mainloop->io_new(y->core->mainloop, fd, PA_IO_EVENT_INPUT|PA_IO_EVENT_HANGUP,
+        rfcomm_io_callback, t);
+    t->acquire = bluez5_sco_acquire_cb;
+    t->release = bluez5_sco_release_cb;
+    pa_bluetooth_transport_put(t);
+
+    pa_log_debug("Transport %s available for profile %s", t->path, pa_bluetooth_profile_to_string(t->profile));
+
+    pa_assert_se(r = dbus_message_new_method_return(m));
+
+    return r;
+
+fail:
+    pa_assert_se(r = dbus_message_new_error(m, "org.bluez.Error.InvalidArguments", "Unable to handle new connection"));
+    return r;
+refused:
+    pa_log_debug("dbus: NewConnection path=%s rejected", path);
+    pa_assert_se(r = dbus_message_new_error(m, "org.bluez.Error.Rejected", "New connection rejected"));
+    return r;
+}
+
+static DBusMessage *profile_request_disconnection(DBusConnection *conn, DBusMessage *m, void *userdata) {
+    DBusMessage *r;
+
+    pa_assert_se(r = dbus_message_new_method_return(m));
+
+    return r;
+}
+
+static DBusHandlerResult profile_handler(DBusConnection *c, DBusMessage *m, void *userdata) {
+    struct pa_bluetooth_discovery *y = userdata;
+    DBusMessage *r = NULL;
+    const char *path, *interface, *member;
+
+    pa_assert(y);
+
+    path = dbus_message_get_path(m);
+    interface = dbus_message_get_interface(m);
+    member = dbus_message_get_member(m);
+
+    pa_log_debug("dbus: path=%s, interface=%s, member=%s", path, interface, member);
+
+    if (!pa_streq(path, HSP_AG_PROFILE))
+        return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+
+    if (dbus_message_is_method_call(m, "org.freedesktop.DBus.Introspectable", "Introspect")) {
+        const char *xml = PROFILE_INTROSPECT_XML;
+
+        pa_assert_se(r = dbus_message_new_method_return(m));
+        pa_assert_se(dbus_message_append_args(r, DBUS_TYPE_STRING, &xml, DBUS_TYPE_INVALID));
+
+    } else if (dbus_message_is_method_call(m, BLUEZ_PROFILE_INTERFACE, "Release")) {
+    } else if (dbus_message_is_method_call(m, BLUEZ_PROFILE_INTERFACE, "Cancel")) {
+    } else if (dbus_message_is_method_call(m, BLUEZ_PROFILE_INTERFACE, "RequestDisconnection")) {
+        r = profile_request_disconnection(c, m, userdata);
+    } else if (dbus_message_is_method_call(m, BLUEZ_PROFILE_INTERFACE, "NewConnection"))
+        r = profile_new_connection(c, m, userdata);
+    else
+        return DBUS_HANDLER_RESULT_NOT_YET_HANDLED;
+
+    if (r) {
+        pa_assert_se(dbus_connection_send(pa_dbus_connection_get(y->connection), r, NULL));
+        dbus_message_unref(r);
+    }
+
+    return DBUS_HANDLER_RESULT_HANDLED;
+}
+
+static void profile_init(pa_bluetooth_discovery *y, pa_bluetooth_profile_t profile) {
+    static const DBusObjectPathVTable vtable_profile = {
+        .message_function = profile_handler,
+    };
+    const char *object_name;
+    const char *uuid;
+
+    pa_assert(y);
+
+    switch(profile) {
+        case PA_BLUETOOTH_PROFILE_HSP_AG:
+            object_name = HSP_AG_PROFILE;
+            uuid = PA_BLUETOOTH_UUID_HSP_AG;
+            break;
+        default:
+            pa_assert_not_reached();
+            break;
+    }
+    pa_assert_se(dbus_connection_register_object_path(pa_dbus_connection_get(y->connection),
+                  object_name, &vtable_profile, y));
+    register_profile (y, object_name, uuid);
+}
+
+static void profile_done(pa_bluetooth_discovery *y, pa_bluetooth_profile_t profile) {
+    pa_assert(y);
+
+    switch(profile) {
+        case PA_BLUETOOTH_PROFILE_HSP_AG:
+            dbus_connection_unregister_object_path(pa_dbus_connection_get(y->connection), HSP_AG_PROFILE);
+            break;
+        default:
+            pa_assert_not_reached();
+            break;
+    }
+}
+
 pa_bluetooth_discovery* pa_bluetooth_discovery_get(pa_core *c) {
     pa_bluetooth_discovery *y;
     DBusError err;
@@ -1589,6 +1963,8 @@ pa_bluetooth_discovery* pa_bluetooth_discovery_get(pa_core *c) {
 
     get_managed_objects(y);
 
+    profile_init(y, PA_BLUETOOTH_PROFILE_HSP_AG);
+
     return y;
 
 fail:
@@ -1650,6 +2026,7 @@ void pa_bluetooth_discovery_unref(pa_bluetooth_discovery *y) {
 
         endpoint_done(y, PA_BLUETOOTH_PROFILE_A2DP_SINK);
         endpoint_done(y, PA_BLUETOOTH_PROFILE_A2DP_SOURCE);
+        profile_done(y, PA_BLUETOOTH_PROFILE_HSP_AG);
 
         pa_dbus_connection_unref(y->connection);
     }
diff --git a/src/modules/bluetooth/bluez5-util.h b/src/modules/bluetooth/bluez5-util.h
index 63bae35ee..9fc7477e9 100644
--- a/src/modules/bluetooth/bluez5-util.h
+++ b/src/modules/bluetooth/bluez5-util.h
@@ -26,6 +26,8 @@
 
 #define PA_BLUETOOTH_UUID_A2DP_SOURCE "0000110a-0000-1000-8000-00805f9b34fb"
 #define PA_BLUETOOTH_UUID_A2DP_SINK   "0000110b-0000-1000-8000-00805f9b34fb"
+#define PA_BLUETOOTH_UUID_HSP_HS      "00001108-0000-1000-8000-00805f9b34fb"
+#define PA_BLUETOOTH_UUID_HSP_AG      "00001112-0000-1000-8000-00805f9b34fb"
 
 typedef struct pa_bluetooth_transport pa_bluetooth_transport;
 typedef struct pa_bluetooth_device pa_bluetooth_device;
@@ -41,6 +43,7 @@ typedef enum pa_bluetooth_hook {
 typedef enum profile {
     PA_BLUETOOTH_PROFILE_A2DP_SINK,
     PA_BLUETOOTH_PROFILE_A2DP_SOURCE,
+    PA_BLUETOOTH_PROFILE_HSP_AG,
     PA_BLUETOOTH_PROFILE_OFF
 } pa_bluetooth_profile_t;
 #define PA_BLUETOOTH_PROFILE_COUNT PA_BLUETOOTH_PROFILE_OFF
@@ -70,6 +73,9 @@ struct pa_bluetooth_transport {
     pa_bluetooth_transport_acquire_cb acquire;
     pa_bluetooth_transport_release_cb release;
     void *userdata;
+
+    int rfcomm_fd;
+    pa_io_event *rfcomm_io;
 };
 
 struct pa_bluetooth_device {
diff --git a/src/modules/bluetooth/module-bluez5-device.c b/src/modules/bluetooth/module-bluez5-device.c
index 57b279181..8752694f3 100644
--- a/src/modules/bluetooth/module-bluez5-device.c
+++ b/src/modules/bluetooth/module-bluez5-device.c
@@ -34,6 +34,7 @@
 
 #include <pulsecore/core-error.h>
 #include <pulsecore/core-util.h>
+#include <pulsecore/core-rtclock.h>
 #include <pulsecore/i18n.h>
 #include <pulsecore/module.h>
 #include <pulsecore/modargs.h>
@@ -551,6 +552,163 @@ static void a2dp_reduce_bitpool(struct userdata *u) {
     a2dp_set_bitpool(u, bitpool);
 }
 
+static int sco_process_render(struct userdata *u) {
+    int ret = 0;
+
+    pa_assert(u);
+    pa_assert(u->profile == PA_BLUETOOTH_PROFILE_HSP_AG);
+    pa_assert(u->sink);
+
+    /* First, render some data */
+    if (!u->write_memchunk.memblock)
+        pa_sink_render_full(u->sink, u->write_block_size, &u->write_memchunk);
+
+    pa_assert(u->write_memchunk.length == u->write_block_size);
+
+    for (;;) {
+        ssize_t l;
+        const void *p;
+
+        /* Now write that data to the socket. The socket is of type
+         * SEQPACKET, and we generated the data of the MTU size, so this
+         * should just work. */
+
+        p = (const uint8_t *) pa_memblock_acquire_chunk(&u->write_memchunk);
+        l = pa_write(u->stream_fd, p, u->write_memchunk.length, &u->stream_write_type);
+        pa_memblock_release(u->write_memchunk.memblock);
+
+        pa_assert(l != 0);
+
+        if (l < 0) {
+
+            if (errno == EINTR)
+                /* Retry right away if we got interrupted */
+                continue;
+            else if (errno == EAGAIN)
+                /* Hmm, apparently the socket was not writable, give up for now */
+                break;
+
+            pa_log_error("Failed to write data to SCO socket: %s", pa_cstrerror(errno));
+            ret = -1;
+            break;
+        }
+
+        pa_assert((size_t) l <= u->write_memchunk.length);
+
+        if ((size_t) l != u->write_memchunk.length) {
+            pa_log_error("Wrote memory block to socket only partially! %llu written, wanted to write %llu.",
+                        (unsigned long long) l,
+                        (unsigned long long) u->write_memchunk.length);
+            ret = -1;
+            break;
+        }
+
+        u->write_index += (uint64_t) u->write_memchunk.length;
+        pa_memblock_unref(u->write_memchunk.memblock);
+        pa_memchunk_reset(&u->write_memchunk);
+
+        ret = 1;
+        break;
+    }
+
+    return ret;
+}
+/* Run from IO thread */
+static int sco_process_push(struct userdata *u) {
+    int ret = 0;
+    pa_memchunk memchunk;
+
+    pa_assert(u);
+    pa_assert(u->profile == PA_BLUETOOTH_PROFILE_HSP_AG);
+    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 (;;) {
+        ssize_t l;
+        void *p;
+        struct msghdr m;
+        struct cmsghdr *cm;
+        uint8_t aux[1024];
+        struct iovec iov;
+        bool found_tstamp = false;
+        pa_usec_t tstamp;
+
+        memset(&m, 0, sizeof(m));
+        memset(&aux, 0, sizeof(aux));
+        memset(&iov, 0, sizeof(iov));
+
+        m.msg_iov = &iov;
+        m.msg_iovlen = 1;
+        m.msg_control = aux;
+        m.msg_controllen = sizeof(aux);
+
+        p = pa_memblock_acquire(memchunk.memblock);
+        iov.iov_base = p;
+        iov.iov_len = pa_memblock_get_length(memchunk.memblock);
+        l = recvmsg(u->stream_fd, &m, 0);
+        pa_memblock_release(memchunk.memblock);
+
+        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 SCO socket: %s", l < 0 ? pa_cstrerror(errno) : "EOF");
+            ret = -1;
+            break;
+        }
+        pa_assert((size_t) l <= pa_memblock_get_length(memchunk.memblock));
+
+        /* In some rare occasions, we might receive packets of a very strange
+         * size. This could potentially be possible if the SCO packet was
+         * received partially over-the-air, or more probably due to hardware
+         * issues in our Bluetooth adapter. In these cases, in order to avoid
+         * an assertion failure due to unaligned data, just discard the whole
+         * packet */
+        if (!pa_frame_aligned(l, &u->sample_spec)) {
+            pa_log_warn("SCO packet received of unaligned size: %zu", l);
+            break;
+        }
+
+        memchunk.length = (size_t) l;
+        u->read_index += (uint64_t) l;
+
+        for (cm = CMSG_FIRSTHDR(&m); cm; cm = CMSG_NXTHDR(&m, cm))
+            if (cm->cmsg_level == SOL_SOCKET && cm->cmsg_type == SO_TIMESTAMP) {
+                struct timeval *tv = (struct timeval*) CMSG_DATA(cm);
+                pa_rtclock_from_wallclock(tv);
+                tstamp = pa_timeval_load(tv);
+                found_tstamp = true;
+                break;
+            }
+
+        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);
+
+        pa_source_post(u->source, &memchunk);
+
+        ret = l;
+        break;
+    }
+
+    pa_memblock_unref(memchunk.memblock);
+
+    return ret;
+}
+
 static void teardown_stream(struct userdata *u) {
     if (u->rtpoll_item) {
         pa_rtpoll_item_free(u->rtpoll_item);
@@ -611,13 +769,18 @@ static void transport_release(struct userdata *u) {
 
 /* Run from I/O thread */
 static void transport_config_mtu(struct userdata *u) {
-    u->read_block_size =
-        (u->read_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
-        / u->sbc_info.frame_length * u->sbc_info.codesize;
+    if (u->profile == PA_BLUETOOTH_PROFILE_HSP_AG) {
+        u->read_block_size = u->read_link_mtu;
+        u->write_block_size = u->write_link_mtu;
+    } else {
+        u->read_block_size =
+            (u->read_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+            / u->sbc_info.frame_length * u->sbc_info.codesize;
 
-    u->write_block_size =
-        (u->write_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
-        / u->sbc_info.frame_length * u->sbc_info.codesize;
+        u->write_block_size =
+            (u->write_link_mtu - sizeof(struct rtp_header) - sizeof(struct rtp_payload))
+            / u->sbc_info.frame_length * u->sbc_info.codesize;
+    }
 
     if (u->sink) {
         pa_sink_set_max_request_within_thread(u->sink, u->write_block_size);
@@ -761,6 +924,7 @@ static int add_source(struct userdata *u) {
     if (!u->transport_acquired)
         switch (u->profile) {
             case PA_BLUETOOTH_PROFILE_A2DP_SOURCE:
+            case PA_BLUETOOTH_PROFILE_HSP_AG:
                 data.suspend_cause = PA_SUSPEND_USER;
                 break;
             case PA_BLUETOOTH_PROFILE_A2DP_SINK:
@@ -878,6 +1042,7 @@ static int add_sink(struct userdata *u) {
             case PA_BLUETOOTH_PROFILE_A2DP_SINK:
                 /* Profile switch should have failed */
             case PA_BLUETOOTH_PROFILE_A2DP_SOURCE:
+            case PA_BLUETOOTH_PROFILE_HSP_AG:
             case PA_BLUETOOTH_PROFILE_OFF:
                 pa_assert_not_reached();
                 break;
@@ -897,7 +1062,7 @@ static int add_sink(struct userdata *u) {
 }
 
 /* Run from main thread */
-static void transport_config(struct userdata *u) {
+static void transport_config_a2dp(struct userdata *u) {
     sbc_info_t *sbc_info = &u->sbc_info;
     a2dp_sbc_t *config;
 
@@ -1005,6 +1170,15 @@ static void transport_config(struct userdata *u) {
                 sbc_info->sbc.allocation, sbc_info->sbc.subbands, sbc_info->sbc.blocks, sbc_info->sbc.bitpool);
 }
 
+static void transport_config(struct userdata *u) {
+    if (u->profile == PA_BLUETOOTH_PROFILE_HSP_AG) {
+        u->sample_spec.format = PA_SAMPLE_S16LE;
+        u->sample_spec.channels = 1;
+        u->sample_spec.rate = 8000;
+    } else
+        transport_config_a2dp(u);
+}
+
 /* Run from main thread */
 static int setup_transport(struct userdata *u) {
     pa_bluetooth_transport *t;
@@ -1032,6 +1206,8 @@ static int setup_transport(struct userdata *u) {
     return 0;
 }
 
+static pa_direction_t get_profile_direction(pa_bluetooth_profile_t p);
+
 /* Run from main thread */
 static int init_profile(struct userdata *u) {
     int r = 0;
@@ -1043,11 +1219,11 @@ static int init_profile(struct userdata *u) {
 
     pa_assert(u->transport);
 
-    if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK)
+    if (get_profile_direction (u->profile) & PA_DIRECTION_OUTPUT)
         if (add_sink(u) < 0)
             r = -1;
 
-    if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SOURCE)
+    if (get_profile_direction (u->profile) & PA_DIRECTION_INPUT)
         if (add_source(u) < 0)
             r = -1;
 
@@ -1111,7 +1287,10 @@ static void thread_func(void *userdata) {
             if (pollfd && (pollfd->revents & POLLIN)) {
                 int n_read;
 
-                n_read = a2dp_process_push(u);
+                if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SOURCE)
+                    n_read = a2dp_process_push(u);
+                else
+                    n_read = sco_process_push(u);
 
                 if (n_read < 0)
                     goto fail;
@@ -1182,8 +1361,13 @@ static void thread_func(void *userdata) {
                     if (u->write_index <= 0)
                         u->started_at = pa_rtclock_now();
 
-                    if ((n_written = a2dp_process_render(u)) < 0)
-                        goto fail;
+                    if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK) {
+                        if ((n_written = a2dp_process_render(u)) < 0)
+                            goto fail;
+                    } else {
+                        if ((n_written = sco_process_render(u)) < 0)
+                            goto fail;
+                    }
 
                     if (n_written == 0)
                         pa_log("Broken kernel: we got EAGAIN on write() after POLLOUT!");
@@ -1363,6 +1547,7 @@ static pa_direction_t get_profile_direction(pa_bluetooth_profile_t p) {
     static const pa_direction_t profile_direction[] = {
         [PA_BLUETOOTH_PROFILE_A2DP_SINK] = PA_DIRECTION_OUTPUT,
         [PA_BLUETOOTH_PROFILE_A2DP_SOURCE] = PA_DIRECTION_INPUT,
+        [PA_BLUETOOTH_PROFILE_HSP_AG] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
         [PA_BLUETOOTH_PROFILE_OFF] = 0
     };
 
@@ -1540,8 +1725,19 @@ static pa_card_profile *create_card_profile(struct userdata *u, const char *uuid
 
         p = PA_CARD_PROFILE_DATA(cp);
         *p = PA_BLUETOOTH_PROFILE_A2DP_SOURCE;
-    }
+    } else if (pa_streq(uuid, PA_BLUETOOTH_UUID_HSP_HS)) {
+        cp = pa_card_profile_new("hsp_ag", _("Headset Audio Gateway"), sizeof(pa_bluetooth_profile_t));
+        cp->priority = 20;
+        cp->n_sinks = 1;
+        cp->n_sources = 1;
+        cp->max_sink_channels = 1;
+        cp->max_source_channels = 1;
+        pa_hashmap_put(input_port->profiles, cp->name, cp);
+        pa_hashmap_put(output_port->profiles, cp->name, cp);
 
+        p = PA_CARD_PROFILE_DATA(cp);
+        *p = PA_BLUETOOTH_PROFILE_HSP_AG;
+    }
     if (cp && u->device->transports[*p])
         cp->available = transport_state_to_availability(u->device->transports[*p]->state);