3 * Bluetooth low-complexity, subband codec (SBC) library
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>
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.
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.
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
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
42 #include <sys/types.h>
46 #include "sbc_tables.h"
49 #include "sbc_primitives.h"
51 #define SBC_SYNCWORD 0x9C
53 /* This structure contains an unpacked SBC frame.
54 Yes, there is probably quite some unused space herein */
61 DUAL_CHANNEL = SBC_MODE_DUAL_CHANNEL,
62 STEREO = SBC_MODE_STEREO,
63 JOINT_STEREO = SBC_MODE_JOINT_STEREO
67 LOUDNESS = SBC_AM_LOUDNESS,
76 /* bit number x set means joint stereo has been used in subband x */
79 /* only the lower 4 bits of every element are to be used */
80 uint32_t scale_factor[2][8];
82 /* raw integer subband samples in the frame */
83 int32_t SBC_ALIGNED sb_sample_f[16][2][8];
85 /* modified subband samples */
86 int32_t SBC_ALIGNED sb_sample[16][2][8];
88 /* original pcm audio samples */
89 int16_t SBC_ALIGNED pcm_sample[2][16*8];
92 struct sbc_decoder_state {
99 * Calculates the CRC-8 of the first len bits in data
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
136 static uint8_t sbc_crc8(const uint8_t *data, size_t len)
142 for (i = 0; i < len / 8; i++)
143 crc = crc_table[crc ^ data[i]];
146 for (i = 0; i < len % 8; i++) {
147 char bit = ((octet ^ crc) & 0x80) >> 7;
149 crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
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)
162 static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8])
164 uint8_t sf = frame->frequency;
166 if (frame->mode == MONO || frame->mode == DUAL_CHANNEL) {
167 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
170 for (ch = 0; ch < frame->channels; ch++) {
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];
179 for (sb = 0; sb < frame->subbands; sb++) {
180 if (frame->scale_factor[ch][sb] == 0)
181 bitneed[ch][sb] = -5;
183 if (frame->subbands == 4)
184 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
186 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
188 bitneed[ch][sb] = loudness / 2;
190 bitneed[ch][sb] = loudness;
192 if (bitneed[ch][sb] > max_bitneed)
193 max_bitneed = bitneed[ch][sb];
199 bitslice = max_bitneed + 1;
202 bitcount += slicecount;
204 for (sb = 0; sb < frame->subbands; sb++) {
205 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
207 else if (bitneed[ch][sb] == bitslice + 1)
210 } while (bitcount + slicecount < frame->bitpool);
212 if (bitcount + slicecount == frame->bitpool) {
213 bitcount += slicecount;
217 for (sb = 0; sb < frame->subbands; sb++) {
218 if (bitneed[ch][sb] < bitslice + 2)
221 bits[ch][sb] = bitneed[ch][sb] - bitslice;
222 if (bits[ch][sb] > 16)
227 for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
228 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
231 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
237 for (sb = 0; bitcount < frame->bitpool && sb < frame->subbands; sb++) {
238 if (bits[ch][sb] < 16) {
246 } else if (frame->mode == STEREO || frame->mode == JOINT_STEREO) {
247 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
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];
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;
265 if (frame->subbands == 4)
266 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
268 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
270 bitneed[ch][sb] = loudness / 2;
272 bitneed[ch][sb] = loudness;
274 if (bitneed[ch][sb] > max_bitneed)
275 max_bitneed = bitneed[ch][sb];
282 bitslice = max_bitneed + 1;
285 bitcount += slicecount;
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))
291 else if (bitneed[ch][sb] == bitslice + 1)
295 } while (bitcount + slicecount < frame->bitpool);
297 if (bitcount + slicecount == frame->bitpool) {
298 bitcount += slicecount;
302 for (ch = 0; ch < 2; ch++) {
303 for (sb = 0; sb < frame->subbands; sb++) {
304 if (bitneed[ch][sb] < bitslice + 2) {
307 bits[ch][sb] = bitneed[ch][sb] - bitslice;
308 if (bits[ch][sb] > 16)
316 while (bitcount < frame->bitpool) {
317 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) {
320 } else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1)) {
327 if (sb >= frame->subbands) break;
334 while (bitcount < frame->bitpool) {
335 if (bits[ch][sb] < 16) {
342 if (sb >= frame->subbands) break;
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:
357 * -1 Data stream too short
358 * -2 Sync byte incorrect
360 * -4 Bitpool value out of bounds
362 static int sbc_unpack_frame(const uint8_t *data, struct sbc_frame *frame,
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 };
373 int ch, sb, blk, bit; /* channel, subband, block and bit standard
375 int bits[2][8]; /* bits distribution */
376 uint32_t levels[2][8]; /* levels derived from that */
381 if (data[0] != SBC_SYNCWORD)
384 frame->frequency = (data[1] >> 6) & 0x03;
386 frame->block_mode = (data[1] >> 4) & 0x03;
387 switch (frame->block_mode) {
402 frame->mode = (data[1] >> 2) & 0x03;
403 switch (frame->mode) {
407 case DUAL_CHANNEL: /* fall-through */
414 frame->allocation = (data[1] >> 1) & 0x01;
416 frame->subband_mode = (data[1] & 0x01);
417 frame->subbands = frame->subband_mode ? 8 : 4;
419 frame->bitpool = data[2];
421 if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
422 frame->bitpool > 16 * frame->subbands)
425 if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
426 frame->bitpool > 32 * frame->subbands)
429 /* data[3] is crc, we're checking it later */
433 crc_header[0] = data[1];
434 crc_header[1] = data[2];
437 if (frame->mode == JOINT_STEREO) {
438 if (len * 8 < consumed + frame->subbands)
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;
447 crc_header[crc_pos / 8] = data[4];
449 consumed += frame->subbands;
450 crc_pos += frame->subbands;
453 if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
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));
469 if (data[3] != sbc_crc8(crc_header, crc_pos))
472 sbc_calculate_bits(frame, bits);
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;
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) {
484 for (bit = 0; bit < bits[ch][sb]; bit++) {
485 if (consumed > len * 8)
488 if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
489 audio_sample |= 1 << (bits[ch][sb] - bit - 1);
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]);
498 frame->sb_sample[blk][ch][sb] = 0;
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;
518 if ((consumed & 0x7) != 0)
519 consumed += 8 - (consumed & 0x7);
521 return consumed >> 3;
524 static void sbc_decoder_init(struct sbc_decoder_state *state,
525 const struct sbc_frame *frame)
529 memset(state->V, 0, sizeof(state->V));
530 state->subbands = frame->subbands;
532 for (ch = 0; ch < 2; ch++)
533 for (i = 0; i < frame->subbands * 2; i++)
534 state->offset[ch][i] = (10 * i + 10);
537 static inline void sbc_synthesize_four(struct sbc_decoder_state *state,
538 struct sbc_frame *frame, int ch, int blk)
541 int32_t *v = state->V[ch];
542 int *offset = state->offset[ch];
544 for (i = 0; i < 8; i++) {
549 memcpy(v + 80, v, 9 * sizeof(*v));
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])))));
560 /* Compute the samples */
561 for (idx = 0, i = 0; i < 4; i++, idx += 5) {
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])))))))))));
579 static inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
580 struct sbc_frame *frame, int ch, int blk)
583 int *offset = state->offset[ch];
585 for (i = 0; i < 16; i++) {
590 for (j = 0; j < 9; j++)
591 state->V[ch][j + 160] = state->V[ch][j];
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])))))))));
606 /* Compute the samples */
607 for (idx = 0, i = 0; i < 8; i++, idx += 5) {
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])))))))))));
625 static int sbc_synthesize_audio(struct sbc_decoder_state *state,
626 struct sbc_frame *frame)
630 switch (frame->subbands) {
632 for (ch = 0; ch < frame->channels; ch++) {
633 for (blk = 0; blk < frame->blocks; blk++)
634 sbc_synthesize_four(state, frame, ch, blk);
636 return frame->blocks * 4;
639 for (ch = 0; ch < frame->channels; ch++) {
640 for (blk = 0; blk < frame->blocks; blk++)
641 sbc_synthesize_eight(state, frame, ch, blk);
643 return frame->blocks * 8;
650 static int sbc_analyze_audio(struct sbc_encoder_state *state,
651 struct sbc_frame *frame)
656 switch (frame->subbands) {
658 for (ch = 0; ch < frame->channels; ch++) {
659 x = &state->X[ch][state->position - 16 +
661 for (blk = 0; blk < frame->blocks; blk += 4) {
662 state->sbc_analyze_4b_4s(
664 frame->sb_sample_f[blk][ch],
665 frame->sb_sample_f[blk + 1][ch] -
666 frame->sb_sample_f[blk][ch]);
670 return frame->blocks * 4;
673 for (ch = 0; ch < frame->channels; ch++) {
674 x = &state->X[ch][state->position - 32 +
676 for (blk = 0; blk < frame->blocks; blk += 4) {
677 state->sbc_analyze_4b_8s(
679 frame->sb_sample_f[blk][ch],
680 frame->sb_sample_f[blk + 1][ch] -
681 frame->sb_sample_f[blk][ch]);
685 return frame->blocks * 8;
692 /* Supplementary bitstream writing macros for 'sbc_pack_frame' */
694 #define PUT_BITS(data_ptr, bits_cache, bits_count, v, n) \
696 bits_cache = (v) | (bits_cache << (n)); \
698 if (bits_count >= 16) { \
700 *data_ptr++ = (uint8_t) \
701 (bits_cache >> bits_count); \
703 *data_ptr++ = (uint8_t) \
704 (bits_cache >> bits_count); \
708 #define FLUSH_BITS(data_ptr, bits_cache, bits_count) \
710 while (bits_count >= 8) { \
712 *data_ptr++ = (uint8_t) \
713 (bits_cache >> bits_count); \
715 if (bits_count > 0) \
716 *data_ptr++ = (uint8_t) \
717 (bits_cache << (8 - bits_count)); \
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.
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
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)
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;
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 };
748 uint32_t audio_sample;
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];
755 data[0] = SBC_SYNCWORD;
757 data[1] = (frame->frequency & 0x03) << 6;
759 data[1] |= (frame->block_mode & 0x03) << 4;
761 data[1] |= (frame->mode & 0x03) << 2;
763 data[1] |= (frame->allocation & 0x01) << 1;
765 switch (frame_subbands) {
777 data[2] = frame->bitpool;
779 if ((frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&
780 frame->bitpool > frame_subbands << 4)
783 if ((frame->mode == STEREO || frame->mode == JOINT_STEREO) &&
784 frame->bitpool > frame_subbands << 5)
787 /* Can't fill in crc yet */
789 crc_header[0] = data[1];
790 crc_header[1] = data[2];
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];
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;
809 for (blk = 0; blk < frame->blocks; blk++) {
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);
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) {
823 scalefactor_j[0] *= 2;
825 tmp = fabs(sb_sample_j[blk][1]);
826 while (scalefactor_j[1] < tmp) {
828 scalefactor_j[1] *= 2;
832 /* decide whether to join this subband */
833 if ((frame->scale_factor[0][sb] +
834 frame->scale_factor[1][sb]) >
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] =
845 frame->sb_sample_f[blk][1][sb] =
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;
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;
867 /* align the last crc byte */
869 crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8);
871 data[3] = sbc_crc8(crc_header, crc_pos);
873 sbc_calculate_bits(frame, bits);
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] +
886 for (blk = 0; blk < frame->blocks; blk++) {
887 for (ch = 0; ch < frame_channels; ch++) {
888 for (sb = 0; sb < frame_subbands; sb++) {
890 if (bits[ch][sb] == 0)
893 audio_sample = ((uint64_t) levels[ch][sb] *
894 (sb_sample_delta[ch][sb] +
895 frame->sb_sample_f[blk][ch][sb])) >> 32;
897 PUT_BITS(data_ptr, bits_cache, bits_count,
898 audio_sample, bits[ch][sb]);
903 FLUSH_BITS(data_ptr, bits_cache, bits_count);
905 return data_ptr - data;
908 static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len)
910 if (frame->subbands == 4) {
911 if (frame->channels == 1)
912 return sbc_pack_frame_internal(data, frame, len, 4, 1);
914 return sbc_pack_frame_internal(data, frame, len, 4, 2);
916 if (frame->channels == 1)
917 return sbc_pack_frame_internal(data, frame, len, 8, 1);
919 return sbc_pack_frame_internal(data, frame, len, 8, 2);
923 static void sbc_encoder_init(struct sbc_encoder_state *state,
924 const struct sbc_frame *frame)
926 memset(&state->X, 0, sizeof(state->X));
927 state->position = SBC_X_BUFFER_SIZE - frame->subbands * 9;
929 sbc_init_primitives(state);
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;
939 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags)
941 sbc->frequency = SBC_FREQ_44100;
942 sbc->mode = SBC_MODE_STEREO;
943 sbc->subbands = SBC_SB_8;
944 sbc->blocks = SBC_BLK_16;
946 #if __BYTE_ORDER == __LITTLE_ENDIAN
947 sbc->endian = SBC_LE;
948 #elif __BYTE_ORDER == __BIG_ENDIAN
949 sbc->endian = SBC_BE;
951 #error "Unknown byte order"
955 int sbc_init(sbc_t *sbc, unsigned long flags)
960 memset(sbc, 0, sizeof(sbc_t));
962 sbc->priv_alloc_base = malloc(sizeof(struct sbc_priv) + SBC_ALIGN_MASK);
963 if (!sbc->priv_alloc_base)
966 sbc->priv = (void *) (((uintptr_t) sbc->priv_alloc_base +
967 SBC_ALIGN_MASK) & ~((uintptr_t) SBC_ALIGN_MASK));
969 memset(sbc->priv, 0, sizeof(struct sbc_priv));
971 sbc_set_defaults(sbc, flags);
976 ssize_t sbc_parse(sbc_t *sbc, const void *input, size_t input_len)
978 return sbc_decode(sbc, input, input_len, NULL, 0, NULL);
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)
984 struct sbc_priv *priv;
986 int i, ch, framelen, samples;
993 framelen = sbc_unpack_frame(input, &priv->frame, input_len);
996 sbc_decoder_init(&priv->dec_state, &priv->frame);
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;
1006 priv->frame.codesize = sbc_get_codesize(sbc);
1007 priv->frame.length = framelen;
1019 samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
1023 if (output_len < (size_t) (samples * priv->frame.channels * 2))
1024 samples = output_len / (priv->frame.channels * 2);
1026 for (i = 0; i < samples; i++) {
1027 for (ch = 0; ch < priv->frame.channels; ch++) {
1029 s = priv->frame.pcm_sample[ch][i];
1031 if (sbc->endian == SBC_BE) {
1032 *ptr++ = (s & 0xff00) >> 8;
1033 *ptr++ = (s & 0x00ff);
1035 *ptr++ = (s & 0x00ff);
1036 *ptr++ = (s & 0xff00) >> 8;
1042 *written = samples * priv->frame.channels * 2;
1047 ssize_t sbc_encode(sbc_t *sbc, const void *input, size_t input_len,
1048 void *output, size_t output_len, size_t *written)
1050 struct sbc_priv *priv;
1051 int framelen, samples;
1052 int (*sbc_enc_process_input)(int position,
1053 const uint8_t *pcm, int16_t X[2][SBC_X_BUFFER_SIZE],
1054 int nsamples, int nchannels);
1065 priv->frame.frequency = sbc->frequency;
1066 priv->frame.mode = sbc->mode;
1067 priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1068 priv->frame.allocation = sbc->allocation;
1069 priv->frame.subband_mode = sbc->subbands;
1070 priv->frame.subbands = sbc->subbands ? 8 : 4;
1071 priv->frame.block_mode = sbc->blocks;
1072 priv->frame.blocks = 4 + (sbc->blocks * 4);
1073 priv->frame.bitpool = sbc->bitpool;
1074 priv->frame.codesize = sbc_get_codesize(sbc);
1075 priv->frame.length = sbc_get_frame_length(sbc);
1077 sbc_encoder_init(&priv->enc_state, &priv->frame);
1081 /* input must be large enough to encode a complete frame */
1082 if (input_len < priv->frame.codesize)
1085 /* output must be large enough to receive the encoded frame */
1086 if (!output || output_len < priv->frame.length)
1089 /* Select the needed input data processing function and call it */
1090 if (priv->frame.subbands == 8) {
1091 if (sbc->endian == SBC_BE)
1092 sbc_enc_process_input =
1093 priv->enc_state.sbc_enc_process_input_8s_be;
1095 sbc_enc_process_input =
1096 priv->enc_state.sbc_enc_process_input_8s_le;
1098 if (sbc->endian == SBC_BE)
1099 sbc_enc_process_input =
1100 priv->enc_state.sbc_enc_process_input_4s_be;
1102 sbc_enc_process_input =
1103 priv->enc_state.sbc_enc_process_input_4s_le;
1106 priv->enc_state.position = sbc_enc_process_input(
1107 priv->enc_state.position, (const uint8_t *) input,
1108 priv->enc_state.X, priv->frame.subbands * priv->frame.blocks,
1109 priv->frame.channels);
1111 samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
1113 priv->enc_state.sbc_calc_scalefactors(
1114 priv->frame.sb_sample_f, priv->frame.scale_factor,
1115 priv->frame.blocks, priv->frame.channels, priv->frame.subbands);
1117 framelen = sbc_pack_frame(output, &priv->frame, output_len);
1120 *written = framelen;
1122 return samples * priv->frame.channels * 2;
1125 void sbc_finish(sbc_t *sbc)
1130 if (sbc->priv_alloc_base)
1131 free(sbc->priv_alloc_base);
1133 memset(sbc, 0, sizeof(sbc_t));
1136 size_t sbc_get_frame_length(sbc_t *sbc)
1139 uint8_t subbands, channels, blocks, joint, bitpool;
1140 struct sbc_priv *priv;
1144 return priv->frame.length;
1146 subbands = sbc->subbands ? 8 : 4;
1147 blocks = 4 + (sbc->blocks * 4);
1148 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1149 joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0;
1150 bitpool = sbc->bitpool;
1152 ret = 4 + (4 * subbands * channels) / 8;
1153 /* This term is not always evenly divide so we round it up */
1155 ret += ((blocks * channels * bitpool) + 7) / 8;
1157 ret += (((joint ? subbands : 0) + blocks * bitpool) + 7) / 8;
1162 unsigned sbc_get_frame_duration(sbc_t *sbc)
1164 uint8_t subbands, blocks;
1166 struct sbc_priv *priv;
1170 subbands = sbc->subbands ? 8 : 4;
1171 blocks = 4 + (sbc->blocks * 4);
1173 subbands = priv->frame.subbands;
1174 blocks = priv->frame.blocks;
1177 switch (sbc->frequency) {
1178 case SBC_FREQ_16000:
1182 case SBC_FREQ_32000:
1186 case SBC_FREQ_44100:
1190 case SBC_FREQ_48000:
1197 return (1000000 * blocks * subbands) / frequency;
1200 size_t sbc_get_codesize(sbc_t *sbc)
1202 uint16_t subbands, channels, blocks;
1203 struct sbc_priv *priv;
1207 subbands = sbc->subbands ? 8 : 4;
1208 blocks = 4 + (sbc->blocks * 4);
1209 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2;
1211 subbands = priv->frame.subbands;
1212 blocks = priv->frame.blocks;
1213 channels = priv->frame.channels;
1216 return subbands * blocks * channels * 2;
1219 const char *sbc_get_implementation_info(sbc_t *sbc)
1221 struct sbc_priv *priv;
1230 return priv->enc_state.implementation_info;
1233 int sbc_reinit(sbc_t *sbc, unsigned long flags)
1235 struct sbc_priv *priv;
1237 if (!sbc || !sbc->priv)
1242 if (priv->init == 1)
1243 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1245 sbc_set_defaults(sbc, flags);