bluez5: Add G722 codec for ASHA

For documentation on ASHA, see
https://source.android.com/docs/core/connect/bluetooth/asha

meson_options.txt

@@ -145,6 +145,10 @@ option('bluez5-codec-lc3',
         description: 'Enable LC3 open source codec implementation',
         type: 'feature',
         value: 'auto')
+option('bluez5-codec-g722',
+        description: 'Enable G722 open source codec implementation',
+        type: 'feature',
+        value: 'auto')
 option('control',
        description: 'Enable control spa plugin integration',
        type: 'feature',

spa/include/spa/param/bluetooth/audio.h

@@ -44,6 +44,9 @@ enum spa_bluetooth_audio_codec {
 
 	/* BAP */
 	SPA_BLUETOOTH_AUDIO_CODEC_LC3 = 0x200,
+
+	/* ASHA */
+	SPA_BLUETOOTH_AUDIO_CODEC_G722 = 0x300,
 };
 
 /**

spa/meson.build

@@ -89,6 +89,9 @@ if get_option('spa-plugins').allowed()
       summary({'ModemManager': mm_dep.found()}, bool_yn: true, section: 'Bluetooth backends')
     endif
     cdata.set('HAVE_LC3', get_option('bluez5-codec-lc3').allowed() and lc3_dep.found())
+    g722_codec_option = get_option('bluez5-codec-g722')
+    summary({'G722': g722_codec_option.allowed()}, bool_yn: true, section: 'Bluetooth audio codecs')
+    cdata.set('HAVE_G722', g722_codec_option.allowed())
   endif
 
   have_vulkan = false

spa/plugins/bluez5/a2dp-codec-caps.h

@@ -486,4 +486,6 @@ typedef struct {
 	uint8_t data;
 } __attribute__ ((packed)) a2dp_opus_g_t;
 
+#define ASHA_CODEC_G722	0x63
+
 #endif

spa/plugins/bluez5/asha-codec-g722.c

@@ -0,0 +1,291 @@
+/* Spa ASHA G722 codec */
+/* SPDX-FileCopyrightText: Copyright © 2024 Wim Taymans */
+/* SPDX-License-Identifier: MIT */
+
+#include <spa/param/audio/format.h>
+#include <spa/utils/dict.h>
+#include <spa/debug/log.h>
+
+#include <libavcodec/avcodec.h>
+#include <libavutil/channel_layout.h>
+#include <libavutil/common.h>
+#include <libavutil/frame.h>
+#include <libavutil/samplefmt.h>
+
+#include "rtp.h"
+#include "media-codecs.h"
+
+#define ASHA_HEADER_SZ       1 /* 1 byte sequence number */
+#define ASHA_ENCODED_PKT_SZ  160
+
+static struct spa_log *spalog;
+
+struct impl {
+	const AVCodec *avcodec;
+	AVCodecContext *ctx;
+	AVFrame *frame;
+	AVPacket *pkt;
+
+	uint8_t seqnum;
+	unsigned int codesize;
+};
+
+static int codec_reduce_bitpool(void *data)
+{
+	return -ENOTSUP;
+}
+
+static int codec_increase_bitpool(void *data)
+{
+	return -ENOTSUP;
+}
+
+static int codec_abr_process (void *data, size_t unsent)
+{
+	return -ENOTSUP;
+}
+
+static int codec_get_block_size(void *data)
+{
+	struct impl *this = data;
+	return this->codesize;
+}
+
+static int codec_start_encode (void *data,
+		void *dst, size_t dst_size, uint16_t seqnum, uint32_t timestamp)
+{
+	return 0;
+}
+
+static int codec_enum_config(const struct media_codec *codec, uint32_t flags,
+		const void *caps, size_t caps_size, uint32_t id, uint32_t idx,
+		struct spa_pod_builder *b, struct spa_pod **param)
+{
+	struct spa_pod_frame f[1];
+	uint32_t position[1];
+
+	if (idx > 0)
+		return 0;
+
+	spa_pod_builder_push_object(b, &f[0], SPA_TYPE_OBJECT_Format, id);
+	spa_pod_builder_add(b,
+			SPA_FORMAT_mediaType,      SPA_POD_Id(SPA_MEDIA_TYPE_audio),
+			SPA_FORMAT_mediaSubtype,   SPA_POD_Id(SPA_MEDIA_SUBTYPE_raw),
+			SPA_FORMAT_AUDIO_format,   SPA_POD_Id(SPA_AUDIO_FORMAT_S16),
+			0);
+	spa_pod_builder_add(b,
+		SPA_FORMAT_AUDIO_rate, SPA_POD_Int(16000),
+		0);
+
+	spa_pod_builder_add(b,
+			SPA_FORMAT_AUDIO_channels, SPA_POD_Int(1),
+			0);
+	position[0] = SPA_AUDIO_CHANNEL_MONO;
+	spa_pod_builder_add(b,
+			SPA_FORMAT_AUDIO_channels, SPA_POD_Int(1),
+			SPA_FORMAT_AUDIO_position, SPA_POD_Array(sizeof(uint32_t),
+				SPA_TYPE_Id, 1, position),
+			0);
+
+	*param = spa_pod_builder_pop(b, &f[0]);
+
+	return *param == NULL ? -EIO : 1;
+}
+
+static void codec_deinit(void *data)
+{
+	struct impl *this = data;
+
+	if (this->frame)
+		av_frame_free(&this->frame);
+	if (this->pkt)
+		av_packet_free(&this->pkt);
+	if (this->ctx)
+		avcodec_free_context(&this->ctx);
+}
+
+static void *codec_init(const struct media_codec *codec, uint32_t flags,
+		void *config, size_t config_len, const struct spa_audio_info *info,
+		void *props, size_t mtu)
+{
+	struct impl *this;
+	const AVCodec *avcodec;
+	AVCodecContext *c = NULL;
+	AVFrame *frame = NULL;
+	AVPacket *pkt = NULL;
+
+	if ((this = calloc(1, sizeof(struct impl))) == NULL)
+		goto error;
+
+	avcodec = avcodec_find_encoder(AV_CODEC_ID_ADPCM_G722);
+	if (!avcodec) {
+		spa_log_error(spalog, "Codec not found");
+		goto error;
+	}
+
+	c = avcodec_alloc_context3(avcodec);
+	if (!c) {
+		spa_log_error(spalog, "Codec context allocation failed");
+		goto error;
+	}
+
+	/* https://source.android.com/docs/core/connect/bluetooth/asha#audio-packet-format-and-timing */
+	c->bit_rate = 64000;
+	c->sample_rate = 16000;
+	c->sample_fmt = AV_SAMPLE_FMT_S16;
+	av_channel_layout_copy(&c->ch_layout, &(AVChannelLayout)AV_CHANNEL_LAYOUT_MONO);
+
+	spa_log_info(spalog, "Opening codec, format: %d, rate: %d, layout: %d", c->sample_fmt, c->sample_rate, c->ch_layout.nb_channels);
+
+	if (avcodec_open2(c, avcodec, NULL) < 0) {
+		spa_log_error(spalog, "Could not open codec");
+		goto error;
+	}
+
+	frame = av_frame_alloc();
+	if (!frame) {
+		spa_log_error(spalog, "Could not allocate frame");
+		goto error;
+	}
+
+	frame->nb_samples = c->frame_size;
+	frame->format = c->sample_fmt;
+	if (av_channel_layout_copy (&frame->ch_layout, &c->ch_layout)) {
+		spa_log_error(spalog, "Failed to copy channel layout");
+		goto error;
+	}
+
+	spa_log_info(spalog, "Frame info, samples: %d, format: %d, layout: %d", frame->nb_samples, frame->format, frame->ch_layout.nb_channels);
+
+	if (av_frame_get_buffer(frame, 0)) {
+		spa_log_error(spalog, "Failed to allocate buffer for frame");
+		goto error;
+	}
+
+	pkt = av_packet_alloc();
+	if (!pkt) {
+		spa_log_error(spalog, "Could not allocate packet");
+		goto error;
+	}
+
+	/*
+	 * G722 has a compression ratio of 4. Considering 160 bytes of encoded
+	 * payload, we need 640 bytes for generating an encoded frame.
+	 */
+	this->codesize = ASHA_ENCODED_PKT_SZ * 4;
+	this->seqnum = 0;
+
+	this->avcodec = avcodec;
+	this->ctx = c;
+	this->frame = frame;
+	this->pkt = pkt;
+
+	spa_log_debug(spalog, "Codec initialized");
+
+	return this;
+
+error:
+	if (frame)
+		av_frame_free(&frame);
+	if (c)
+		avcodec_free_context(&c);
+
+	return NULL;
+}
+
+static int codec_encode(void *data,
+		const void *src, size_t src_size,
+		void *dst, size_t dst_size,
+		size_t *dst_out, int *need_flush)
+{
+	struct impl *this = data;
+	uint8_t *dest = (uint8_t *)dst;
+	AVFrame *frame = this->frame;
+	AVPacket *pkt = this->pkt;
+	size_t src_sz;
+	int ret;
+
+	if (src_size < this->codesize) {
+		spa_log_trace(spalog, "Insufficient bytes for encoding, %lu", src_size);
+		return 0;
+	}
+	if (dst_size < (ASHA_HEADER_SZ + ASHA_ENCODED_PKT_SZ)) {
+		spa_log_trace(spalog, "No space for encoded output, %lu", dst_size);
+		return 0;
+	}
+
+	spa_log_trace(spalog, "%lu bytes to encode", src_size);
+
+	ret = av_frame_make_writable(this->frame);
+	if (ret < 0) {
+		spa_log_error(spalog, "Failed to make frame writable");
+		return -EIO;
+	}
+
+	src_sz = (src_size > this->codesize) ? this->codesize : src_size;
+	frame->data[0] = (uint8_t *)src;
+	frame->linesize[0] = src_sz;
+
+	spa_log_trace(spalog, "Encoding %lu bytes", src_sz);
+
+	ret = avcodec_send_frame(this->ctx, frame);
+	if (ret < 0) {
+		spa_log_error(spalog, "Failed to send frame: %d", ret);
+		return ret;
+	}
+
+	ret = avcodec_receive_packet(this->ctx, pkt);
+	if (ret == AVERROR(EAGAIN) || ret == AVERROR_EOF) {
+		spa_log_warn(spalog, "Receive packet EOF/EAGAIN: %d", ret);
+		return 0;
+	}
+
+	if (ret < 0) {
+		spa_log_error(spalog, "Receive packet error: %d", ret);
+		return -EIO;
+	}
+
+	spa_log_trace(spalog, "%d encoded bytes with sequence: %d", pkt->size, this->seqnum);
+
+	/* Payload for ASHA must be preceded by 1-byte sequence number */
+	*dest = this->seqnum;
+	dest++;
+	memcpy((void *)dest, pkt->data, pkt->size);
+
+	*dst_out = pkt->size + ASHA_HEADER_SZ;
+	*need_flush = NEED_FLUSH_ALL;
+
+	this->seqnum++;
+
+	return src_sz;
+}
+
+static void codec_set_log(struct spa_log *global_log)
+{
+	spalog = global_log;
+	spa_log_topic_init(spalog, &codec_plugin_log_topic);
+}
+
+const struct media_codec asha_codec_g722 = {
+	.id = SPA_BLUETOOTH_AUDIO_CODEC_G722,
+	.codec_id = ASHA_CODEC_G722,
+	.name = "g722",
+	.asha = true,
+	.description = "G722",
+	.fill_caps = NULL,
+	.enum_config = codec_enum_config,
+	.init = codec_init,
+	.deinit = codec_deinit,
+	.get_block_size = codec_get_block_size,
+	.start_encode = codec_start_encode,
+	.encode = codec_encode,
+	.abr_process = codec_abr_process,
+	.reduce_bitpool = codec_reduce_bitpool,
+	.increase_bitpool = codec_increase_bitpool,
+	.set_log = codec_set_log,
+};
+
+MEDIA_CODEC_EXPORT_DEF(
+	"g722",
+	&asha_codec_g722
+);

spa/plugins/bluez5/bluez5-dbus.c

@@ -5672,7 +5672,7 @@ static void interfaces_removed(struct spa_bt_monitor *monitor, DBusMessageIter *
 					struct spa_bt_device *d = transport->device;
 					if (d != NULL){
 						device_free(d);
-					}		
+					}
 				} else if (transport->profile == SPA_BT_PROFILE_BAP_BROADCAST_SOURCE) {
 					/*
 					 * For each transport that has a broadcast source profile,
@@ -5682,7 +5682,7 @@ static void interfaces_removed(struct spa_bt_monitor *monitor, DBusMessageIter *
 					 * for this case will have the scanned device to the transport
 					 * "/fd0" and create new devices for the other transports from this device
 					 * that appear only in case of multiple BISes per BIG.
-					 * 
+					 *
 					 * Here we delete the created devices.
 					 */
 					char *pos = strstr(transport->path, "/fd0");

spa/plugins/bluez5/codec-loader.c

@@ -51,6 +51,7 @@ static int codec_order(const struct media_codec *c)
 		SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_DUPLEX,
 		SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_PRO,
 		SPA_BLUETOOTH_AUDIO_CODEC_AAC_ELD,
+		SPA_BLUETOOTH_AUDIO_CODEC_G722,
 	};
 	size_t i;
 	for (i = 0; i < SPA_N_ELEMENTS(order); ++i)
