1/***
2  This file is part of PulseAudio.
3
4  Copyright (C) 2020 Asymptotic <sanchayan@asymptotic.io>
5
6  PulseAudio is free software; you can redistribute it and/or modify
7  it under the terms of the GNU Lesser General Public License as
8  published by the Free Software Foundation; either version 2.1 of the
9  License, or (at your option) any later version.
10
11  PulseAudio is distributed in the hope that it will be useful, but
12  WITHOUT ANY WARRANTY; without even the implied warranty of
13  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
14  General Public License for more details.
15
16  You should have received a copy of the GNU Lesser General Public
17  License along with PulseAudio; if not, see <http://www.gnu.org/licenses/>.
18***/
19
20#ifdef HAVE_CONFIG_H
21#include <config.h>
22#endif
23
24#include <pulsecore/log.h>
25#include <pulsecore/macro.h>
26#include <pulsecore/once.h>
27#include <pulse/sample.h>
28
29#include <arpa/inet.h>
30
31#include "a2dp-codecs.h"
32#include "a2dp-codec-api.h"
33#include "a2dp-codec-gst.h"
34#include "rtp.h"
35
36static bool can_be_supported(bool for_encoding) {
37    GstElementFactory *element_factory;
38
39    if (!for_encoding)
40        return false;
41
42    element_factory = gst_element_factory_find("ldacenc");
43    if (element_factory == NULL) {
44        pa_log_info("LDAC encoder element `ldacenc` not found");
45        return false;
46    }
47    gst_object_unref(element_factory);
48
49    return true;
50}
51
52static bool can_accept_capabilities_common(const a2dp_ldac_t *capabilities, uint32_t vendor_id, uint16_t codec_id) {
53    if (A2DP_GET_VENDOR_ID(capabilities->info) != vendor_id || A2DP_GET_CODEC_ID(capabilities->info) != codec_id)
54        return false;
55
56    if (!(capabilities->frequency & (LDAC_SAMPLING_FREQ_44100 | LDAC_SAMPLING_FREQ_48000 |
57                                     LDAC_SAMPLING_FREQ_88200 | LDAC_SAMPLING_FREQ_96000)))
58        return false;
59
60    if (!(capabilities->channel_mode & LDAC_CHANNEL_MODE_STEREO))
61        return false;
62
63    return true;
64}
65
66static bool can_accept_capabilities(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
67    const a2dp_ldac_t *capabilities = (const a2dp_ldac_t *) capabilities_buffer;
68
69    if (capabilities_size != sizeof(*capabilities))
70        return false;
71
72    return can_accept_capabilities_common(capabilities, LDAC_VENDOR_ID, LDAC_CODEC_ID);
73}
74
75static const char *choose_remote_endpoint(const pa_hashmap *capabilities_hashmap, const pa_sample_spec *default_sample_spec, bool for_encoding) {
76    const pa_a2dp_codec_capabilities *a2dp_capabilities;
77    const char *key;
78    void *state;
79
80    /* There is no preference, just choose random valid entry */
81    PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
82        if (can_accept_capabilities(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
83            return key;
84    }
85
86    return NULL;
87}
88
89static void fill_capabilities_common(a2dp_ldac_t *capabilities, uint32_t vendor_id, uint16_t codec_id) {
90    capabilities->info = A2DP_SET_VENDOR_ID_CODEC_ID(vendor_id, codec_id);
91    capabilities->channel_mode = LDAC_CHANNEL_MODE_STEREO;
92    capabilities->frequency = LDAC_SAMPLING_FREQ_44100 | LDAC_SAMPLING_FREQ_48000 |
93                              LDAC_SAMPLING_FREQ_88200 | LDAC_SAMPLING_FREQ_96000;
94}
95
96static uint8_t fill_capabilities(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
97    a2dp_ldac_t *capabilities = (a2dp_ldac_t *) capabilities_buffer;
98
99    pa_zero(*capabilities);
100    fill_capabilities_common(capabilities, LDAC_VENDOR_ID, LDAC_CODEC_ID);
101    return sizeof(*capabilities);
102}
103
104static bool is_configuration_valid(const uint8_t *config_buffer, uint8_t config_size) {
105    const a2dp_ldac_t *config = (const a2dp_ldac_t *) config_buffer;
106
107    if (config_size != sizeof(*config)) {
108        pa_log_error("Invalid size of config buffer");
109        return false;
110    }
111
112    if (A2DP_GET_VENDOR_ID(config->info) != LDAC_VENDOR_ID || A2DP_GET_CODEC_ID(config->info) != LDAC_CODEC_ID) {
113        pa_log_error("Invalid vendor codec information in configuration");
114        return false;
115    }
116
117    if (config->frequency != LDAC_SAMPLING_FREQ_44100 && config->frequency != LDAC_SAMPLING_FREQ_48000 &&
118        config->frequency != LDAC_SAMPLING_FREQ_88200 && config->frequency != LDAC_SAMPLING_FREQ_96000) {
119        pa_log_error("Invalid sampling frequency in configuration");
120        return false;
121    }
122
123    if (config->channel_mode != LDAC_CHANNEL_MODE_STEREO) {
124        pa_log_error("Invalid channel mode in configuration");
125        return false;
126    }
127
128    return true;
129}
130
131static int fill_preferred_configuration_common(const pa_sample_spec *default_sample_spec, const a2dp_ldac_t *capabilities, a2dp_ldac_t *config, uint32_t vendor_id, uint16_t codec_id) {
132    int i;
133
134    static const struct {
135        uint32_t rate;
136        uint8_t cap;
137    } freq_table[] = {
138        { 44100U, LDAC_SAMPLING_FREQ_44100 },
139        { 48000U, LDAC_SAMPLING_FREQ_48000 },
140        { 88200U, LDAC_SAMPLING_FREQ_88200 },
141        { 96000U, LDAC_SAMPLING_FREQ_96000 }
142    };
143
144    if (A2DP_GET_VENDOR_ID(capabilities->info) != LDAC_VENDOR_ID || A2DP_GET_CODEC_ID(capabilities->info) != LDAC_CODEC_ID) {
145        pa_log_error("No supported vendor codec information");
146        return -1;
147    }
148
149    config->info = A2DP_SET_VENDOR_ID_CODEC_ID(vendor_id, codec_id);
150
151    if (!(capabilities->channel_mode & LDAC_CHANNEL_MODE_STEREO)) {
152        pa_log_error("No supported channel modes");
153        return -1;
154    }
155
156    config->channel_mode = LDAC_CHANNEL_MODE_STEREO;
157
158    /* Find the lowest freq that is at least as high as the requested sampling rate */
159    for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++) {
160        if (freq_table[i].rate >= default_sample_spec->rate && (capabilities->frequency & freq_table[i].cap)) {
161            config->frequency = freq_table[i].cap;
162            break;
163        }
164    }
165
166    if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
167        for (--i; i >= 0; i--) {
168            if (capabilities->frequency & freq_table[i].cap) {
169                config->frequency = freq_table[i].cap;
170                break;
171            }
172        }
173
174        if (i < 0) {
175            pa_log_error("Not suitable sample rate");
176            return false;
177        }
178    }
179
180    return 0;
181}
182
183static uint8_t fill_preferred_configuration(const pa_sample_spec *default_sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[MAX_A2DP_CAPS_SIZE]) {
184    a2dp_ldac_t *config = (a2dp_ldac_t *) config_buffer;
185    const a2dp_ldac_t *capabilities = (const a2dp_ldac_t *) capabilities_buffer;
186
187    if (capabilities_size != sizeof(*capabilities)) {
188        pa_log_error("Invalid size of capabilities buffer");
189        return 0;
190    }
191
192    pa_zero(*config);
193
194    if (fill_preferred_configuration_common(default_sample_spec, capabilities, config, LDAC_VENDOR_ID, LDAC_CODEC_ID) < 0)
195        return 0;
196
197    return sizeof(*config);
198}
199
200GstElement *gst_init_ldac(struct gst_info *info, pa_sample_spec *ss, bool for_encoding) {
201    GstElement *bin;
202    GstElement *enc;
203    GstPad *pad;
204
205    if (!for_encoding) {
206        pa_log_error("LDAC does not support decoding");
207        return NULL;
208    }
209
210    ss->format = PA_SAMPLE_FLOAT32LE;
211
212    switch (info->a2dp_codec_t.ldac_config->frequency) {
213        case LDAC_SAMPLING_FREQ_44100:
214            ss->rate = 44100u;
215            break;
216        case LDAC_SAMPLING_FREQ_48000:
217            ss->rate = 48000u;
218            break;
219        case LDAC_SAMPLING_FREQ_88200:
220            ss->rate = 88200;
221            break;
222        case LDAC_SAMPLING_FREQ_96000:
223            ss->rate = 96000;
224            break;
225        default:
226            pa_log_error("LDAC invalid frequency %d", info->a2dp_codec_t.ldac_config->frequency);
227            goto fail;
228    }
229
230    switch (info->a2dp_codec_t.ldac_config->channel_mode) {
231        case LDAC_CHANNEL_MODE_STEREO:
232            ss->channels = 2;
233            break;
234        case LDAC_CHANNEL_MODE_MONO:
235            ss->channels = 1;
236            break;
237        case LDAC_CHANNEL_MODE_DUAL:
238            ss->channels = 1;
239            break;
240        default:
241            pa_log_error("LDAC invalid channel mode %d", info->a2dp_codec_t.ldac_config->channel_mode);
242            goto fail;
243    }
244
245    enc = gst_element_factory_make("ldacenc", "ldac_enc");
246    if (!enc) {
247        pa_log_error("Could not create LDAC encoder element");
248        goto fail;
249    }
250
251    switch (info->codec_type) {
252        case LDAC_EQMID_HQ:
253            g_object_set(enc, "eqmid", 0, NULL);
254            break;
255        case LDAC_EQMID_SQ:
256            g_object_set(enc, "eqmid", 1, NULL);
257            break;
258        case LDAC_EQMID_MQ:
259            g_object_set(enc, "eqmid", 2, NULL);
260            break;
261        default:
262            goto fail;
263    }
264
265    bin = gst_bin_new("ldac_enc_bin");
266    pa_assert(bin);
267
268    gst_bin_add_many(GST_BIN(bin), enc, NULL);
269
270    pad = gst_element_get_static_pad(enc, "sink");
271    pa_assert_se(gst_element_add_pad(bin, gst_ghost_pad_new("sink", pad)));
272    gst_object_unref(GST_OBJECT(pad));
273
274    pad = gst_element_get_static_pad(enc, "src");
275    pa_assert_se(gst_element_add_pad(bin, gst_ghost_pad_new("src", pad)));
276    gst_object_unref(GST_OBJECT(pad));
277
278    return bin;
279
280fail:
281    pa_log_error("LDAC encoder initialisation failed");
282    return NULL;
283}
284
285static void *init_common(enum a2dp_codec_type codec_type, bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
286    GstElement *bin;
287    struct gst_info *info = NULL;
288
289    if (!for_encoding) {
290        pa_log_error("LDAC decoder not supported");
291        return NULL;
292    }
293
294    info = pa_xnew0(struct gst_info, 1);
295    pa_assert(info);
296
297    info->core = core;
298    info->ss = sample_spec;
299
300    info->codec_type = codec_type;
301    info->a2dp_codec_t.ldac_config = (const a2dp_ldac_t *) config_buffer;
302    pa_assert(config_size == sizeof(*(info->a2dp_codec_t.ldac_config)));
303
304    if (!(bin = gst_init_ldac(info, sample_spec, for_encoding)))
305        goto fail;
306
307    if (!gst_codec_init(info, for_encoding, bin))
308        goto fail;
309
310    return info;
311
312fail:
313    if (info)
314        pa_xfree(info);
315
316    return NULL;
317}
318
319static void *init_hq(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
320    return init_common(LDAC_EQMID_HQ, for_encoding, for_backchannel, config_buffer, config_size, sample_spec, core);
321}
322
323static void *init_sq(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
324    return init_common(LDAC_EQMID_SQ, for_encoding, for_backchannel, config_buffer, config_size, sample_spec, core);
325}
326
327static void *init_mq(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
328    return init_common(LDAC_EQMID_MQ, for_encoding, for_backchannel, config_buffer, config_size, sample_spec, core);
329}
330
331static void deinit(void *codec_info) {
332    return gst_codec_deinit(codec_info);
333}
334
335static int reset(void *codec_info) {
336    return 0;
337}
338
339static uint32_t get_ldac_num_samples(void *codec_info) {
340    struct gst_info *info = (struct gst_info *) codec_info;
341
342    switch (info->a2dp_codec_t.ldac_config->frequency) {
343        case LDAC_SAMPLING_FREQ_44100:
344        case LDAC_SAMPLING_FREQ_48000:
345            return 128;
346            break;
347        case LDAC_SAMPLING_FREQ_88200:
348        case LDAC_SAMPLING_FREQ_96000:
349            return 256;
350            break;
351        default:
352            break;
353    }
354
355    return 128;
356}
357
358static uint8_t get_ldac_num_frames(void *codec_info, enum a2dp_codec_type codec_type) {
359    struct gst_info *info = (struct gst_info *) codec_info;
360    uint8_t channels;
361
362    switch (info->a2dp_codec_t.ldac_config->channel_mode) {
363        case LDAC_CHANNEL_MODE_STEREO:
364            channels = 2;
365            break;
366        case LDAC_CHANNEL_MODE_MONO:
367        case LDAC_CHANNEL_MODE_DUAL:
368            channels = 1;
369            break;
370        default:
371            break;
372    }
373
374    switch (codec_type) {
375        case LDAC_EQMID_HQ:
376            return 4 / channels;
377        case LDAC_EQMID_SQ:
378            return 6 / channels;
379        case LDAC_EQMID_MQ:
380            return 12 / channels;
381        default:
382            break;
383    }
384
385    return 6 / channels;
386}
387
388static size_t get_block_size(void *codec_info, size_t link_mtu) {
389    struct gst_info *info = (struct gst_info *) codec_info;
390
391    return get_ldac_num_samples(codec_info) * get_ldac_num_frames(codec_info, info->codec_type) * pa_frame_size(info->ss);
392}
393
394static size_t get_encoded_block_size(void *codec_info, size_t input_size) {
395    /* encoded block size is not exactly known, report input_size */
396    return input_size;
397}
398
399static size_t reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
400    return 0;
401}
402
403static 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) {
404    struct gst_info *info = (struct gst_info *) codec_info;
405    struct rtp_header *header;
406    struct rtp_payload *payload;
407    size_t written;
408
409    if (PA_UNLIKELY(output_size < sizeof(*header) + sizeof(*payload))) {
410        *processed = 0;
411        return 0;
412    }
413
414    written = gst_transcode_buffer(codec_info, timestamp, input_buffer, input_size, output_buffer + sizeof(*header) + sizeof(*payload), output_size - sizeof(*header) - sizeof(*payload), processed);
415    if (PA_UNLIKELY(*processed != input_size))
416        pa_log_error("LDAC encoding error");
417
418    if (PA_LIKELY(written > 0)) {
419        header = (struct rtp_header *) output_buffer;
420        pa_zero(*header);
421        header->v = 2;
422        header->pt = 96;
423        header->sequence_number = htons(info->seq_num++);
424        header->timestamp = htonl(timestamp);
425        header->ssrc = htonl(1);
426        payload = (struct rtp_payload*) (output_buffer + sizeof(*header));
427        payload->frame_count = get_ldac_num_frames(codec_info, info->codec_type);
428        written += sizeof(*header) + sizeof(*payload);
429    }
430
431    return written;
432}
433
434const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_ldac_eqmid_hq = {
435    .id = { A2DP_CODEC_VENDOR, LDAC_VENDOR_ID, LDAC_CODEC_ID },
436    .support_backchannel = false,
437    .can_be_supported = can_be_supported,
438    .can_accept_capabilities = can_accept_capabilities,
439    .choose_remote_endpoint = choose_remote_endpoint,
440    .fill_capabilities = fill_capabilities,
441    .is_configuration_valid = is_configuration_valid,
442    .fill_preferred_configuration = fill_preferred_configuration,
443    .bt_codec = {
444        .name = "ldac_hq",
445        .description = "LDAC (High Quality)",
446        .init = init_hq,
447        .deinit = deinit,
448        .reset = reset,
449        .get_read_block_size = get_block_size,
450        .get_write_block_size = get_block_size,
451        .get_encoded_block_size = get_encoded_block_size,
452        .reduce_encoder_bitrate = reduce_encoder_bitrate,
453        .encode_buffer = encode_buffer,
454    },
455};
456
457const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_ldac_eqmid_sq = {
458    .id = { A2DP_CODEC_VENDOR, LDAC_VENDOR_ID, LDAC_CODEC_ID },
459    .support_backchannel = false,
460    .can_be_supported = can_be_supported,
461    .can_accept_capabilities = can_accept_capabilities,
462    .choose_remote_endpoint = choose_remote_endpoint,
463    .fill_capabilities = fill_capabilities,
464    .is_configuration_valid = is_configuration_valid,
465    .fill_preferred_configuration = fill_preferred_configuration,
466    .bt_codec = {
467        .name = "ldac_sq",
468        .description = "LDAC (Standard Quality)",
469        .init = init_sq,
470        .deinit = deinit,
471        .reset = reset,
472        .get_read_block_size = get_block_size,
473        .get_write_block_size = get_block_size,
474        .get_encoded_block_size = get_encoded_block_size,
475        .reduce_encoder_bitrate = reduce_encoder_bitrate,
476        .encode_buffer = encode_buffer,
477    },
478};
479
480const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_ldac_eqmid_mq = {
481    .id = { A2DP_CODEC_VENDOR, LDAC_VENDOR_ID, LDAC_CODEC_ID },
482    .support_backchannel = false,
483    .can_be_supported = can_be_supported,
484    .can_accept_capabilities = can_accept_capabilities,
485    .choose_remote_endpoint = choose_remote_endpoint,
486    .fill_capabilities = fill_capabilities,
487    .is_configuration_valid = is_configuration_valid,
488    .fill_preferred_configuration = fill_preferred_configuration,
489    .bt_codec = {
490        .name = "ldac_mq",
491        .description = "LDAC (Mobile Quality)",
492        .init = init_mq,
493        .deinit = deinit,
494        .reset = reset,
495        .get_read_block_size = get_block_size,
496        .get_write_block_size = get_block_size,
497        .get_encoded_block_size = get_encoded_block_size,
498        .reduce_encoder_bitrate = reduce_encoder_bitrate,
499        .encode_buffer = encode_buffer,
500    },
501};
502