1 /*
2 *
3 * Bluetooth low-complexity, subband codec (SBC) library
4 *
5 * Copyright (C) 2008-2010 Nokia Corporation
6 * Copyright (C) 2004-2010 Marcel Holtmann <marcel@holtmann.org>
7 * Copyright (C) 2004-2005 Henryk Ploetz <henryk@ploetzli.ch>
8 * Copyright (C) 2005-2008 Brad Midgley <bmidgley@xmission.com>
9 *
10 *
11 * This library is free software; you can redistribute it and/or
12 * modify it under the terms of the GNU Lesser General Public
13 * License as published by the Free Software Foundation; either
14 * version 2.1 of the License, or (at your option) any later version.
15 *
16 * This library is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
19 * Lesser General Public License for more details.
20 *
21 * You should have received a copy of the GNU Lesser General Public
22 * License along with this library; if not, write to the Free Software
23 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
24 *
25 */
26
27 /* todo items:
28
29 use a log2 table for byte integer scale factors calculation (sum log2 results
30 for high and low bytes) fill bitpool by 16 bits instead of one at a time in
31 bits allocation/bitpool generation port to the dsp
32
33 */
34
35 #ifdef HAVE_CONFIG_H
36 #include <config.h>
37 #endif
38
39 #include <stdio.h>
40 #include <errno.h>
41 #include <string.h>
42 #include <stdlib.h>
43 #include <sys/types.h>
44 #include <limits.h>
45
46 #include "sbc_math.h"
47 #include "sbc_tables.h"
48
49 #include "sbc.h"
50 #include "sbc_primitives.h"
51
52 #define SBC_SYNCWORD 0x9C
53
54 /* This structure contains an unpacked SBC frame.
55 Yes, there is probably quite some unused space herein */
56 struct sbc_frame {
57 uint8_t frequency;
58 uint8_t block_mode;
59 uint8_t blocks;
60 enum {
61 MONO = SBC_MODE_MONO,
62 DUAL_CHANNEL = SBC_MODE_DUAL_CHANNEL,
63 STEREO = SBC_MODE_STEREO,
64 JOINT_STEREO = SBC_MODE_JOINT_STEREO
65 } mode;
66 uint8_t channels;
67 enum {
68 LOUDNESS = SBC_AM_LOUDNESS,
69 SNR = SBC_AM_SNR
70 } allocation;
71 uint8_t subband_mode;
72 uint8_t subbands;
73 uint8_t bitpool;
74 uint16_t codesize;
75 uint8_t length;
76
77 /* bit number x set means joint stereo has been used in subband x */
78 uint8_t joint;
79
80 /* only the lower 4 bits of every element are to be used */
81 uint32_t SBC_ALIGNED scale_factor[2][8];
82
83 /* raw integer subband samples in the frame */
84 int32_t SBC_ALIGNED sb_sample_f[16][2][8];
85
86 /* modified subband samples */
87 int32_t SBC_ALIGNED sb_sample[16][2][8];
88
89 /* original pcm audio samples */
90 int16_t SBC_ALIGNED pcm_sample[2][16*8];
91 };
92
93 struct sbc_decoder_state {
94 int subbands;
95 int32_t V[2][170];
96 int offset[2][16];
97 };
98
99 /*
100 * Calculates the CRC-8 of the first len bits in data
101 */
102 static const uint8_t crc_table[256] = {
103 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
104 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
105 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
106 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
107 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
108 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
109 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
110 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
111 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
112 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
113 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
114 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
115 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
116 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
117 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
118 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
119 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
120 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
121 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
122 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
123 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
124 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
125 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
126 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
127 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
128 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
129 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
130 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
131 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
132 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
133 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
134 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
135 };
136
sbc_crc8(const uint8_t * data,size_t len)137 static uint8_t sbc_crc8(const uint8_t *data, size_t len)
138 {
139 uint8_t crc = 0x0f;
140 size_t i;
141 uint8_t octet;
142
143 for (i = 0; i < len / 8; i++)
144 crc = crc_table[crc ^ data[i]];
145
146 octet = data[i];
147 for (i = 0; i < len % 8; i++) {
148 char bit = ((octet ^ crc) & 0x80) >> 7;
149
150 crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
151
152 octet = octet << 1;
153 }
154
155 return crc;
156 }
157
158 /*
159 * Code straight from the spec to calculate the bits array
160 * Takes a pointer to the frame in question, a pointer to the bits array and
161 * the sampling frequency (as 2 bit integer)
162 */
sbc_calculate_bits_internal(const struct sbc_frame * frame,int (* bits)[8],int subbands)163 static SBC_ALWAYS_INLINE void sbc_calculate_bits_internal(
164 const struct sbc_frame *frame, int (*bits)[8], int subbands)
165 {
166 uint8_t sf = frame->frequency;
167
168 if (frame->mode == MONO || frame->mode == DUAL_CHANNEL) {
169 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
170 int ch, sb;
171
172 for (ch = 0; ch < frame->channels; ch++) {
173 max_bitneed = 0;
174 if (frame->allocation == SNR) {
175 for (sb = 0; sb < subbands; sb++) {
176 bitneed[ch][sb] = frame->scale_factor[ch][sb];
177 if (bitneed[ch][sb] > max_bitneed)
178 max_bitneed = bitneed[ch][sb];
179 }
180 } else {
181 for (sb = 0; sb < subbands; sb++) {
182 if (frame->scale_factor[ch][sb] == 0)
183 bitneed[ch][sb] = -5;
184 else {
185 if (subbands == 4)
186 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
187 else
188 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
189 if (loudness > 0)
190 bitneed[ch][sb] = loudness / 2;
191 else
192 bitneed[ch][sb] = loudness;
193 }
194 if (bitneed[ch][sb] > max_bitneed)
195 max_bitneed = bitneed[ch][sb];
196 }
197 }
198
199 bitcount = 0;
200 slicecount = 0;
201 bitslice = max_bitneed + 1;
202 do {
203 bitslice--;
204 bitcount += slicecount;
205 slicecount = 0;
206 for (sb = 0; sb < subbands; sb++) {
207 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
208 slicecount++;
209 else if (bitneed[ch][sb] == bitslice + 1)
210 slicecount += 2;
211 }
212 } while (bitcount + slicecount < frame->bitpool);
213
214 if (bitcount + slicecount == frame->bitpool) {
215 bitcount += slicecount;
216 bitslice--;
217 }
218
219 for (sb = 0; sb < subbands; sb++) {
220 if (bitneed[ch][sb] < bitslice + 2)
221 bits[ch][sb] = 0;
222 else {
223 bits[ch][sb] = bitneed[ch][sb] - bitslice;
224 if (bits[ch][sb] > 16)
225 bits[ch][sb] = 16;
226 }
227 }
228
229 for (sb = 0; bitcount < frame->bitpool &&
230 sb < subbands; sb++) {
231 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
232 bits[ch][sb]++;
233 bitcount++;
234 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
235 bits[ch][sb] = 2;
236 bitcount += 2;
237 }
238 }
239
240 for (sb = 0; bitcount < frame->bitpool &&
241 sb < subbands; sb++) {
242 if (bits[ch][sb] < 16) {
243 bits[ch][sb]++;
244 bitcount++;
245 }
246 }
247
248 }
249
250 } else if (frame->mode == STEREO || frame->mode == JOINT_STEREO) {
251 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
252 int ch, sb;
253
254 max_bitneed = 0;
255 if (frame->allocation == SNR) {
256 for (ch = 0; ch < 2; ch++) {
257 for (sb = 0; sb < subbands; sb++) {
258 bitneed[ch][sb] = frame->scale_factor[ch][sb];
259 if (bitneed[ch][sb] > max_bitneed)
260 max_bitneed = bitneed[ch][sb];
261 }
262 }
263 } else {
264 for (ch = 0; ch < 2; ch++) {
265 for (sb = 0; sb < subbands; sb++) {
266 if (frame->scale_factor[ch][sb] == 0)
267 bitneed[ch][sb] = -5;
268 else {
269 if (subbands == 4)
270 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
271 else
272 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
273 if (loudness > 0)
274 bitneed[ch][sb] = loudness / 2;
275 else
276 bitneed[ch][sb] = loudness;
277 }
278 if (bitneed[ch][sb] > max_bitneed)
279 max_bitneed = bitneed[ch][sb];
280 }
281 }
282 }
283
284 bitcount = 0;
285 slicecount = 0;
286 bitslice = max_bitneed + 1;
287 do {
288 bitslice--;
289 bitcount += slicecount;
290 slicecount = 0;
291 for (ch = 0; ch < 2; ch++) {
292 for (sb = 0; sb < subbands; sb++) {
293 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
294 slicecount++;
295 else if (bitneed[ch][sb] == bitslice + 1)
296 slicecount += 2;
297 }
298 }
299 } while (bitcount + slicecount < frame->bitpool);
300
301 if (bitcount + slicecount == frame->bitpool) {
302 bitcount += slicecount;
303 bitslice--;
304 }
305
306 for (ch = 0; ch < 2; ch++) {
307 for (sb = 0; sb < subbands; sb++) {
308 if (bitneed[ch][sb] < bitslice + 2) {
309 bits[ch][sb] = 0;
310 } else {
311 bits[ch][sb] = bitneed[ch][sb] - bitslice;
312 if (bits[ch][sb] > 16)
313 bits[ch][sb] = 16;
314 }
315 }
316 }
317
318 ch = 0;
319 sb = 0;
320 while (bitcount < frame->bitpool) {
321 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
322 bits[ch][sb]++;
323 bitcount++;
324 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
325 bits[ch][sb] = 2;
326 bitcount += 2;
327 }
328 if (ch == 1) {
329 ch = 0;
330 sb++;
331 if (sb >= subbands)
332 break;
333 } else
334 ch = 1;
335 }
336
337 ch = 0;
338 sb = 0;
339 while (bitcount < frame->bitpool) {
340 if (bits[ch][sb] < 16) {
341 bits[ch][sb]++;
342 bitcount++;
343 }
344 if (ch == 1) {
345 ch = 0;
346 sb++;
347 if (sb >= subbands)
348 break;
349 } else
350 ch = 1;
351 }
352
353 }
354
355 }
356
sbc_calculate_bits(const struct sbc_frame * frame,int (* bits)[8])357 static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8])
358 {
359 if (frame->subbands == 4)
360 sbc_calculate_bits_internal(frame, bits, 4);
361 else
362 sbc_calculate_bits_internal(frame, bits, 8);
363 }
364
365 /*
366 * Unpacks a SBC frame at the beginning of the stream in data,
367 * which has at most len bytes into frame.
368 * Returns the length in bytes of the packed frame, or a negative
369 * value on error. The error codes are:
370 *
371 * -1 Data stream too short
372 * -2 Sync byte incorrect
373 * -3 CRC8 incorrect
374 * -4 Bitpool value out of bounds
375 */
sbc_unpack_frame(const uint8_t * data,struct sbc_frame * frame,size_t len)376 static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
377 size_t len)
378 {
379 unsigned int consumed;
380 /* Will copy the parts of the header that are relevant to crc
381 * calculation here */
382 uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
383 int crc_pos = 0;
384 int32_t temp;
385
386 int audio_sample;
387 int ch, sb, blk, bit; /* channel, subband, block and bit standard
388 counters */
389 int bits[2][8]; /* bits distribution */
390 uint32_t levels[2][8]; /* levels derived from that */
391
392 if (len < 4)
393 return -1;
394
395 if (data[0] != SBC_SYNCWORD)
396 return -2;
397
398 frame->frequency = (data[1] >> 6) & 0x03;
399
400 frame->block_mode = (data[1] >> 4) & 0x03;
401 switch (frame->block_mode) {
402 case SBC_BLK_4:
403 frame->blocks = 4;
404 break;
405 case SBC_BLK_8:
406 frame->blocks = 8;
407 break;
408 case SBC_BLK_12:
409 frame->blocks = 12;
410 break;
411 case SBC_BLK_16:
412 frame->blocks = 16;
413 break;
414 }
415
416 frame->mode = (data[1] >> 2) & 0x03;
417 switch (frame->mode) {
418 case MONO:
419 frame->channels = 1;
420 break;
421 case DUAL_CHANNEL: /* fall-through */
422 case STEREO:
423 case JOINT_STEREO:
424 frame->channels = 2;
425 break;
426 }
427
428 frame->allocation = (data[1] >> 1) & 0x01;
429
430 frame->subband_mode = (data[1] & 0x01);
431 frame->subbands = frame->subband_mode ? 8 : 4;
432
433 frame->bitpool = data[2];
434
435 if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
436 frame->bitpool > 16 * frame->subbands)
437 return -4;
438
439 if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
440 frame->bitpool > 32 * frame->subbands)
441 return -4;
442
443 /* data[3] is crc, we're checking it later */
444
445 consumed = 32;
446
447 crc_header[0] = data[1];
448 crc_header[1] = data[2];
449 crc_pos = 16;
450
451 if (frame->mode == JOINT_STEREO) {
452 if (len * 8 < consumed + frame->subbands)
453 return -1;
454
455 frame->joint = 0x00;
456 for (sb = 0; sb < frame->subbands - 1; sb++)
457 frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb;
458 if (frame->subbands == 4)
459 crc_header[crc_pos / 8] = data[4] & 0xf0;
460 else
461 crc_header[crc_pos / 8] = data[4];
462
463 consumed += frame->subbands;
464 crc_pos += frame->subbands;
465 }
466
467 if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
468 return -1;
469
470 for (ch = 0; ch < frame->channels; ch++) {
471 for (sb = 0; sb < frame->subbands; sb++) {
472 /* FIXME assert(consumed % 4 == 0); */
473 frame->scale_factor[ch][sb] =
474 (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
475 crc_header[crc_pos >> 3] |=
476 frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));
477
478 consumed += 4;
479 crc_pos += 4;
480 }
481 }
482
483 if (data[3] != sbc_crc8(crc_header, crc_pos))
484 return -3;
485
486 sbc_calculate_bits(frame, bits);
487
488 for (ch = 0; ch < frame->channels; ch++) {
489 for (sb = 0; sb < frame->subbands; sb++)
490 levels[ch][sb] = (1 << bits[ch][sb]) - 1;
491 }
492
493 for (blk = 0; blk < frame->blocks; blk++) {
494 for (ch = 0; ch < frame->channels; ch++) {
495 for (sb = 0; sb < frame->subbands; sb++) {
496 if (levels[ch][sb] > 0) {
497 audio_sample = 0;
498 for (bit = 0; bit < bits[ch][sb]; bit++) {
499 if (consumed > len * 8)
500 return -1;
501
502 if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
503 audio_sample |= 1 << (bits[ch][sb] - bit - 1);
504
505 consumed++;
506 }
507
508 frame->sb_sample[blk][ch][sb] =
509 (((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) /
510 levels[ch][sb] - (1 << frame->scale_factor[ch][sb]);
511 } else
512 frame->sb_sample[blk][ch][sb] = 0;
513 }
514 }
515 }
516
517 if (frame->mode == JOINT_STEREO) {
518 for (blk = 0; blk < frame->blocks; blk++) {
519 for (sb = 0; sb < frame->subbands; sb++) {
520 if (frame->joint & (0x01 << sb)) {
521 temp = frame->sb_sample[blk][0][sb] +
522 frame->sb_sample[blk][1][sb];
523 frame->sb_sample[blk][1][sb] =
524 frame->sb_sample[blk][0][sb] -
525 frame->sb_sample[blk][1][sb];
526 frame->sb_sample[blk][0][sb] = temp;
527 }
528 }
529 }
530 }
531
532 if ((consumed & 0x7) != 0)
533 consumed += 8 - (consumed & 0x7);
534
535 return consumed >> 3;
536 }
537
sbc_decoder_init(struct sbc_decoder_state * state,const struct sbc_frame * frame)538 static void sbc_decoder_init(struct sbc_decoder_state *state,
539 const struct sbc_frame *frame)
540 {
541 int i, ch;
542
543 memset(state->V, 0, sizeof(state->V));
544 state->subbands = frame->subbands;
545
546 for (ch = 0; ch < 2; ch++)
547 for (i = 0; i < frame->subbands * 2; i++)
548 state->offset[ch][i] = (10 * i + 10);
549 }
550
sbc_clip16(int32_t s)551 static SBC_ALWAYS_INLINE int16_t sbc_clip16(int32_t s)
552 {
553 if (s > 0x7FFF)
554 return 0x7FFF;
555 else if (s < -0x8000)
556 return -0x8000;
557 else
558 return s;
559 }
560
sbc_synthesize_four(struct sbc_decoder_state * state,struct sbc_frame * frame,int ch,int blk)561 static inline void sbc_synthesize_four(struct sbc_decoder_state *state,
562 struct sbc_frame *frame, int ch, int blk)
563 {
564 int i, k, idx;
565 int32_t *v = state->V[ch];
566 int *offset = state->offset[ch];
567
568 for (i = 0; i < 8; i++) {
569 /* Shifting */
570 offset[i]--;
571 if (offset[i] < 0) {
572 offset[i] = 79;
573 memcpy(v + 80, v, 9 * sizeof(*v));
574 }
575
576 /* Distribute the new matrix value to the shifted position */
577 v[offset[i]] = SCALE4_STAGED1(
578 MULA(synmatrix4[i][0], frame->sb_sample[blk][ch][0],
579 MULA(synmatrix4[i][1], frame->sb_sample[blk][ch][1],
580 MULA(synmatrix4[i][2], frame->sb_sample[blk][ch][2],
581 MUL (synmatrix4[i][3], frame->sb_sample[blk][ch][3])))));
582 }
583
584 /* Compute the samples */
585 for (idx = 0, i = 0; i < 4; i++, idx += 5) {
586 k = (i + 4) & 0xf;
587
588 /* Store in output, Q0 */
589 frame->pcm_sample[ch][blk * 4 + i] = sbc_clip16(SCALE4_STAGED1(
590 MULA(v[offset[i] + 0], sbc_proto_4_40m0[idx + 0],
591 MULA(v[offset[k] + 1], sbc_proto_4_40m1[idx + 0],
592 MULA(v[offset[i] + 2], sbc_proto_4_40m0[idx + 1],
593 MULA(v[offset[k] + 3], sbc_proto_4_40m1[idx + 1],
594 MULA(v[offset[i] + 4], sbc_proto_4_40m0[idx + 2],
595 MULA(v[offset[k] + 5], sbc_proto_4_40m1[idx + 2],
596 MULA(v[offset[i] + 6], sbc_proto_4_40m0[idx + 3],
597 MULA(v[offset[k] + 7], sbc_proto_4_40m1[idx + 3],
598 MULA(v[offset[i] + 8], sbc_proto_4_40m0[idx + 4],
599 MUL( v[offset[k] + 9], sbc_proto_4_40m1[idx + 4]))))))))))));
600 }
601 }
602
sbc_synthesize_eight(struct sbc_decoder_state * state,struct sbc_frame * frame,int ch,int blk)603 static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
604 struct sbc_frame *frame, int ch, int blk)
605 {
606 int i, j, k, idx;
607 int *offset = state->offset[ch];
608
609 for (i = 0; i < 16; i++) {
610 /* Shifting */
611 offset[i]--;
612 if (offset[i] < 0) {
613 offset[i] = 159;
614 for (j = 0; j < 9; j++)
615 state->V[ch][j + 160] = state->V[ch][j];
616 }
617
618 /* Distribute the new matrix value to the shifted position */
619 state->V[ch][offset[i]] = SCALE8_STAGED1(
620 MULA(synmatrix8[i][0], frame->sb_sample[blk][ch][0],
621 MULA(synmatrix8[i][1], frame->sb_sample[blk][ch][1],
622 MULA(synmatrix8[i][2], frame->sb_sample[blk][ch][2],
623 MULA(synmatrix8[i][3], frame->sb_sample[blk][ch][3],
624 MULA(synmatrix8[i][4], frame->sb_sample[blk][ch][4],
625 MULA(synmatrix8[i][5], frame->sb_sample[blk][ch][5],
626 MULA(synmatrix8[i][6], frame->sb_sample[blk][ch][6],
627 MUL( synmatrix8[i][7], frame->sb_sample[blk][ch][7])))))))));
628 }
629
630 /* Compute the samples */
631 for (idx = 0, i = 0; i < 8; i++, idx += 5) {
632 k = (i + 8) & 0xf;
633
634 /* Store in output, Q0 */
635 frame->pcm_sample[ch][blk * 8 + i] = sbc_clip16(SCALE8_STAGED1(
636 MULA(state->V[ch][offset[i] + 0], sbc_proto_8_80m0[idx + 0],
637 MULA(state->V[ch][offset[k] + 1], sbc_proto_8_80m1[idx + 0],
638 MULA(state->V[ch][offset[i] + 2], sbc_proto_8_80m0[idx + 1],
639 MULA(state->V[ch][offset[k] + 3], sbc_proto_8_80m1[idx + 1],
640 MULA(state->V[ch][offset[i] + 4], sbc_proto_8_80m0[idx + 2],
641 MULA(state->V[ch][offset[k] + 5], sbc_proto_8_80m1[idx + 2],
642 MULA(state->V[ch][offset[i] + 6], sbc_proto_8_80m0[idx + 3],
643 MULA(state->V[ch][offset[k] + 7], sbc_proto_8_80m1[idx + 3],
644 MULA(state->V[ch][offset[i] + 8], sbc_proto_8_80m0[idx + 4],
645 MUL( state->V[ch][offset[k] + 9], sbc_proto_8_80m1[idx + 4]))))))))))));
646 }
647 }
648
sbc_synthesize_audio(struct sbc_decoder_state * state,struct sbc_frame * frame)649 static int sbc_synthesize_audio(struct sbc_decoder_state *state,
650 struct sbc_frame *frame)
651 {
652 int ch, blk;
653
654 switch (frame->subbands) {
655 case 4:
656 for (ch = 0; ch < frame->channels; ch++) {
657 for (blk = 0; blk < frame->blocks; blk++)
658 sbc_synthesize_four(state, frame, ch, blk);
659 }
660 return frame->blocks * 4;
661
662 case 8:
663 for (ch = 0; ch < frame->channels; ch++) {
664 for (blk = 0; blk < frame->blocks; blk++)
665 sbc_synthesize_eight(state, frame, ch, blk);
666 }
667 return frame->blocks * 8;
668
669 default:
670 return -EIO;
671 }
672 }
673
sbc_analyze_audio(struct sbc_encoder_state * state,struct sbc_frame * frame)674 static int sbc_analyze_audio(struct sbc_encoder_state *state,
675 struct sbc_frame *frame)
676 {
677 int ch, blk;
678 int16_t *x;
679
680 switch (frame->subbands) {
681 case 4:
682 for (ch = 0; ch < frame->channels; ch++) {
683 x = &state->X[ch][state->position - 16 +
684 frame->blocks * 4];
685 for (blk = 0; blk < frame->blocks; blk += 4) {
686 state->sbc_analyze_4b_4s(
687 x,
688 frame->sb_sample_f[blk][ch],
689 frame->sb_sample_f[blk + 1][ch] -
690 frame->sb_sample_f[blk][ch]);
691 x -= 16;
692 }
693 }
694 return frame->blocks * 4;
695
696 case 8:
697 for (ch = 0; ch < frame->channels; ch++) {
698 x = &state->X[ch][state->position - 32 +
699 frame->blocks * 8];
700 for (blk = 0; blk < frame->blocks; blk += 4) {
701 state->sbc_analyze_4b_8s(
702 x,
703 frame->sb_sample_f[blk][ch],
704 frame->sb_sample_f[blk + 1][ch] -
705 frame->sb_sample_f[blk][ch]);
706 x -= 32;
707 }
708 }
709 return frame->blocks * 8;
710
711 default:
712 return -EIO;
713 }
714 }
715
716 /* Supplementary bitstream writing macros for 'sbc_pack_frame' */
717
718 #define PUT_BITS(data_ptr, bits_cache, bits_count, v, n) \
719 do { \
720 bits_cache = (v) | (bits_cache << (n)); \
721 bits_count += (n); \
722 if (bits_count >= 16) { \
723 bits_count -= 8; \
724 *data_ptr++ = (uint8_t) \
725 (bits_cache >> bits_count); \
726 bits_count -= 8; \
727 *data_ptr++ = (uint8_t) \
728 (bits_cache >> bits_count); \
729 } \
730 } while (0)
731
732 #define FLUSH_BITS(data_ptr, bits_cache, bits_count) \
733 do { \
734 while (bits_count >= 8) { \
735 bits_count -= 8; \
736 *data_ptr++ = (uint8_t) \
737 (bits_cache >> bits_count); \
738 } \
739 if (bits_count > 0) \
740 *data_ptr++ = (uint8_t) \
741 (bits_cache << (8 - bits_count)); \
742 } while (0)
743
744 /*
745 * Packs the SBC frame from frame into the memory at data. At most len
746 * bytes will be used, should more memory be needed an appropriate
747 * error code will be returned. Returns the length of the packed frame
748 * on success or a negative value on error.
749 *
750 * The error codes are:
751 * -1 Not enough memory reserved
752 * -2 Unsupported sampling rate
753 * -3 Unsupported number of blocks
754 * -4 Unsupported number of subbands
755 * -5 Bitpool value out of bounds
756 * -99 not implemented
757 */
758
sbc_pack_frame_internal(uint8_t * data,struct sbc_frame * frame,size_t len,int frame_subbands,int frame_channels,int joint)759 static SBC_ALWAYS_INLINE ssize_t sbc_pack_frame_internal(uint8_t *data,
760 struct sbc_frame *frame, size_t len,
761 int frame_subbands, int frame_channels,
762 int joint)
763 {
764 /* Bitstream writer starts from the fourth byte */
765 uint8_t *data_ptr = data + 4;
766 uint32_t bits_cache = 0;
767 uint32_t bits_count = 0;
768
769 /* Will copy the header parts for CRC-8 calculation here */
770 uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
771 int crc_pos = 0;
772
773 uint32_t audio_sample;
774
775 int ch, sb, blk; /* channel, subband, block and bit counters */
776 int bits[2][8]; /* bits distribution */
777 uint32_t levels[2][8]; /* levels are derived from that */
778 uint32_t sb_sample_delta[2][8];
779
780 data[0] = SBC_SYNCWORD;
781
782 data[1] = (frame->frequency & 0x03) << 6;
783
784 data[1] |= (frame->block_mode & 0x03) << 4;
785
786 data[1] |= (frame->mode & 0x03) << 2;
787
788 data[1] |= (frame->allocation & 0x01) << 1;
789
790 switch (frame_subbands) {
791 case 4:
792 /* Nothing to do */
793 break;
794 case 8:
795 data[1] |= 0x01;
796 break;
797 default:
798 return -4;
799 break;
800 }
801
802 data[2] = frame->bitpool;
803
804 if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
805 frame->bitpool > frame_subbands << 4)
806 return -5;
807
808 if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
809 frame->bitpool > frame_subbands << 5)
810 return -5;
811
812 /* Can't fill in crc yet */
813
814 crc_header[0] = data[1];
815 crc_header[1] = data[2];
816 crc_pos = 16;
817
818 if (frame->mode == JOINT_STEREO) {
819 PUT_BITS(data_ptr, bits_cache, bits_count,
820 joint, frame_subbands);
821 crc_header[crc_pos >> 3] = joint;
822 crc_pos += frame_subbands;
823 }
824
825 for (ch = 0; ch < frame_channels; ch++) {
826 for (sb = 0; sb < frame_subbands; sb++) {
827 PUT_BITS(data_ptr, bits_cache, bits_count,
828 frame->scale_factor[ch][sb] & 0x0F, 4);
829 crc_header[crc_pos >> 3] <<= 4;
830 crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
831 crc_pos += 4;
832 }
833 }
834
835 /* align the last crc byte */
836 if (crc_pos % 8)
837 crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);
838
839 data[3] = sbc_crc8(crc_header, crc_pos);
840
841 sbc_calculate_bits(frame, bits);
842
843 for (ch = 0; ch < frame_channels; ch++) {
844 for (sb = 0; sb < frame_subbands; sb++) {
845 levels[ch][sb] = ((1 << bits[ch][sb]) - 1) <<
846 (32 - (frame->scale_factor[ch][sb] +
847 SCALE_OUT_BITS + 2));
848 sb_sample_delta[ch][sb] = (uint32_t) 1 <<
849 (frame->scale_factor[ch][sb] +
850 SCALE_OUT_BITS + 1);
851 }
852 }
853
854 for (blk = 0; blk < frame->blocks; blk++) {
855 for (ch = 0; ch < frame_channels; ch++) {
856 for (sb = 0; sb < frame_subbands; sb++) {
857
858 if (bits[ch][sb] == 0)
859 continue;
860
861 audio_sample = ((uint64_t) levels[ch][sb] *
862 (sb_sample_delta[ch][sb] +
863 frame->sb_sample_f[blk][ch][sb])) >> 32;
864
865 PUT_BITS(data_ptr, bits_cache, bits_count,
866 audio_sample, bits[ch][sb]);
867 }
868 }
869 }
870
871 FLUSH_BITS(data_ptr, bits_cache, bits_count);
872
873 return data_ptr - data;
874 }
875
sbc_pack_frame(uint8_t * data,struct sbc_frame * frame,size_t len,int joint)876 static ssize_t sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len,
877 int joint)
878 {
879 if (frame->subbands == 4) {
880 if (frame->channels == 1)
881 return sbc_pack_frame_internal(
882 data, frame, len, 4, 1, joint);
883 else
884 return sbc_pack_frame_internal(
885 data, frame, len, 4, 2, joint);
886 } else {
887 if (frame->channels == 1)
888 return sbc_pack_frame_internal(
889 data, frame, len, 8, 1, joint);
890 else
891 return sbc_pack_frame_internal(
892 data, frame, len, 8, 2, joint);
893 }
894 }
895
sbc_encoder_init(struct sbc_encoder_state * state,const struct sbc_frame * frame)896 static void sbc_encoder_init(struct sbc_encoder_state *state,
897 const struct sbc_frame *frame)
898 {
899 memset(&state->X, 0, sizeof(state->X));
900 state->position = (SBC_X_BUFFER_SIZE - frame->subbands * 9) & ~7;
901
902 sbc_init_primitives(state);
903 }
904
905 struct sbc_priv {
906 int init;
907 struct SBC_ALIGNED sbc_frame frame;
908 struct SBC_ALIGNED sbc_decoder_state dec_state;
909 struct SBC_ALIGNED sbc_encoder_state enc_state;
910 };
911
sbc_set_defaults(sbc_t * sbc,unsigned long flags)912 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
913 {
914 sbc->frequency = SBC_FREQ_44100;
915 sbc->mode = SBC_MODE_STEREO;
916 sbc->subbands = SBC_SB_8;
917 sbc->blocks = SBC_BLK_16;
918 sbc->bitpool = 32;
919 #if __BYTE_ORDER == __LITTLE_ENDIAN
920 sbc->endian = SBC_LE;
921 #elif __BYTE_ORDER == __BIG_ENDIAN
922 sbc->endian = SBC_BE;
923 #else
924 #error "Unknown byte order"
925 #endif
926 }
927
sbc_init(sbc_t * sbc,unsigned long flags)928 int sbc_init(sbc_t *sbc, unsigned long flags)
929 {
930 if (!sbc)
931 return -EIO;
932
933 memset(sbc, 0, sizeof(sbc_t));
934
935 sbc->priv_alloc_base = malloc(sizeof(struct sbc_priv) + SBC_ALIGN_MASK);
936 if (!sbc->priv_alloc_base)
937 return -ENOMEM;
938
939 sbc->priv = (void *) (((uintptr_t) sbc->priv_alloc_base +
940 SBC_ALIGN_MASK) & ~((uintptr_t) SBC_ALIGN_MASK));
941
942 memset(sbc->priv, 0, sizeof(struct sbc_priv));
943
944 sbc_set_defaults(sbc, flags);
945
946 return 0;
947 }
948
sbc_parse(sbc_t * sbc,const void * input,size_t input_len)949 ssize_t sbc_parse(sbc_t *sbc, const void *input, size_t input_len)
950 {
951 return sbc_decode(sbc, input, input_len, NULL, 0, NULL);
952 }
953
sbc_decode(sbc_t * sbc,const void * input,size_t input_len,void * output,size_t output_len,size_t * written)954 ssize_t sbc_decode(sbc_t *sbc, const void *input, size_t input_len,
955 void *output, size_t output_len, size_t *written)
956 {
957 struct sbc_priv *priv;
958 char *ptr;
959 int i, ch, framelen, samples;
960
961 if (!sbc || !input)
962 return -EIO;
963
964 priv = sbc->priv;
965
966 framelen = sbc_unpack_frame(input, &priv->frame, input_len);
967
968 if (!priv->init) {
969 sbc_decoder_init(&priv->dec_state, &priv->frame);
970 priv->init = 1;
971
972 sbc->frequency = priv->frame.frequency;
973 sbc->mode = priv->frame.mode;
974 sbc->subbands = priv->frame.subband_mode;
975 sbc->blocks = priv->frame.block_mode;
976 sbc->allocation = priv->frame.allocation;
977 sbc->bitpool = priv->frame.bitpool;
978
979 priv->frame.codesize = sbc_get_codesize(sbc);
980 priv->frame.length = framelen;
981 } else if (priv->frame.bitpool != sbc->bitpool) {
982 priv->frame.length = framelen;
983 sbc->bitpool = priv->frame.bitpool;
984 }
985
986 if (!output)
987 return framelen;
988
989 if (written)
990 *written = 0;
991
992 if (framelen <= 0)
993 return framelen;
994
995 samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
996
997 ptr = output;
998
999 if (output_len < (size_t) (samples * priv->frame.channels * 2))
1000 samples = output_len / (priv->frame.channels * 2);
1001
1002 for (i = 0; i < samples; i++) {
1003 for (ch = 0; ch < priv->frame.channels; ch++) {
1004 int16_t s;
1005 s = priv->frame.pcm_sample[ch][i];
1006
1007 if (sbc->endian == SBC_BE) {
1008 *ptr++ = (s & 0xff00) >> 8;
1009 *ptr++ = (s & 0x00ff);
1010 } else {
1011 *ptr++ = (s & 0x00ff);
1012 *ptr++ = (s & 0xff00) >> 8;
1013 }
1014 }
1015 }
1016
1017 if (written)
1018 *written = samples * priv->frame.channels * 2;
1019
1020 return framelen;
1021 }
1022
sbc_encode(sbc_t * sbc,const void * input,size_t input_len,void * output,size_t output_len,ssize_t * written)1023 ssize_t sbc_encode(sbc_t *sbc, const void *input, size_t input_len,
1024 void *output, size_t output_len, ssize_t *written)
1025 {
1026 struct sbc_priv *priv;
1027 int samples;
1028 ssize_t framelen;
1029 int (*sbc_enc_process_input)(int position,
1030 const uint8_t *pcm, int16_t X[2][SBC_X_BUFFER_SIZE],
1031 int nsamples, int nchannels);
1032
1033 if (!sbc || !input)
1034 return -EIO;
1035
1036 priv = sbc->priv;
1037
1038 if (written)
1039 *written = 0;
1040
1041 if (!priv->init) {
1042 priv->frame.frequency = sbc->frequency;
1043 priv->frame.mode = sbc->mode;
1044 priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1045 priv->frame.allocation = sbc->allocation;
1046 priv->frame.subband_mode = sbc->subbands;
1047 priv->frame.subbands = sbc->subbands ? 8 : 4;
1048 priv->frame.block_mode = sbc->blocks;
1049 priv->frame.blocks = 4 + (sbc->blocks * 4);
1050 priv->frame.bitpool = sbc->bitpool;
1051 priv->frame.codesize = sbc_get_codesize(sbc);
1052 priv->frame.length = sbc_get_frame_length(sbc);
1053
1054 sbc_encoder_init(&priv->enc_state, &priv->frame);
1055 priv->init = 1;
1056 } else if (priv->frame.bitpool != sbc->bitpool) {
1057 priv->frame.length = sbc_get_frame_length(sbc);
1058 priv->frame.bitpool = sbc->bitpool;
1059 }
1060
1061 /* input must be large enough to encode a complete frame */
1062 if (input_len < priv->frame.codesize)
1063 return 0;
1064
1065 /* output must be large enough to receive the encoded frame */
1066 if (!output || output_len < priv->frame.length)
1067 return -ENOSPC;
1068
1069 /* Select the needed input data processing function and call it */
1070 if (priv->frame.subbands == 8) {
1071 if (sbc->endian == SBC_BE)
1072 sbc_enc_process_input =
1073 priv->enc_state.sbc_enc_process_input_8s_be;
1074 else
1075 sbc_enc_process_input =
1076 priv->enc_state.sbc_enc_process_input_8s_le;
1077 } else {
1078 if (sbc->endian == SBC_BE)
1079 sbc_enc_process_input =
1080 priv->enc_state.sbc_enc_process_input_4s_be;
1081 else
1082 sbc_enc_process_input =
1083 priv->enc_state.sbc_enc_process_input_4s_le;
1084 }
1085
1086 priv->enc_state.position = sbc_enc_process_input(
1087 priv->enc_state.position, (const uint8_t *) input,
1088 priv->enc_state.X, priv->frame.subbands * priv->frame.blocks,
1089 priv->frame.channels);
1090
1091 samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
1092
1093 if (priv->frame.mode == JOINT_STEREO) {
1094 int j = priv->enc_state.sbc_calc_scalefactors_j(
1095 priv->frame.sb_sample_f, priv->frame.scale_factor,
1096 priv->frame.blocks, priv->frame.subbands);
1097 framelen = sbc_pack_frame(output, &priv->frame, output_len, j);
1098 } else {
1099 priv->enc_state.sbc_calc_scalefactors(
1100 priv->frame.sb_sample_f, priv->frame.scale_factor,
1101 priv->frame.blocks, priv->frame.channels,
1102 priv->frame.subbands);
1103 framelen = sbc_pack_frame(output, &priv->frame, output_len, 0);
1104 }
1105
1106 if (written)
1107 *written = framelen;
1108
1109 return samples * priv->frame.channels * 2;
1110 }
1111
sbc_finish(sbc_t * sbc)1112 void sbc_finish(sbc_t *sbc)
1113 {
1114 if (!sbc)
1115 return;
1116
1117 free(sbc->priv_alloc_base);
1118
1119 memset(sbc, 0, sizeof(sbc_t));
1120 }
1121
sbc_get_frame_length(sbc_t * sbc)1122 size_t sbc_get_frame_length(sbc_t *sbc)
1123 {
1124 int ret;
1125 uint8_t subbands, channels, blocks, joint, bitpool;
1126 struct sbc_priv *priv;
1127
1128 priv = sbc->priv;
1129 if (priv->init && priv->frame.bitpool == sbc->bitpool)
1130 return priv->frame.length;
1131
1132 subbands = sbc->subbands ? 8 : 4;
1133 blocks = 4 + (sbc->blocks * 4);
1134 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1135 joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;
1136 bitpool = sbc->bitpool;
1137
1138 ret = 4 + (4 * subbands * channels) / 8;
1139 /* This term is not always evenly divide so we round it up */
1140 if (channels == 1)
1141 ret += ((blocks * channels * bitpool) + 7) / 8;
1142 else
1143 ret += (((joint ? subbands : 0) + blocks * bitpool) + 7) / 8;
1144
1145 return ret;
1146 }
1147
sbc_get_frame_duration(sbc_t * sbc)1148 unsigned sbc_get_frame_duration(sbc_t *sbc)
1149 {
1150 uint8_t subbands, blocks;
1151 uint16_t frequency;
1152 struct sbc_priv *priv;
1153
1154 priv = sbc->priv;
1155 if (!priv->init) {
1156 subbands = sbc->subbands ? 8 : 4;
1157 blocks = 4 + (sbc->blocks * 4);
1158 } else {
1159 subbands = priv->frame.subbands;
1160 blocks = priv->frame.blocks;
1161 }
1162
1163 switch (sbc->frequency) {
1164 case SBC_FREQ_16000:
1165 frequency = 16000;
1166 break;
1167
1168 case SBC_FREQ_32000:
1169 frequency = 32000;
1170 break;
1171
1172 case SBC_FREQ_44100:
1173 frequency = 44100;
1174 break;
1175
1176 case SBC_FREQ_48000:
1177 frequency = 48000;
1178 break;
1179 default:
1180 return 0;
1181 }
1182
1183 return (1000000 * blocks * subbands) / frequency;
1184 }
1185
sbc_get_codesize(sbc_t * sbc)1186 size_t sbc_get_codesize(sbc_t *sbc)
1187 {
1188 uint16_t subbands, channels, blocks;
1189 struct sbc_priv *priv;
1190
1191 priv = sbc->priv;
1192 if (!priv->init) {
1193 subbands = sbc->subbands ? 8 : 4;
1194 blocks = 4 + (sbc->blocks * 4);
1195 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1196 } else {
1197 subbands = priv->frame.subbands;
1198 blocks = priv->frame.blocks;
1199 channels = priv->frame.channels;
1200 }
1201
1202 return subbands * blocks * channels * 2;
1203 }
1204
sbc_get_implementation_info(sbc_t * sbc)1205 const char *sbc_get_implementation_info(sbc_t *sbc)
1206 {
1207 struct sbc_priv *priv;
1208
1209 if (!sbc)
1210 return NULL;
1211
1212 priv = sbc->priv;
1213 if (!priv)
1214 return NULL;
1215
1216 return priv->enc_state.implementation_info;
1217 }
1218
sbc_reinit(sbc_t * sbc,unsigned long flags)1219 int sbc_reinit(sbc_t *sbc, unsigned long flags)
1220 {
1221 struct sbc_priv *priv;
1222
1223 if (!sbc || !sbc->priv)
1224 return -EIO;
1225
1226 priv = sbc->priv;
1227
1228 if (priv->init == 1)
1229 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1230
1231 sbc_set_defaults(sbc, flags);
1232
1233 return 0;
1234 }
1235