@@ -172,7 +173,8 @@ const struct media_codec * const *load_media_codecs(struct spa_plugin_loader *lo
 		MEDIA_CODEC_FACTORY_LIB("lc3plus"),
 		MEDIA_CODEC_FACTORY_LIB("opus"),
 		MEDIA_CODEC_FACTORY_LIB("opus-g"),
-		MEDIA_CODEC_FACTORY_LIB("lc3")
+		MEDIA_CODEC_FACTORY_LIB("lc3"),
+		MEDIA_CODEC_FACTORY_LIB("g722")
 #undef MEDIA_CODEC_FACTORY_LIB
 	};
 

spa/plugins/bluez5/media-codecs.h

@@ -71,6 +71,7 @@ struct media_codec {
 	a2dp_vendor_codec_t vendor;
 
 	bool bap;
+	bool asha;
 
 	const char *name;
 	const char *description;

spa/plugins/bluez5/meson.build

@@ -173,6 +173,16 @@ if get_option('bluez5-codec-lc3').allowed() and lc3_dep.found()
     install_dir : spa_plugindir / 'bluez5')
 endif
 
+if get_option('bluez5-codec-g722').allowed() and ffmpeg.allowed()
+  bluez_codec_g722 = shared_library('spa-codec-bluez5-g722',
+    [ 'asha-codec-g722.c', 'media-codecs.c' ],
+    include_directories : [ configinc ],
+    c_args : codec_args,
+    dependencies : [ spa_dep, avcodec_dep, avformat_dep, avutil_dep ],
+    install : true,
+    install_dir : spa_plugindir / 'bluez5')
+endif
+
 test_apps = [
   'test-midi',
 ]