From 61c585c8e60c207386f5b4379dbb07d2d31193c5 Mon Sep 17 00:00:00 2001 From: Pauli Virtanen Date: Sun, 28 Jan 2024 22:15:00 +0200 Subject: [PATCH] bluez5: support Google's Opus A2DP vendor codec --- spa/include/spa/param/bluetooth/audio.h | 1 + spa/include/spa/param/bluetooth/type-info.h | 1 + spa/plugins/bluez5/a2dp-codec-caps.h | 28 ++ spa/plugins/bluez5/a2dp-codec-opus-g.c | 526 ++++++++++++++++++++ spa/plugins/bluez5/codec-loader.c | 2 + spa/plugins/bluez5/media-sink.c | 5 +- spa/plugins/bluez5/meson.build | 8 + 7 files changed, 570 insertions(+), 1 deletion(-) create mode 100644 spa/plugins/bluez5/a2dp-codec-opus-g.c diff --git a/spa/include/spa/param/bluetooth/audio.h b/spa/include/spa/param/bluetooth/audio.h index dd55db86f..abdf7ccba 100644 --- a/spa/include/spa/param/bluetooth/audio.h +++ b/spa/include/spa/param/bluetooth/audio.h @@ -35,6 +35,7 @@ enum spa_bluetooth_audio_codec { SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_71, SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_DUPLEX, SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_PRO, + SPA_BLUETOOTH_AUDIO_CODEC_OPUS_G, /* HFP */ SPA_BLUETOOTH_AUDIO_CODEC_CVSD = 0x100, diff --git a/spa/include/spa/param/bluetooth/type-info.h b/spa/include/spa/param/bluetooth/type-info.h index 5b82ea81f..878c5b9b0 100644 --- a/spa/include/spa/param/bluetooth/type-info.h +++ b/spa/include/spa/param/bluetooth/type-info.h @@ -39,6 +39,7 @@ static const struct spa_type_info spa_type_bluetooth_audio_codec[] = { { SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_71, SPA_TYPE_Int, SPA_TYPE_INFO_BLUETOOTH_AUDIO_CODEC_BASE "opus_05_71", NULL }, { SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_DUPLEX, SPA_TYPE_Int, SPA_TYPE_INFO_BLUETOOTH_AUDIO_CODEC_BASE "opus_05_duplex", NULL }, { SPA_BLUETOOTH_AUDIO_CODEC_OPUS_05_PRO, SPA_TYPE_Int, SPA_TYPE_INFO_BLUETOOTH_AUDIO_CODEC_BASE "opus_05_pro", NULL }, + { SPA_BLUETOOTH_AUDIO_CODEC_OPUS_G, SPA_TYPE_Int, SPA_TYPE_INFO_BLUETOOTH_AUDIO_CODEC_BASE "opus_g", NULL }, { SPA_BLUETOOTH_AUDIO_CODEC_CVSD, SPA_TYPE_Int, SPA_TYPE_INFO_BLUETOOTH_AUDIO_CODEC_BASE "cvsd", NULL }, { SPA_BLUETOOTH_AUDIO_CODEC_MSBC, SPA_TYPE_Int, SPA_TYPE_INFO_BLUETOOTH_AUDIO_CODEC_BASE "msbc", NULL }, diff --git a/spa/plugins/bluez5/a2dp-codec-caps.h b/spa/plugins/bluez5/a2dp-codec-caps.h index 2a9b06734..2bb883ec6 100644 --- a/spa/plugins/bluez5/a2dp-codec-caps.h +++ b/spa/plugins/bluez5/a2dp-codec-caps.h @@ -282,6 +282,29 @@ #define OPUS_05_SET_BITRATE(a, v) OPUS_05_SET_UINT16(a, bitrate, v) +#define OPUS_G_VENDOR_ID 0x000000e0 +#define OPUS_G_CODEC_ID 0x0001 + +#define OPUS_G_FREQUENCY_MASK 0x80 +#define OPUS_G_FREQUENCY_48000 0x80 + +#define OPUS_G_DURATION_MASK 0x18 +#define OPUS_G_DURATION_100 0x08 +#define OPUS_G_DURATION_200 0x10 + +#define OPUS_G_CHANNELS_MASK 0x07 +#define OPUS_G_CHANNELS_MONO 0x01 +#define OPUS_G_CHANNELS_STEREO 0x02 +#define OPUS_G_CHANNELS_MONO_2 0x04 + +#define OPUS_G_GET_FREQUENCY(a) ((a).data & OPUS_G_FREQUENCY_MASK) +#define OPUS_G_GET_DURATION(a) ((a).data & OPUS_G_DURATION_MASK) +#define OPUS_G_GET_CHANNELS(a) ((a).data & OPUS_G_CHANNELS_MASK) + +#define OPUS_G_SET(a, freq, dur, ch) \ + (a).data = ((freq) & OPUS_G_FREQUENCY_MASK) | ((dur) & OPUS_G_DURATION_MASK) | ((ch) & OPUS_G_CHANNELS_MASK) + + typedef struct { uint32_t vendor_id; uint16_t codec_id; @@ -458,4 +481,9 @@ typedef struct { a2dp_opus_05_direction_t bidi; } __attribute__ ((packed)) a2dp_opus_05_t; +typedef struct { + a2dp_vendor_codec_t info; + uint8_t data; +} __attribute__ ((packed)) a2dp_opus_g_t; + #endif diff --git a/spa/plugins/bluez5/a2dp-codec-opus-g.c b/spa/plugins/bluez5/a2dp-codec-opus-g.c new file mode 100644 index 000000000..df1ad7765 --- /dev/null +++ b/spa/plugins/bluez5/a2dp-codec-opus-g.c @@ -0,0 +1,526 @@ +/* Spa A2DP Opus Codec */ +/* SPDX-FileCopyrightText: Copyright © 2020 Wim Taymans */ +/* SPDX-FileCopyrightText: Copyright © 2022 Pauli Virtanen */ +/* SPDX-License-Identifier: MIT */ + +#include +#include +#include +#include +#include +#if __BYTE_ORDER != __LITTLE_ENDIAN +#include +#endif + +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "rtp.h" +#include "media-codecs.h" + +static struct spa_log *log; + +struct dec_data { +}; + +struct enc_data { + struct rtp_header *header; + struct rtp_payload *payload; + + int samples; + int codesize; + int frame_dms; + int bitrate; + int packet_size; +}; + +struct impl { + OpusEncoder *enc; + OpusDecoder *dec; + + int mtu; + int samplerate; + int channels; + int application; + + struct dec_data d; + struct enc_data e; +}; + +static int codec_fill_caps(const struct media_codec *codec, uint32_t flags, + uint8_t caps[A2DP_MAX_CAPS_SIZE]) +{ + a2dp_opus_g_t conf = { + .info = codec->vendor, + }; + + OPUS_G_SET(conf, + OPUS_G_FREQUENCY_48000, + OPUS_G_DURATION_100 | OPUS_G_DURATION_200, + OPUS_G_CHANNELS_MONO | OPUS_G_CHANNELS_STEREO | OPUS_G_CHANNELS_MONO_2); + + memcpy(caps, &conf, sizeof(conf)); + return sizeof(conf); +} + +static int codec_select_config(const struct media_codec *codec, uint32_t flags, + const void *caps, size_t caps_size, + const struct media_codec_audio_info *info, + const struct spa_dict *global_settings, uint8_t config[A2DP_MAX_CAPS_SIZE]) +{ + a2dp_opus_g_t conf; + int frequency, duration, channels; + + if (caps_size < sizeof(conf)) + return -EINVAL; + + memcpy(&conf, caps, sizeof(conf)); + + if (codec->vendor.vendor_id != conf.info.vendor_id || + codec->vendor.codec_id != conf.info.codec_id) + return -ENOTSUP; + + if (OPUS_G_GET_FREQUENCY(conf) & OPUS_G_FREQUENCY_48000) + frequency = OPUS_G_FREQUENCY_48000; + else + return -EINVAL; + + if (OPUS_G_GET_DURATION(conf) & OPUS_G_DURATION_200) + duration = OPUS_G_DURATION_200; + else if (OPUS_G_GET_DURATION(conf) & OPUS_G_DURATION_100) + duration = OPUS_G_DURATION_100; + else + return -EINVAL; + + if (OPUS_G_GET_CHANNELS(conf) & OPUS_G_CHANNELS_STEREO) + channels = OPUS_G_CHANNELS_STEREO; + else if (OPUS_G_GET_CHANNELS(conf) & OPUS_G_CHANNELS_MONO) + channels = OPUS_G_CHANNELS_MONO; + else if (OPUS_G_GET_CHANNELS(conf) & OPUS_G_CHANNELS_MONO_2) + channels = OPUS_G_CHANNELS_MONO_2; + else + return -EINVAL; + + OPUS_G_SET(conf, frequency, duration, channels); + + memcpy(config, &conf, sizeof(conf)); + + return sizeof(conf); +} + +static int codec_caps_preference_cmp(const struct media_codec *codec, uint32_t flags, const void *caps1, size_t caps1_size, + const void *caps2, size_t caps2_size, const struct media_codec_audio_info *info, + const struct spa_dict *global_settings) +{ + a2dp_opus_g_t conf1, conf2, cap1, cap2; + a2dp_opus_g_t *conf; + int res1, res2; + int a, b; + + /* Order selected configurations by preference */ + res1 = codec->select_config(codec, flags, caps1, caps1_size, info, global_settings, (uint8_t *)&conf1); + res2 = codec->select_config(codec, flags, caps2, caps2_size, info, global_settings, (uint8_t *)&conf2); + +#define PREFER_EXPR(expr) \ + do { \ + conf = &conf1; \ + a = (expr); \ + conf = &conf2; \ + b = (expr); \ + if (a != b) \ + return b - a; \ + } while (0) + +#define PREFER_BOOL(expr) PREFER_EXPR((expr) ? 1 : 0) + + /* Prefer valid */ + a = (res1 > 0 && (size_t)res1 == sizeof(a2dp_opus_g_t)) ? 1 : 0; + b = (res2 > 0 && (size_t)res2 == sizeof(a2dp_opus_g_t)) ? 1 : 0; + if (!a || !b) + return b - a; + + memcpy(&cap1, caps1, sizeof(cap1)); + memcpy(&cap2, caps2, sizeof(cap2)); + + PREFER_EXPR(OPUS_G_GET_CHANNELS(*conf) & OPUS_G_CHANNELS_STEREO); + + return 0; + +#undef PREFER_EXPR +#undef PREFER_BOOL +} + +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) +{ + a2dp_opus_g_t conf; + struct spa_pod_frame f[1]; + uint32_t position[SPA_AUDIO_MAX_CHANNELS]; + int channels; + + if (caps_size < sizeof(conf)) + return -EINVAL; + + memcpy(&conf, caps, sizeof(conf)); + + if (idx > 0) + return 0; + + switch (OPUS_G_GET_CHANNELS(conf)) { + case OPUS_G_CHANNELS_STEREO: + channels = 2; + position[0] = SPA_AUDIO_CHANNEL_FL; + position[1] = SPA_AUDIO_CHANNEL_FR; + break; + case OPUS_G_CHANNELS_MONO: + channels = 1; + position[0] = SPA_AUDIO_CHANNEL_MONO; + break; + case OPUS_G_CHANNELS_MONO_2: + channels = 2; + position[0] = SPA_AUDIO_CHANNEL_AUX0; + position[1] = SPA_AUDIO_CHANNEL_AUX1; + break; + default: + return -EINVAL; + } + + 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_F32), + SPA_FORMAT_AUDIO_rate, SPA_POD_CHOICE_ENUM_Int(6, + 48000, 48000, 24000, 16000, 12000, 8000), + SPA_FORMAT_AUDIO_channels, SPA_POD_Int(channels), + SPA_FORMAT_AUDIO_position, SPA_POD_Array(sizeof(uint32_t), + SPA_TYPE_Id, channels, position), + 0); + + *param = spa_pod_builder_pop(b, &f[0]); + return *param == NULL ? -EIO : 1; +} + +static int codec_validate_config(const struct media_codec *codec, uint32_t flags, + const void *caps, size_t caps_size, + struct spa_audio_info *info) +{ + a2dp_opus_g_t conf; + + if (caps == NULL || caps_size < sizeof(conf)) + return -EINVAL; + + memcpy(&conf, caps, sizeof(conf)); + + spa_zero(*info); + info->media_type = SPA_MEDIA_TYPE_audio; + info->media_subtype = SPA_MEDIA_SUBTYPE_raw; + + info->info.raw.format = SPA_AUDIO_FORMAT_F32; + info->info.raw.rate = 0; /* not specified by config */ + + switch (OPUS_G_GET_FREQUENCY(conf)) { + case OPUS_G_FREQUENCY_48000: + break; + default: + return -EINVAL; + } + + switch (OPUS_G_GET_DURATION(conf)) { + case OPUS_G_DURATION_100: + case OPUS_G_DURATION_200: + break; + default: + return -EINVAL; + } + + switch (OPUS_G_GET_CHANNELS(conf)) { + case OPUS_G_CHANNELS_STEREO: + info->info.raw.channels = 2; + info->info.raw.position[0] = SPA_AUDIO_CHANNEL_FL; + info->info.raw.position[1] = SPA_AUDIO_CHANNEL_FR; + break; + case OPUS_G_CHANNELS_MONO: + info->info.raw.channels = 1; + info->info.raw.position[0] = SPA_AUDIO_CHANNEL_MONO; + break; + case OPUS_G_CHANNELS_MONO_2: + info->info.raw.channels = 2; + info->info.raw.position[0] = SPA_AUDIO_CHANNEL_AUX0; + info->info.raw.position[1] = SPA_AUDIO_CHANNEL_AUX1; + break; + default: + return -EINVAL; + } + + return 0; +} + +static int parse_frame_dms(int value) +{ + switch (value) { + case OPUS_G_DURATION_100: + return 100; + case OPUS_G_DURATION_200: + return 200; + default: + return -EINVAL; + } +} + +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) +{ + a2dp_opus_g_t conf; + struct impl *this = NULL; + struct spa_audio_info config_info; + int res; + + if (config_len < sizeof(conf)) { + res = -EINVAL; + goto error; + } + memcpy(&conf, config, sizeof(conf)); + + if (info->media_type != SPA_MEDIA_TYPE_audio || + info->media_subtype != SPA_MEDIA_SUBTYPE_raw || + info->info.raw.format != SPA_AUDIO_FORMAT_F32) { + res = -EINVAL; + goto error; + } + + if ((this = calloc(1, sizeof(struct impl))) == NULL) + goto error_errno; + + if ((res = codec_validate_config(codec, flags, config, config_len, &config_info)) < 0) + goto error; + if (config_info.info.raw.channels != info->info.raw.channels) { + res = -EINVAL; + goto error; + } + + this->mtu = mtu; + this->samplerate = info->info.raw.rate; + this->channels = config_info.info.raw.channels; + this->application = OPUS_APPLICATION_AUDIO; + + /* + * Setup encoder + */ + this->enc = opus_encoder_create(this->samplerate, this->channels, this->application, &res); + if (this->enc == NULL) { + res = -EINVAL; + goto error; + } + + if ((this->e.frame_dms = parse_frame_dms(OPUS_G_GET_DURATION(conf))) < 0) { + res = -EINVAL; + goto error; + } + + this->e.samples = this->e.frame_dms * this->samplerate / 10000; + this->e.codesize = this->e.samples * (int)this->channels * sizeof(float); + + int header_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload); + this->e.bitrate = SPA_MIN(128000 * this->channels, + (int64_t)8 * (this->mtu - header_size) * 10000 / this->e.frame_dms); + + opus_encoder_ctl(this->enc, OPUS_SET_BITRATE(this->e.bitrate)); + + /* + * Setup decoder + */ + this->dec = opus_decoder_create(this->samplerate, this->channels, &res); + if (this->dec == NULL) { + res = -EINVAL; + goto error; + } + + return this; + +error_errno: + res = -errno; + goto error; + +error: + if (this && this->enc) + opus_encoder_destroy(this->enc); + if (this && this->dec) + opus_decoder_destroy(this->dec); + free(this); + errno = -res; + return NULL; +} + +static void codec_deinit(void *data) +{ + struct impl *this = data; + + opus_encoder_destroy(this->enc); + opus_decoder_destroy(this->dec); + free(this); +} + +static int codec_get_block_size(void *data) +{ + struct impl *this = data; + return this->e.codesize; +} + +static int codec_start_encode (void *data, + void *dst, size_t dst_size, uint16_t seqnum, uint32_t timestamp) +{ + struct impl *this = data; + size_t header_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload); + + if (dst_size <= header_size) + return -EINVAL; + + this->e.header = (struct rtp_header *)dst; + this->e.payload = SPA_PTROFF(dst, sizeof(struct rtp_header), struct rtp_payload); + memset(dst, 0, header_size); + + this->e.payload->frame_count = 0; + this->e.header->v = 2; + this->e.header->pt = 96; + this->e.header->sequence_number = htons(seqnum); + this->e.header->timestamp = htonl(timestamp); + this->e.header->ssrc = htonl(1); + + this->e.packet_size = header_size; + return this->e.packet_size; +} + +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; + int res; + + if (src_size < (size_t)this->e.codesize) { + *dst_out = 0; + return 0; + } + if (this->e.packet_size >= this->mtu) + return -EINVAL; + + dst_size = SPA_MIN(dst_size, (size_t)(this->mtu - this->e.packet_size)); + + res = opus_encode_float(this->enc, src, this->e.samples, dst, dst_size); + if (res < 0) + return -EINVAL; + + *dst_out = res; + + this->e.packet_size += res; + this->e.payload->frame_count++; + + *need_flush = NEED_FLUSH_ALL; + + return this->e.codesize; +} + +static int codec_start_decode (void *data, + const void *src, size_t src_size, uint16_t *seqnum, uint32_t *timestamp) +{ + struct impl SPA_UNUSED *this = data; + const struct rtp_header *header = src; + const struct rtp_payload *payload = SPA_PTROFF(src, sizeof(struct rtp_header), void); + size_t header_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload); + + spa_return_val_if_fail (src_size > header_size, -EINVAL); + + if (seqnum) + *seqnum = ntohs(header->sequence_number); + if (timestamp) + *timestamp = ntohl(header->timestamp); + + if (payload->is_fragmented) + return -EINVAL; /* fragmentation not supported */ + if (payload->frame_count != 1) + return -EINVAL; /* wrong number of frames in packet */ + + return header_size; +} + +static int codec_decode(void *data, + const void *src, size_t src_size, + void *dst, size_t dst_size, + size_t *dst_out) +{ + struct impl SPA_UNUSED *this = data; + int consumed = src_size; + int res; + int dst_samples; + + dst_samples = dst_size / (sizeof(float) * this->channels); + res = opus_decode_float(this->dec, src, src_size, dst, dst_samples, 0); + if (res < 0) + return -EINVAL; + + *dst_out = (size_t)res * this->channels * sizeof(float); + + return consumed; +} + +static int codec_abr_process (void *data, size_t unsent) +{ + return -ENOTSUP; +} + +static int codec_reduce_bitpool(void *data) +{ + return 0; +} + +static int codec_increase_bitpool(void *data) +{ + return 0; +} + +static void codec_set_log(struct spa_log *global_log) +{ + log = global_log; + spa_log_topic_init(log, &codec_plugin_log_topic); +} + +const struct media_codec a2dp_codec_opus_g = { + .id = SPA_BLUETOOTH_AUDIO_CODEC_OPUS_G, + .codec_id = A2DP_CODEC_VENDOR, + .vendor = { .vendor_id = OPUS_G_VENDOR_ID, + .codec_id = OPUS_G_CODEC_ID }, + .select_config = codec_select_config, + .enum_config = codec_enum_config, + .validate_config = codec_validate_config, + .caps_preference_cmp = codec_caps_preference_cmp, + .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, + .start_decode = codec_start_decode, + .decode = codec_decode, + .name = "opus_g", + .description = "Opus", + .fill_caps = codec_fill_caps, +}; + +MEDIA_CODEC_EXPORT_DEF( + "opus-g", + &a2dp_codec_opus_g +); diff --git a/spa/plugins/bluez5/codec-loader.c b/spa/plugins/bluez5/codec-loader.c index e38c9c786..3b908779f 100644 --- a/spa/plugins/bluez5/codec-loader.c +++ b/spa/plugins/bluez5/codec-loader.c @@ -36,6 +36,7 @@ static int codec_order(const struct media_codec *c) SPA_BLUETOOTH_AUDIO_CODEC_APTX_HD, SPA_BLUETOOTH_AUDIO_CODEC_APTX, SPA_BLUETOOTH_AUDIO_CODEC_AAC, + SPA_BLUETOOTH_AUDIO_CODEC_OPUS_G, SPA_BLUETOOTH_AUDIO_CODEC_LC3PLUS_HR, SPA_BLUETOOTH_AUDIO_CODEC_MPEG, SPA_BLUETOOTH_AUDIO_CODEC_SBC, @@ -170,6 +171,7 @@ const struct media_codec * const *load_media_codecs(struct spa_plugin_loader *lo MEDIA_CODEC_FACTORY_LIB("sbc"), MEDIA_CODEC_FACTORY_LIB("lc3plus"), MEDIA_CODEC_FACTORY_LIB("opus"), + MEDIA_CODEC_FACTORY_LIB("opus-g"), MEDIA_CODEC_FACTORY_LIB("lc3") #undef MEDIA_CODEC_FACTORY_LIB }; diff --git a/spa/plugins/bluez5/media-sink.c b/spa/plugins/bluez5/media-sink.c index fb2544225..546bc7f01 100644 --- a/spa/plugins/bluez5/media-sink.c +++ b/spa/plugins/bluez5/media-sink.c @@ -1228,8 +1228,11 @@ static int transport_start(struct impl *this) &port->current_format, this->codec_props, this->transport->write_mtu); - if (this->codec_data == NULL) + if (this->codec_data == NULL) { + spa_log_error(this->log, "%p: codec %s initialization failed", this, + this->codec->description); return -EIO; + } } else { this->own_codec_data = false; this->codec_data = this->transport->iso_io->codec_data; diff --git a/spa/plugins/bluez5/meson.build b/spa/plugins/bluez5/meson.build index 745448e42..035e4b7ab 100644 --- a/spa/plugins/bluez5/meson.build +++ b/spa/plugins/bluez5/meson.build @@ -153,6 +153,14 @@ if get_option('bluez5-codec-opus').allowed() and opus_dep.found() dependencies : [ spa_dep, opus_dep, mathlib ], install : true, install_dir : spa_plugindir / 'bluez5') + + bluez_codec_opus_g = shared_library('spa-codec-bluez5-opus-g', + [ 'a2dp-codec-opus-g.c', 'media-codecs.c' ], + include_directories : [ configinc ], + c_args : opus_args, + dependencies : [ spa_dep, opus_dep, mathlib ], + install : true, + install_dir : spa_plugindir / 'bluez5') endif if get_option('bluez5-codec-lc3').allowed() and lc3_dep.found()