backend-native: implement volume controlheadset2

Parse the gain changed AT commands from the headset and fire 2 new
hooks as a result. The device will connect to those hooks and change the
source/sink volumes.

When the source/sink volume changes, set the gain on the microphone or
speaker respectively. Make sure we do nothing if the transport can not
handle the gain changes.

3 files changed, 196 insertions, 0 deletions

diff --git a/src/modules/bluetooth/backend-native.c b/src/modules/bluetooth/backend-native.c
index b3c4220ef..2302754a6 100644
--- a/src/modules/bluetooth/backend-native.c
+++ b/src/modules/bluetooth/backend-native.c
@@ -113,6 +113,7 @@ static int bluez5_sco_acquire_cb(pa_bluetooth_transport *t, bool optional, size_
     src_addr = d->adapter->address;
     dst_addr = d->address;
 
+    /* don't use ba2str to avoid -lbluetooth */
     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)
@@ -226,11 +227,21 @@ static void rfcomm_io_callback(pa_mainloop_api *io, pa_io_event *e, int fd, pa_i
     if (events & PA_IO_EVENT_INPUT) {
         char buf[512];
         ssize_t len;
+        int gain;
 
         len = read (fd, buf, 511);
         buf[len] = 0;
         pa_log("RFCOMM << %s", buf);
 
+        if (sscanf (buf, "AT+VGS=%d", &gain) == 1) {
+          t->speaker_gain = gain;
+          pa_hook_fire(pa_bluetooth_discovery_hook (t->device->discovery, PA_BLUETOOTH_HOOK_TRANSPORT_SPEAKER_GAIN_CHANGED), t);
+
+        } else if (sscanf (buf, "AT+VGM=%d", &gain) == 1) {
+          t->microphone_gain = gain;
+          pa_hook_fire(pa_bluetooth_discovery_hook (t->device->discovery, PA_BLUETOOTH_HOOK_TRANSPORT_MICROPHONE_GAIN_CHANGED), t);
+        }
+
         pa_log("RFCOMM >> OK");
         len = write (fd, "\r\nOK\r\n", 5);
         /* we ignore any errors, it's not critical and real errors should
@@ -256,6 +267,40 @@ static void transport_dispose(pa_bluetooth_transport *t) {
     pa_xfree(trfc);
 }
 
+static void set_speaker_gain(pa_bluetooth_transport *t, uint16_t gain) {
+    struct transport_rfcomm *trfc = t->userdata;
+    char buf[512];
+    ssize_t len, written;
+
+    if (t->speaker_gain == gain)
+      return;
+
+    t->speaker_gain = gain;
+
+    len = sprintf (buf, "+VGS=%d\r\n", gain);
+    pa_log("RFCOMM >> +VGS=%d", gain);
+    written = write (trfc->rfcomm_fd, buf, len);
+    if (written != len)
+        pa_log_error("RFCOMM write error: %s", pa_cstrerror(errno));
+}
+
+static void set_microphone_gain(pa_bluetooth_transport *t, uint16_t gain) {
+    struct transport_rfcomm *trfc = t->userdata;
+    char buf[512];
+    ssize_t len, written;
+
+    if (t->microphone_gain == gain)
+      return;
+
+    t->microphone_gain = gain;
+
+    len = sprintf (buf, "+VGM=%d\r\n", gain);
+    pa_log("RFCOMM >> +VGM=%d", gain);
+    written = write (trfc->rfcomm_fd, buf, len);
+    if (written != len)
+        pa_log_error("RFCOMM write error: %s", pa_cstrerror(errno));
+
+}
 
 static DBusMessage *profile_new_connection(DBusConnection *conn, DBusMessage *m, void *userdata) {
     pa_bluetooth_backend *b = userdata;
@@ -303,6 +348,8 @@ static DBusMessage *profile_new_connection(DBusConnection *conn, DBusMessage *m,
     t->acquire = bluez5_sco_acquire_cb;
     t->release = bluez5_sco_release_cb;
     t->dispose = transport_dispose;
+    t->set_speaker_gain = set_speaker_gain;
+    t->set_microphone_gain = set_microphone_gain;
 
     trfc =  pa_xnew0(struct transport_rfcomm, 1);
     trfc->rfcomm_fd = fd;
diff --git a/src/modules/bluetooth/bluez5-util.h b/src/modules/bluetooth/bluez5-util.h
index fde2e7834..1258f07eb 100644
--- a/src/modules/bluetooth/bluez5-util.h
+++ b/src/modules/bluetooth/bluez5-util.h
@@ -40,6 +40,8 @@ typedef struct pa_bluetooth_backend pa_bluetooth_backend;
 typedef enum pa_bluetooth_hook {
     PA_BLUETOOTH_HOOK_DEVICE_CONNECTION_CHANGED,          /* Call data: pa_bluetooth_device */
     PA_BLUETOOTH_HOOK_TRANSPORT_STATE_CHANGED,            /* Call data: pa_bluetooth_transport */
+    PA_BLUETOOTH_HOOK_TRANSPORT_MICROPHONE_GAIN_CHANGED,  /* Call data: pa_bluetooth_transport */
+    PA_BLUETOOTH_HOOK_TRANSPORT_SPEAKER_GAIN_CHANGED,     /* Call data: pa_bluetooth_transport */
     PA_BLUETOOTH_HOOK_MAX
 } pa_bluetooth_hook_t;
 
@@ -61,6 +63,8 @@ typedef enum pa_bluetooth_transport_state {
 typedef int (*pa_bluetooth_transport_acquire_cb)(pa_bluetooth_transport *t, bool optional, size_t *imtu, size_t *omtu);
 typedef void (*pa_bluetooth_transport_release_cb)(pa_bluetooth_transport *t);
 typedef void (*pa_bluetooth_transport_dispose_cb)(pa_bluetooth_transport *t);
+typedef void (*pa_bluetooth_transport_set_speaker_gain_cb)(pa_bluetooth_transport *t, uint16_t gain);
+typedef void (*pa_bluetooth_transport_set_microphone_gain_cb)(pa_bluetooth_transport *t, uint16_t gain);
 
 struct pa_bluetooth_transport {
     pa_bluetooth_device *device;
@@ -73,11 +77,16 @@ struct pa_bluetooth_transport {
     uint8_t *config;
     size_t config_size;
 
+    uint16_t microphone_gain;
+    uint16_t speaker_gain;
+
     pa_bluetooth_transport_state_t state;
 
     pa_bluetooth_transport_acquire_cb acquire;
     pa_bluetooth_transport_release_cb release;
     pa_bluetooth_transport_dispose_cb dispose;
+    pa_bluetooth_transport_set_speaker_gain_cb set_speaker_gain;
+    pa_bluetooth_transport_set_microphone_gain_cb set_microphone_gain;
     void *userdata;
 };
 
diff --git a/src/modules/bluetooth/module-bluez5-device.c b/src/modules/bluetooth/module-bluez5-device.c
index 15731d147..8406e955d 100644
--- a/src/modules/bluetooth/module-bluez5-device.c
+++ b/src/modules/bluetooth/module-bluez5-device.c
@@ -66,6 +66,7 @@ PA_MODULE_USAGE("path=<device object path>");
 
 #define BITPOOL_DEC_LIMIT 32
 #define BITPOOL_DEC_STEP 5
+#define HSP_MAX_GAIN 15
 
 static const char* const valid_modargs[] = {
     "path",
@@ -103,6 +104,8 @@ struct userdata {
 
     pa_hook_slot *device_connection_changed_slot;
     pa_hook_slot *transport_state_changed_slot;
+    pa_hook_slot *transport_speaker_gain_changed_slot;
+    pa_hook_slot *transport_microphone_gain_changed_slot;
 
     pa_bluetooth_discovery *discovery;
     pa_bluetooth_device *device;
@@ -903,6 +906,40 @@ static int source_process_msg(pa_msgobject *o, int code, void *data, int64_t off
 }
 
 /* Run from main thread */
+static void source_set_volume_cb(pa_source *s) {
+    uint16_t gain;
+    pa_volume_t volume;
+    struct userdata *u;
+
+    pa_assert(s);
+    pa_assert(s->core);
+
+    u = s->userdata;
+
+    pa_assert(u);
+    pa_assert(u->source == s);
+    pa_assert(u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT);
+
+    if (u->transport->set_microphone_gain == NULL)
+      return;
+
+    gain = (pa_cvolume_max(&s->real_volume) * HSP_MAX_GAIN) / PA_VOLUME_NORM;
+
+    if (gain > HSP_MAX_GAIN)
+        gain = HSP_MAX_GAIN;
+
+    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(&s->real_volume, u->sample_spec.channels, volume);
+
+    u->transport->set_microphone_gain (u->transport, gain);
+}
+
+/* Run from main thread */
 static int add_source(struct userdata *u) {
     pa_source_new_data data;
 
@@ -944,6 +981,10 @@ static int add_source(struct userdata *u) {
     u->source->userdata = u;
     u->source->parent.process_msg = source_process_msg;
 
+    if (u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT) {
+        pa_source_set_set_volume_callback(u->source, source_set_volume_cb);
+        u->source->n_volume_steps = 16;
+    }
     return 0;
 }
 
@@ -1022,6 +1063,40 @@ static int sink_process_msg(pa_msgobject *o, int code, void *data, int64_t offse
 }
 
 /* Run from main thread */
+static void sink_set_volume_cb(pa_sink *s) {
+    uint16_t gain;
+    pa_volume_t volume;
+    struct userdata *u;
+
+    pa_assert(s);
+    pa_assert(s->core);
+
+    u = s->userdata;
+
+    pa_assert(u);
+    pa_assert(u->sink == s);
+    pa_assert(u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT);
+
+    if (u->transport->set_speaker_gain == NULL)
+      return;
+
+    gain = (pa_cvolume_max(&s->real_volume) * HSP_MAX_GAIN) / PA_VOLUME_NORM;
+
+    if (gain > HSP_MAX_GAIN)
+        gain = HSP_MAX_GAIN;
+
+    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(&s->real_volume, u->sample_spec.channels, volume);
+
+    u->transport->set_speaker_gain (u->transport, gain);
+}
+
+/* Run from main thread */
 static int add_sink(struct userdata *u) {
     pa_sink_new_data data;
 
@@ -1064,6 +1139,10 @@ static int add_sink(struct userdata *u) {
     u->sink->userdata = u;
     u->sink->parent.process_msg = sink_process_msg;
 
+    if (u->profile == PA_BLUETOOTH_PROFILE_HEADSET_HEAD_UNIT) {
+        pa_sink_set_set_volume_callback(u->sink, sink_set_volume_cb);
+        u->sink->n_volume_steps = 16;
+    }
     return 0;
 }
 
@@ -1975,6 +2054,54 @@ static pa_hook_result_t transport_state_changed_cb(pa_bluetooth_discovery *y, pa
     return PA_HOOK_OK;
 }
 
+static pa_hook_result_t transport_speaker_gain_changed_cb(pa_bluetooth_discovery *y, pa_bluetooth_transport *t, struct userdata *u) {
+    pa_volume_t volume;
+    pa_cvolume v;
+    uint16_t gain;
+
+    pa_assert(t);
+    pa_assert(u);
+
+    if (t != u->transport)
+      return PA_HOOK_OK;
+
+    gain = t->speaker_gain;
+    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, volume);
+    pa_sink_volume_changed(u->sink, &v);
+
+    return PA_HOOK_OK;
+}
+
+static pa_hook_result_t transport_microphone_gain_changed_cb(pa_bluetooth_discovery *y, pa_bluetooth_transport *t, struct userdata *u) {
+    pa_volume_t volume;
+    pa_cvolume v;
+    uint16_t gain;
+
+    pa_assert(t);
+    pa_assert(u);
+
+    if (t != u->transport)
+      return PA_HOOK_OK;
+
+    gain = t->microphone_gain;
+    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, volume);
+    pa_source_volume_changed(u->source, &v);
+
+    return PA_HOOK_OK;
+}
+
 /* Run 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 *m = BLUETOOTH_MSG(obj);
@@ -2039,6 +2166,13 @@ int pa__init(pa_module* m) {
         pa_hook_connect(pa_bluetooth_discovery_hook(u->discovery, PA_BLUETOOTH_HOOK_TRANSPORT_STATE_CHANGED),
                         PA_HOOK_NORMAL, (pa_hook_cb_t) transport_state_changed_cb, u);
 
+    u->transport_speaker_gain_changed_slot =
+        pa_hook_connect(pa_bluetooth_discovery_hook(u->discovery, PA_BLUETOOTH_HOOK_TRANSPORT_SPEAKER_GAIN_CHANGED), PA_HOOK_NORMAL, (pa_hook_cb_t) transport_speaker_gain_changed_cb, u);
+
+    u->transport_microphone_gain_changed_slot =
+        pa_hook_connect(pa_bluetooth_discovery_hook(u->discovery, PA_BLUETOOTH_HOOK_TRANSPORT_MICROPHONE_GAIN_CHANGED), PA_HOOK_NORMAL, (pa_hook_cb_t) transport_microphone_gain_changed_cb, u);
+
+
     if (add_card(u) < 0)
         goto fail;
 
@@ -2091,6 +2225,12 @@ void pa__done(pa_module *m) {
     if (u->transport_state_changed_slot)
         pa_hook_slot_free(u->transport_state_changed_slot);
 
+    if (u->transport_speaker_gain_changed_slot)
+        pa_hook_slot_free(u->transport_speaker_gain_changed_slot);
+
+    if (u->transport_microphone_gain_changed_slot)
+        pa_hook_slot_free(u->transport_microphone_gain_changed_slot);
+
     if (u->sbc_info.buffer)
         pa_xfree(u->sbc_info.buffer);