bluetooth: add CVSD codec implementation

Part-of: <https://gitlab.freedesktop.org/pulseaudio/pulseaudio/-/merge_requests/507>

9 files changed, 209 insertions, 72 deletions

diff --git a/src/modules/bluetooth/a2dp-codec-util.c b/src/modules/bluetooth/a2dp-codec-util.c
index 873c270cb..23860eafb 100644
--- a/src/modules/bluetooth/a2dp-codec-util.c
+++ b/src/modules/bluetooth/a2dp-codec-util.c
@@ -29,6 +29,14 @@
 
 #include "a2dp-codec-util.h"
 
+extern const pa_a2dp_codec pa_bt_codec_cvsd;
+
+/* List of HSP/HFP codecs.
+ */
+static const pa_a2dp_codec *pa_hf_codecs[] = {
+    &pa_bt_codec_cvsd,
+};
+
 extern const pa_a2dp_codec pa_a2dp_codec_sbc;
 extern const pa_a2dp_codec pa_a2dp_codec_sbc_xq_453;
 extern const pa_a2dp_codec pa_a2dp_codec_sbc_xq_512;
@@ -70,6 +78,17 @@ const pa_a2dp_codec *pa_bluetooth_a2dp_codec_iter(unsigned int i) {
     return pa_a2dp_codecs[i];
 }
 
