1 /***
2 This file is part of PulseAudio.
3
4 Copyright 2018-2019 Pali Rohár <pali.rohar@gmail.com>
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/core-util.h>
25 #include <pulsecore/log.h>
26 #include <pulsecore/macro.h>
27 #include <pulsecore/once.h>
28 #include <pulse/sample.h>
29 #include <pulse/xmalloc.h>
30
31 #include <arpa/inet.h>
32
33 #include <sbc/sbc.h>
34
35 #include "a2dp-codecs.h"
36 #include "a2dp-codec-api.h"
37 #include "rtp.h"
38
39 #define SBC_BITPOOL_DEC_STEP 5
40 #define SBC_BITPOOL_INC_STEP 1
41
42 #define SBC_SYNCWORD 0x9C
43
44 struct sbc_info {
45 sbc_t sbc; /* Codec data */
46 size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */
47 uint16_t seq_num; /* Cumulative packet sequence */
48 uint8_t frequency;
49 uint8_t blocks;
50 uint8_t subbands;
51 uint8_t mode;
52 uint8_t allocation;
53 uint8_t initial_bitpool;
54 uint8_t min_bitpool;
55 uint8_t max_bitpool;
56
57 uint8_t nr_blocks;
58 uint8_t nr_subbands;
59
60 bool boost_source_volume;
61 /* Size of SBC frame fragment left over from previous decoding iteration */
62 size_t frame_fragment_size;
63 /* Maximum SBC frame size is 512 bytes when SBC compression ratio > 1 */
64 uint8_t frame_fragment[512];
65 };
66
can_be_supported(bool for_encoding)67 static bool can_be_supported(bool for_encoding) {
68 return true;
69 }
70
can_accept_capabilities(const uint8_t * capabilities_buffer,uint8_t capabilities_size,bool for_encoding)71 static bool can_accept_capabilities(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
72 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
73
74 if (capabilities_size != sizeof(*capabilities))
75 return false;
76
77 if (!(capabilities->frequency & (SBC_SAMPLING_FREQ_16000 | SBC_SAMPLING_FREQ_32000 | SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000)))
78 return false;
79
80 if (!(capabilities->channel_mode & (SBC_CHANNEL_MODE_MONO | SBC_CHANNEL_MODE_DUAL_CHANNEL | SBC_CHANNEL_MODE_STEREO | SBC_CHANNEL_MODE_JOINT_STEREO)))
81 return false;
82
83 if (!(capabilities->allocation_method & (SBC_ALLOCATION_SNR | SBC_ALLOCATION_LOUDNESS)))
84 return false;
85
86 if (!(capabilities->subbands & (SBC_SUBBANDS_4 | SBC_SUBBANDS_8)))
87 return false;
88
89 if (!(capabilities->block_length & (SBC_BLOCK_LENGTH_4 | SBC_BLOCK_LENGTH_8 | SBC_BLOCK_LENGTH_12 | SBC_BLOCK_LENGTH_16)))
90 return false;
91
92 return true;
93 }
94
can_accept_capabilities_xq(const uint8_t * capabilities_buffer,uint8_t capabilities_size,bool for_encoding)95 static bool can_accept_capabilities_xq(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
96 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
97
98 if (capabilities_size != sizeof(*capabilities))
99 return false;
100
101 if (!(capabilities->frequency & (SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000)))
102 return false;
103
104 if (!(capabilities->channel_mode & (SBC_CHANNEL_MODE_DUAL_CHANNEL)))
105 return false;
106
107 if (!(capabilities->allocation_method & (SBC_ALLOCATION_LOUDNESS)))
108 return false;
109
110 if (!(capabilities->subbands & (SBC_SUBBANDS_8)))
111 return false;
112
113 if (!(capabilities->block_length & (SBC_BLOCK_LENGTH_16)))
114 return false;
115
116 return true;
117 }
118
can_accept_capabilities_faststream(const uint8_t * capabilities_buffer,uint8_t capabilities_size,bool for_encoding)119 static bool can_accept_capabilities_faststream(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
120 const a2dp_faststream_t *capabilities = (const a2dp_faststream_t *) capabilities_buffer;
121
122 if (capabilities_size != sizeof(*capabilities))
123 return false;
124
125 if (!(capabilities->direction & (FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE)))
126 return false;
127
128 if (!(capabilities->sink_frequency & (FASTSTREAM_SINK_SAMPLING_FREQ_44100 | FASTSTREAM_SINK_SAMPLING_FREQ_48000)))
129 return false;
130
131 if (!(capabilities->source_frequency & FASTSTREAM_SOURCE_SAMPLING_FREQ_16000))
132 return false;
133
134 return true;
135 }
136
choose_remote_endpoint(const pa_hashmap * capabilities_hashmap,const pa_sample_spec * default_sample_spec,bool for_encoding)137 static const char *choose_remote_endpoint(const pa_hashmap *capabilities_hashmap, const pa_sample_spec *default_sample_spec, bool for_encoding) {
138 const pa_a2dp_codec_capabilities *a2dp_capabilities;
139 const char *key;
140 void *state;
141
142 /* There is no preference, just choose random valid entry */
143 PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
144 if (can_accept_capabilities(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
145 return key;
146 }
147
148 return NULL;
149 }
150
choose_remote_endpoint_xq(const pa_hashmap * capabilities_hashmap,const pa_sample_spec * default_sample_spec,bool for_encoding)151 static const char *choose_remote_endpoint_xq(const pa_hashmap *capabilities_hashmap, const pa_sample_spec *default_sample_spec, bool for_encoding) {
152 const pa_a2dp_codec_capabilities *a2dp_capabilities;
153 const char *key;
154 void *state;
155
156 /* There is no preference, just choose random valid entry */
157 PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
158 if (can_accept_capabilities_xq(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
159 return key;
160 }
161
162 return NULL;
163 }
164
choose_remote_endpoint_faststream(const pa_hashmap * capabilities_hashmap,const pa_sample_spec * default_sample_spec,bool for_encoding)165 static const char *choose_remote_endpoint_faststream(const pa_hashmap *capabilities_hashmap, const pa_sample_spec *default_sample_spec, bool for_encoding) {
166 const pa_a2dp_codec_capabilities *a2dp_capabilities;
167 const char *key;
168 void *state;
169
170 /* There is no preference, just choose random valid entry */
171 PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
172 pa_log_debug("choose_remote_endpoint_faststream checking peer endpoint '%s'", key);
173 if (can_accept_capabilities_faststream(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
174 return key;
175 }
176
177 pa_log_debug("choose_remote_endpoint_faststream matched no peer endpoint");
178
179 return NULL;
180 }
181
fill_capabilities(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])182 static uint8_t fill_capabilities(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
183 a2dp_sbc_t *capabilities = (a2dp_sbc_t *) capabilities_buffer;
184
185 pa_zero(*capabilities);
186
187 capabilities->channel_mode = SBC_CHANNEL_MODE_MONO | SBC_CHANNEL_MODE_DUAL_CHANNEL | SBC_CHANNEL_MODE_STEREO |
188 SBC_CHANNEL_MODE_JOINT_STEREO;
189 capabilities->frequency = SBC_SAMPLING_FREQ_16000 | SBC_SAMPLING_FREQ_32000 | SBC_SAMPLING_FREQ_44100 |
190 SBC_SAMPLING_FREQ_48000;
191 capabilities->allocation_method = SBC_ALLOCATION_SNR | SBC_ALLOCATION_LOUDNESS;
192 capabilities->subbands = SBC_SUBBANDS_4 | SBC_SUBBANDS_8;
193 capabilities->block_length = SBC_BLOCK_LENGTH_4 | SBC_BLOCK_LENGTH_8 | SBC_BLOCK_LENGTH_12 | SBC_BLOCK_LENGTH_16;
194 capabilities->min_bitpool = SBC_MIN_BITPOOL;
195 capabilities->max_bitpool = SBC_BITPOOL_HQ_JOINT_STEREO_44100;
196
197 return sizeof(*capabilities);
198 }
199
set_info_and_sample_spec_from_sbc_config(struct sbc_info * sbc_info,pa_sample_spec * sample_spec,const a2dp_sbc_t * config)200 static void set_info_and_sample_spec_from_sbc_config(struct sbc_info *sbc_info, pa_sample_spec *sample_spec, const a2dp_sbc_t *config) {
201 switch (config->frequency) {
202 case SBC_SAMPLING_FREQ_16000:
203 sbc_info->frequency = SBC_FREQ_16000;
204 sample_spec->rate = 16000U;
205 break;
206 case SBC_SAMPLING_FREQ_32000:
207 sbc_info->frequency = SBC_FREQ_32000;
208 sample_spec->rate = 32000U;
209 break;
210 case SBC_SAMPLING_FREQ_44100:
211 sbc_info->frequency = SBC_FREQ_44100;
212 sample_spec->rate = 44100U;
213 break;
214 case SBC_SAMPLING_FREQ_48000:
215 sbc_info->frequency = SBC_FREQ_48000;
216 sample_spec->rate = 48000U;
217 break;
218 default:
219 pa_assert_not_reached();
220 }
221
222 switch (config->channel_mode) {
223 case SBC_CHANNEL_MODE_MONO:
224 sbc_info->mode = SBC_MODE_MONO;
225 sample_spec->channels = 1;
226 break;
227 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
228 sbc_info->mode = SBC_MODE_DUAL_CHANNEL;
229 sample_spec->channels = 2;
230 break;
231 case SBC_CHANNEL_MODE_STEREO:
232 sbc_info->mode = SBC_MODE_STEREO;
233 sample_spec->channels = 2;
234 break;
235 case SBC_CHANNEL_MODE_JOINT_STEREO:
236 sbc_info->mode = SBC_MODE_JOINT_STEREO;
237 sample_spec->channels = 2;
238 break;
239 default:
240 pa_assert_not_reached();
241 }
242
243 switch (config->allocation_method) {
244 case SBC_ALLOCATION_SNR:
245 sbc_info->allocation = SBC_AM_SNR;
246 break;
247 case SBC_ALLOCATION_LOUDNESS:
248 sbc_info->allocation = SBC_AM_LOUDNESS;
249 break;
250 default:
251 pa_assert_not_reached();
252 }
253
254 switch (config->subbands) {
255 case SBC_SUBBANDS_4:
256 sbc_info->subbands = SBC_SB_4;
257 sbc_info->nr_subbands = 4;
258 break;
259 case SBC_SUBBANDS_8:
260 sbc_info->subbands = SBC_SB_8;
261 sbc_info->nr_subbands = 8;
262 break;
263 default:
264 pa_assert_not_reached();
265 }
266
267 switch (config->block_length) {
268 case SBC_BLOCK_LENGTH_4:
269 sbc_info->blocks = SBC_BLK_4;
270 sbc_info->nr_blocks = 4;
271 break;
272 case SBC_BLOCK_LENGTH_8:
273 sbc_info->blocks = SBC_BLK_8;
274 sbc_info->nr_blocks = 8;
275 break;
276 case SBC_BLOCK_LENGTH_12:
277 sbc_info->blocks = SBC_BLK_12;
278 sbc_info->nr_blocks = 12;
279 break;
280 case SBC_BLOCK_LENGTH_16:
281 sbc_info->blocks = SBC_BLK_16;
282 sbc_info->nr_blocks = 16;
283 break;
284 default:
285 pa_assert_not_reached();
286 }
287
288 sbc_info->min_bitpool = config->min_bitpool;
289 sbc_info->max_bitpool = config->max_bitpool;
290 }
291
set_params(struct sbc_info * sbc_info)292 static void set_params(struct sbc_info *sbc_info) {
293 sbc_info->sbc.frequency = sbc_info->frequency;
294 sbc_info->sbc.blocks = sbc_info->blocks;
295 sbc_info->sbc.subbands = sbc_info->subbands;
296 sbc_info->sbc.mode = sbc_info->mode;
297 sbc_info->sbc.allocation = sbc_info->allocation;
298 sbc_info->sbc.bitpool = sbc_info->initial_bitpool;
299 sbc_info->sbc.endian = SBC_LE;
300
301 sbc_info->codesize = sbc_get_codesize(&sbc_info->sbc);
302 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
303 }
304
sbc_get_max_bitpool_below_rate(a2dp_sbc_t * config,uint8_t lower_bound,uint8_t upper_bound,uint32_t bitrate_cap)305 static uint8_t sbc_get_max_bitpool_below_rate(a2dp_sbc_t *config, uint8_t lower_bound, uint8_t upper_bound, uint32_t bitrate_cap) {
306 pa_sample_spec sample_spec;
307 struct sbc_info sbc_info;
308 int ret;
309
310 pa_assert(config);
311
312 ret = sbc_init(&sbc_info.sbc, 0);
313 if (ret != 0) {
314 pa_log_error("SBC initialization failed: %d", ret);
315 return lower_bound;
316 }
317
318 set_info_and_sample_spec_from_sbc_config(&sbc_info, &sample_spec, config);
319
320 while (upper_bound - lower_bound > 1) {
321 size_t midpoint = (upper_bound + lower_bound) / 2;
322
323 sbc_info.initial_bitpool = midpoint;
324 set_params(&sbc_info);
325
326 size_t bitrate = sbc_info.frame_length * 8 * sample_spec.rate / (sbc_info.nr_subbands * sbc_info.nr_blocks);
327
328 if (bitrate > bitrate_cap)
329 upper_bound = midpoint;
330 else
331 lower_bound = midpoint;
332 }
333
334 sbc_finish(&sbc_info.sbc);
335
336 pa_log_debug("SBC target bitrate %u bitpool %u sample rate %u", bitrate_cap, lower_bound, sample_spec.rate);
337
338 return lower_bound;
339 }
340
341 /* SBC XQ
342 *
343 * References:
344 * https://habr.com/en/post/456476/
345 * http://soundexpert.org/articles/-/blogs/audio-quality-of-sbc-xq-bluetooth-audio-codec
346 *
347 */
fill_capabilities_xq(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE],uint32_t bitrate_cap)348 static uint8_t fill_capabilities_xq(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE], uint32_t bitrate_cap) {
349 a2dp_sbc_t *capabilities = (a2dp_sbc_t *) capabilities_buffer;
350
351 pa_zero(*capabilities);
352
353 /* Bitpool value increases with sample rate. Prepare to calculate maximum viable
354 * bitpool value at specified bitrate_cap, with rest of SBC parameters fixed. */
355 capabilities->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
356 capabilities->frequency = SBC_SAMPLING_FREQ_48000;
357 capabilities->allocation_method = SBC_ALLOCATION_LOUDNESS;
358 capabilities->subbands = SBC_SUBBANDS_8;
359 capabilities->block_length = SBC_BLOCK_LENGTH_16;
360 capabilities->min_bitpool = SBC_MIN_BITPOOL;
361 capabilities->max_bitpool = SBC_MAX_BITPOOL; /* Upper boundary in calculation below. */
362
363 /* Now calculate and write it back to be exposed through endpoint capabilities. */
364 capabilities->max_bitpool = sbc_get_max_bitpool_below_rate(capabilities, capabilities->min_bitpool, capabilities->max_bitpool, bitrate_cap);
365
366 /* Add back all supported frequencies exposed through endpoint capabilities, rest of SBC parameters are still fixed. */
367 capabilities->frequency = SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000;
368
369 return sizeof(*capabilities);
370 }
371
fill_capabilities_faststream(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])372 static uint8_t fill_capabilities_faststream(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
373 a2dp_faststream_t *capabilities = (a2dp_faststream_t *) capabilities_buffer;
374
375 pa_zero(*capabilities);
376
377 capabilities->info = A2DP_SET_VENDOR_ID_CODEC_ID(FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID);
378
379 capabilities->direction = FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE;
380 capabilities->sink_frequency = FASTSTREAM_SINK_SAMPLING_FREQ_44100 | FASTSTREAM_SINK_SAMPLING_FREQ_48000;
381 capabilities->source_frequency = FASTSTREAM_SOURCE_SAMPLING_FREQ_16000;
382
383 return sizeof(*capabilities);
384 }
385
is_configuration_valid_faststream(const uint8_t * config_buffer,uint8_t config_size)386 static bool is_configuration_valid_faststream(const uint8_t *config_buffer, uint8_t config_size) {
387 const a2dp_faststream_t *config = (const a2dp_faststream_t *) config_buffer;
388
389 if (config_size != sizeof(*config)) {
390 pa_log_error("Invalid size of config buffer");
391 return false;
392 }
393
394 if (!(config->direction & (FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE))) {
395 pa_log_error("Invalid FastStream direction in configuration");
396 return false;
397 }
398
399 if (config->sink_frequency != FASTSTREAM_SINK_SAMPLING_FREQ_44100 && config->sink_frequency != FASTSTREAM_SINK_SAMPLING_FREQ_48000) {
400 pa_log_error("Invalid FastStream sink sampling frequency in configuration");
401 return false;
402 }
403
404 if (config->source_frequency != FASTSTREAM_SOURCE_SAMPLING_FREQ_16000) {
405 pa_log_error("Invalid FastStream source sampling frequency in configuration");
406 return false;
407 }
408
409 return true;
410 }
411
is_configuration_valid(const uint8_t * config_buffer,uint8_t config_size)412 static bool is_configuration_valid(const uint8_t *config_buffer, uint8_t config_size) {
413 const a2dp_sbc_t *config = (const a2dp_sbc_t *) config_buffer;
414
415 if (config_size != sizeof(*config)) {
416 pa_log_error("Invalid size of config buffer");
417 return false;
418 }
419
420 if (config->frequency != SBC_SAMPLING_FREQ_16000 && config->frequency != SBC_SAMPLING_FREQ_32000 &&
421 config->frequency != SBC_SAMPLING_FREQ_44100 && config->frequency != SBC_SAMPLING_FREQ_48000) {
422 pa_log_error("Invalid sampling frequency in configuration");
423 return false;
424 }
425
426 if (config->channel_mode != SBC_CHANNEL_MODE_MONO && config->channel_mode != SBC_CHANNEL_MODE_DUAL_CHANNEL &&
427 config->channel_mode != SBC_CHANNEL_MODE_STEREO && config->channel_mode != SBC_CHANNEL_MODE_JOINT_STEREO) {
428 pa_log_error("Invalid channel mode in configuration");
429 return false;
430 }
431
432 if (config->allocation_method != SBC_ALLOCATION_SNR && config->allocation_method != SBC_ALLOCATION_LOUDNESS) {
433 pa_log_error("Invalid allocation method in configuration");
434 return false;
435 }
436
437 if (config->subbands != SBC_SUBBANDS_4 && config->subbands != SBC_SUBBANDS_8) {
438 pa_log_error("Invalid SBC subbands in configuration");
439 return false;
440 }
441
442 if (config->block_length != SBC_BLOCK_LENGTH_4 && config->block_length != SBC_BLOCK_LENGTH_8 &&
443 config->block_length != SBC_BLOCK_LENGTH_12 && config->block_length != SBC_BLOCK_LENGTH_16) {
444 pa_log_error("Invalid block length in configuration");
445 return false;
446 }
447
448 if (config->min_bitpool > config->max_bitpool) {
449 pa_log_error("Invalid bitpool in configuration");
450 return false;
451 }
452
453 return true;
454 }
455
default_bitpool(uint8_t freq,uint8_t mode)456 static uint8_t default_bitpool(uint8_t freq, uint8_t mode) {
457 /* These bitpool values were chosen based on the A2DP spec recommendation */
458 switch (freq) {
459 case SBC_SAMPLING_FREQ_16000:
460 case SBC_SAMPLING_FREQ_32000:
461 switch (mode) {
462 case SBC_CHANNEL_MODE_MONO:
463 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
464 case SBC_CHANNEL_MODE_STEREO:
465 case SBC_CHANNEL_MODE_JOINT_STEREO:
466 return SBC_BITPOOL_HQ_JOINT_STEREO_44100;
467 }
468 break;
469
470 case SBC_SAMPLING_FREQ_44100:
471 switch (mode) {
472 case SBC_CHANNEL_MODE_MONO:
473 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
474 return SBC_BITPOOL_HQ_MONO_44100;
475
476 case SBC_CHANNEL_MODE_STEREO:
477 case SBC_CHANNEL_MODE_JOINT_STEREO:
478 return SBC_BITPOOL_HQ_JOINT_STEREO_44100;
479 }
480 break;
481
482 case SBC_SAMPLING_FREQ_48000:
483 switch (mode) {
484 case SBC_CHANNEL_MODE_MONO:
485 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
486 return SBC_BITPOOL_HQ_MONO_48000;
487
488 case SBC_CHANNEL_MODE_STEREO:
489 case SBC_CHANNEL_MODE_JOINT_STEREO:
490 return SBC_BITPOOL_HQ_JOINT_STEREO_48000;
491 }
492 break;
493 }
494
495 pa_assert_not_reached();
496 }
497
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])498 static 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]) {
499 a2dp_sbc_t *config = (a2dp_sbc_t *) config_buffer;
500 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
501 int i;
502
503 static const struct {
504 uint32_t rate;
505 uint8_t cap;
506 } freq_table[] = {
507 { 16000U, SBC_SAMPLING_FREQ_16000 },
508 { 32000U, SBC_SAMPLING_FREQ_32000 },
509 { 44100U, SBC_SAMPLING_FREQ_44100 },
510 { 48000U, SBC_SAMPLING_FREQ_48000 }
511 };
512
513 if (capabilities_size != sizeof(*capabilities)) {
514 pa_log_error("Invalid size of capabilities buffer");
515 return 0;
516 }
517
518 pa_zero(*config);
519
520 /* Find the lowest freq that is at least as high as the requested sampling rate */
521 for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
522 if (freq_table[i].rate >= default_sample_spec->rate && (capabilities->frequency & freq_table[i].cap)) {
523 config->frequency = freq_table[i].cap;
524 break;
525 }
526
527 if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
528 for (--i; i >= 0; i--) {
529 if (capabilities->frequency & freq_table[i].cap) {
530 config->frequency = freq_table[i].cap;
531 break;
532 }
533 }
534
535 if (i < 0) {
536 pa_log_error("Not suitable sample rate");
537 return 0;
538 }
539 }
540
541 pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
542
543 if (default_sample_spec->channels <= 1) {
544 if (capabilities->channel_mode & SBC_CHANNEL_MODE_MONO)
545 config->channel_mode = SBC_CHANNEL_MODE_MONO;
546 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_JOINT_STEREO)
547 config->channel_mode = SBC_CHANNEL_MODE_JOINT_STEREO;
548 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_STEREO)
549 config->channel_mode = SBC_CHANNEL_MODE_STEREO;
550 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
551 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
552 else {
553 pa_log_error("No supported channel modes");
554 return 0;
555 }
556 } else {
557 if (capabilities->channel_mode & SBC_CHANNEL_MODE_JOINT_STEREO)
558 config->channel_mode = SBC_CHANNEL_MODE_JOINT_STEREO;
559 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_STEREO)
560 config->channel_mode = SBC_CHANNEL_MODE_STEREO;
561 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
562 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
563 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_MONO)
564 config->channel_mode = SBC_CHANNEL_MODE_MONO;
565 else {
566 pa_log_error("No supported channel modes");
567 return 0;
568 }
569 }
570
571 if (capabilities->block_length & SBC_BLOCK_LENGTH_16)
572 config->block_length = SBC_BLOCK_LENGTH_16;
573 else if (capabilities->block_length & SBC_BLOCK_LENGTH_12)
574 config->block_length = SBC_BLOCK_LENGTH_12;
575 else if (capabilities->block_length & SBC_BLOCK_LENGTH_8)
576 config->block_length = SBC_BLOCK_LENGTH_8;
577 else if (capabilities->block_length & SBC_BLOCK_LENGTH_4)
578 config->block_length = SBC_BLOCK_LENGTH_4;
579 else {
580 pa_log_error("No supported block lengths");
581 return 0;
582 }
583
584 if (capabilities->subbands & SBC_SUBBANDS_8)
585 config->subbands = SBC_SUBBANDS_8;
586 else if (capabilities->subbands & SBC_SUBBANDS_4)
587 config->subbands = SBC_SUBBANDS_4;
588 else {
589 pa_log_error("No supported subbands");
590 return 0;
591 }
592
593 if (capabilities->allocation_method & SBC_ALLOCATION_LOUDNESS)
594 config->allocation_method = SBC_ALLOCATION_LOUDNESS;
595 else if (capabilities->allocation_method & SBC_ALLOCATION_SNR)
596 config->allocation_method = SBC_ALLOCATION_SNR;
597 else {
598 pa_log_error("No supported allocation method");
599 return 0;
600 }
601
602 config->min_bitpool = (uint8_t) PA_MAX(SBC_MIN_BITPOOL, capabilities->min_bitpool);
603 config->max_bitpool = (uint8_t) PA_MIN(default_bitpool(config->frequency, config->channel_mode), capabilities->max_bitpool);
604
605 if (config->min_bitpool > config->max_bitpool) {
606 pa_log_error("No supported bitpool");
607 return 0;
608 }
609
610 return sizeof(*config);
611 }
612
fill_preferred_configuration_faststream(const pa_sample_spec * default_sample_spec,const uint8_t * capabilities_buffer,uint8_t capabilities_size,uint8_t config_buffer[MAX_A2DP_CAPS_SIZE])613 static uint8_t fill_preferred_configuration_faststream(const pa_sample_spec *default_sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[MAX_A2DP_CAPS_SIZE]) {
614 a2dp_faststream_t *config = (a2dp_faststream_t *) config_buffer;
615 const a2dp_faststream_t *capabilities = (const a2dp_faststream_t *) capabilities_buffer;
616 int i;
617
618 static const struct {
619 uint32_t rate;
620 uint8_t cap;
621 } sink_freq_table[] = {
622 { 44100U, FASTSTREAM_SINK_SAMPLING_FREQ_44100 },
623 { 48000U, FASTSTREAM_SINK_SAMPLING_FREQ_48000 }
624 };
625
626 static const struct {
627 uint32_t rate;
628 uint8_t cap;
629 } source_freq_table[] = {
630 { 16000U, FASTSTREAM_SOURCE_SAMPLING_FREQ_16000 }
631 };
632
633 if (capabilities_size != sizeof(*capabilities)) {
634 pa_log_error("Invalid size of FastStream capabilities buffer");
635 return 0;
636 }
637
638 pa_zero(*config);
639
640 /* Find the lowest freq that is at least as high as the requested sampling rate */
641 for (i = 0; (unsigned) i < PA_ELEMENTSOF(sink_freq_table); i++)
642 if (sink_freq_table[i].rate >= default_sample_spec->rate && (capabilities->sink_frequency & sink_freq_table[i].cap)) {
643 config->sink_frequency = sink_freq_table[i].cap;
644 break;
645 }
646
647 /* Match with endpoint capabilities */
648 if ((unsigned) i == PA_ELEMENTSOF(sink_freq_table)) {
649 for (--i; i >= 0; i--) {
650 if (capabilities->sink_frequency & sink_freq_table[i].cap) {
651 config->sink_frequency = sink_freq_table[i].cap;
652 break;
653 }
654 }
655
656 if (i < 0) {
657 pa_log_error("Not suitable FastStream sink sample rate");
658 return 0;
659 }
660 }
661
662 pa_assert((unsigned) i < PA_ELEMENTSOF(sink_freq_table));
663
664 /* Only single frequency (for now?) */
665 config->source_frequency = FASTSTREAM_SOURCE_SAMPLING_FREQ_16000;
666 i = 0;
667
668 /* Match with endpoint capabilities */
669 if ((unsigned) i == PA_ELEMENTSOF(source_freq_table)) {
670 for (--i; i >= 0; i--) {
671 if (capabilities->source_frequency & source_freq_table[i].cap) {
672 config->source_frequency = source_freq_table[i].cap;
673 break;
674 }
675 }
676
677 if (i < 0) {
678 pa_log_error("Not suitable FastStream source sample rate");
679 return 0;
680 }
681 }
682
683 pa_assert((unsigned) i < PA_ELEMENTSOF(source_freq_table));
684
685 config->direction = FASTSTREAM_DIRECTION_SINK | FASTSTREAM_DIRECTION_SOURCE;
686
687 config->info = A2DP_SET_VENDOR_ID_CODEC_ID(FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID);
688
689 return sizeof(*config);
690 }
691
fill_preferred_configuration_xq(const pa_sample_spec * default_sample_spec,const uint8_t * capabilities_buffer,uint8_t capabilities_size,uint8_t config_buffer[MAX_A2DP_CAPS_SIZE],uint32_t bitrate_cap)692 static uint8_t fill_preferred_configuration_xq(const pa_sample_spec *default_sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[MAX_A2DP_CAPS_SIZE], uint32_t bitrate_cap) {
693 a2dp_sbc_t *config = (a2dp_sbc_t *) config_buffer;
694 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
695 int i;
696
697 static const struct {
698 uint32_t rate;
699 uint8_t cap;
700 } freq_table[] = {
701 { 16000U, SBC_SAMPLING_FREQ_16000 },
702 { 32000U, SBC_SAMPLING_FREQ_32000 },
703 { 44100U, SBC_SAMPLING_FREQ_44100 },
704 { 48000U, SBC_SAMPLING_FREQ_48000 }
705 };
706
707 if (capabilities_size != sizeof(*capabilities)) {
708 pa_log_error("Invalid size of capabilities buffer");
709 return 0;
710 }
711
712 pa_zero(*config);
713
714 /* Find the lowest freq that is at least as high as the requested sampling rate */
715 for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
716 if (freq_table[i].rate >= default_sample_spec->rate && (capabilities->frequency & freq_table[i].cap)) {
717 config->frequency = freq_table[i].cap;
718 break;
719 }
720
721 if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
722 for (--i; i >= 0; i--) {
723 if (capabilities->frequency & freq_table[i].cap) {
724 config->frequency = freq_table[i].cap;
725 break;
726 }
727 }
728
729 if (i < 0) {
730 pa_log_error("Not suitable sample rate");
731 return 0;
732 }
733 }
734
735 pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
736
737 if (default_sample_spec->channels <= 1) {
738 if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
739 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
740 else {
741 pa_log_error("No supported channel modes");
742 return 0;
743 }
744 } else {
745 if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
746 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
747 else {
748 pa_log_error("No supported channel modes");
749 return 0;
750 }
751 }
752
753 if (capabilities->block_length & SBC_BLOCK_LENGTH_16)
754 config->block_length = SBC_BLOCK_LENGTH_16;
755 else {
756 pa_log_error("No supported block lengths");
757 return 0;
758 }
759
760 if (capabilities->subbands & SBC_SUBBANDS_8)
761 config->subbands = SBC_SUBBANDS_8;
762 else {
763 pa_log_error("No supported subbands");
764 return 0;
765 }
766
767 if (capabilities->allocation_method & SBC_ALLOCATION_LOUDNESS)
768 config->allocation_method = SBC_ALLOCATION_LOUDNESS;
769 else {
770 pa_log_error("No supported allocation method");
771 return 0;
772 }
773
774 config->min_bitpool = (uint8_t) PA_MAX(SBC_MIN_BITPOOL, capabilities->min_bitpool);
775 config->max_bitpool = sbc_get_max_bitpool_below_rate(config, config->min_bitpool, capabilities->max_bitpool, bitrate_cap);
776
777 if (config->min_bitpool > config->max_bitpool) {
778 pa_log_error("No supported bitpool");
779 return 0;
780 }
781
782 return sizeof(*config);
783 }
784
fill_capabilities_xq_453kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])785 static uint8_t fill_capabilities_xq_453kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
786 return fill_capabilities_xq(capabilities_buffer, 453000);
787 }
788
fill_preferred_configuration_xq_453kbps(const pa_sample_spec * default_sample_spec,const uint8_t * capabilities_buffer,uint8_t capabilities_size,uint8_t config_buffer[MAX_A2DP_CAPS_SIZE])789 static uint8_t fill_preferred_configuration_xq_453kbps(const pa_sample_spec *default_sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[MAX_A2DP_CAPS_SIZE]) {
790 return fill_preferred_configuration_xq(default_sample_spec, capabilities_buffer, capabilities_size, config_buffer, 453000);
791 }
792
fill_capabilities_xq_512kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])793 static uint8_t fill_capabilities_xq_512kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
794 return fill_capabilities_xq(capabilities_buffer, 512000);
795 }
796
fill_preferred_configuration_xq_512kbps(const pa_sample_spec * default_sample_spec,const uint8_t * capabilities_buffer,uint8_t capabilities_size,uint8_t config_buffer[MAX_A2DP_CAPS_SIZE])797 static uint8_t fill_preferred_configuration_xq_512kbps(const pa_sample_spec *default_sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[MAX_A2DP_CAPS_SIZE]) {
798 return fill_preferred_configuration_xq(default_sample_spec, capabilities_buffer, capabilities_size, config_buffer, 512000);
799 }
800
fill_capabilities_xq_552kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])801 static uint8_t fill_capabilities_xq_552kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
802 return fill_capabilities_xq(capabilities_buffer, 552000);
803 }
804
fill_preferred_configuration_xq_552kbps(const pa_sample_spec * default_sample_spec,const uint8_t * capabilities_buffer,uint8_t capabilities_size,uint8_t config_buffer[MAX_A2DP_CAPS_SIZE])805 static uint8_t fill_preferred_configuration_xq_552kbps(const pa_sample_spec *default_sample_spec, const uint8_t *capabilities_buffer, uint8_t capabilities_size, uint8_t config_buffer[MAX_A2DP_CAPS_SIZE]) {
806 return fill_preferred_configuration_xq(default_sample_spec, capabilities_buffer, capabilities_size, config_buffer, 552000);
807 }
808
init(bool for_encoding,bool for_backchannel,const uint8_t * config_buffer,uint8_t config_size,pa_sample_spec * sample_spec,pa_core * core)809 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) {
810 struct sbc_info *sbc_info;
811 const a2dp_sbc_t *config = (const a2dp_sbc_t *) config_buffer;
812 int ret;
813
814 pa_assert(config_size == sizeof(*config));
815 pa_assert(!for_backchannel);
816
817 sbc_info = pa_xnew0(struct sbc_info, 1);
818
819 ret = sbc_init(&sbc_info->sbc, 0);
820 if (ret != 0) {
821 pa_xfree(sbc_info);
822 pa_log_error("SBC initialization failed: %d", ret);
823 return NULL;
824 }
825
826 sample_spec->format = PA_SAMPLE_S16LE;
827
828 set_info_and_sample_spec_from_sbc_config(sbc_info, sample_spec, config);
829
830 /* Set minimum bitpool for source to get the maximum possible block_size
831 * in get_block_size() function. This block_size is length of buffer used
832 * for decoded audio data and so is inversely proportional to frame length
833 * which depends on bitpool value. Bitpool is controlled by other side from
834 * range [min_bitpool, max_bitpool]. */
835 sbc_info->initial_bitpool = for_encoding ? sbc_info->max_bitpool : sbc_info->min_bitpool;
836
837 set_params(sbc_info);
838
839 pa_log_info("SBC parameters: allocation=%s, subbands=%u, blocks=%u, mode=%s bitpool=%u codesize=%u frame_length=%u",
840 sbc_info->sbc.allocation ? "SNR" : "Loudness", sbc_info->sbc.subbands ? 8 : 4,
841 (sbc_info->sbc.blocks+1)*4, sbc_info->sbc.mode == SBC_MODE_MONO ? "Mono" :
842 sbc_info->sbc.mode == SBC_MODE_DUAL_CHANNEL ? "DualChannel" :
843 sbc_info->sbc.mode == SBC_MODE_STEREO ? "Stereo" : "JointStereo",
844 sbc_info->sbc.bitpool, (unsigned)sbc_info->codesize, (unsigned)sbc_info->frame_length);
845
846 return sbc_info;
847 }
848
init_faststream(bool for_encoding,bool for_backchannel,const uint8_t * config_buffer,uint8_t config_size,pa_sample_spec * sample_spec,pa_core * core)849 static void *init_faststream(bool for_encoding, bool for_backchannel, const uint8_t *config_buffer, uint8_t config_size, pa_sample_spec *sample_spec, pa_core *core) {
850 struct sbc_info *sbc_info;
851 const a2dp_faststream_t *config = (const a2dp_faststream_t *) config_buffer;
852 int ret;
853
854 pa_assert(config_size == sizeof(*config));
855
856 sbc_info = pa_xnew0(struct sbc_info, 1);
857
858 ret = sbc_init(&sbc_info->sbc, 0);
859 if (ret != 0) {
860 pa_xfree(sbc_info);
861 pa_log_error("SBC initialization failed: %d", ret);
862 return NULL;
863 }
864
865 sample_spec->format = PA_SAMPLE_S16LE;
866
867 if (for_encoding != for_backchannel) {
868 switch (config->sink_frequency) {
869 case FASTSTREAM_SINK_SAMPLING_FREQ_44100:
870 sbc_info->frequency = SBC_FREQ_44100;
871 sample_spec->rate = 44100U;
872 break;
873 case FASTSTREAM_SINK_SAMPLING_FREQ_48000:
874 sbc_info->frequency = SBC_FREQ_48000;
875 sample_spec->rate = 48000U;
876 break;
877 default:
878 pa_assert_not_reached();
879 }
880
881 sample_spec->channels = 2;
882
883 sbc_info->mode = SBC_MODE_JOINT_STEREO;
884 sbc_info->initial_bitpool = sbc_info->min_bitpool = sbc_info->max_bitpool = 29;
885 } else {
886 switch (config->source_frequency) {
887 case FASTSTREAM_SOURCE_SAMPLING_FREQ_16000:
888 sbc_info->frequency = SBC_FREQ_16000;
889 sample_spec->rate = 16000U;
890 break;
891 default:
892 pa_assert_not_reached();
893 }
894
895 sample_spec->channels = 2;
896
897 sbc_info->mode = SBC_MODE_MONO;
898 sbc_info->initial_bitpool = sbc_info->min_bitpool = sbc_info->max_bitpool = 32;
899 }
900
901 sbc_info->allocation = SBC_AM_LOUDNESS;
902 sbc_info->subbands = SBC_SB_8;
903 sbc_info->nr_subbands = 8;
904 sbc_info->blocks = SBC_BLK_16;
905 sbc_info->nr_blocks = 16;
906
907 set_params(sbc_info);
908 if (sbc_info->frame_length & 1)
909 ++sbc_info->frame_length;
910
911 pa_log_info("FastStream %s SBC parameters: allocation=%s, subbands=%u, blocks=%u, mode=%s bitpool=%u codesize=%u frame_length=%u",
912 for_encoding ? "encoder" : "decoder",
913 sbc_info->sbc.allocation ? "SNR" : "Loudness", sbc_info->sbc.subbands ? 8 : 4,
914 (sbc_info->sbc.blocks+1)*4, sbc_info->sbc.mode == SBC_MODE_MONO ? "Mono" :
915 sbc_info->sbc.mode == SBC_MODE_DUAL_CHANNEL ? "DualChannel" :
916 sbc_info->sbc.mode == SBC_MODE_STEREO ? "Stereo" : "JointStereo",
917 sbc_info->sbc.bitpool, (unsigned)sbc_info->codesize, (unsigned)sbc_info->frame_length);
918
919 return sbc_info;
920 }
921
deinit(void * codec_info)922 static void deinit(void *codec_info) {
923 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
924
925 sbc_finish(&sbc_info->sbc);
926 pa_xfree(sbc_info);
927 }
928
set_bitpool(struct sbc_info * sbc_info,uint8_t bitpool)929 static void set_bitpool(struct sbc_info *sbc_info, uint8_t bitpool) {
930 if (bitpool > sbc_info->max_bitpool)
931 bitpool = sbc_info->max_bitpool;
932 else if (bitpool < sbc_info->min_bitpool)
933 bitpool = sbc_info->min_bitpool;
934
935 sbc_info->sbc.bitpool = bitpool;
936
937 sbc_info->codesize = sbc_get_codesize(&sbc_info->sbc);
938 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
939
940 pa_log_debug("Bitpool has changed to %u", sbc_info->sbc.bitpool);
941 }
942
reset(void * codec_info)943 static int reset(void *codec_info) {
944 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
945 int ret;
946
947 /* forget about source volume boost */
948 sbc_info->boost_source_volume = false;
949
950 /* forget last saved frame fragment */
951 sbc_info->frame_fragment_size = 0;
952
953 ret = sbc_reinit(&sbc_info->sbc, 0);
954 if (ret != 0) {
955 pa_log_error("SBC reinitialization failed: %d", ret);
956 return -1;
957 }
958
959 /* sbc_reinit() sets also default parameters, so reset them back */
960 set_params(sbc_info);
961
962 sbc_info->seq_num = 0;
963 return 0;
964 }
965
reset_faststream(void * codec_info)966 static int reset_faststream(void *codec_info) {
967 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
968 int ret;
969
970 ret = sbc_reinit(&sbc_info->sbc, 0);
971 if (ret != 0) {
972 pa_log_error("SBC reinitialization failed: %d", ret);
973 return -1;
974 }
975
976 /* sbc_reinit() sets also default parameters, so reset them back */
977 set_params(sbc_info);
978 if (sbc_info->frame_length & 1)
979 ++sbc_info->frame_length;
980
981 sbc_info->seq_num = 0;
982 return 0;
983 }
984
get_block_size(void * codec_info,size_t link_mtu)985 static size_t get_block_size(void *codec_info, size_t link_mtu) {
986 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
987 size_t rtp_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
988 size_t frame_count = (link_mtu - rtp_size) / sbc_info->frame_length;
989
990 /* frame_count is only 4 bit number */
991 if (frame_count > 15)
992 frame_count = 15;
993
994 /* Code dealing with read/write block size expects it to be
995 * non-zero to make progress, make it at least one frame.
996 */
997 if (frame_count < 1) {
998 pa_log_warn("SBC packet size %lu is larger than link MTU %lu", sbc_info->frame_length + rtp_size, link_mtu);
999 frame_count = 1;
1000 }
1001
1002 return frame_count * sbc_info->codesize;
1003 }
1004
get_write_block_size_faststream(void * codec_info,size_t link_mtu)1005 static size_t get_write_block_size_faststream(void *codec_info, size_t link_mtu) {
1006 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1007 size_t frame_count = link_mtu / sbc_info->frame_length;
1008
1009 /* 3 frames seem to work best, with minimal glitches */
1010 if (frame_count > 3)
1011 frame_count = 3;
1012
1013 return frame_count * sbc_info->codesize;
1014 }
1015
get_read_block_size_faststream(void * codec_info,size_t link_mtu)1016 static size_t get_read_block_size_faststream(void *codec_info, size_t link_mtu) {
1017 /* With SBC bitpool >= 29 and any combination of blocks, subbands
1018 * and channels maximum compression ratio 4:1 is achieved with
1019 * blocks=16, subbands=8, channels=2, bitpool=29
1020 *
1021 * Though smaller bitpools can yield higher compression ratio, faststream is
1022 * assumed to have fixed bitpool so maximum output size is link_mtu * 4.
1023 */
1024 return link_mtu * 4;
1025 }
1026
get_encoded_block_size(void * codec_info,size_t input_size)1027 static size_t get_encoded_block_size(void *codec_info, size_t input_size) {
1028 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1029 size_t rtp_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
1030
1031 /* input size should be aligned to codec input block size */
1032 pa_assert_fp(input_size % sbc_info->codesize == 0);
1033
1034 return (input_size / sbc_info->codesize) * sbc_info->frame_length + rtp_size;
1035 }
1036
get_encoded_block_size_faststream(void * codec_info,size_t input_size)1037 static size_t get_encoded_block_size_faststream(void *codec_info, size_t input_size) {
1038 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1039
1040 /* input size should be aligned to codec input block size */
1041 pa_assert_fp(input_size % sbc_info->codesize == 0);
1042
1043 return (input_size / sbc_info->codesize) * sbc_info->frame_length;
1044 }
1045
reduce_encoder_bitrate(void * codec_info,size_t write_link_mtu)1046 static size_t reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
1047 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1048 uint8_t bitpool;
1049
1050 bitpool = PA_MAX(sbc_info->sbc.bitpool - SBC_BITPOOL_DEC_STEP, sbc_info->min_bitpool);
1051
1052 if (sbc_info->sbc.bitpool == bitpool)
1053 return 0;
1054
1055 set_bitpool(sbc_info, bitpool);
1056 return get_block_size(codec_info, write_link_mtu);
1057 }
1058
increase_encoder_bitrate(void * codec_info,size_t write_link_mtu)1059 static size_t increase_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
1060 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1061 uint8_t bitpool;
1062
1063 bitpool = PA_MIN(sbc_info->sbc.bitpool + SBC_BITPOOL_INC_STEP, sbc_info->max_bitpool);
1064
1065 if (sbc_info->sbc.bitpool == bitpool)
1066 return 0;
1067
1068 set_bitpool(sbc_info, bitpool);
1069 return get_block_size(codec_info, write_link_mtu);
1070 }
1071
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)1072 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) {
1073 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1074 struct rtp_header *header;
1075 struct rtp_payload *payload;
1076 uint8_t *d;
1077 const uint8_t *p;
1078 size_t to_write, to_encode;
1079 uint8_t frame_count;
1080
1081 header = (struct rtp_header*) output_buffer;
1082 payload = (struct rtp_payload*) (output_buffer + sizeof(*header));
1083
1084 frame_count = 0;
1085
1086 p = input_buffer;
1087 to_encode = input_size;
1088
1089 d = output_buffer + sizeof(*header) + sizeof(*payload);
1090 to_write = output_size - sizeof(*header) - sizeof(*payload);
1091
1092 /* frame_count is only 4 bit number */
1093 while (PA_LIKELY(to_encode > 0 && to_write > 0 && frame_count < 15)) {
1094 ssize_t written;
1095 ssize_t encoded;
1096
1097 encoded = sbc_encode(&sbc_info->sbc,
1098 p, to_encode,
1099 d, to_write,
1100 &written);
1101
1102 if (PA_UNLIKELY(encoded <= 0)) {
1103 pa_log_error("SBC encoding error (%li)", (long) encoded);
1104 break;
1105 }
1106
1107 if (PA_UNLIKELY(written < 0)) {
1108 pa_log_error("SBC encoding error (%li)", (long) written);
1109 break;
1110 }
1111
1112 pa_assert_fp((size_t) encoded <= to_encode);
1113 pa_assert_fp((size_t) encoded == sbc_info->codesize);
1114
1115 pa_assert_fp((size_t) written <= to_write);
1116 pa_assert_fp((size_t) written == sbc_info->frame_length);
1117
1118 p += encoded;
1119 to_encode -= encoded;
1120
1121 d += written;
1122 to_write -= written;
1123
1124 frame_count++;
1125 }
1126
1127 PA_ONCE_BEGIN {
1128 pa_log_debug("Using SBC codec implementation: %s", pa_strnull(sbc_get_implementation_info(&sbc_info->sbc)));
1129 } PA_ONCE_END;
1130
1131 if (PA_UNLIKELY(frame_count == 0)) {
1132 *processed = 0;
1133 return 0;
1134 }
1135
1136 /* write it to the fifo */
1137 pa_memzero(output_buffer, sizeof(*header) + sizeof(*payload));
1138 header->v = 2;
1139
1140 /* A2DP spec: "A payload type in the RTP dynamic range shall be chosen".
1141 * RFC3551 defines the dynamic range to span from 96 to 127, and 96 appears
1142 * to be the most common choice in A2DP implementations. */
1143 header->pt = 96;
1144
1145 header->sequence_number = htons(sbc_info->seq_num++);
1146 header->timestamp = htonl(timestamp);
1147 header->ssrc = htonl(1);
1148 payload->frame_count = frame_count;
1149
1150 *processed = p - input_buffer;
1151 return d - output_buffer;
1152 }
1153
encode_buffer_faststream(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)1154 static size_t encode_buffer_faststream(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) {
1155 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1156 uint8_t *d;
1157 const uint8_t *p;
1158 size_t to_write, to_encode;
1159 uint8_t frame_count;
1160
1161 frame_count = 0;
1162
1163 p = input_buffer;
1164 to_encode = input_size;
1165
1166 d = output_buffer;
1167 to_write = output_size;
1168
1169 /* frame_count is only 4 bit number */
1170 while (PA_LIKELY(to_encode > 0 && to_write > 0)) {
1171 ssize_t written;
1172 ssize_t encoded;
1173
1174 encoded = sbc_encode(&sbc_info->sbc,
1175 p, to_encode,
1176 d, to_write,
1177 &written);
1178
1179 if (PA_UNLIKELY(encoded <= 0)) {
1180 pa_log_error("SBC encoding error (%li)", (long) encoded);
1181 break;
1182 }
1183
1184 if (PA_UNLIKELY(written < 0)) {
1185 pa_log_error("SBC encoding error (%li)", (long) written);
1186 break;
1187 }
1188
1189 while (written < sbc_info->frame_length && written < to_write)
1190 d[written++] = 0;
1191
1192 pa_assert_fp((size_t) encoded <= to_encode);
1193 pa_assert_fp((size_t) encoded == sbc_info->codesize);
1194
1195 pa_assert_fp((size_t) written <= to_write);
1196 pa_assert_fp((size_t) written == sbc_info->frame_length);
1197
1198 p += encoded;
1199 to_encode -= encoded;
1200
1201 d += written;
1202 to_write -= written;
1203
1204 frame_count++;
1205 }
1206
1207 PA_ONCE_BEGIN {
1208 pa_log_debug("Using SBC codec implementation: %s", pa_strnull(sbc_get_implementation_info(&sbc_info->sbc)));
1209 } PA_ONCE_END;
1210
1211 if (PA_UNLIKELY(frame_count == 0)) {
1212 *processed = 0;
1213 return 0;
1214 }
1215
1216 *processed = p - input_buffer;
1217 return d - output_buffer;
1218 }
1219
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)1220 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) {
1221 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1222
1223 struct rtp_header *header;
1224 struct rtp_payload *payload;
1225 const uint8_t *p;
1226 uint8_t *d;
1227 size_t to_write, to_decode;
1228 uint8_t frame_count;
1229
1230 header = (struct rtp_header *) input_buffer;
1231 payload = (struct rtp_payload*) (input_buffer + sizeof(*header));
1232
1233 frame_count = payload->frame_count;
1234
1235 /* TODO: Add support for decoding fragmented SBC frames */
1236 if (payload->is_fragmented) {
1237 pa_log_error("Unsupported fragmented SBC frame");
1238 *processed = 0;
1239 return 0;
1240 }
1241
1242 p = input_buffer + sizeof(*header) + sizeof(*payload);
1243 to_decode = input_size - sizeof(*header) - sizeof(*payload);
1244
1245 d = output_buffer;
1246 to_write = output_size;
1247
1248 while (PA_LIKELY(to_decode > 0 && to_write > 0 && frame_count > 0)) {
1249 size_t written;
1250 ssize_t decoded;
1251
1252 decoded = sbc_decode(&sbc_info->sbc,
1253 p, to_decode,
1254 d, to_write,
1255 &written);
1256
1257 if (PA_UNLIKELY(decoded <= 0)) {
1258 pa_log_error("SBC decoding error (%li)", (long) decoded);
1259 break;
1260 }
1261
1262 /* Reset frame length, it can be changed due to bitpool change */
1263 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
1264
1265 pa_assert_fp((size_t) decoded <= to_decode);
1266 pa_assert_fp((size_t) decoded == sbc_info->frame_length);
1267
1268 pa_assert_fp((size_t) written <= to_write);
1269 pa_assert_fp((size_t) written == sbc_info->codesize);
1270
1271 p += decoded;
1272 to_decode -= decoded;
1273
1274 d += written;
1275 to_write -= written;
1276
1277 frame_count--;
1278 }
1279
1280 *processed = p - input_buffer;
1281 return d - output_buffer;
1282 }
1283
decode_buffer_faststream(void * codec_info,const uint8_t * input_buffer,size_t input_size,uint8_t * output_buffer,size_t output_size,size_t * processed)1284 static size_t decode_buffer_faststream(void *codec_info, const uint8_t *input_buffer, size_t input_size, uint8_t *output_buffer, size_t output_size, size_t *processed) {
1285 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1286
1287 const uint8_t *p;
1288 uint8_t *d;
1289 size_t to_write, to_decode;
1290 pa_sample_spec decoded_sample_spec = {
1291 .format = PA_SAMPLE_S16LE,
1292 .channels = 2,
1293 .rate = 16000U
1294 };
1295 uint8_t decode_buffer[4096];
1296 uint8_t frame_buffer[4096];
1297
1298 to_decode = input_size;
1299
1300 /* append input buffer to fragment left from previous decode call */
1301 if (sbc_info->frame_fragment_size) {
1302
1303 if (sbc_info->frame_fragment_size + to_decode > sizeof(frame_buffer)) {
1304 pa_log_debug("FastStream SBC input (saved + incoming) size %lu larger than buffer size %lu, input truncated to fit",
1305 sbc_info->frame_fragment_size + to_decode, sizeof(frame_buffer));
1306 to_decode = sizeof(frame_buffer) - sbc_info->frame_fragment_size;
1307 }
1308
1309 memcpy(frame_buffer, sbc_info->frame_fragment, sbc_info->frame_fragment_size);
1310 memcpy(frame_buffer + sbc_info->frame_fragment_size, input_buffer, to_decode);
1311
1312 to_decode += sbc_info->frame_fragment_size;
1313 p = frame_buffer;
1314
1315 /* clear saved fragment */
1316 sbc_info->frame_fragment_size = 0;
1317 } else
1318 p = input_buffer;
1319
1320 d = output_buffer;
1321 to_write = output_size;
1322
1323 while (PA_LIKELY(to_decode > 0 && to_write > 0)) {
1324 size_t written = 0;
1325 ssize_t decoded;
1326
1327 /* skip to SBC sync word before attempting decode */
1328 if (*p != SBC_SYNCWORD) {
1329 ++p;
1330 --to_decode;
1331 continue;
1332 }
1333
1334 if (to_decode < sbc_info->frame_length) {
1335 pa_log_debug("FastStream SBC input %lu is too short (expected frame length %lu)", to_decode, sbc_info->frame_length);
1336 break;
1337 }
1338
1339 decoded = sbc_decode(&sbc_info->sbc,
1340 p, to_decode,
1341 decode_buffer, sizeof(decode_buffer),
1342 &written);
1343
1344 if (PA_UNLIKELY(decoded <= 0)) {
1345 /* sbc_decode returns -1 if input too short,
1346 * break from loop to save this frame fragment for next decode iteration */
1347 if (decoded == -1) {
1348 pa_log_debug("FastStream SBC decoding error (%li) input %lu is too short", (long) decoded, to_decode);
1349 break;
1350 }
1351
1352 /* otherwise failed to decode frame, skip to next SBC sync word */
1353 pa_log_error("FastStream SBC decoding error (%li)", (long) decoded);
1354 decoded = 1;
1355 } else {
1356 /* Reset codesize and frame_length to values found by decoder */
1357 sbc_info->codesize = sbc_get_codesize(&sbc_info->sbc);
1358 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
1359
1360 if (sbc_info->mode != sbc_info->sbc.mode)
1361 sbc_info->mode = sbc_info->sbc.mode;
1362
1363 if (sbc_info->frequency != sbc_info->sbc.frequency) {
1364 /* some devices unexpectedly return SBC frequency different from 16000
1365 * remember this, and keep incoming sample rate at 16000 */
1366 pa_log_debug("FastStream decoder detected SBC frequency %u, expected %u", sbc_info->sbc.frequency, sbc_info->frequency);
1367 sbc_info->frequency = sbc_info->sbc.frequency;
1368
1369 /* volume is too low for known devices with unexpected source SBC frequency */
1370 pa_log_debug("FastStream decoder requesting 20dB boost for source volume");
1371 sbc_info->boost_source_volume = true;
1372 }
1373
1374 if (sbc_info->sbc.mode == SBC_MODE_MONO) {
1375 const void *interleave_buf[2] = {decode_buffer, decode_buffer};
1376 /* mono->stereo conversion needs to fit into remaining output space */
1377 written = PA_MIN(to_write / 2, written);
1378 pa_interleave(interleave_buf, 2, d, pa_sample_size(&decoded_sample_spec), written / pa_sample_size(&decoded_sample_spec));
1379 written *= 2;
1380 } else
1381 memcpy(d, decode_buffer, written);
1382 }
1383
1384 pa_assert_fp((size_t) decoded <= to_decode);
1385 pa_assert_fp((size_t) written <= to_write);
1386
1387 p += decoded;
1388 to_decode -= decoded;
1389
1390 d += written;
1391 to_write -= written;
1392 }
1393
1394 if (to_decode) {
1395 if (to_decode > sizeof(sbc_info->frame_fragment)) {
1396 pa_log_debug("FastStream remaining SBC fragment size %lu larger than buffer size %lu, remainder truncated to fit",
1397 to_decode, sizeof(sbc_info->frame_fragment));
1398 p += to_decode - sizeof(sbc_info->frame_fragment);
1399 to_decode = sizeof(sbc_info->frame_fragment);
1400 }
1401
1402 pa_log_debug("FastStream saving SBC fragment size %lu for next decoding iteration", to_decode);
1403 memcpy(sbc_info->frame_fragment, p, to_decode);
1404 sbc_info->frame_fragment_size = to_decode;
1405 }
1406
1407 *processed = input_size;
1408
1409 return d - output_buffer;
1410 }
1411
1412 /* Boost sink backchannel mic volume by 20dB as it appears too quiet */
get_source_output_volume_factor_dB_faststream(void * codec_info)1413 double get_source_output_volume_factor_dB_faststream(void *codec_info) {
1414 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
1415
1416 if (sbc_info->boost_source_volume)
1417 return 20.;
1418
1419 return 1.0;
1420 }
1421
1422 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc = {
1423 .id = { A2DP_CODEC_SBC, 0, 0 },
1424 .can_be_supported = can_be_supported,
1425 .can_accept_capabilities = can_accept_capabilities,
1426 .choose_remote_endpoint = choose_remote_endpoint,
1427 .fill_capabilities = fill_capabilities,
1428 .is_configuration_valid = is_configuration_valid,
1429 .fill_preferred_configuration = fill_preferred_configuration,
1430 .bt_codec = {
1431 .name = "sbc",
1432 .description = "SBC",
1433 .init = init,
1434 .deinit = deinit,
1435 .reset = reset,
1436 .get_read_block_size = get_block_size,
1437 .get_write_block_size = get_block_size,
1438 .get_encoded_block_size = get_encoded_block_size,
1439 .reduce_encoder_bitrate = reduce_encoder_bitrate,
1440 .increase_encoder_bitrate = increase_encoder_bitrate,
1441 .encode_buffer = encode_buffer,
1442 .decode_buffer = decode_buffer,
1443 },
1444 };
1445
1446 /* There are multiple definitions of SBC XQ, but in all cases this is
1447 * SBC codec in Dual Channel mode, 8 bands, block length 16, allocation method Loudness,
1448 * with bitpool adjusted to match target bitrates.
1449 *
1450 * Most commonly choosen bitrates and reasons are:
1451 * 453000 - this yields most efficient packing of frames on Android for bluetooth EDR 2mbps
1452 * 512000 - this looks to be old limit stated in bluetooth documents
1453 * 552000 - this yields most efficient packing of frames on Android for bluetooth EDR 3mbps
1454 *
1455 * Efficient packing considerations do not apply on Linux (yet?) but still
1456 * we can gain from increased bitrate.
1457 */
1458
1459 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc_xq_453 = {
1460 .id = { A2DP_CODEC_SBC, 0, 0 },
1461 .can_be_supported = can_be_supported,
1462 .can_accept_capabilities = can_accept_capabilities_xq,
1463 .choose_remote_endpoint = choose_remote_endpoint_xq,
1464 .fill_capabilities = fill_capabilities_xq_453kbps,
1465 .is_configuration_valid = is_configuration_valid,
1466 .fill_preferred_configuration = fill_preferred_configuration_xq_453kbps,
1467 .bt_codec = {
1468 .name = "sbc_xq_453",
1469 .description = "SBC XQ 453kbps",
1470 .init = init,
1471 .deinit = deinit,
1472 .reset = reset,
1473 .get_read_block_size = get_block_size,
1474 .get_write_block_size = get_block_size,
1475 .get_encoded_block_size = get_encoded_block_size,
1476 .reduce_encoder_bitrate = reduce_encoder_bitrate,
1477 .increase_encoder_bitrate = increase_encoder_bitrate,
1478 .encode_buffer = encode_buffer,
1479 .decode_buffer = decode_buffer,
1480 },
1481 };
1482
1483 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc_xq_512 = {
1484 .id = { A2DP_CODEC_SBC, 0, 0 },
1485 .can_be_supported = can_be_supported,
1486 .can_accept_capabilities = can_accept_capabilities_xq,
1487 .choose_remote_endpoint = choose_remote_endpoint_xq,
1488 .fill_capabilities = fill_capabilities_xq_512kbps,
1489 .is_configuration_valid = is_configuration_valid,
1490 .fill_preferred_configuration = fill_preferred_configuration_xq_512kbps,
1491 .bt_codec = {
1492 .name = "sbc_xq_512",
1493 .description = "SBC XQ 512kbps",
1494 .init = init,
1495 .deinit = deinit,
1496 .reset = reset,
1497 .get_read_block_size = get_block_size,
1498 .get_write_block_size = get_block_size,
1499 .get_encoded_block_size = get_encoded_block_size,
1500 .reduce_encoder_bitrate = reduce_encoder_bitrate,
1501 .increase_encoder_bitrate = increase_encoder_bitrate,
1502 .encode_buffer = encode_buffer,
1503 .decode_buffer = decode_buffer,
1504 },
1505 };
1506
1507 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc_xq_552 = {
1508 .id = { A2DP_CODEC_SBC, 0, 0 },
1509 .can_be_supported = can_be_supported,
1510 .can_accept_capabilities = can_accept_capabilities_xq,
1511 .choose_remote_endpoint = choose_remote_endpoint_xq,
1512 .fill_capabilities = fill_capabilities_xq_552kbps,
1513 .is_configuration_valid = is_configuration_valid,
1514 .fill_preferred_configuration = fill_preferred_configuration_xq_552kbps,
1515 .bt_codec = {
1516 .name = "sbc_xq_552",
1517 .description = "SBC XQ 552kbps",
1518 .init = init,
1519 .deinit = deinit,
1520 .reset = reset,
1521 .get_read_block_size = get_block_size,
1522 .get_write_block_size = get_block_size,
1523 .get_encoded_block_size = get_encoded_block_size,
1524 .reduce_encoder_bitrate = reduce_encoder_bitrate,
1525 .increase_encoder_bitrate = increase_encoder_bitrate,
1526 .encode_buffer = encode_buffer,
1527 .decode_buffer = decode_buffer,
1528 },
1529 };
1530
1531 /* FastStream codec is just SBC codec with fixed parameters.
1532 *
1533 * Sink stream parameters:
1534 * 48.0kHz or 44.1kHz,
1535 * Blocks 16,
1536 * Sub-bands 8,
1537 * Joint Stereo,
1538 * Allocation method Loudness,
1539 * Bitpool = 29
1540 * (data rate = 212kbps, packet size = (71+1)3 <= DM5 = 220, with 3 SBC frames).
1541 * SBC frame size is 71 bytes, but FastStream is zero-padded to the even size (72).
1542 *
1543 * Source stream parameters:
1544 * 16kHz,
1545 * Mono,
1546 * Blocks 16,
1547 * Sub-bands 8,
1548 * Allocation method Loudness,
1549 * Bitpool = 32
1550 * (data rate = 72kbps, packet size = 723 <= DM5 = 220, with 3 SBC frames).
1551 */
1552
1553 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_faststream = {
1554 .id = { A2DP_CODEC_VENDOR, FASTSTREAM_VENDOR_ID, FASTSTREAM_CODEC_ID },
1555 .can_be_supported = can_be_supported,
1556 .can_accept_capabilities = can_accept_capabilities_faststream,
1557 .choose_remote_endpoint = choose_remote_endpoint_faststream,
1558 .fill_capabilities = fill_capabilities_faststream,
1559 .is_configuration_valid = is_configuration_valid_faststream,
1560 .fill_preferred_configuration = fill_preferred_configuration_faststream,
1561 .bt_codec = {
1562 .name = "faststream",
1563 .description = "FastStream",
1564 .support_backchannel = true,
1565 .init = init_faststream,
1566 .deinit = deinit,
1567 .reset = reset_faststream,
1568 .get_read_block_size = get_read_block_size_faststream,
1569 .get_write_block_size = get_write_block_size_faststream,
1570 .get_encoded_block_size = get_encoded_block_size_faststream,
1571 .encode_buffer = encode_buffer_faststream,
1572 .decode_buffer = decode_buffer_faststream,
1573 .get_source_output_volume_factor_dB = get_source_output_volume_factor_dB_faststream,
1574 },
1575 };
1576