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