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 struct sbc_info {
43 sbc_t sbc; /* Codec data */
44 size_t codesize, frame_length; /* SBC Codesize, frame_length. We simply cache those values here */
45 uint16_t seq_num; /* Cumulative packet sequence */
46 uint8_t frequency;
47 uint8_t blocks;
48 uint8_t subbands;
49 uint8_t mode;
50 uint8_t allocation;
51 uint8_t initial_bitpool;
52 uint8_t min_bitpool;
53 uint8_t max_bitpool;
54
55 uint8_t nr_blocks;
56 uint8_t nr_subbands;
57 };
58
can_be_supported(bool for_encoding)59 static bool can_be_supported(bool for_encoding) {
60 return true;
61 }
62
can_accept_capabilities(const uint8_t * capabilities_buffer,uint8_t capabilities_size,bool for_encoding)63 static bool can_accept_capabilities(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
64 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
65
66 if (capabilities_size != sizeof(*capabilities))
67 return false;
68
69 if (!(capabilities->frequency & (SBC_SAMPLING_FREQ_16000 | SBC_SAMPLING_FREQ_32000 | SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000)))
70 return false;
71
72 if (!(capabilities->channel_mode & (SBC_CHANNEL_MODE_MONO | SBC_CHANNEL_MODE_DUAL_CHANNEL | SBC_CHANNEL_MODE_STEREO | SBC_CHANNEL_MODE_JOINT_STEREO)))
73 return false;
74
75 if (!(capabilities->allocation_method & (SBC_ALLOCATION_SNR | SBC_ALLOCATION_LOUDNESS)))
76 return false;
77
78 if (!(capabilities->subbands & (SBC_SUBBANDS_4 | SBC_SUBBANDS_8)))
79 return false;
80
81 if (!(capabilities->block_length & (SBC_BLOCK_LENGTH_4 | SBC_BLOCK_LENGTH_8 | SBC_BLOCK_LENGTH_12 | SBC_BLOCK_LENGTH_16)))
82 return false;
83
84 return true;
85 }
86
can_accept_capabilities_xq(const uint8_t * capabilities_buffer,uint8_t capabilities_size,bool for_encoding)87 static bool can_accept_capabilities_xq(const uint8_t *capabilities_buffer, uint8_t capabilities_size, bool for_encoding) {
88 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
89
90 if (capabilities_size != sizeof(*capabilities))
91 return false;
92
93 if (!(capabilities->frequency & (SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000)))
94 return false;
95
96 if (!(capabilities->channel_mode & (SBC_CHANNEL_MODE_DUAL_CHANNEL)))
97 return false;
98
99 if (!(capabilities->allocation_method & (SBC_ALLOCATION_LOUDNESS)))
100 return false;
101
102 if (!(capabilities->subbands & (SBC_SUBBANDS_8)))
103 return false;
104
105 if (!(capabilities->block_length & (SBC_BLOCK_LENGTH_16)))
106 return false;
107
108 return true;
109 }
110
choose_remote_endpoint(const pa_hashmap * capabilities_hashmap,const pa_sample_spec * default_sample_spec,bool for_encoding)111 static const char *choose_remote_endpoint(const pa_hashmap *capabilities_hashmap, const pa_sample_spec *default_sample_spec, bool for_encoding) {
112 const pa_a2dp_codec_capabilities *a2dp_capabilities;
113 const char *key;
114 void *state;
115
116 /* There is no preference, just choose random valid entry */
117 PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
118 if (can_accept_capabilities(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
119 return key;
120 }
121
122 return NULL;
123 }
124
choose_remote_endpoint_xq(const pa_hashmap * capabilities_hashmap,const pa_sample_spec * default_sample_spec,bool for_encoding)125 static const char *choose_remote_endpoint_xq(const pa_hashmap *capabilities_hashmap, const pa_sample_spec *default_sample_spec, bool for_encoding) {
126 const pa_a2dp_codec_capabilities *a2dp_capabilities;
127 const char *key;
128 void *state;
129
130 /* There is no preference, just choose random valid entry */
131 PA_HASHMAP_FOREACH_KV(key, a2dp_capabilities, capabilities_hashmap, state) {
132 if (can_accept_capabilities_xq(a2dp_capabilities->buffer, a2dp_capabilities->size, for_encoding))
133 return key;
134 }
135
136 return NULL;
137 }
138
fill_capabilities(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])139 static uint8_t fill_capabilities(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
140 a2dp_sbc_t *capabilities = (a2dp_sbc_t *) capabilities_buffer;
141
142 pa_zero(*capabilities);
143
144 capabilities->channel_mode = SBC_CHANNEL_MODE_MONO | SBC_CHANNEL_MODE_DUAL_CHANNEL | SBC_CHANNEL_MODE_STEREO |
145 SBC_CHANNEL_MODE_JOINT_STEREO;
146 capabilities->frequency = SBC_SAMPLING_FREQ_16000 | SBC_SAMPLING_FREQ_32000 | SBC_SAMPLING_FREQ_44100 |
147 SBC_SAMPLING_FREQ_48000;
148 capabilities->allocation_method = SBC_ALLOCATION_SNR | SBC_ALLOCATION_LOUDNESS;
149 capabilities->subbands = SBC_SUBBANDS_4 | SBC_SUBBANDS_8;
150 capabilities->block_length = SBC_BLOCK_LENGTH_4 | SBC_BLOCK_LENGTH_8 | SBC_BLOCK_LENGTH_12 | SBC_BLOCK_LENGTH_16;
151 capabilities->min_bitpool = SBC_MIN_BITPOOL;
152 capabilities->max_bitpool = SBC_BITPOOL_HQ_JOINT_STEREO_44100;
153
154 return sizeof(*capabilities);
155 }
156
set_info_and_sample_spec_from_sbc_config(struct sbc_info * sbc_info,pa_sample_spec * sample_spec,const a2dp_sbc_t * config)157 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) {
158 switch (config->frequency) {
159 case SBC_SAMPLING_FREQ_16000:
160 sbc_info->frequency = SBC_FREQ_16000;
161 sample_spec->rate = 16000U;
162 break;
163 case SBC_SAMPLING_FREQ_32000:
164 sbc_info->frequency = SBC_FREQ_32000;
165 sample_spec->rate = 32000U;
166 break;
167 case SBC_SAMPLING_FREQ_44100:
168 sbc_info->frequency = SBC_FREQ_44100;
169 sample_spec->rate = 44100U;
170 break;
171 case SBC_SAMPLING_FREQ_48000:
172 sbc_info->frequency = SBC_FREQ_48000;
173 sample_spec->rate = 48000U;
174 break;
175 default:
176 pa_assert_not_reached();
177 }
178
179 switch (config->channel_mode) {
180 case SBC_CHANNEL_MODE_MONO:
181 sbc_info->mode = SBC_MODE_MONO;
182 sample_spec->channels = 1;
183 break;
184 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
185 sbc_info->mode = SBC_MODE_DUAL_CHANNEL;
186 sample_spec->channels = 2;
187 break;
188 case SBC_CHANNEL_MODE_STEREO:
189 sbc_info->mode = SBC_MODE_STEREO;
190 sample_spec->channels = 2;
191 break;
192 case SBC_CHANNEL_MODE_JOINT_STEREO:
193 sbc_info->mode = SBC_MODE_JOINT_STEREO;
194 sample_spec->channels = 2;
195 break;
196 default:
197 pa_assert_not_reached();
198 }
199
200 switch (config->allocation_method) {
201 case SBC_ALLOCATION_SNR:
202 sbc_info->allocation = SBC_AM_SNR;
203 break;
204 case SBC_ALLOCATION_LOUDNESS:
205 sbc_info->allocation = SBC_AM_LOUDNESS;
206 break;
207 default:
208 pa_assert_not_reached();
209 }
210
211 switch (config->subbands) {
212 case SBC_SUBBANDS_4:
213 sbc_info->subbands = SBC_SB_4;
214 sbc_info->nr_subbands = 4;
215 break;
216 case SBC_SUBBANDS_8:
217 sbc_info->subbands = SBC_SB_8;
218 sbc_info->nr_subbands = 8;
219 break;
220 default:
221 pa_assert_not_reached();
222 }
223
224 switch (config->block_length) {
225 case SBC_BLOCK_LENGTH_4:
226 sbc_info->blocks = SBC_BLK_4;
227 sbc_info->nr_blocks = 4;
228 break;
229 case SBC_BLOCK_LENGTH_8:
230 sbc_info->blocks = SBC_BLK_8;
231 sbc_info->nr_blocks = 8;
232 break;
233 case SBC_BLOCK_LENGTH_12:
234 sbc_info->blocks = SBC_BLK_12;
235 sbc_info->nr_blocks = 12;
236 break;
237 case SBC_BLOCK_LENGTH_16:
238 sbc_info->blocks = SBC_BLK_16;
239 sbc_info->nr_blocks = 16;
240 break;
241 default:
242 pa_assert_not_reached();
243 }
244
245 sbc_info->min_bitpool = config->min_bitpool;
246 sbc_info->max_bitpool = config->max_bitpool;
247 }
248
set_params(struct sbc_info * sbc_info)249 static void set_params(struct sbc_info *sbc_info) {
250 sbc_info->sbc.frequency = sbc_info->frequency;
251 sbc_info->sbc.blocks = sbc_info->blocks;
252 sbc_info->sbc.subbands = sbc_info->subbands;
253 sbc_info->sbc.mode = sbc_info->mode;
254 sbc_info->sbc.allocation = sbc_info->allocation;
255 sbc_info->sbc.bitpool = sbc_info->initial_bitpool;
256 sbc_info->sbc.endian = SBC_LE;
257
258 sbc_info->codesize = sbc_get_codesize(&sbc_info->sbc);
259 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
260 }
261
sbc_get_max_bitpool_below_rate(a2dp_sbc_t * config,uint8_t lower_bound,uint8_t upper_bound,uint32_t bitrate_cap)262 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) {
263 pa_sample_spec sample_spec;
264 struct sbc_info sbc_info;
265 int ret;
266
267 pa_assert(config);
268
269 ret = sbc_init(&sbc_info.sbc, 0);
270 if (ret != 0) {
271 pa_log_error("SBC initialization failed: %d", ret);
272 return lower_bound;
273 }
274
275 set_info_and_sample_spec_from_sbc_config(&sbc_info, &sample_spec, config);
276
277 while (upper_bound - lower_bound > 1) {
278 size_t midpoint = (upper_bound + lower_bound) / 2;
279
280 sbc_info.initial_bitpool = midpoint;
281 set_params(&sbc_info);
282
283 size_t bitrate = sbc_info.frame_length * 8 * sample_spec.rate / (sbc_info.nr_subbands * sbc_info.nr_blocks);
284
285 if (bitrate > bitrate_cap)
286 upper_bound = midpoint;
287 else
288 lower_bound = midpoint;
289 }
290
291 sbc_finish(&sbc_info.sbc);
292
293 pa_log_debug("SBC target bitrate %u bitpool %u sample rate %u", bitrate_cap, lower_bound, sample_spec.rate);
294
295 return lower_bound;
296 }
297
298 /* SBC XQ
299 *
300 * References:
301 * https://habr.com/en/post/456476/
302 * http://soundexpert.org/articles/-/blogs/audio-quality-of-sbc-xq-bluetooth-audio-codec
303 *
304 */
fill_capabilities_xq(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE],uint32_t bitrate_cap)305 static uint8_t fill_capabilities_xq(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE], uint32_t bitrate_cap) {
306 a2dp_sbc_t *capabilities = (a2dp_sbc_t *) capabilities_buffer;
307
308 pa_zero(*capabilities);
309
310 /* Bitpool value increases with sample rate. Prepare to calculate maximum viable
311 * bitpool value at specified bitrate_cap, with rest of SBC parameters fixed. */
312 capabilities->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
313 capabilities->frequency = SBC_SAMPLING_FREQ_48000;
314 capabilities->allocation_method = SBC_ALLOCATION_LOUDNESS;
315 capabilities->subbands = SBC_SUBBANDS_8;
316 capabilities->block_length = SBC_BLOCK_LENGTH_16;
317 capabilities->min_bitpool = SBC_MIN_BITPOOL;
318 capabilities->max_bitpool = SBC_MAX_BITPOOL; /* Upper boundary in calculation below. */
319
320 /* Now calculate and write it back to be exposed through endpoint capabilities. */
321 capabilities->max_bitpool = sbc_get_max_bitpool_below_rate(capabilities, capabilities->min_bitpool, capabilities->max_bitpool, bitrate_cap);
322
323 /* Add back all supported frequencies exposed through endpoint capabilities, rest of SBC parameters are still fixed. */
324 capabilities->frequency = SBC_SAMPLING_FREQ_44100 | SBC_SAMPLING_FREQ_48000;
325
326 return sizeof(*capabilities);
327 }
328
is_configuration_valid(const uint8_t * config_buffer,uint8_t config_size)329 static bool is_configuration_valid(const uint8_t *config_buffer, uint8_t config_size) {
330 const a2dp_sbc_t *config = (const a2dp_sbc_t *) config_buffer;
331
332 if (config_size != sizeof(*config)) {
333 pa_log_error("Invalid size of config buffer");
334 return false;
335 }
336
337 if (config->frequency != SBC_SAMPLING_FREQ_16000 && config->frequency != SBC_SAMPLING_FREQ_32000 &&
338 config->frequency != SBC_SAMPLING_FREQ_44100 && config->frequency != SBC_SAMPLING_FREQ_48000) {
339 pa_log_error("Invalid sampling frequency in configuration");
340 return false;
341 }
342
343 if (config->channel_mode != SBC_CHANNEL_MODE_MONO && config->channel_mode != SBC_CHANNEL_MODE_DUAL_CHANNEL &&
344 config->channel_mode != SBC_CHANNEL_MODE_STEREO && config->channel_mode != SBC_CHANNEL_MODE_JOINT_STEREO) {
345 pa_log_error("Invalid channel mode in configuration");
346 return false;
347 }
348
349 if (config->allocation_method != SBC_ALLOCATION_SNR && config->allocation_method != SBC_ALLOCATION_LOUDNESS) {
350 pa_log_error("Invalid allocation method in configuration");
351 return false;
352 }
353
354 if (config->subbands != SBC_SUBBANDS_4 && config->subbands != SBC_SUBBANDS_8) {
355 pa_log_error("Invalid SBC subbands in configuration");
356 return false;
357 }
358
359 if (config->block_length != SBC_BLOCK_LENGTH_4 && config->block_length != SBC_BLOCK_LENGTH_8 &&
360 config->block_length != SBC_BLOCK_LENGTH_12 && config->block_length != SBC_BLOCK_LENGTH_16) {
361 pa_log_error("Invalid block length in configuration");
362 return false;
363 }
364
365 if (config->min_bitpool > config->max_bitpool) {
366 pa_log_error("Invalid bitpool in configuration");
367 return false;
368 }
369
370 return true;
371 }
372
default_bitpool(uint8_t freq,uint8_t mode)373 static uint8_t default_bitpool(uint8_t freq, uint8_t mode) {
374 /* These bitpool values were chosen based on the A2DP spec recommendation */
375 switch (freq) {
376 case SBC_SAMPLING_FREQ_16000:
377 case SBC_SAMPLING_FREQ_32000:
378 switch (mode) {
379 case SBC_CHANNEL_MODE_MONO:
380 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
381 case SBC_CHANNEL_MODE_STEREO:
382 case SBC_CHANNEL_MODE_JOINT_STEREO:
383 return SBC_BITPOOL_HQ_JOINT_STEREO_44100;
384 }
385 break;
386
387 case SBC_SAMPLING_FREQ_44100:
388 switch (mode) {
389 case SBC_CHANNEL_MODE_MONO:
390 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
391 return SBC_BITPOOL_HQ_MONO_44100;
392
393 case SBC_CHANNEL_MODE_STEREO:
394 case SBC_CHANNEL_MODE_JOINT_STEREO:
395 return SBC_BITPOOL_HQ_JOINT_STEREO_44100;
396 }
397 break;
398
399 case SBC_SAMPLING_FREQ_48000:
400 switch (mode) {
401 case SBC_CHANNEL_MODE_MONO:
402 case SBC_CHANNEL_MODE_DUAL_CHANNEL:
403 return SBC_BITPOOL_HQ_MONO_48000;
404
405 case SBC_CHANNEL_MODE_STEREO:
406 case SBC_CHANNEL_MODE_JOINT_STEREO:
407 return SBC_BITPOOL_HQ_JOINT_STEREO_48000;
408 }
409 break;
410 }
411
412 pa_assert_not_reached();
413 }
414
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])415 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]) {
416 a2dp_sbc_t *config = (a2dp_sbc_t *) config_buffer;
417 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
418 int i;
419
420 static const struct {
421 uint32_t rate;
422 uint8_t cap;
423 } freq_table[] = {
424 { 16000U, SBC_SAMPLING_FREQ_16000 },
425 { 32000U, SBC_SAMPLING_FREQ_32000 },
426 { 44100U, SBC_SAMPLING_FREQ_44100 },
427 { 48000U, SBC_SAMPLING_FREQ_48000 }
428 };
429
430 if (capabilities_size != sizeof(*capabilities)) {
431 pa_log_error("Invalid size of capabilities buffer");
432 return 0;
433 }
434
435 pa_zero(*config);
436
437 /* Find the lowest freq that is at least as high as the requested sampling rate */
438 for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
439 if (freq_table[i].rate >= default_sample_spec->rate && (capabilities->frequency & freq_table[i].cap)) {
440 config->frequency = freq_table[i].cap;
441 break;
442 }
443
444 if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
445 for (--i; i >= 0; i--) {
446 if (capabilities->frequency & freq_table[i].cap) {
447 config->frequency = freq_table[i].cap;
448 break;
449 }
450 }
451
452 if (i < 0) {
453 pa_log_error("Not suitable sample rate");
454 return 0;
455 }
456 }
457
458 pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
459
460 if (default_sample_spec->channels <= 1) {
461 if (capabilities->channel_mode & SBC_CHANNEL_MODE_MONO)
462 config->channel_mode = SBC_CHANNEL_MODE_MONO;
463 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_JOINT_STEREO)
464 config->channel_mode = SBC_CHANNEL_MODE_JOINT_STEREO;
465 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_STEREO)
466 config->channel_mode = SBC_CHANNEL_MODE_STEREO;
467 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
468 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
469 else {
470 pa_log_error("No supported channel modes");
471 return 0;
472 }
473 } else {
474 if (capabilities->channel_mode & SBC_CHANNEL_MODE_JOINT_STEREO)
475 config->channel_mode = SBC_CHANNEL_MODE_JOINT_STEREO;
476 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_STEREO)
477 config->channel_mode = SBC_CHANNEL_MODE_STEREO;
478 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
479 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
480 else if (capabilities->channel_mode & SBC_CHANNEL_MODE_MONO)
481 config->channel_mode = SBC_CHANNEL_MODE_MONO;
482 else {
483 pa_log_error("No supported channel modes");
484 return 0;
485 }
486 }
487
488 if (capabilities->block_length & SBC_BLOCK_LENGTH_16)
489 config->block_length = SBC_BLOCK_LENGTH_16;
490 else if (capabilities->block_length & SBC_BLOCK_LENGTH_12)
491 config->block_length = SBC_BLOCK_LENGTH_12;
492 else if (capabilities->block_length & SBC_BLOCK_LENGTH_8)
493 config->block_length = SBC_BLOCK_LENGTH_8;
494 else if (capabilities->block_length & SBC_BLOCK_LENGTH_4)
495 config->block_length = SBC_BLOCK_LENGTH_4;
496 else {
497 pa_log_error("No supported block lengths");
498 return 0;
499 }
500
501 if (capabilities->subbands & SBC_SUBBANDS_8)
502 config->subbands = SBC_SUBBANDS_8;
503 else if (capabilities->subbands & SBC_SUBBANDS_4)
504 config->subbands = SBC_SUBBANDS_4;
505 else {
506 pa_log_error("No supported subbands");
507 return 0;
508 }
509
510 if (capabilities->allocation_method & SBC_ALLOCATION_LOUDNESS)
511 config->allocation_method = SBC_ALLOCATION_LOUDNESS;
512 else if (capabilities->allocation_method & SBC_ALLOCATION_SNR)
513 config->allocation_method = SBC_ALLOCATION_SNR;
514 else {
515 pa_log_error("No supported allocation method");
516 return 0;
517 }
518
519 config->min_bitpool = (uint8_t) PA_MAX(SBC_MIN_BITPOOL, capabilities->min_bitpool);
520 config->max_bitpool = (uint8_t) PA_MIN(default_bitpool(config->frequency, config->channel_mode), capabilities->max_bitpool);
521
522 if (config->min_bitpool > config->max_bitpool) {
523 pa_log_error("No supported bitpool");
524 return 0;
525 }
526
527 return sizeof(*config);
528 }
529
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)530 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) {
531 a2dp_sbc_t *config = (a2dp_sbc_t *) config_buffer;
532 const a2dp_sbc_t *capabilities = (const a2dp_sbc_t *) capabilities_buffer;
533 int i;
534
535 static const struct {
536 uint32_t rate;
537 uint8_t cap;
538 } freq_table[] = {
539 { 16000U, SBC_SAMPLING_FREQ_16000 },
540 { 32000U, SBC_SAMPLING_FREQ_32000 },
541 { 44100U, SBC_SAMPLING_FREQ_44100 },
542 { 48000U, SBC_SAMPLING_FREQ_48000 }
543 };
544
545 if (capabilities_size != sizeof(*capabilities)) {
546 pa_log_error("Invalid size of capabilities buffer");
547 return 0;
548 }
549
550 pa_zero(*config);
551
552 /* Find the lowest freq that is at least as high as the requested sampling rate */
553 for (i = 0; (unsigned) i < PA_ELEMENTSOF(freq_table); i++)
554 if (freq_table[i].rate >= default_sample_spec->rate && (capabilities->frequency & freq_table[i].cap)) {
555 config->frequency = freq_table[i].cap;
556 break;
557 }
558
559 if ((unsigned) i == PA_ELEMENTSOF(freq_table)) {
560 for (--i; i >= 0; i--) {
561 if (capabilities->frequency & freq_table[i].cap) {
562 config->frequency = freq_table[i].cap;
563 break;
564 }
565 }
566
567 if (i < 0) {
568 pa_log_error("Not suitable sample rate");
569 return 0;
570 }
571 }
572
573 pa_assert((unsigned) i < PA_ELEMENTSOF(freq_table));
574
575 if (default_sample_spec->channels <= 1) {
576 if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
577 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
578 else {
579 pa_log_error("No supported channel modes");
580 return 0;
581 }
582 } else {
583 if (capabilities->channel_mode & SBC_CHANNEL_MODE_DUAL_CHANNEL)
584 config->channel_mode = SBC_CHANNEL_MODE_DUAL_CHANNEL;
585 else {
586 pa_log_error("No supported channel modes");
587 return 0;
588 }
589 }
590
591 if (capabilities->block_length & SBC_BLOCK_LENGTH_16)
592 config->block_length = SBC_BLOCK_LENGTH_16;
593 else {
594 pa_log_error("No supported block lengths");
595 return 0;
596 }
597
598 if (capabilities->subbands & SBC_SUBBANDS_8)
599 config->subbands = SBC_SUBBANDS_8;
600 else {
601 pa_log_error("No supported subbands");
602 return 0;
603 }
604
605 if (capabilities->allocation_method & SBC_ALLOCATION_LOUDNESS)
606 config->allocation_method = SBC_ALLOCATION_LOUDNESS;
607 else {
608 pa_log_error("No supported allocation method");
609 return 0;
610 }
611
612 config->min_bitpool = (uint8_t) PA_MAX(SBC_MIN_BITPOOL, capabilities->min_bitpool);
613 config->max_bitpool = sbc_get_max_bitpool_below_rate(config, config->min_bitpool, capabilities->max_bitpool, bitrate_cap);
614
615 if (config->min_bitpool > config->max_bitpool) {
616 pa_log_error("No supported bitpool");
617 return 0;
618 }
619
620 return sizeof(*config);
621 }
622
fill_capabilities_xq_453kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])623 static uint8_t fill_capabilities_xq_453kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
624 return fill_capabilities_xq(capabilities_buffer, 453000);
625 }
626
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])627 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]) {
628 return fill_preferred_configuration_xq(default_sample_spec, capabilities_buffer, capabilities_size, config_buffer, 453000);
629 }
630
fill_capabilities_xq_512kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])631 static uint8_t fill_capabilities_xq_512kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
632 return fill_capabilities_xq(capabilities_buffer, 512000);
633 }
634
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])635 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]) {
636 return fill_preferred_configuration_xq(default_sample_spec, capabilities_buffer, capabilities_size, config_buffer, 512000);
637 }
638
fill_capabilities_xq_552kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE])639 static uint8_t fill_capabilities_xq_552kbps(uint8_t capabilities_buffer[MAX_A2DP_CAPS_SIZE]) {
640 return fill_capabilities_xq(capabilities_buffer, 552000);
641 }
642
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])643 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]) {
644 return fill_preferred_configuration_xq(default_sample_spec, capabilities_buffer, capabilities_size, config_buffer, 552000);
645 }
646
init(bool for_encoding,bool for_backchannel,const uint8_t * config_buffer,uint8_t config_size,pa_sample_spec * sample_spec,pa_core * core)647 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) {
648 struct sbc_info *sbc_info;
649 const a2dp_sbc_t *config = (const a2dp_sbc_t *) config_buffer;
650 int ret;
651
652 pa_assert(config_size == sizeof(*config));
653 pa_assert(!for_backchannel);
654
655 sbc_info = pa_xnew0(struct sbc_info, 1);
656
657 ret = sbc_init(&sbc_info->sbc, 0);
658 if (ret != 0) {
659 pa_xfree(sbc_info);
660 pa_log_error("SBC initialization failed: %d", ret);
661 return NULL;
662 }
663
664 sample_spec->format = PA_SAMPLE_S16LE;
665
666 set_info_and_sample_spec_from_sbc_config(sbc_info, sample_spec, config);
667
668 /* Set minimum bitpool for source to get the maximum possible block_size
669 * in get_block_size() function. This block_size is length of buffer used
670 * for decoded audio data and so is inversely proportional to frame length
671 * which depends on bitpool value. Bitpool is controlled by other side from
672 * range [min_bitpool, max_bitpool]. */
673 sbc_info->initial_bitpool = for_encoding ? sbc_info->max_bitpool : sbc_info->min_bitpool;
674
675 set_params(sbc_info);
676
677 pa_log_info("SBC parameters: allocation=%s, subbands=%u, blocks=%u, mode=%s bitpool=%u codesize=%u frame_length=%u",
678 sbc_info->sbc.allocation ? "SNR" : "Loudness", sbc_info->sbc.subbands ? 8 : 4,
679 (sbc_info->sbc.blocks+1)*4, sbc_info->sbc.mode == SBC_MODE_MONO ? "Mono" :
680 sbc_info->sbc.mode == SBC_MODE_DUAL_CHANNEL ? "DualChannel" :
681 sbc_info->sbc.mode == SBC_MODE_STEREO ? "Stereo" : "JointStereo",
682 sbc_info->sbc.bitpool, (unsigned)sbc_info->codesize, (unsigned)sbc_info->frame_length);
683
684 return sbc_info;
685 }
686
deinit(void * codec_info)687 static void deinit(void *codec_info) {
688 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
689
690 sbc_finish(&sbc_info->sbc);
691 pa_xfree(sbc_info);
692 }
693
set_bitpool(struct sbc_info * sbc_info,uint8_t bitpool)694 static void set_bitpool(struct sbc_info *sbc_info, uint8_t bitpool) {
695 if (bitpool > sbc_info->max_bitpool)
696 bitpool = sbc_info->max_bitpool;
697 else if (bitpool < sbc_info->min_bitpool)
698 bitpool = sbc_info->min_bitpool;
699
700 sbc_info->sbc.bitpool = bitpool;
701
702 sbc_info->codesize = sbc_get_codesize(&sbc_info->sbc);
703 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
704
705 pa_log_debug("Bitpool has changed to %u", sbc_info->sbc.bitpool);
706 }
707
reset(void * codec_info)708 static int reset(void *codec_info) {
709 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
710 int ret;
711
712 ret = sbc_reinit(&sbc_info->sbc, 0);
713 if (ret != 0) {
714 pa_log_error("SBC reinitialization failed: %d", ret);
715 return -1;
716 }
717
718 /* sbc_reinit() sets also default parameters, so reset them back */
719 set_params(sbc_info);
720
721 sbc_info->seq_num = 0;
722 return 0;
723 }
724
get_block_size(void * codec_info,size_t link_mtu)725 static size_t get_block_size(void *codec_info, size_t link_mtu) {
726 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
727 size_t rtp_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
728 size_t frame_count = (link_mtu - rtp_size) / sbc_info->frame_length;
729
730 /* frame_count is only 4 bit number */
731 if (frame_count > 15)
732 frame_count = 15;
733
734 /* Code dealing with read/write block size expects it to be
735 * non-zero to make progress, make it at least one frame.
736 */
737 if (frame_count < 1) {
738 pa_log_warn("SBC packet size %lu is larger than link MTU %lu", sbc_info->frame_length + rtp_size, link_mtu);
739 frame_count = 1;
740 }
741
742 return frame_count * sbc_info->codesize;
743 }
744
get_encoded_block_size(void * codec_info,size_t input_size)745 static size_t get_encoded_block_size(void *codec_info, size_t input_size) {
746 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
747 size_t rtp_size = sizeof(struct rtp_header) + sizeof(struct rtp_payload);
748
749 /* input size should be aligned to codec input block size */
750 pa_assert_fp(input_size % sbc_info->codesize == 0);
751
752 return (input_size / sbc_info->codesize) * sbc_info->frame_length + rtp_size;
753 }
754
reduce_encoder_bitrate(void * codec_info,size_t write_link_mtu)755 static size_t reduce_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
756 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
757 uint8_t bitpool;
758
759 bitpool = PA_MAX(sbc_info->sbc.bitpool - SBC_BITPOOL_DEC_STEP, sbc_info->min_bitpool);
760
761 if (sbc_info->sbc.bitpool == bitpool)
762 return 0;
763
764 set_bitpool(sbc_info, bitpool);
765 return get_block_size(codec_info, write_link_mtu);
766 }
767
increase_encoder_bitrate(void * codec_info,size_t write_link_mtu)768 static size_t increase_encoder_bitrate(void *codec_info, size_t write_link_mtu) {
769 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
770 uint8_t bitpool;
771
772 bitpool = PA_MIN(sbc_info->sbc.bitpool + SBC_BITPOOL_INC_STEP, sbc_info->max_bitpool);
773
774 if (sbc_info->sbc.bitpool == bitpool)
775 return 0;
776
777 set_bitpool(sbc_info, bitpool);
778 return get_block_size(codec_info, write_link_mtu);
779 }
780
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)781 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) {
782 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
783 struct rtp_header *header;
784 struct rtp_payload *payload;
785 uint8_t *d;
786 const uint8_t *p;
787 size_t to_write, to_encode;
788 uint8_t frame_count;
789
790 header = (struct rtp_header*) output_buffer;
791 payload = (struct rtp_payload*) (output_buffer + sizeof(*header));
792
793 frame_count = 0;
794
795 p = input_buffer;
796 to_encode = input_size;
797
798 d = output_buffer + sizeof(*header) + sizeof(*payload);
799 to_write = output_size - sizeof(*header) - sizeof(*payload);
800
801 /* frame_count is only 4 bit number */
802 while (PA_LIKELY(to_encode > 0 && to_write > 0 && frame_count < 15)) {
803 ssize_t written;
804 ssize_t encoded;
805
806 encoded = sbc_encode(&sbc_info->sbc,
807 p, to_encode,
808 d, to_write,
809 &written);
810
811 if (PA_UNLIKELY(encoded <= 0)) {
812 pa_log_error("SBC encoding error (%li)", (long) encoded);
813 break;
814 }
815
816 if (PA_UNLIKELY(written < 0)) {
817 pa_log_error("SBC encoding error (%li)", (long) written);
818 break;
819 }
820
821 pa_assert_fp((size_t) encoded <= to_encode);
822 pa_assert_fp((size_t) encoded == sbc_info->codesize);
823
824 pa_assert_fp((size_t) written <= to_write);
825 pa_assert_fp((size_t) written == sbc_info->frame_length);
826
827 p += encoded;
828 to_encode -= encoded;
829
830 d += written;
831 to_write -= written;
832
833 frame_count++;
834 }
835
836 PA_ONCE_BEGIN {
837 pa_log_debug("Using SBC codec implementation: %s", pa_strnull(sbc_get_implementation_info(&sbc_info->sbc)));
838 } PA_ONCE_END;
839
840 if (PA_UNLIKELY(frame_count == 0)) {
841 *processed = 0;
842 return 0;
843 }
844
845 /* write it to the fifo */
846 pa_memzero(output_buffer, sizeof(*header) + sizeof(*payload));
847 header->v = 2;
848
849 /* A2DP spec: "A payload type in the RTP dynamic range shall be chosen".
850 * RFC3551 defines the dynamic range to span from 96 to 127, and 96 appears
851 * to be the most common choice in A2DP implementations. */
852 header->pt = 96;
853
854 header->sequence_number = htons(sbc_info->seq_num++);
855 header->timestamp = htonl(timestamp);
856 header->ssrc = htonl(1);
857 payload->frame_count = frame_count;
858
859 *processed = p - input_buffer;
860 return d - output_buffer;
861 }
862
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)863 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) {
864 struct sbc_info *sbc_info = (struct sbc_info *) codec_info;
865
866 struct rtp_header *header;
867 struct rtp_payload *payload;
868 const uint8_t *p;
869 uint8_t *d;
870 size_t to_write, to_decode;
871 uint8_t frame_count;
872
873 header = (struct rtp_header *) input_buffer;
874 payload = (struct rtp_payload*) (input_buffer + sizeof(*header));
875
876 frame_count = payload->frame_count;
877
878 /* TODO: Add support for decoding fragmented SBC frames */
879 if (payload->is_fragmented) {
880 pa_log_error("Unsupported fragmented SBC frame");
881 *processed = 0;
882 return 0;
883 }
884
885 p = input_buffer + sizeof(*header) + sizeof(*payload);
886 to_decode = input_size - sizeof(*header) - sizeof(*payload);
887
888 d = output_buffer;
889 to_write = output_size;
890
891 while (PA_LIKELY(to_decode > 0 && to_write > 0 && frame_count > 0)) {
892 size_t written;
893 ssize_t decoded;
894
895 decoded = sbc_decode(&sbc_info->sbc,
896 p, to_decode,
897 d, to_write,
898 &written);
899
900 if (PA_UNLIKELY(decoded <= 0)) {
901 pa_log_error("SBC decoding error (%li)", (long) decoded);
902 break;
903 }
904
905 /* Reset frame length, it can be changed due to bitpool change */
906 sbc_info->frame_length = sbc_get_frame_length(&sbc_info->sbc);
907
908 pa_assert_fp((size_t) decoded <= to_decode);
909 pa_assert_fp((size_t) decoded == sbc_info->frame_length);
910
911 pa_assert_fp((size_t) written <= to_write);
912 pa_assert_fp((size_t) written == sbc_info->codesize);
913
914 p += decoded;
915 to_decode -= decoded;
916
917 d += written;
918 to_write -= written;
919
920 frame_count--;
921 }
922
923 *processed = p - input_buffer;
924 return d - output_buffer;
925 }
926
927 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc = {
928 .id = { A2DP_CODEC_SBC, 0, 0 },
929 .support_backchannel = false,
930 .can_be_supported = can_be_supported,
931 .can_accept_capabilities = can_accept_capabilities,
932 .choose_remote_endpoint = choose_remote_endpoint,
933 .fill_capabilities = fill_capabilities,
934 .is_configuration_valid = is_configuration_valid,
935 .fill_preferred_configuration = fill_preferred_configuration,
936 .bt_codec = {
937 .name = "sbc",
938 .description = "SBC",
939 .init = init,
940 .deinit = deinit,
941 .reset = reset,
942 .get_read_block_size = get_block_size,
943 .get_write_block_size = get_block_size,
944 .get_encoded_block_size = get_encoded_block_size,
945 .reduce_encoder_bitrate = reduce_encoder_bitrate,
946 .increase_encoder_bitrate = increase_encoder_bitrate,
947 .encode_buffer = encode_buffer,
948 .decode_buffer = decode_buffer,
949 },
950 };
951
952 /* There are multiple definitions of SBC XQ, but in all cases this is
953 * SBC codec in Dual Channel mode, 8 bands, block length 16, allocation method Loudness,
954 * with bitpool adjusted to match target bitrates.
955 *
956 * Most commonly choosen bitrates and reasons are:
957 * 453000 - this yields most efficient packing of frames on Android for bluetooth EDR 2mbps
958 * 512000 - this looks to be old limit stated in bluetooth documents
959 * 552000 - this yields most efficient packing of frames on Android for bluetooth EDR 3mbps
960 *
961 * Efficient packing considerations do not apply on Linux (yet?) but still
962 * we can gain from increased bitrate.
963 */
964
965 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc_xq_453 = {
966 .id = { A2DP_CODEC_SBC, 0, 0 },
967 .support_backchannel = false,
968 .can_be_supported = can_be_supported,
969 .can_accept_capabilities = can_accept_capabilities_xq,
970 .choose_remote_endpoint = choose_remote_endpoint_xq,
971 .fill_capabilities = fill_capabilities_xq_453kbps,
972 .is_configuration_valid = is_configuration_valid,
973 .fill_preferred_configuration = fill_preferred_configuration_xq_453kbps,
974 .bt_codec = {
975 .name = "sbc_xq_453",
976 .description = "SBC XQ 453kbps",
977 .init = init,
978 .deinit = deinit,
979 .reset = reset,
980 .get_read_block_size = get_block_size,
981 .get_write_block_size = get_block_size,
982 .get_encoded_block_size = get_encoded_block_size,
983 .reduce_encoder_bitrate = reduce_encoder_bitrate,
984 .increase_encoder_bitrate = increase_encoder_bitrate,
985 .encode_buffer = encode_buffer,
986 .decode_buffer = decode_buffer,
987 },
988 };
989
990 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc_xq_512 = {
991 .id = { A2DP_CODEC_SBC, 0, 0 },
992 .support_backchannel = false,
993 .can_be_supported = can_be_supported,
994 .can_accept_capabilities = can_accept_capabilities_xq,
995 .choose_remote_endpoint = choose_remote_endpoint_xq,
996 .fill_capabilities = fill_capabilities_xq_512kbps,
997 .is_configuration_valid = is_configuration_valid,
998 .fill_preferred_configuration = fill_preferred_configuration_xq_512kbps,
999 .bt_codec = {
1000 .name = "sbc_xq_512",
1001 .description = "SBC XQ 512kbps",
1002 .init = init,
1003 .deinit = deinit,
1004 .reset = reset,
1005 .get_read_block_size = get_block_size,
1006 .get_write_block_size = get_block_size,
1007 .get_encoded_block_size = get_encoded_block_size,
1008 .reduce_encoder_bitrate = reduce_encoder_bitrate,
1009 .increase_encoder_bitrate = increase_encoder_bitrate,
1010 .encode_buffer = encode_buffer,
1011 .decode_buffer = decode_buffer,
1012 },
1013 };
1014
1015 const pa_a2dp_endpoint_conf pa_a2dp_endpoint_conf_sbc_xq_552 = {
1016 .id = { A2DP_CODEC_SBC, 0, 0 },
1017 .support_backchannel = false,
1018 .can_be_supported = can_be_supported,
1019 .can_accept_capabilities = can_accept_capabilities_xq,
1020 .choose_remote_endpoint = choose_remote_endpoint_xq,
1021 .fill_capabilities = fill_capabilities_xq_552kbps,
1022 .is_configuration_valid = is_configuration_valid,
1023 .fill_preferred_configuration = fill_preferred_configuration_xq_552kbps,
1024 .bt_codec = {
1025 .name = "sbc_xq_552",
1026 .description = "SBC XQ 552kbps",
1027 .init = init,
1028 .deinit = deinit,
1029 .reset = reset,
1030 .get_read_block_size = get_block_size,
1031 .get_write_block_size = get_block_size,
1032 .get_encoded_block_size = get_encoded_block_size,
1033 .reduce_encoder_bitrate = reduce_encoder_bitrate,
1034 .increase_encoder_bitrate = increase_encoder_bitrate,
1035 .encode_buffer = encode_buffer,
1036 .decode_buffer = decode_buffer,
1037 },
1038 };
1039