]> code.delx.au - pulseaudio/blob - src/modules/bluetooth/sbc.c
delx.au / code / pulseaudio / blob
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
Merge commit 'coling/master'
[pulseaudio] / src / modules / bluetooth / sbc.c
1 /*
2 *
3 * Bluetooth low-complexity, subband codec (SBC) library
4 *
5 * Copyright (C) 2004-2008 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
44 #include "sbc_math.h"
45 #include "sbc_tables.h"
46
47 #include "sbc.h"
48
49 #define SBC_SYNCWORD 0x9C
50
51 /* This structure contains an unpacked SBC frame.
52 Yes, there is probably quite some unused space herein */
53 struct sbc_frame {
54 uint8_t frequency;
55 uint8_t block_mode;
56 uint8_t blocks;
57 enum {
58 MONO = SBC_MODE_MONO,
59 DUAL_CHANNEL = SBC_MODE_DUAL_CHANNEL,
60 STEREO = SBC_MODE_STEREO,
61 JOINT_STEREO = SBC_MODE_JOINT_STEREO
62 } mode;
63 uint8_t channels;
64 enum {
65 LOUDNESS = SBC_AM_LOUDNESS,
66 SNR = SBC_AM_SNR
67 } allocation;
68 uint8_t subband_mode;
69 uint8_t subbands;
70 uint8_t bitpool;
71 uint8_t codesize;
72 uint8_t length;
73
74 /* bit number x set means joint stereo has been used in subband x */
75 uint8_t joint;
76
77 /* only the lower 4 bits of every element are to be used */
78 uint8_t scale_factor[2][8];
79
80 /* raw integer subband samples in the frame */
81
82 int32_t sb_sample_f[16][2][8];
83 int32_t sb_sample[16][2][8]; /* modified subband samples */
84 int16_t pcm_sample[2][16*8]; /* original pcm audio samples */
85 };
86
87 struct sbc_decoder_state {
88 int subbands;
89 int32_t V[2][170];
90 int offset[2][16];
91 };
92
93 struct sbc_encoder_state {
94 int subbands;
95 int position[2];
96 int32_t X[2][160];
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
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 unsigned 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 */
163 static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8])
164 {
165 uint8_t sf = frame->frequency;
166
167 if (frame->mode == MONO || frame->mode == DUAL_CHANNEL) {
168 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
169 int ch, sb;
170
171 for (ch = 0; ch < frame->channels; ch++) {
172 max_bitneed = 0;
173 if (frame->allocation == SNR) {
174 for (sb = 0; sb < frame->subbands; sb++) {
175 bitneed[ch][sb] = frame->scale_factor[ch][sb];
176 if (bitneed[ch][sb] > max_bitneed)
177 max_bitneed = bitneed[ch][sb];
178 }
179 } else {
180 for (sb = 0; sb < frame->subbands; sb++) {
181 if (frame->scale_factor[ch][sb] == 0)
182 bitneed[ch][sb] = -5;
183 else {
184 if (frame->subbands == 4)
185 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
186 else
187 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
188 if (loudness > 0)
189 bitneed[ch][sb] = loudness / 2;
190 else
191 bitneed[ch][sb] = loudness;
192 }
193 if (bitneed[ch][sb] > max_bitneed)
194 max_bitneed = bitneed[ch][sb];
195 }
196 }
197
198 bitcount = 0;
199 slicecount = 0;
200 bitslice = max_bitneed + 1;
201 do {
202 bitslice--;
203 bitcount += slicecount;
204 slicecount = 0;
205 for (sb = 0; sb < frame->subbands; sb++) {
206 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
207 slicecount++;
208 else if (bitneed[ch][sb] == bitslice + 1)
209 slicecount += 2;
210 }
211 } while (bitcount + slicecount < frame->bitpool);
212
213 if (bitcount + slicecount == frame->bitpool) {
214 bitcount += slicecount;
215 bitslice--;
216 }
217
218 for (sb = 0; sb < frame->subbands; sb++) {
219 if (bitneed[ch][sb] < bitslice + 2)
220 bits[ch][sb] = 0;
221 else {
222 bits[ch][sb] = bitneed[ch][sb] - bitslice;
223 if (bits[ch][sb] > 16)
224 bits[ch][sb] = 16;
225 }
226 }
227
228 for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
229 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
230 bits[ch][sb]++;
231 bitcount++;
232 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
233 bits[ch][sb] = 2;
234 bitcount += 2;
235 }
236 }
237
238 for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
239 if (bits[ch][sb] < 16) {
240 bits[ch][sb]++;
241 bitcount++;
242 }
243 }
244
245 }
246
247 } else if (frame->mode == STEREO || frame->mode == JOINT_STEREO) {
248 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
249 int ch, sb;
250
251 max_bitneed = 0;
252 if (frame->allocation == SNR) {
253 for (ch = 0; ch < 2; ch++) {
254 for (sb = 0; sb < frame->subbands; sb++) {
255 bitneed[ch][sb] = frame->scale_factor[ch][sb];
256 if (bitneed[ch][sb] > max_bitneed)
257 max_bitneed = bitneed[ch][sb];
258 }
259 }
260 } else {
261 for (ch = 0; ch < 2; ch++) {
262 for (sb = 0; sb < frame->subbands; sb++) {
263 if (frame->scale_factor[ch][sb] == 0)
264 bitneed[ch][sb] = -5;
265 else {
266 if (frame->subbands == 4)
267 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
268 else
269 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
270 if (loudness > 0)
271 bitneed[ch][sb] = loudness / 2;
272 else
273 bitneed[ch][sb] = loudness;
274 }
275 if (bitneed[ch][sb] > max_bitneed)
276 max_bitneed = bitneed[ch][sb];
277 }
278 }
279 }
280
281 bitcount = 0;
282 slicecount = 0;
283 bitslice = max_bitneed + 1;
284 do {
285 bitslice--;
286 bitcount += slicecount;
287 slicecount = 0;
288 for (ch = 0; ch < 2; ch++) {
289 for (sb = 0; sb < frame->subbands; sb++) {
290 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
291 slicecount++;
292 else if (bitneed[ch][sb] == bitslice + 1)
293 slicecount += 2;
294 }
295 }
296 } while (bitcount + slicecount < frame->bitpool);
297
298 if (bitcount + slicecount == frame->bitpool) {
299 bitcount += slicecount;
300 bitslice--;
301 }
302
303 for (ch = 0; ch < 2; ch++) {
304 for (sb = 0; sb < frame->subbands; sb++) {
305 if (bitneed[ch][sb] < bitslice + 2) {
306 bits[ch][sb] = 0;
307 } else {
308 bits[ch][sb] = bitneed[ch][sb] - bitslice;
309 if (bits[ch][sb] > 16)
310 bits[ch][sb] = 16;
311 }
312 }
313 }
314
315 ch = 0;
316 sb = 0;
317 while (bitcount < frame->bitpool) {
318 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
319 bits[ch][sb]++;
320 bitcount++;
321 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
322 bits[ch][sb] = 2;
323 bitcount += 2;
324 }
325 if (ch == 1) {
326 ch = 0;
327 sb++;
328 if (sb >= frame->subbands) break;
329 } else
330 ch = 1;
331 }
332
333 ch = 0;
334 sb = 0;
335 while (bitcount < frame->bitpool) {
336 if (bits[ch][sb] < 16) {
337 bits[ch][sb]++;
338 bitcount++;
339 }
340 if (ch == 1) {
341 ch = 0;
342 sb++;
343 if (sb >= frame->subbands) break;
344 } else
345 ch = 1;
346 }
347
348 }
349
350 }
351
352 /*
353 * Unpacks a SBC frame at the beginning of the stream in data,
354 * which has at most len bytes into frame.
355 * Returns the length in bytes of the packed frame, or a negative
356 * value on error. The error codes are:
357 *
358 * -1 Data stream too short
359 * -2 Sync byte incorrect
360 * -3 CRC8 incorrect
361 * -4 Bitpool value out of bounds
362 */
363 static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
364 size_t len)
365 {
366 int consumed;
367 /* Will copy the parts of the header that are relevant to crc
368 * calculation here */
369 uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
370 int crc_pos = 0;
371 int32_t temp;
372
373 int audio_sample;
374 int ch, sb, blk, bit; /* channel, subband, block and bit standard
375 counters */
376 int bits[2][8]; /* bits distribution */
377 uint32_t levels[2][8]; /* levels derived from that */
378
379 if (len < 4)
380 return -1;
381
382 if (data[0] != SBC_SYNCWORD)
383 return -2;
384
385 frame->frequency = (data[1] >> 6) & 0x03;
386
387 frame->block_mode = (data[1] >> 4) & 0x03;
388 switch (frame->block_mode) {
389 case SBC_BLK_4:
390 frame->blocks = 4;
391 break;
392 case SBC_BLK_8:
393 frame->blocks = 8;
394 break;
395 case SBC_BLK_12:
396 frame->blocks = 12;
397 break;
398 case SBC_BLK_16:
399 frame->blocks = 16;
400 break;
401 }
402
403 frame->mode = (data[1] >> 2) & 0x03;
404 switch (frame->mode) {
405 case MONO:
406 frame->channels = 1;
407 break;
408 case DUAL_CHANNEL: /* fall-through */
409 case STEREO:
410 case JOINT_STEREO:
411 frame->channels = 2;
412 break;
413 }
414
415 frame->allocation = (data[1] >> 1) & 0x01;
416
417 frame->subband_mode = (data[1] & 0x01);
418 frame->subbands = frame->subband_mode ? 8 : 4;
419
420 frame->bitpool = data[2];
421
422 if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
423 frame->bitpool > 16 * frame->subbands)
424 return -4;
425
426 if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
427 frame->bitpool > 32 * frame->subbands)
428 return -4;
429
430 /* data[3] is crc, we're checking it later */
431
432 consumed = 32;
433
434 crc_header[0] = data[1];
435 crc_header[1] = data[2];
436 crc_pos = 16;
437
438 if (frame->mode == JOINT_STEREO) {
439 if (len * 8 < consumed + frame->subbands)
440 return -1;
441
442 frame->joint = 0x00;
443 for (sb = 0; sb < frame->subbands - 1; sb++)
444 frame->joint |= ((data[4] >> (7 - sb)) & 0x01) << sb;
445 if (frame->subbands == 4)
446 crc_header[crc_pos / 8] = data[4] & 0xf0;
447 else
448 crc_header[crc_pos / 8] = data[4];
449
450 consumed += frame->subbands;
451 crc_pos += frame->subbands;
452 }
453
454 if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
455 return -1;
456
457 for (ch = 0; ch < frame->channels; ch++) {
458 for (sb = 0; sb < frame->subbands; sb++) {
459 /* FIXME assert(consumed % 4 == 0); */
460 frame->scale_factor[ch][sb] =
461 (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
462 crc_header[crc_pos >> 3] |=
463 frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));
464
465 consumed += 4;
466 crc_pos += 4;
467 }
468 }
469
470 if (data[3] != sbc_crc8(crc_header, crc_pos))
471 return -3;
472
473 sbc_calculate_bits(frame, bits);
474
475 for (ch = 0; ch < frame->channels; ch++) {
476 for (sb = 0; sb < frame->subbands; sb++)
477 levels[ch][sb] = (1 << bits[ch][sb]) - 1;
478 }
479
480 for (blk = 0; blk < frame->blocks; blk++) {
481 for (ch = 0; ch < frame->channels; ch++) {
482 for (sb = 0; sb < frame->subbands; sb++) {
483 if (levels[ch][sb] > 0) {
484 audio_sample = 0;
485 for (bit = 0; bit < bits[ch][sb]; bit++) {
486 if (consumed > len * 8)
487 return -1;
488
489 if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
490 audio_sample |= 1 << (bits[ch][sb] - bit - 1);
491
492 consumed++;
493 }
494
495 frame->sb_sample[blk][ch][sb] =
496 (((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) /
497 levels[ch][sb] - (1 << frame->scale_factor[ch][sb]);
498 } else
499 frame->sb_sample[blk][ch][sb] = 0;
500 }
501 }
502 }
503
504 if (frame->mode == JOINT_STEREO) {
505 for (blk = 0; blk < frame->blocks; blk++) {
506 for (sb = 0; sb < frame->subbands; sb++) {
507 if (frame->joint & (0x01 << sb)) {
508 temp = frame->sb_sample[blk][0][sb] +
509 frame->sb_sample[blk][1][sb];
510 frame->sb_sample[blk][1][sb] =
511 frame->sb_sample[blk][0][sb] -
512 frame->sb_sample[blk][1][sb];
513 frame->sb_sample[blk][0][sb] = temp;
514 }
515 }
516 }
517 }
518
519 if ((consumed & 0x7) != 0)
520 consumed += 8 - (consumed & 0x7);
521
522 return consumed >> 3;
523 }
524
525 static void sbc_decoder_init(struct sbc_decoder_state *state,
526 const struct sbc_frame *frame)
527 {
528 int i, ch;
529
530 memset(state->V, 0, sizeof(state->V));
531 state->subbands = frame->subbands;
532
533 for (ch = 0; ch < 2; ch++)
534 for (i = 0; i < frame->subbands * 2; i++)
535 state->offset[ch][i] = (10 * i + 10);
536 }
537
538 static inline void sbc_synthesize_four(struct sbc_decoder_state *state,
539 struct sbc_frame *frame, int ch, int blk)
540 {
541 int i, k, idx;
542 int32_t *v = state->V[ch];
543 int *offset = state->offset[ch];
544
545 for (i = 0; i < 8; i++) {
546 /* Shifting */
547 offset[i]--;
548 if (offset[i] < 0) {
549 offset[i] = 79;
550 memcpy(v + 80, v, 9 * sizeof(*v));
551 }
552
553 /* Distribute the new matrix value to the shifted position */
554 v[offset[i]] = SCALE4_STAGED1(
555 MULA(synmatrix4[i][0], frame->sb_sample[blk][ch][0],
556 MULA(synmatrix4[i][1], frame->sb_sample[blk][ch][1],
557 MULA(synmatrix4[i][2], frame->sb_sample[blk][ch][2],
558 MUL (synmatrix4[i][3], frame->sb_sample[blk][ch][3])))));
559 }
560
561 /* Compute the samples */
562 for (idx = 0, i = 0; i < 4; i++, idx += 5) {
563 k = (i + 4) & 0xf;
564
565 /* Store in output, Q0 */
566 frame->pcm_sample[ch][blk * 4 + i] = SCALE4_STAGED2(
567 MULA(v[offset[i] + 0], sbc_proto_4_40m0[idx + 0],
568 MULA(v[offset[k] + 1], sbc_proto_4_40m1[idx + 0],
569 MULA(v[offset[i] + 2], sbc_proto_4_40m0[idx + 1],
570 MULA(v[offset[k] + 3], sbc_proto_4_40m1[idx + 1],
571 MULA(v[offset[i] + 4], sbc_proto_4_40m0[idx + 2],
572 MULA(v[offset[k] + 5], sbc_proto_4_40m1[idx + 2],
573 MULA(v[offset[i] + 6], sbc_proto_4_40m0[idx + 3],
574 MULA(v[offset[k] + 7], sbc_proto_4_40m1[idx + 3],
575 MULA(v[offset[i] + 8], sbc_proto_4_40m0[idx + 4],
576 MUL( v[offset[k] + 9], sbc_proto_4_40m1[idx + 4])))))))))));
577 }
578 }
579
580 static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
581 struct sbc_frame *frame, int ch, int blk)
582 {
583 int i, j, k, idx;
584 int *offset = state->offset[ch];
585
586 for (i = 0; i < 16; i++) {
587 /* Shifting */
588 offset[i]--;
589 if (offset[i] < 0) {
590 offset[i] = 159;
591 for (j = 0; j < 9; j++)
592 state->V[ch][j + 160] = state->V[ch][j];
593 }
594
595 /* Distribute the new matrix value to the shifted position */
596 state->V[ch][offset[i]] = SCALE8_STAGED1(
597 MULA(synmatrix8[i][0], frame->sb_sample[blk][ch][0],
598 MULA(synmatrix8[i][1], frame->sb_sample[blk][ch][1],
599 MULA(synmatrix8[i][2], frame->sb_sample[blk][ch][2],
600 MULA(synmatrix8[i][3], frame->sb_sample[blk][ch][3],
601 MULA(synmatrix8[i][4], frame->sb_sample[blk][ch][4],
602 MULA(synmatrix8[i][5], frame->sb_sample[blk][ch][5],
603 MULA(synmatrix8[i][6], frame->sb_sample[blk][ch][6],
604 MUL( synmatrix8[i][7], frame->sb_sample[blk][ch][7])))))))));
605 }
606
607 /* Compute the samples */
608 for (idx = 0, i = 0; i < 8; i++, idx += 5) {
609 k = (i + 8) & 0xf;
610
611 /* Store in output */
612 frame->pcm_sample[ch][blk * 8 + i] = SCALE8_STAGED2( // Q0
613 MULA(state->V[ch][offset[i] + 0], sbc_proto_8_80m0[idx + 0],
614 MULA(state->V[ch][offset[k] + 1], sbc_proto_8_80m1[idx + 0],
615 MULA(state->V[ch][offset[i] + 2], sbc_proto_8_80m0[idx + 1],
616 MULA(state->V[ch][offset[k] + 3], sbc_proto_8_80m1[idx + 1],
617 MULA(state->V[ch][offset[i] + 4], sbc_proto_8_80m0[idx + 2],
618 MULA(state->V[ch][offset[k] + 5], sbc_proto_8_80m1[idx + 2],
619 MULA(state->V[ch][offset[i] + 6], sbc_proto_8_80m0[idx + 3],
620 MULA(state->V[ch][offset[k] + 7], sbc_proto_8_80m1[idx + 3],
621 MULA(state->V[ch][offset[i] + 8], sbc_proto_8_80m0[idx + 4],
622 MUL( state->V[ch][offset[k] + 9], sbc_proto_8_80m1[idx + 4])))))))))));
623 }
624 }
625
626 static int sbc_synthesize_audio(struct sbc_decoder_state *state,
627 struct sbc_frame *frame)
628 {
629 int ch, blk;
630
631 switch (frame->subbands) {
632 case 4:
633 for (ch = 0; ch < frame->channels; ch++) {
634 for (blk = 0; blk < frame->blocks; blk++)
635 sbc_synthesize_four(state, frame, ch, blk);
636 }
637 return frame->blocks * 4;
638
639 case 8:
640 for (ch = 0; ch < frame->channels; ch++) {
641 for (blk = 0; blk < frame->blocks; blk++)
642 sbc_synthesize_eight(state, frame, ch, blk);
643 }
644 return frame->blocks * 8;
645
646 default:
647 return -EIO;
648 }
649 }
650
651 static void sbc_encoder_init(struct sbc_encoder_state *state,
652 const struct sbc_frame *frame)
653 {
654 memset(&state->X, 0, sizeof(state->X));
655 state->subbands = frame->subbands;
656 state->position[0] = state->position[1] = 9 * frame->subbands;
657 }
658
659 static inline void _sbc_analyze_four(const int32_t *in, int32_t *out)
660 {
661 sbc_fixed_t t[8], s[5];
662
663 t[0] = SCALE4_STAGE1( /* Q8 */
664 MULA(_sbc_proto_4[0], in[8] - in[32], /* Q18 */
665 MUL( _sbc_proto_4[1], in[16] - in[24])));
666
667 t[1] = SCALE4_STAGE1(
668 MULA(_sbc_proto_4[2], in[1],
669 MULA(_sbc_proto_4[3], in[9],
670 MULA(_sbc_proto_4[4], in[17],
671 MULA(_sbc_proto_4[5], in[25],
672 MUL( _sbc_proto_4[6], in[33]))))));
673
674 t[2] = SCALE4_STAGE1(
675 MULA(_sbc_proto_4[7], in[2],
676 MULA(_sbc_proto_4[8], in[10],
677 MULA(_sbc_proto_4[9], in[18],
678 MULA(_sbc_proto_4[10], in[26],
679 MUL( _sbc_proto_4[11], in[34]))))));
680
681 t[3] = SCALE4_STAGE1(
682 MULA(_sbc_proto_4[12], in[3],
683 MULA(_sbc_proto_4[13], in[11],
684 MULA(_sbc_proto_4[14], in[19],
685 MULA(_sbc_proto_4[15], in[27],
686 MUL( _sbc_proto_4[16], in[35]))))));
687
688 t[4] = SCALE4_STAGE1(
689 MULA(_sbc_proto_4[17], in[4] + in[36],
690 MULA(_sbc_proto_4[18], in[12] + in[28],
691 MUL( _sbc_proto_4[19], in[20]))));
692
693 t[5] = SCALE4_STAGE1(
694 MULA(_sbc_proto_4[16], in[5],
695 MULA(_sbc_proto_4[15], in[13],
696 MULA(_sbc_proto_4[14], in[21],
697 MULA(_sbc_proto_4[13], in[29],
698 MUL( _sbc_proto_4[12], in[37]))))));
699
700 /* don't compute t[6]... this term always multiplies
701 * with cos(pi/2) = 0 */
702
703 t[7] = SCALE4_STAGE1(
704 MULA(_sbc_proto_4[6], in[7],
705 MULA(_sbc_proto_4[5], in[15],
706 MULA(_sbc_proto_4[4], in[23],
707 MULA(_sbc_proto_4[3], in[31],
708 MUL( _sbc_proto_4[2], in[39]))))));
709
710 s[0] = MUL( _anamatrix4[0], t[0] + t[4]);
711 s[1] = MUL( _anamatrix4[2], t[2]);
712 s[2] = MULA(_anamatrix4[1], t[1] + t[3],
713 MUL(_anamatrix4[3], t[5]));
714 s[3] = MULA(_anamatrix4[3], t[1] + t[3],
715 MUL(_anamatrix4[1], -t[5] + t[7]));
716 s[4] = MUL( _anamatrix4[3], t[7]);
717
718 out[0] = SCALE4_STAGE2( s[0] + s[1] + s[2] + s[4]); /* Q0 */
719 out[1] = SCALE4_STAGE2(-s[0] + s[1] + s[3]);
720 out[2] = SCALE4_STAGE2(-s[0] + s[1] - s[3]);
721 out[3] = SCALE4_STAGE2( s[0] + s[1] - s[2] - s[4]);
722 }
723
724 static inline void sbc_analyze_four(struct sbc_encoder_state *state,
725 struct sbc_frame *frame, int ch, int blk)
726 {
727 int32_t *x = &state->X[ch][state->position[ch]];
728 int16_t *pcm = &frame->pcm_sample[ch][blk * 4];
729
730 /* Input 4 Audio Samples */
731 x[40] = x[0] = pcm[3];
732 x[41] = x[1] = pcm[2];
733 x[42] = x[2] = pcm[1];
734 x[43] = x[3] = pcm[0];
735
736 _sbc_analyze_four(x, frame->sb_sample_f[blk][ch]);
737
738 state->position[ch] -= 4;
739 if (state->position[ch] < 0)
740 state->position[ch] = 36;
741 }
742
743 static inline void _sbc_analyze_eight(const int32_t *in, int32_t *out)
744 {
745 sbc_fixed_t t[8], s[8];
746
747 t[0] = SCALE8_STAGE1( /* Q10 */
748 MULA(_sbc_proto_8[0], (in[16] - in[64]), /* Q18 = Q18 * Q0 */
749 MULA(_sbc_proto_8[1], (in[32] - in[48]),
750 MULA(_sbc_proto_8[2], in[4],
751 MULA(_sbc_proto_8[3], in[20],
752 MULA(_sbc_proto_8[4], in[36],
753 MUL( _sbc_proto_8[5], in[52])))))));
754
755 t[1] = SCALE8_STAGE1(
756 MULA(_sbc_proto_8[6], in[2],
757 MULA(_sbc_proto_8[7], in[18],
758 MULA(_sbc_proto_8[8], in[34],
759 MULA(_sbc_proto_8[9], in[50],
760 MUL(_sbc_proto_8[10], in[66]))))));
761
762 t[2] = SCALE8_STAGE1(
763 MULA(_sbc_proto_8[11], in[1],
764 MULA(_sbc_proto_8[12], in[17],
765 MULA(_sbc_proto_8[13], in[33],
766 MULA(_sbc_proto_8[14], in[49],
767 MULA(_sbc_proto_8[15], in[65],
768 MULA(_sbc_proto_8[16], in[3],
769 MULA(_sbc_proto_8[17], in[19],
770 MULA(_sbc_proto_8[18], in[35],
771 MULA(_sbc_proto_8[19], in[51],
772 MUL( _sbc_proto_8[20], in[67])))))))))));
773
774 t[3] = SCALE8_STAGE1(
775 MULA( _sbc_proto_8[21], in[5],
776 MULA( _sbc_proto_8[22], in[21],
777 MULA( _sbc_proto_8[23], in[37],
778 MULA( _sbc_proto_8[24], in[53],
779 MULA( _sbc_proto_8[25], in[69],
780 MULA(-_sbc_proto_8[15], in[15],
781 MULA(-_sbc_proto_8[14], in[31],
782 MULA(-_sbc_proto_8[13], in[47],
783 MULA(-_sbc_proto_8[12], in[63],
784 MUL( -_sbc_proto_8[11], in[79])))))))))));
785
786 t[4] = SCALE8_STAGE1(
787 MULA( _sbc_proto_8[26], in[6],
788 MULA( _sbc_proto_8[27], in[22],
789 MULA( _sbc_proto_8[28], in[38],
790 MULA( _sbc_proto_8[29], in[54],
791 MULA( _sbc_proto_8[30], in[70],
792 MULA(-_sbc_proto_8[10], in[14],
793 MULA(-_sbc_proto_8[9], in[30],
794 MULA(-_sbc_proto_8[8], in[46],
795 MULA(-_sbc_proto_8[7], in[62],
796 MUL( -_sbc_proto_8[6], in[78])))))))))));
797
798 t[5] = SCALE8_STAGE1(
799 MULA( _sbc_proto_8[31], in[7],
800 MULA( _sbc_proto_8[32], in[23],
801 MULA( _sbc_proto_8[33], in[39],
802 MULA( _sbc_proto_8[34], in[55],
803 MULA( _sbc_proto_8[35], in[71],
804 MULA(-_sbc_proto_8[20], in[13],
805 MULA(-_sbc_proto_8[19], in[29],
806 MULA(-_sbc_proto_8[18], in[45],
807 MULA(-_sbc_proto_8[17], in[61],
808 MUL( -_sbc_proto_8[16], in[77])))))))))));
809
810 t[6] = SCALE8_STAGE1(
811 MULA( _sbc_proto_8[36], (in[8] + in[72]),
812 MULA( _sbc_proto_8[37], (in[24] + in[56]),
813 MULA( _sbc_proto_8[38], in[40],
814 MULA(-_sbc_proto_8[39], in[12],
815 MULA(-_sbc_proto_8[5], in[28],
816 MULA(-_sbc_proto_8[4], in[44],
817 MULA(-_sbc_proto_8[3], in[60],
818 MUL( -_sbc_proto_8[2], in[76])))))))));
819
820 t[7] = SCALE8_STAGE1(
821 MULA( _sbc_proto_8[35], in[9],
822 MULA( _sbc_proto_8[34], in[25],
823 MULA( _sbc_proto_8[33], in[41],
824 MULA( _sbc_proto_8[32], in[57],
825 MULA( _sbc_proto_8[31], in[73],
826 MULA(-_sbc_proto_8[25], in[11],
827 MULA(-_sbc_proto_8[24], in[27],
828 MULA(-_sbc_proto_8[23], in[43],
829 MULA(-_sbc_proto_8[22], in[59],
830 MUL( -_sbc_proto_8[21], in[75])))))))))));
831
832 s[0] = MULA( _anamatrix8[0], t[0],
833 MUL( _anamatrix8[1], t[6]));
834 s[1] = MUL( _anamatrix8[7], t[1]);
835 s[2] = MULA( _anamatrix8[2], t[2],
836 MULA( _anamatrix8[3], t[3],
837 MULA( _anamatrix8[4], t[5],
838 MUL( _anamatrix8[5], t[7]))));
839 s[3] = MUL( _anamatrix8[6], t[4]);
840 s[4] = MULA( _anamatrix8[3], t[2],
841 MULA(-_anamatrix8[5], t[3],
842 MULA(-_anamatrix8[2], t[5],
843 MUL( -_anamatrix8[4], t[7]))));
844 s[5] = MULA( _anamatrix8[4], t[2],
845 MULA(-_anamatrix8[2], t[3],
846 MULA( _anamatrix8[5], t[5],
847 MUL( _anamatrix8[3], t[7]))));
848 s[6] = MULA( _anamatrix8[1], t[0],
849 MUL( -_anamatrix8[0], t[6]));
850 s[7] = MULA( _anamatrix8[5], t[2],
851 MULA(-_anamatrix8[4], t[3],
852 MULA( _anamatrix8[3], t[5],
853 MUL( -_anamatrix8[2], t[7]))));
854
855 out[0] = SCALE8_STAGE2( s[0] + s[1] + s[2] + s[3]);
856 out[1] = SCALE8_STAGE2( s[1] - s[3] + s[4] + s[6]);
857 out[2] = SCALE8_STAGE2( s[1] - s[3] + s[5] - s[6]);
858 out[3] = SCALE8_STAGE2(-s[0] + s[1] + s[3] + s[7]);
859 out[4] = SCALE8_STAGE2(-s[0] + s[1] + s[3] - s[7]);
860 out[5] = SCALE8_STAGE2( s[1] - s[3] - s[5] - s[6]);
861 out[6] = SCALE8_STAGE2( s[1] - s[3] - s[4] + s[6]);
862 out[7] = SCALE8_STAGE2( s[0] + s[1] - s[2] + s[3]);
863 }
864
865 static inline void sbc_analyze_eight(struct sbc_encoder_state *state,
866 struct sbc_frame *frame, int ch,
867 int blk)
868 {
869 int32_t *x = &state->X[ch][state->position[ch]];
870 int16_t *pcm = &frame->pcm_sample[ch][blk * 8];
871
872 /* Input 8 Audio Samples */
873 x[80] = x[0] = pcm[7];
874 x[81] = x[1] = pcm[6];
875 x[82] = x[2] = pcm[5];
876 x[83] = x[3] = pcm[4];
877 x[84] = x[4] = pcm[3];
878 x[85] = x[5] = pcm[2];
879 x[86] = x[6] = pcm[1];
880 x[87] = x[7] = pcm[0];
881
882 _sbc_analyze_eight(x, frame->sb_sample_f[blk][ch]);
883
884 state->position[ch] -= 8;
885 if (state->position[ch] < 0)
886 state->position[ch] = 72;
887 }
888
889 static int sbc_analyze_audio(struct sbc_encoder_state *state,
890 struct sbc_frame *frame)
891 {
892 int ch, blk;
893
894 switch (frame->subbands) {
895 case 4:
896 for (ch = 0; ch < frame->channels; ch++)
897 for (blk = 0; blk < frame->blocks; blk++)
898 sbc_analyze_four(state, frame, ch, blk);
899 return frame->blocks * 4;
900
901 case 8:
902 for (ch = 0; ch < frame->channels; ch++)
903 for (blk = 0; blk < frame->blocks; blk++)
904 sbc_analyze_eight(state, frame, ch, blk);
905 return frame->blocks * 8;
906
907 default:
908 return -EIO;
909 }
910 }
911
912 /*
913 * Packs the SBC frame from frame into the memory at data. At most len
914 * bytes will be used, should more memory be needed an appropriate
915 * error code will be returned. Returns the length of the packed frame
916 * on success or a negative value on error.
917 *
918 * The error codes are:
919 * -1 Not enough memory reserved
920 * -2 Unsupported sampling rate
921 * -3 Unsupported number of blocks
922 * -4 Unsupported number of subbands
923 * -5 Bitpool value out of bounds
924 * -99 not implemented
925 */
926
927 static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len)
928 {
929 int produced;
930 /* Will copy the header parts for CRC-8 calculation here */
931 uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
932 int crc_pos = 0;
933
934 uint16_t audio_sample;
935
936 int ch, sb, blk, bit; /* channel, subband, block and bit counters */
937 int bits[2][8]; /* bits distribution */
938 int levels[2][8]; /* levels are derived from that */
939
940 u_int32_t scalefactor[2][8]; /* derived from frame->scale_factor */
941
942 data[0] = SBC_SYNCWORD;
943
944 data[1] = (frame->frequency & 0x03) << 6;
945
946 data[1] |= (frame->block_mode & 0x03) << 4;
947
948 data[1] |= (frame->mode & 0x03) << 2;
949
950 data[1] |= (frame->allocation & 0x01) << 1;
951
952 switch (frame->subbands) {
953 case 4:
954 /* Nothing to do */
955 break;
956 case 8:
957 data[1] |= 0x01;
958 break;
959 default:
960 return -4;
961 break;
962 }
963
964 data[2] = frame->bitpool;
965
966 if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
967 frame->bitpool > frame->subbands << 4)
968 return -5;
969
970 if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
971 frame->bitpool > frame->subbands << 5)
972 return -5;
973
974 /* Can't fill in crc yet */
975
976 produced = 32;
977
978 crc_header[0] = data[1];
979 crc_header[1] = data[2];
980 crc_pos = 16;
981
982 for (ch = 0; ch < frame->channels; ch++) {
983 for (sb = 0; sb < frame->subbands; sb++) {
984 frame->scale_factor[ch][sb] = 0;
985 scalefactor[ch][sb] = 2;
986 for (blk = 0; blk < frame->blocks; blk++) {
987 while (scalefactor[ch][sb] < fabs(frame->sb_sample_f[blk][ch][sb])) {
988 frame->scale_factor[ch][sb]++;
989 scalefactor[ch][sb] *= 2;
990 }
991 }
992 }
993 }
994
995 if (frame->mode == JOINT_STEREO) {
996 /* like frame->sb_sample but joint stereo */
997 int32_t sb_sample_j[16][2];
998 /* scalefactor and scale_factor in joint case */
999 u_int32_t scalefactor_j[2];
1000 uint8_t scale_factor_j[2];
1001
1002 frame->joint = 0;
1003
1004 for (sb = 0; sb < frame->subbands - 1; sb++) {
1005 scale_factor_j[0] = 0;
1006 scalefactor_j[0] = 2;
1007 scale_factor_j[1] = 0;
1008 scalefactor_j[1] = 2;
1009
1010 for (blk = 0; blk < frame->blocks; blk++) {
1011 /* Calculate joint stereo signal */
1012 sb_sample_j[blk][0] =
1013 (frame->sb_sample_f[blk][0][sb] +
1014 frame->sb_sample_f[blk][1][sb]) >> 1;
1015 sb_sample_j[blk][1] =
1016 (frame->sb_sample_f[blk][0][sb] -
1017 frame->sb_sample_f[blk][1][sb]) >> 1;
1018
1019 /* calculate scale_factor_j and scalefactor_j for joint case */
1020 while (scalefactor_j[0] < fabs(sb_sample_j[blk][0])) {
1021 scale_factor_j[0]++;
1022 scalefactor_j[0] *= 2;
1023 }
1024 while (scalefactor_j[1] < fabs(sb_sample_j[blk][1])) {
1025 scale_factor_j[1]++;
1026 scalefactor_j[1] *= 2;
1027 }
1028 }
1029
1030 /* decide whether to join this subband */
1031 if ((scalefactor[0][sb] + scalefactor[1][sb]) >
1032 (scalefactor_j[0] + scalefactor_j[1]) ) {
1033 /* use joint stereo for this subband */
1034 frame->joint |= 1 << sb;
1035 frame->scale_factor[0][sb] = scale_factor_j[0];
1036 frame->scale_factor[1][sb] = scale_factor_j[1];
1037 scalefactor[0][sb] = scalefactor_j[0];
1038 scalefactor[1][sb] = scalefactor_j[1];
1039 for (blk = 0; blk < frame->blocks; blk++) {
1040 frame->sb_sample_f[blk][0][sb] =
1041 sb_sample_j[blk][0];
1042 frame->sb_sample_f[blk][1][sb] =
1043 sb_sample_j[blk][1];
1044 }
1045 }
1046 }
1047
1048 data[4] = 0;
1049 for (sb = 0; sb < frame->subbands - 1; sb++)
1050 data[4] |= ((frame->joint >> sb) & 0x01) << (frame->subbands - 1 - sb);
1051
1052 crc_header[crc_pos >> 3] = data[4];
1053
1054 produced += frame->subbands;
1055 crc_pos += frame->subbands;
1056 }
1057
1058 for (ch = 0; ch < frame->channels; ch++) {
1059 for (sb = 0; sb < frame->subbands; sb++) {
1060 data[produced >> 3] <<= 4;
1061 crc_header[crc_pos >> 3] <<= 4;
1062 data[produced >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
1063 crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F;
1064
1065 produced += 4;
1066 crc_pos += 4;
1067 }
1068 }
1069
1070 /* align the last crc byte */
1071 if (crc_pos % 8)
1072 crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);
1073
1074 data[3] = sbc_crc8(crc_header, crc_pos);
1075
1076 sbc_calculate_bits(frame, bits);
1077
1078 for (ch = 0; ch < frame->channels; ch++) {
1079 for (sb = 0; sb < frame->subbands; sb++)
1080 levels[ch][sb] = (1 << bits[ch][sb]) - 1;
1081 }
1082
1083 for (blk = 0; blk < frame->blocks; blk++) {
1084 for (ch = 0; ch < frame->channels; ch++) {
1085 for (sb = 0; sb < frame->subbands; sb++) {
1086 if (levels[ch][sb] > 0) {
1087 audio_sample =
1088 (uint16_t) ((((frame->sb_sample_f[blk][ch][sb]*levels[ch][sb]) >>
1089 (frame->scale_factor[ch][sb] + 1)) +
1090 levels[ch][sb]) >> 1);
1091 audio_sample <<= 16 - bits[ch][sb];
1092 for (bit = 0; bit < bits[ch][sb]; bit++) {
1093 data[produced >> 3] <<= 1;
1094 if (audio_sample & 0x8000)
1095 data[produced >> 3] |= 0x1;
1096 audio_sample <<= 1;
1097 produced++;
1098 }
1099 }
1100 }
1101 }
1102 }
1103
1104 /* align the last byte */
1105 if (produced % 8) {
1106 data[produced >> 3] <<= 8 - (produced % 8);
1107 }
1108
1109 return (produced + 7) >> 3;
1110 }
1111
1112 struct sbc_priv {
1113 int init;
1114 struct sbc_frame frame;
1115 struct sbc_decoder_state dec_state;
1116 struct sbc_encoder_state enc_state;
1117 };
1118
1119 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
1120 {
1121 sbc->frequency = SBC_FREQ_44100;
1122 sbc->mode = SBC_MODE_STEREO;
1123 sbc->subbands = SBC_SB_8;
1124 sbc->blocks = SBC_BLK_16;
1125 sbc->bitpool = 32;
1126 #if __BYTE_ORDER == __LITTLE_ENDIAN
1127 sbc->endian = SBC_LE;
1128 #elif __BYTE_ORDER == __BIG_ENDIAN
1129 sbc->endian = SBC_BE;
1130 #else
1131 #error "Unknown byte order"
1132 #endif
1133 }
1134
1135 int sbc_init(sbc_t *sbc, unsigned long flags)
1136 {
1137 if (!sbc)
1138 return -EIO;
1139
1140 memset(sbc, 0, sizeof(sbc_t));
1141
1142 sbc->priv = malloc(sizeof(struct sbc_priv));
1143 if (!sbc->priv)
1144 return -ENOMEM;
1145
1146 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1147
1148 sbc_set_defaults(sbc, flags);
1149
1150 return 0;
1151 }
1152
1153 int sbc_parse(sbc_t *sbc, void *input, int input_len)
1154 {
1155 return sbc_decode(sbc, input, input_len, NULL, 0, NULL);
1156 }
1157
1158 int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output,
1159 int output_len, int *written)
1160 {
1161 struct sbc_priv *priv;
1162 char *ptr;
1163 int i, ch, framelen, samples;
1164
1165 if (!sbc && !input)
1166 return -EIO;
1167
1168 priv = sbc->priv;
1169
1170 framelen = sbc_unpack_frame(input, &priv->frame, input_len);
1171
1172 if (!priv->init) {
1173 sbc_decoder_init(&priv->dec_state, &priv->frame);
1174 priv->init = 1;
1175
1176 sbc->frequency = priv->frame.frequency;
1177 sbc->mode = priv->frame.mode;
1178 sbc->subbands = priv->frame.subband_mode;
1179 sbc->blocks = priv->frame.block_mode;
1180 sbc->allocation = priv->frame.allocation;
1181 sbc->bitpool = priv->frame.bitpool;
1182
1183 priv->frame.codesize = sbc_get_codesize(sbc);
1184 priv->frame.length = sbc_get_frame_length(sbc);
1185 }
1186
1187 if (!output)
1188 return framelen;
1189
1190 if (written)
1191 *written = 0;
1192
1193 samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
1194
1195 ptr = output;
1196
1197 if (output_len < samples * priv->frame.channels * 2)
1198 samples = output_len / (priv->frame.channels * 2);
1199
1200 for (i = 0; i < samples; i++) {
1201 for (ch = 0; ch < priv->frame.channels; ch++) {
1202 int16_t s;
1203 s = priv->frame.pcm_sample[ch][i];
1204
1205 #if __BYTE_ORDER == __LITTLE_ENDIAN
1206 if (sbc->endian == SBC_BE) {
1207 #elif __BYTE_ORDER == __BIG_ENDIAN
1208 if (sbc->endian == SBC_LE) {
1209 #else
1210 #error "Unknown byte order"
1211 #endif
1212 *ptr++ = (s & 0xff00) >> 8;
1213 *ptr++ = (s & 0x00ff);
1214 } else {
1215 *ptr++ = (s & 0x00ff);
1216 *ptr++ = (s & 0xff00) >> 8;
1217 }
1218 }
1219 }
1220
1221 if (written)
1222 *written = samples * priv->frame.channels * 2;
1223
1224 return framelen;
1225 }
1226
1227 int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output,
1228 int output_len, int *written)
1229 {
1230 struct sbc_priv *priv;
1231 char *ptr;
1232 int i, ch, framelen, samples;
1233
1234 if (!sbc && !input)
1235 return -EIO;
1236
1237 priv = sbc->priv;
1238
1239 if (written)
1240 *written = 0;
1241
1242 if (!priv->init) {
1243 priv->frame.frequency = sbc->frequency;
1244 priv->frame.mode = sbc->mode;
1245 priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1246 priv->frame.allocation = sbc->allocation;
1247 priv->frame.subband_mode = sbc->subbands;
1248 priv->frame.subbands = sbc->subbands ? 8 : 4;
1249 priv->frame.block_mode = sbc->blocks;
1250 priv->frame.blocks = 4 + (sbc->blocks * 4);
1251 priv->frame.bitpool = sbc->bitpool;
1252 priv->frame.codesize = sbc_get_codesize(sbc);
1253 priv->frame.length = sbc_get_frame_length(sbc);
1254
1255 sbc_encoder_init(&priv->enc_state, &priv->frame);
1256 priv->init = 1;
1257 }
1258
1259 /* input must be large enough to encode a complete frame */
1260 if (input_len < priv->frame.codesize)
1261 return 0;
1262
1263 /* output must be large enough to receive the encoded frame */
1264 if (!output || output_len < priv->frame.length)
1265 return -ENOSPC;
1266
1267 ptr = input;
1268
1269 for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++) {
1270 for (ch = 0; ch < priv->frame.channels; ch++) {
1271 int16_t s;
1272 #if __BYTE_ORDER == __LITTLE_ENDIAN
1273 if (sbc->endian == SBC_BE)
1274 #elif __BYTE_ORDER == __BIG_ENDIAN
1275 if (sbc->endian == SBC_LE)
1276 #else
1277 #error "Unknown byte order"
1278 #endif
1279 s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff);
1280 else
1281 s = (ptr[0] & 0xff) | (ptr[1] & 0xff) << 8;
1282 ptr += 2;
1283 priv->frame.pcm_sample[ch][i] = s;
1284 }
1285 }
1286
1287 samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
1288
1289 framelen = sbc_pack_frame(output, &priv->frame, output_len);
1290
1291 if (written)
1292 *written = framelen;
1293
1294 return samples * priv->frame.channels * 2;
1295 }
1296
1297 void sbc_finish(sbc_t *sbc)
1298 {
1299 if (!sbc)
1300 return;
1301
1302 if (sbc->priv)
1303 free(sbc->priv);
1304
1305 memset(sbc, 0, sizeof(sbc_t));
1306 }
1307
1308 int sbc_get_frame_length(sbc_t *sbc)
1309 {
1310 int ret;
1311 uint8_t subbands, channels, blocks, joint;
1312 struct sbc_priv *priv;
1313
1314 priv = sbc->priv;
1315 if (!priv->init) {
1316 subbands = sbc->subbands ? 8 : 4;
1317 blocks = 4 + (sbc->blocks * 4);
1318 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1319 joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;
1320 } else {
1321 subbands = priv->frame.subbands;
1322 blocks = priv->frame.blocks;
1323 channels = priv->frame.channels;
1324 joint = priv->frame.joint;
1325 }
1326
1327 ret = 4 + (4 * subbands * channels) / 8;
1328
1329 /* This term is not always evenly divide so we round it up */
1330 if (channels == 1)
1331 ret += ((blocks * channels * sbc->bitpool) + 7) / 8;
1332 else
1333 ret += (((joint ? subbands : 0) + blocks * sbc->bitpool) + 7)
1334 / 8;
1335
1336 return ret;
1337 }
1338
1339 int sbc_get_frame_duration(sbc_t *sbc)
1340 {
1341 uint8_t subbands, blocks;
1342 uint16_t frequency;
1343 struct sbc_priv *priv;
1344
1345 priv = sbc->priv;
1346 if (!priv->init) {
1347 subbands = sbc->subbands ? 8 : 4;
1348 blocks = 4 + (sbc->blocks * 4);
1349 } else {
1350 subbands = priv->frame.subbands;
1351 blocks = priv->frame.blocks;
1352 }
1353
1354 switch (sbc->frequency) {
1355 case SBC_FREQ_16000:
1356 frequency = 16000;
1357 break;
1358
1359 case SBC_FREQ_32000:
1360 frequency = 32000;
1361 break;
1362
1363 case SBC_FREQ_44100:
1364 frequency = 44100;
1365 break;
1366
1367 case SBC_FREQ_48000:
1368 frequency = 48000;
1369 break;
1370 default:
1371 return 0;
1372 }
1373
1374 return (1000000 * blocks * subbands) / frequency;
1375 }
1376
1377 int sbc_get_codesize(sbc_t *sbc)
1378 {
1379 uint8_t subbands, channels, blocks;
1380 struct sbc_priv *priv;
1381
1382 priv = sbc->priv;
1383 if (!priv->init) {
1384 subbands = sbc->subbands ? 8 : 4;
1385 blocks = 4 + (sbc->blocks * 4);
1386 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1387 } else {
1388 subbands = priv->frame.subbands;
1389 blocks = priv->frame.blocks;
1390 channels = priv->frame.channels;
1391 }
1392
1393 return subbands * blocks * channels * 2;
1394 }
1395
1396 int sbc_reinit(sbc_t *sbc, unsigned long flags)
1397 {
1398 struct sbc_priv *priv;
1399
1400 if (!sbc || !sbc->priv)
1401 return -EIO;
1402
1403 priv = sbc->priv;
1404
1405 if (priv->init == 1)
1406 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1407
1408 sbc_set_defaults(sbc, flags);
1409
1410 return 0;
1411 }