+const pa_a2dp_codec *pa_bluetooth_get_hf_codec(const char *name) {
+    unsigned int i;
+
+    for (i = 0; i < PA_ELEMENTSOF(pa_hf_codecs); ++i) {
+        if (pa_streq(pa_hf_codecs[i]->name, name))
+            return pa_hf_codecs[i];
+    }
+
+    return NULL;
+}
+
 const pa_a2dp_codec *pa_bluetooth_get_a2dp_codec(const char *name) {
     unsigned int i;
     unsigned int count = pa_bluetooth_a2dp_codec_count();
diff --git a/src/modules/bluetooth/a2dp-codec-util.h b/src/modules/bluetooth/a2dp-codec-util.h
index 7b8d0cc87..6330fd8e8 100644
--- a/src/modules/bluetooth/a2dp-codec-util.h
+++ b/src/modules/bluetooth/a2dp-codec-util.h
@@ -37,4 +37,7 @@ bool pa_bluetooth_a2dp_codec_is_available(const pa_a2dp_codec_id *id, bool is_a2
 /* Initialise GStreamer */
 void pa_bluetooth_a2dp_codec_gst_init(void);
 
+/* Get HSP/HFP codec by name */
+const pa_a2dp_codec *pa_bluetooth_get_hf_codec(const char *name);
+
 #endif
diff --git a/src/modules/bluetooth/backend-native.c b/src/modules/bluetooth/backend-native.c
index fbed290e9..cbf6b6eee 100644
--- a/src/modules/bluetooth/backend-native.c
+++ b/src/modules/bluetooth/backend-native.c
@@ -551,6 +551,7 @@ static bool hfp_rfcomm_handle(int fd, pa_bluetooth_transport *t, const char *buf
     } else if ((c->state == 2 || c->state == 3) && pa_startswith(buf, "AT+CMER=")) {
         rfcomm_write_response(fd, "OK");
         c->state = 4;
+        t->bt_codec = pa_bluetooth_get_hf_codec("CVSD");
         transport_put(t);
         return false;
     }
@@ -777,6 +778,7 @@ static DBusMessage *profile_new_connection(DBusConnection *conn, DBusMessage *m,
     t->acquire = sco_acquire_cb;
     t->release = sco_release_cb;
     t->write = sco_transport_write;
+    t->bt_codec = pa_bluetooth_get_hf_codec("CVSD");
     t->destroy = transport_destroy;
 
     /* If PA is the HF/HS we are in control of volume attenuation and
diff --git a/src/modules/bluetooth/backend-ofono.c b/src/modules/bluetooth/backend-ofono.c
index 7c8cbbf62..e0ae3fd19 100644
--- a/src/modules/bluetooth/backend-ofono.c
+++ b/src/modules/bluetooth/backend-ofono.c
@@ -218,7 +218,7 @@ static int card_acquire(struct hf_audio_card *card) {
             close(fd);
             return -1;
         }
-        card->transport->codec = codec;
+        card->transport->bt_codec = pa_bluetooth_get_hf_codec("CVSD");
         card->fd = fd;
         return 0;
     }
@@ -686,7 +686,7 @@ static DBusMessage *hf_audio_agent_new_connection(DBusConnection *c, DBusMessage
 
     card->connecting = false;
     card->fd = fd;
-    card->transport->codec = codec;
+    card->transport->bt_codec = pa_bluetooth_get_hf_codec("CVSD");
 
     pa_bluetooth_transport_set_state(card->transport, PA_BLUETOOTH_TRANSPORT_STATE_PLAYING);
 
diff --git a/src/modules/bluetooth/bluez5-util.c b/src/modules/bluetooth/bluez5-util.c
index 8086f0ac8..554af28f3 100644
--- a/src/modules/bluetooth/bluez5-util.c
+++ b/src/modules/bluetooth/bluez5-util.c
@@ -1945,7 +1945,7 @@ static DBusMessage *endpoint_set_configuration(DBusConnection *conn, DBusMessage
     dbus_message_unref(r);
 
     t = pa_bluetooth_transport_new(d, sender, path, p, config, size);
-    t->a2dp_codec = a2dp_codec;
+    t->bt_codec = a2dp_codec;
     t->acquire = bluez5_transport_acquire_cb;
     t->release = bluez5_transport_release_cb;
     t->write = a2dp_transport_write;
diff --git a/src/modules/bluetooth/bluez5-util.h b/src/modules/bluetooth/bluez5-util.h
index 14885b13f..0093d1a3f 100644
--- a/src/modules/bluetooth/bluez5-util.h
+++ b/src/modules/bluetooth/bluez5-util.h
@@ -97,11 +97,10 @@ struct pa_bluetooth_transport {
     char *path;
     pa_bluetooth_profile_t profile;
 
-    uint8_t codec;
     void *config;
     size_t config_size;
 
-    const pa_a2dp_codec *a2dp_codec;
+    const pa_a2dp_codec *bt_codec;
     int stream_write_type;
 
     pa_volume_t source_volume;
diff --git a/src/modules/bluetooth/bt-codec-cvsd.c b/src/modules/bluetooth/bt-codec-cvsd.c
new file mode 100644
index 000000000..b121dc5a1
--- /dev/null
+++ b/src/modules/bluetooth/bt-codec-cvsd.c
@@ -0,0 +1,122 @@
+/***
+  This file is part of PulseAudio.
+
+  PulseAudio is free software; you can redistribute it and/or modify
+  it under the terms of the GNU Lesser General Public License as
+  published by the Free Software Foundation; either version 2.1 of the
+  License, or (at your option) any later version.
+
+  PulseAudio is distributed in the hope that it will be useful, but
+  WITHOUT ANY WARRANTY; without even the implied warranty of
+  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+  General Public License for more details.
+
+  You should have received a copy of the GNU Lesser General Public
+  License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
+***/
+#ifdef HAVE_CONFIG_H
+#include <config.h>
+#endif
+
+#include "a2dp-codec-api.h"
+
+typedef struct codec_info {
+    pa_sample_spec sample_spec;
+} codec_info_t;
+
+static void *init(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
+    codec_info_t *info;
+
+    info = pa_xnew0(codec_info_t, 1);
+
+    info->sample_spec.format = PA_SAMPLE_S16LE;
+    info->sample_spec.channels = 1;
+    info->sample_spec.rate = 8000;
+
+    *sample_spec = info->sample_spec;
+
+    return info;
+}
+
+static void deinit(void *codec_info) {
+    pa_xfree(codec_info);
+}
+
+static int reset(void *codec_info) {
+    return 0;
+}
+
+static size_t get_block_size(void *codec_info, size_t link_mtu) {
+    codec_info_t *info = (codec_info_t *) codec_info;
+    size_t block_size = link_mtu;
+
+    if (!pa_frame_aligned(block_size, &info->sample_spec)) {
+        pa_log_debug("Got invalid block size: %lu, rounding down", block_size);
+        block_size = pa_frame_align(block_size, &info->sample_spec);
+    }
+
+    return block_size;
+}
+
+static size_t get_encoded_block_size(void *codec_info, size_t input_size) {
+    codec_info_t *info = (codec_info_t *) codec_info;
+
+    /* input size should be aligned to sample spec */
+    pa_assert_fp(pa_frame_aligned(input_size, &info->sample_spec));
+
+    return input_size;
+}
+
+static size_t reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
+    return 0;
+}
+
+static size_t increase_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
+    return 0;
+}
+
+static size_t encode_buffer(void *codec_info, uint32_t timestamp, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
+    pa_assert(input_size <= output_size);
+
+    memcpy(output_buffer, input_buffer, input_size);
+    *processed = input_size;
+
+    return input_size;
+}
+
+static size_t decode_buffer(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
+    codec_info_t *info = (codec_info_t *) codec_info;
+
+    *processed = input_size;
+
+    /* 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(input_size, &info->sample_spec)) {
+        pa_log_warn("SCO packet received of unaligned size: %zu", input_size);
+        return 0;
+    }
+
+    memcpy(output_buffer, input_buffer, input_size);
+
+    return input_size;
+}
+
+/* dummy passthrough codec used with HSP/HFP CVSD */
+const pa_a2dp_codec pa_bt_codec_cvsd = {
+    .name = "CVSD",
+    .description = "CVSD",
+    .init = init,
+    .deinit = deinit,
+    .reset = reset,
+    .get_read_block_size = get_block_size,
+    .get_write_block_size = get_block_size,
+    .get_encoded_block_size = get_encoded_block_size,
+    .reduce_encoder_bitrate = reduce_encoder_bitrate,
+    .increase_encoder_bitrate = increase_encoder_bitrate,
+    .encode_buffer = encode_buffer,
+    .decode_buffer = decode_buffer,
+};
diff --git a/src/modules/bluetooth/meson.build b/src/modules/bluetooth/meson.build
index 6315e667e..7f2e6db7a 100644
--- a/src/modules/bluetooth/meson.build
+++ b/src/modules/bluetooth/meson.build
@@ -2,6 +2,7 @@ libbluez5_util_sources = [
   'a2dp-codec-sbc.c',
   'a2dp-codec-util.c',
   'bluez5-util.c',
+  'bt-codec-cvsd.c',
 ]
 
 libbluez5_util_headers = [
diff --git a/src/modules/bluetooth/module-bluez5-device.c b/src/modules/bluetooth/module-bluez5-device.c
index 89bacf425..e0430dea8 100644
--- a/src/modules/bluetooth/module-bluez5-device.c
+++ b/src/modules/bluetooth/module-bluez5-device.c
@@ -623,28 +623,23 @@ static void handle_sink_block_size_change(struct userdata *u) {
 
 /* Run from I/O thread */
 static void transport_config_mtu(struct userdata *u) {
-    if (u->profile == PA_BLUETOOTH_PROFILE_HSP_HS
-        || u->profile == PA_BLUETOOTH_PROFILE_HSP_AG
-        || u->profile == PA_BLUETOOTH_PROFILE_HFP_HF
-        || u->profile == PA_BLUETOOTH_PROFILE_HFP_AG) {
-        u->read_block_size = u->read_link_mtu;
-        u->write_block_size = u->write_link_mtu;
+    pa_assert(u->bt_codec);
 
-        if (!pa_frame_aligned(u->read_block_size, &u->source->sample_spec)) {
-            pa_log_debug("Got invalid read MTU: %lu, rounding down", u->read_block_size);
-            u->read_block_size = pa_frame_align(u->read_block_size, &u->source->sample_spec);
-        }
+    if (u->encoder_info) {
+        u->write_block_size = u->bt_codec->get_write_block_size(u->encoder_info, u->write_link_mtu);
 
         if (!pa_frame_aligned(u->write_block_size, &u->sink->sample_spec)) {
             pa_log_debug("Got invalid write MTU: %lu, rounding down", u->write_block_size);
             u->write_block_size = pa_frame_align(u->write_block_size, &u->sink->sample_spec);
         }
-    } else {
-        pa_assert(u->bt_codec);
-        if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK) {
-            u->write_block_size = u->bt_codec->get_write_block_size(u->encoder_info, u->write_link_mtu);
-        } else {
-            u->read_block_size = u->bt_codec->get_read_block_size(u->decoder_info, u->read_link_mtu);
+    }
+
+    if (u->decoder_info) {
+        u->read_block_size = u->bt_codec->get_read_block_size(u->decoder_info, u->read_link_mtu);
+
+        if (!pa_frame_aligned(u->read_block_size, &u->source->sample_spec)) {
+            pa_log_debug("Got invalid read MTU: %lu, rounding down", u->read_block_size);
+            u->read_block_size = pa_frame_align(u->read_block_size, &u->source->sample_spec);
         }
     }
 
@@ -671,12 +666,14 @@ static int setup_stream(struct userdata *u) {
 
     pa_log_info("Transport %s resuming", u->transport->path);
 
-    if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK) {
-        pa_assert(u->bt_codec);
+    pa_assert(u->bt_codec);
+
+    if (u->encoder_info) {
         if (u->bt_codec->reset(u->encoder_info) < 0)
             return -1;
-    } else if (u->profile == PA_BLUETOOTH_PROFILE_A2DP_SOURCE) {
-        pa_assert(u->bt_codec);
+    }
+
+    if (u->decoder_info) {
         if (u->bt_codec->reset(u->decoder_info) < 0)
             return -1;
     }
@@ -1194,45 +1191,54 @@ static int add_sink(struct userdata *u) {
 }
 
 /* Run from main thread */
-static int transport_config(struct userdata *u) {
-    /* reset encoder buffer contents */
-    u->encoder_buffer_used = 0;
+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_HS] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
+        [PA_BLUETOOTH_PROFILE_HSP_AG] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
+        [PA_BLUETOOTH_PROFILE_HFP_HF] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
+        [PA_BLUETOOTH_PROFILE_HFP_AG] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
+        [PA_BLUETOOTH_PROFILE_OFF] = 0
+    };
 
-    if (u->profile == PA_BLUETOOTH_PROFILE_HSP_HS
-        || u->profile == PA_BLUETOOTH_PROFILE_HSP_AG
-        || u->profile == PA_BLUETOOTH_PROFILE_HFP_HF
-        || u->profile == PA_BLUETOOTH_PROFILE_HFP_AG) {
-        u->encoder_sample_spec.format = PA_SAMPLE_S16LE;
-        u->encoder_sample_spec.channels = 1;
-        u->encoder_sample_spec.rate = 8000;
-        u->decoder_sample_spec.format = PA_SAMPLE_S16LE;
-        u->decoder_sample_spec.channels = 1;
-        u->decoder_sample_spec.rate = 8000;
-        return 0;
-    } else {
-        bool is_a2dp_sink = u->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK;
-        void *info;
+    return profile_direction[p];
+}
 
-        pa_assert(u->transport);
+/* Run from main thread */
+static int transport_config(struct userdata *u) {
+    pa_assert(u);
+    pa_assert(u->transport);
+    pa_assert(!u->bt_codec);
+    pa_assert(!u->encoder_info);
+    pa_assert(!u->decoder_info);
 
-        pa_assert(!u->bt_codec);
-        pa_assert(!u->encoder_info);
-        pa_assert(!u->decoder_info);
+    u->bt_codec = u->transport->bt_codec;
+    pa_assert(u->bt_codec);
 
-        u->bt_codec = u->transport->a2dp_codec;
-        pa_assert(u->bt_codec);
+    /* reset encoder buffer contents */
+    u->encoder_buffer_used = 0;
 
-        info = u->bt_codec->init(is_a2dp_sink, false, u->transport->config, u->transport->config_size, is_a2dp_sink ? &u->encoder_sample_spec : &u->decoder_sample_spec, u->core);
-        if (is_a2dp_sink)
-            u->encoder_info = info;
-        else
-            u->decoder_info = info;
+    if (get_profile_direction(u->profile) & PA_DIRECTION_OUTPUT) {
+        u->encoder_info = u->bt_codec->init(true, false, u->transport->config, u->transport->config_size, &u->encoder_sample_spec, u->core);
 
-        if (!info)
+        if (!u->encoder_info)
             return -1;
+    }
 
-        return 0;
+    if (get_profile_direction(u->profile) & PA_DIRECTION_INPUT) {
+        u->decoder_info = u->bt_codec->init(false, false, u->transport->config, u->transport->config_size, &u->decoder_sample_spec, u->core);
+
+        if (!u->decoder_info) {
+            if (u->encoder_info) {
+                u->bt_codec->deinit(u->encoder_info);
+                u->encoder_info = NULL;
+            }
+            return -1;
+        }
     }
+
+    return 0;
 }
 
 /* Run from main thread */
@@ -1266,21 +1272,6 @@ static int setup_transport(struct userdata *u) {
 }
 
 /* Run from main thread */
-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_HS] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
-        [PA_BLUETOOTH_PROFILE_HSP_AG] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
-        [PA_BLUETOOTH_PROFILE_HFP_HF] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
-        [PA_BLUETOOTH_PROFILE_HFP_AG] = PA_DIRECTION_INPUT | PA_DIRECTION_OUTPUT,
-        [PA_BLUETOOTH_PROFILE_OFF] = 0
-    };
-
-    return profile_direction[p];
-}
-
-/* Run from main thread */
 static int init_profile(struct userdata *u) {
     int r = 0;
     pa_assert(u);
@@ -1486,7 +1477,7 @@ static void thread_func(void *userdata) {
                                 skip_bytes -= bytes_to_render;
                             }
 
-                            if (u->write_index > 0 && u->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK) {
+                            if (u->write_index > 0 && (get_profile_direction(u->profile) & PA_DIRECTION_OUTPUT)) {
                                 size_t new_write_block_size = u->bt_codec->reduce_encoder_bitrate(u->encoder_info, u->write_link_mtu);
                                 if (new_write_block_size) {
                                     u->write_block_size = new_write_block_size;
@@ -1526,7 +1517,7 @@ static void thread_func(void *userdata) {
                             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->profile == PA_BLUETOOTH_PROFILE_A2DP_SINK && u->write_memchunk.memblock == NULL) {
+                            if ((get_profile_direction(u->profile) & PA_DIRECTION_OUTPUT) && u->write_memchunk.memblock == NULL) {
                                 /* write_block() is keeping up with input, try increasing bitrate */
                                 if (u->bt_codec->increase_encoder_bitrate
                                     && pa_timeval_age(&tv_last_output_rate_change) >= u->device->output_rate_refresh_interval_ms * PA_USEC_PER_MSEC) {