projects / GitHub / LineageOS / G12 / android_hardware_realtek.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
blame | history | raw | HEAD
realtek bt bringup
[GitHub/LineageOS/G12/android_hardware_realtek.git] / rtkbt / code / libbt-vendor / codec / msbc / sbc.c
1 /*
2 *
3 * Bluetooth low-complexity, subband codec (SBC) library
4 *
5 * Copyright (C) 2004-2006 Marcel Holtmann <marcel@holtmann.org>
6 * Copyright (C) 2004-2005 Henryk Ploetz <henryk@ploetzli.ch>
7 * Copyright (C) 2005-2006 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 for high and low bytes)
29 fill bitpool by 16 bits instead of one at a time in bits allocation/bitpool generation
30 port to the dsp
31 don't consume more bytes than passed into the encoder
32
33 */
34
35 #ifdef HAVE_CONFIG_H
36 #include <config.h>
37 #endif
38
39 //#include <stdint.h>
40 #include <malloc.h>
41 #include <string.h>
42 //#include <stdlib.h>
43 #include <sys/types.h>
44 #include <sys/errno.h>
45
46 #include "sbc.h"
47 #include "sbc_math.h"
48 #include "sbc_tables.h"
49 #include "cutils/log.h"
50
51
52 #define SBC_SYNCWORD 0xAD
53 #define SBC_POOLTAG 'CBSR'
54
55 /* sampling frequency */
56 #define SBC_FS_16 0x00
57 #define SBC_FS_32 0x01
58 #define SBC_FS_44 0x02
59 #define SBC_FS_48 0x03
60
61 /* nrof_blocks */
62 #define SBC_NB_4 0x00
63 #define SBC_NB_8 0x01
64 #define SBC_NB_12 0x02
65 #define SBC_NB_16 0x03
66
67 /* channel mode */
68 #define SBC_CM_MONO 0x00
69 #define SBC_CM_DUAL_CHANNEL 0x01
70 #define SBC_CM_STEREO 0x02
71 #define SBC_CM_JOINT_STEREO 0x03
72
73 /* allocation mode */
74 #define SBC_AM_LOUDNESS 0x00
75 #define SBC_AM_SNR 0x01
76
77 /* subbands */
78 #define SBC_SB_4 0x00
79 #define SBC_SB_8 0x01
80
81 #define SBC_ALIGN_BITS 4
82 #define SBC_ALIGN_MASK ((1 << (SBC_ALIGN_BITS)) - 1)
83
84
85 typedef unsigned short uint16_t;
86 typedef unsigned char uint8_t;
87 typedef short int16_t;
88 typedef unsigned int u_int32_t;
89
90 #ifndef UINT
91 typedef unsigned int UINT;
92 #endif
93
94
95 #if defined(WIN32) && !defined(__cplusplus)
96
97 #define inline __inline
98
99 #endif
100
101 typedef long long sbc_extended_t;
102 /*
103 static __inline void sbc_synthesize_four(struct sbc_decoder_state *state,
104 struct sbc_frame *frame, int ch, int blk);
105 */
106
107
108 /* This structure contains an unpacked SBC frame.
109 Yes, there is probably quite some unused space herein */
110 struct sbc_frame
111 {
112 uint16_t sampling_frequency; /* in kHz */
113 unsigned char blocks;
114 enum {
115 MONO = SBC_CM_MONO,
116 DUAL_CHANNEL = SBC_CM_DUAL_CHANNEL,
117 STEREO = SBC_CM_STEREO,
118 JOINT_STEREO = SBC_CM_JOINT_STEREO
119 } channel_mode;
120 uint8_t channels;
121 enum {
122 LOUDNESS = SBC_AM_LOUDNESS,
123 SNR = SBC_AM_SNR
124 } allocation_method;
125 uint8_t subbands;
126 uint8_t bitpool;
127 uint8_t join; /* bit number x set means joint stereo has been used in subband x */
128 int scale_factor[2][8]; /* only the lower 4 bits of every element are to be used */
129 uint16_t audio_sample[16][2][8]; /* raw integer subband samples in the frame */
130
131 int sb_sample_f[16][2][8];
132 int sb_sample[16][2][8]; /* modified subband samples */
133 short pcm_sample[2][16*8]; /* original pcm audio samples */
134 };
135
136 struct sbc_decoder_state {
137 int subbands;
138 long long V[2][170];
139 int offset[2][16];
140 };
141
142 struct sbc_encoder_state {
143 int subbands;
144 int32_t X[2][80];
145 };
146
147 /*
148 * Calculates the CRC-8 of the first len bits in data
149 */
150 static const uint8_t crc_table[256] = {
151 0x00, 0x1D, 0x3A, 0x27, 0x74, 0x69, 0x4E, 0x53,
152 0xE8, 0xF5, 0xD2, 0xCF, 0x9C, 0x81, 0xA6, 0xBB,
153 0xCD, 0xD0, 0xF7, 0xEA, 0xB9, 0xA4, 0x83, 0x9E,
154 0x25, 0x38, 0x1F, 0x02, 0x51, 0x4C, 0x6B, 0x76,
155 0x87, 0x9A, 0xBD, 0xA0, 0xF3, 0xEE, 0xC9, 0xD4,
156 0x6F, 0x72, 0x55, 0x48, 0x1B, 0x06, 0x21, 0x3C,
157 0x4A, 0x57, 0x70, 0x6D, 0x3E, 0x23, 0x04, 0x19,
158 0xA2, 0xBF, 0x98, 0x85, 0xD6, 0xCB, 0xEC, 0xF1,
159 0x13, 0x0E, 0x29, 0x34, 0x67, 0x7A, 0x5D, 0x40,
160 0xFB, 0xE6, 0xC1, 0xDC, 0x8F, 0x92, 0xB5, 0xA8,
161 0xDE, 0xC3, 0xE4, 0xF9, 0xAA, 0xB7, 0x90, 0x8D,
162 0x36, 0x2B, 0x0C, 0x11, 0x42, 0x5F, 0x78, 0x65,
163 0x94, 0x89, 0xAE, 0xB3, 0xE0, 0xFD, 0xDA, 0xC7,
164 0x7C, 0x61, 0x46, 0x5B, 0x08, 0x15, 0x32, 0x2F,
165 0x59, 0x44, 0x63, 0x7E, 0x2D, 0x30, 0x17, 0x0A,
166 0xB1, 0xAC, 0x8B, 0x96, 0xC5, 0xD8, 0xFF, 0xE2,
167 0x26, 0x3B, 0x1C, 0x01, 0x52, 0x4F, 0x68, 0x75,
168 0xCE, 0xD3, 0xF4, 0xE9, 0xBA, 0xA7, 0x80, 0x9D,
169 0xEB, 0xF6, 0xD1, 0xCC, 0x9F, 0x82, 0xA5, 0xB8,
170 0x03, 0x1E, 0x39, 0x24, 0x77, 0x6A, 0x4D, 0x50,
171 0xA1, 0xBC, 0x9B, 0x86, 0xD5, 0xC8, 0xEF, 0xF2,
172 0x49, 0x54, 0x73, 0x6E, 0x3D, 0x20, 0x07, 0x1A,
173 0x6C, 0x71, 0x56, 0x4B, 0x18, 0x05, 0x22, 0x3F,
174 0x84, 0x99, 0xBE, 0xA3, 0xF0, 0xED, 0xCA, 0xD7,
175 0x35, 0x28, 0x0F, 0x12, 0x41, 0x5C, 0x7B, 0x66,
176 0xDD, 0xC0, 0xE7, 0xFA, 0xA9, 0xB4, 0x93, 0x8E,
177 0xF8, 0xE5, 0xC2, 0xDF, 0x8C, 0x91, 0xB6, 0xAB,
178 0x10, 0x0D, 0x2A, 0x37, 0x64, 0x79, 0x5E, 0x43,
179 0xB2, 0xAF, 0x88, 0x95, 0xC6, 0xDB, 0xFC, 0xE1,
180 0x5A, 0x47, 0x60, 0x7D, 0x2E, 0x33, 0x14, 0x09,
181 0x7F, 0x62, 0x45, 0x58, 0x0B, 0x16, 0x31, 0x2C,
182 0x97, 0x8A, 0xAD, 0xB0, 0xE3, 0xFE, 0xD9, 0xC4
183 };
184
185 static uint8_t sbc_crc8(const uint8_t * data, size_t len)
186 {
187 uint8_t crc = 0x0f;
188 size_t i;
189 uint8_t octet;
190
191 for (i = 0; i < len / 8; i++)
192 crc = crc_table[crc ^ data[i]];
193
194 octet = data[i];
195 for (i = 0; i < len % 8; i++)
196 {
197 char bit = ((octet ^ crc) & 0x80) >> 7;
198
199 crc = ((crc & 0x7f) << 1) ^ (bit ? 0x1d : 0);
200
201 octet = octet << 1;
202 }
203
204 return crc;
205 }
206
207 /*
208 * Code straight from the spec to calculate the bits array
209 * Takes a pointer to the frame in question, a pointer to the bits array and the sampling frequency (as 2 bit integer)
210 */
211 static void sbc_calculate_bits(const struct sbc_frame *frame, int (*bits)[8], uint8_t sf)
212 {
213 if (frame->channel_mode == MONO || frame->channel_mode == DUAL_CHANNEL)
214 {
215 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
216 int ch, sb;
217
218 for (ch = 0; ch < frame->channels; ch++)
219 {
220 if (frame->allocation_method == SNR)
221 {
222 for (sb = 0; sb < frame->subbands; sb++)
223 {
224 bitneed[ch][sb] = frame->scale_factor[ch][sb];
225 }
226 }
227
228 else
229 {
230 for (sb = 0; sb < frame->subbands; sb++)
231 {
232 if (frame->scale_factor[ch][sb] == 0)
233 {
234 bitneed[ch][sb] = -5;
235 }
236 else
237 {
238 if (frame->subbands == 4)
239 {
240 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
241 }
242
243 else
244 {
245 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
246 }
247
248 if (loudness > 0)
249 {
250 bitneed[ch][sb] = loudness / 2;
251 }
252
253 else
254 {
255 bitneed[ch][sb] = loudness;
256 }
257 }
258 }
259 }
260
261 max_bitneed = 0;
262 for (sb = 0; sb < frame->subbands; sb++)
263 {
264 if (bitneed[ch][sb] > max_bitneed)
265 max_bitneed = bitneed[ch][sb];
266 }
267
268 bitcount = 0;
269 slicecount = 0;
270 bitslice = max_bitneed + 1;
271 do
272 {
273 bitslice--;
274 bitcount += slicecount;
275 slicecount = 0;
276 for (sb = 0; sb < frame->subbands; sb++)
277 {
278 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
279 {
280 slicecount++;
281 }
282
283 else if (bitneed[ch][sb] == bitslice + 1)
284 {
285 slicecount += 2;
286 }
287 }
288 } while (bitcount + slicecount < frame->bitpool);
289
290 if (bitcount + slicecount == frame->bitpool)
291 {
292 bitcount += slicecount;
293 bitslice--;
294 }
295
296 for (sb = 0; sb < frame->subbands; sb++)
297 {
298 if (bitneed[ch][sb] < bitslice + 2)
299 {
300 bits[ch][sb] = 0;
301 }
302
303 else
304 {
305 bits[ch][sb] = bitneed[ch][sb] - bitslice;
306 if (bits[ch][sb] > 16)
307 bits[ch][sb] = 16;
308 }
309 }
310
311 sb = 0;
312 while (bitcount < frame->bitpool && sb < frame->subbands)
313 {
314 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16))
315 {
316 bits[ch][sb]++;
317 bitcount++;
318 }
319
320 else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1))
321 {
322 bits[ch][sb] = 2;
323 bitcount += 2;
324 }
325 sb++;
326 }
327
328 sb = 0;
329 while (bitcount < frame->bitpool && sb < frame->subbands)
330 {
331 if (bits[ch][sb] < 16)
332 {
333 bits[ch][sb]++;
334 bitcount++;
335 }
336 sb++;
337 }
338
339 }
340
341 }
342
343 else if (frame->channel_mode == STEREO || frame->channel_mode == JOINT_STEREO)
344 {
345 int bitneed[2][8], loudness, max_bitneed, bitcount, slicecount, bitslice;
346 int ch, sb;
347
348 if (frame->allocation_method == SNR)
349 {
350 for (ch = 0; ch < 2; ch++) {
351 for (sb = 0; sb < frame->subbands; sb++)
352 {
353 bitneed[ch][sb] = frame->scale_factor[ch][sb];
354 }
355 }
356 }
357
358 else
359 {
360 for (ch = 0; ch < 2; ch++)
361 {
362 for (sb = 0; sb < frame->subbands; sb++)
363 {
364 if (frame->scale_factor[ch][sb] == 0)
365 {
366 bitneed[ch][sb] = -5;
367 }
368
369 else
370 {
371 if (frame->subbands == 4)
372 {
373 loudness = frame->scale_factor[ch][sb] - sbc_offset4[sf][sb];
374 }
375
376 else
377 {
378 loudness = frame->scale_factor[ch][sb] - sbc_offset8[sf][sb];
379 }
380
381 if (loudness > 0)
382 {
383 bitneed[ch][sb] = loudness / 2;
384 }
385
386 else
387 {
388 bitneed[ch][sb] = loudness;
389 }
390 }
391 }
392 }
393 }
394
395 max_bitneed = 0;
396 for (ch = 0; ch < 2; ch++)
397 {
398 for (sb = 0; sb < frame->subbands; sb++)
399 {
400 if (bitneed[ch][sb] > max_bitneed)
401 max_bitneed = bitneed[ch][sb];
402 }
403 }
404
405 bitcount = 0;
406 slicecount = 0;
407 bitslice = max_bitneed + 1;
408 do {
409 bitslice--;
410 bitcount += slicecount;
411 slicecount = 0;
412 for (ch = 0; ch < 2; ch++)
413 {
414 for (sb = 0; sb < frame->subbands; sb++)
415 {
416 if ((bitneed[ch][sb] > bitslice + 1) && (bitneed[ch][sb] < bitslice + 16))
417 {
418 slicecount++;
419 }
420
421 else if (bitneed[ch][sb] == bitslice + 1)
422 {
423 slicecount += 2;
424 }
425 }
426 }
427 } while (bitcount + slicecount < frame->bitpool);
428
429 if (bitcount + slicecount == frame->bitpool)
430 {
431 bitcount += slicecount;
432 bitslice--;
433 }
434
435 for (ch = 0; ch < 2; ch++)
436 {
437 for (sb = 0; sb < frame->subbands; sb++)
438 {
439 if (bitneed[ch][sb] < bitslice + 2)
440 {
441 bits[ch][sb] = 0;
442 }
443
444 else
445 {
446 bits[ch][sb] = bitneed[ch][sb] - bitslice;
447 if (bits[ch][sb] > 16)
448 bits[ch][sb] = 16;
449 }
450 }
451 }
452
453 ch = 0;
454 sb = 0;
455 while ((bitcount < frame->bitpool) && (sb < frame->subbands))
456 {
457 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16))
458 {
459 bits[ch][sb]++;
460 bitcount++;
461 }
462
463 else if ((bitneed[ch][sb] == bitslice + 1) && (frame->bitpool > bitcount + 1))
464 {
465 bits[ch][sb] = 2;
466 bitcount += 2;
467 }
468 if (ch == 1)
469 {
470 ch = 0;
471 sb++;
472 }
473 else
474 {
475 ch = 1;
476 }
477 }
478
479 ch = 0;
480 sb = 0;
481 while ((bitcount < frame->bitpool) && (sb < frame->subbands))
482 {
483 if (bits[ch][sb] < 16)
484 {
485 bits[ch][sb]++;
486 bitcount++;
487 }
488 if (ch == 1)
489 {
490 ch = 0;
491 sb++;
492 }
493
494 else
495 {
496 ch = 1;
497 }
498 }
499
500 }
501 }
502
503 /*
504 * Unpacks a SBC frame at the beginning of the stream in data,
505 * which has at most len bytes into frame.
506 * Returns the length in bytes of the packed frame, or a negative
507 * value on error. The error codes are:
508 *
509 * -1 Data stream too short
510 * -2 Sync byte incorrect
511 * -3 CRC8 incorrect
512 * -4 Bitpool value out of bounds
513 */
514 static int sbc_unpack_frame(const uint8_t * data, struct sbc_frame *frame, size_t len)
515 {
516 UINT consumed;
517 /* Will copy the parts of the header that are relevant to crc calculation here */
518 uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
519 int crc_pos = 0;
520 int32_t temp;
521
522 uint8_t sf; /* sampling_frequency, temporarily needed as array index */
523
524 // int audio_sample;
525 int ch, sb, blk, bit; /* channel, subband, block and bit standard counters */
526 int bits[2][8]; /* bits distribution */
527 int levels[2][8]; /* levels derived from that */
528
529 if (len < 4)
530 {
531 //DbgPrint("Exit when len < 4\n");
532 return -1;
533 }
534
535 if (data[0] != SBC_SYNCWORD)
536 {
537 //DbgPrint("Exit when data[0] != SBC_SYNCWORD\n");
538 return -2;
539 }
540
541 #if 0
542 sf = (data[1] >> 6) & 0x03;
543 switch (sf) {
544 case SBC_FS_16:
545 frame->sampling_frequency = 16000;
546 break;
547 case SBC_FS_32:
548 frame->sampling_frequency = 32000;
549 break;
550 case SBC_FS_44:
551 frame->sampling_frequency = 44100;
552 break;
553 case SBC_FS_48:
554 frame->sampling_frequency = 48000;
555 break;
556 }
557
558 switch ((data[1] >> 4) & 0x03) {
559 case SBC_NB_4:
560 frame->blocks = 4;
561 break;
562 case SBC_NB_8:
563 frame->blocks = 8;
564 break;
565 case SBC_NB_12:
566 frame->blocks = 12;
567 break;
568 case SBC_NB_16:
569 frame->blocks = 16;
570 break;
571 }
572
573 frame->channel_mode = (data[1] >> 2) & 0x03;
574 switch (frame->channel_mode) {
575 case MONO:
576 frame->channels = 1;
577 break;
578 case DUAL_CHANNEL: /* fall-through */
579 case STEREO:
580 case JOINT_STEREO:
581 frame->channels = 2;
582 break;
583 }
584
585 frame->allocation_method = (data[1] >> 1) & 0x01;
586 frame->subbands = (data[1] & 0x01) ? 8 : 4;
587 frame->bitpool = data[2];
588 #endif
589
590 frame->sampling_frequency = 16000;
591 frame->blocks = 15;
592 frame->channel_mode = MONO;
593 frame->channels = 1;
594 frame->allocation_method = SBC_AM_LOUDNESS;
595 frame->subbands = 8;
596 frame->bitpool = 26;
597
598
599 if (((frame->channel_mode == MONO || frame->channel_mode == DUAL_CHANNEL)
600 && frame->bitpool > 16 * frame->subbands)
601 || ((frame->channel_mode == STEREO || frame->channel_mode == JOINT_STEREO)
602 && frame->bitpool > 32 * frame->subbands))
603 {
604 //DbgPrint("Exit when frame->mode == MONO || frame->mode == DUAL_CHANNEL) &&"
605 // "frame->bitpool > 16 * frame->subbands\n");
606 return -4;
607 }
608
609 /* data[3] is crc, we're checking it later */
610 consumed = 32;
611 #if 1
612 crc_header[0] = data[1];
613 crc_header[1] = data[2];
614 #endif
615
616 #if 0
617 crc_header[0] = 0x31;
618 crc_header[1] = 0x1a;
619 #endif
620
621 crc_pos = 16;
622
623 if (frame->channel_mode == JOINT_STEREO)
624 {
625 if (len * 8 < consumed + frame->subbands)
626 return -1;
627
628 frame->join = 0x00;
629 for (sb = 0; sb < frame->subbands - 1; sb++) {
630 frame->join |= ((data[4] >> (7 - sb)) & 0x01) << sb;
631 }
632 if (frame->subbands == 4) {
633 crc_header[crc_pos / 8] = data[4] & 0xf0;
634 } else {
635 crc_header[crc_pos / 8] = data[4];
636 }
637
638 consumed += frame->subbands;
639 crc_pos += frame->subbands;
640 }
641
642 if (len * 8 < consumed + (4 * frame->subbands * frame->channels))
643 {
644 //DbgPrint("len * 8 < consumed + (4 * frame->subbands * frame->channels\n");
645 return -1;
646 }
647
648 for (ch = 0; ch < frame->channels; ch++)
649 {
650 for (sb = 0; sb < frame->subbands; sb++)
651 {
652 /* FIXME assert(consumed % 4 == 0); */
653 frame->scale_factor[ch][sb] = (data[consumed >> 3] >> (4 - (consumed & 0x7))) & 0x0F;
654 crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] << (4 - (crc_pos & 0x7));
655
656 consumed += 4;
657 crc_pos += 4;
658 }
659 }
660
661 if (data[3] != sbc_crc8(crc_header, crc_pos))
662 {
663 ALOGD("sbc_crc %02x -> %02x\n",data[3],sbc_crc8(crc_header, crc_pos));
664 return -3;
665 }
666
667 sf = SBC_FS_16;
668 sbc_calculate_bits(frame, bits, sf);
669
670 for (ch = 0; ch < frame->channels; ch++)
671 {
672 for (sb = 0; sb < frame->subbands; sb++)
673 {
674 levels[ch][sb] = (1 << bits[ch][sb]) - 1;
675 }
676 }
677 #if 0
678 for (blk = 0; blk < frame->blocks; blk++)
679 {
680 for (ch = 0; ch < frame->channels; ch++)
681 {
682 for (sb = 0; sb < frame->subbands; sb++)
683 {
684 if (levels[ch][sb] > 0)
685 {
686 audio_sample = 0;
687 for (bit = 0; bit < bits[ch][sb]; bit++)
688 {
689 if (consumed > len * 8)
690 {
691 LogEvent("Exit when consumed > len * 8\n");
692 return -1;
693 }
694
695 if ((data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01)
696 audio_sample |= 1 << (bits[ch][sb] - bit - 1);
697
698 consumed++;
699 }
700
701 frame->sb_sample[blk][ch][sb] =
702 (((audio_sample << 1) | 1) << frame->scale_factor[ch][sb]) /
703 levels[ch][sb] - (1 << frame->scale_factor[ch][sb]);
704 }
705
706 else
707 frame->sb_sample[blk][ch][sb] = 0;
708 }
709 }
710 }
711 #endif
712
713 #if 1
714
715 for (blk = 0; blk < frame->blocks; blk++)
716 {
717 for (ch = 0; ch < frame->channels; ch++)
718 {
719 for (sb = 0; sb < frame->subbands; sb++)
720 {
721 frame->audio_sample[blk][ch][sb] = 0;
722 if (bits[ch][sb] == 0)
723 continue;
724
725 for (bit = 0; bit < bits[ch][sb]; bit++)
726 {
727 int b; /* A bit */
728 if (consumed > len * 8)
729 return -1;
730
731 b = (data[consumed >> 3] >> (7 - (consumed & 0x7))) & 0x01;
732 frame->audio_sample[blk][ch][sb] |= b << (bits[ch][sb] - bit - 1);
733
734 consumed++;
735 }
736 }
737 }
738 }
739
740 for (blk = 0; blk < frame->blocks; blk++)
741 {
742 for (ch = 0; ch < frame->channels; ch++)
743 {
744 for (sb = 0; sb < frame->subbands; sb++)
745 {
746 if (levels[ch][sb] > 0)
747 {
748 frame->sb_sample[blk][ch][sb] =
749 (((frame->audio_sample[blk][ch][sb] << 16) | 0x8000) / levels[ch][sb]) - 0x8000;
750
751 frame->sb_sample[blk][ch][sb] >>= 3;
752 frame->sb_sample[blk][ch][sb] = (frame->sb_sample[blk][ch][sb] << (frame->scale_factor[ch][sb] + 1)); // Q13
753 }
754 else
755 {
756 frame->sb_sample[blk][ch][sb] = 0;
757 }
758 }
759 }
760 }
761 #endif
762
763 if (frame->channel_mode == JOINT_STEREO)
764 {
765 for (blk = 0; blk < frame->blocks; blk++)
766 {
767 for (sb = 0; sb < frame->subbands; sb++)
768 {
769 if (frame->join & (0x01 << sb)) {
770 temp = frame->sb_sample[blk][0][sb] + frame->sb_sample[blk][1][sb];
771 frame->sb_sample[blk][1][sb] = frame->sb_sample[blk][0][sb] - frame->sb_sample[blk][1][sb];
772 frame->sb_sample[blk][0][sb] = temp;
773 }
774 }
775 }
776 }
777
778 if ((consumed & 0x7) != 0)
779 consumed += 8 - (consumed & 0x7);
780
781
782 return consumed >> 3;
783 }
784
785 static void sbc_decoder_init(struct sbc_decoder_state *state, const struct sbc_frame *frame)
786 {
787 int i, ch;
788
789 memset(state->V, 0, sizeof(state->V));
790 state->subbands = frame->subbands;
791
792 for (ch = 0; ch < 2; ch++)
793 for (i = 0; i < frame->subbands * 2; i++)
794 state->offset[ch][i] = (10 * i + 10);
795 }
796
797 static __inline void sbc_synthesize_four(struct sbc_decoder_state *state,
798 struct sbc_frame *frame, int ch, int blk)
799 {
800 int i, j, k, idx;
801 sbc_extended_t res;
802
803 for(i = 0; i < 8; i++) {
804 /* Shifting */
805 state->offset[ch][i]--;
806 if(state->offset[ch][i] < 0) {
807 state->offset[ch][i] = 79;
808 for(j = 0; j < 9; j++) {
809 state->V[ch][j+80] = state->V[ch][j];
810 }
811 }
812 }
813
814
815 for(i = 0; i < 8; i++) {
816 /* Distribute the new matrix value to the shifted position */
817 SBC_FIXED_0(res);
818 for (j = 0; j < 4; j++) {
819 MULA(res, synmatrix4[i][j], frame->sb_sample[blk][ch][j]);
820 }
821 state->V[ch][state->offset[ch][i]] = SCALE4_STAGED1(res);
822 }
823
824 /* Compute the samples */
825 for(idx = 0, i = 0; i < 4; i++) {
826 k = (i + 4) & 0xf;
827 SBC_FIXED_0(res);
828 for(j = 0; j < 10; idx++) {
829 MULA(res, state->V[ch][state->offset[ch][i]+j++], sbc_proto_4_40m0[idx]);
830 MULA(res, state->V[ch][state->offset[ch][k]+j++], sbc_proto_4_40m1[idx]);
831 }
832 /* Store in output */
833 frame->pcm_sample[ch][blk * 4 + i] = (short)SCALE4_STAGED2(res); // Q0
834 }
835 }
836
837 static __inline void sbc_synthesize_eight(struct sbc_decoder_state *state,
838 struct sbc_frame *frame, int ch, int blk)
839 {
840 int i, j, k, idx;
841 sbc_extended_t res;
842
843 for(i = 0; i < 16; i++) {
844 /* Shifting */
845 state->offset[ch][i]--;
846 if(state->offset[ch][i] < 0) {
847 state->offset[ch][i] = 159;
848 for(j = 0; j < 9; j++) {
849 state->V[ch][j+160] = state->V[ch][j];
850 }
851 }
852 }
853
854 for(i = 0; i < 16; i++) {
855 /* Distribute the new matrix value to the shifted position */
856 SBC_FIXED_0(res);
857 for (j = 0; j < 8; j++) {
858 MULA(res, synmatrix8[i][j], frame->sb_sample[blk][ch][j]); // Q28 = Q15 * Q13
859 }
860 state->V[ch][state->offset[ch][i]] = SCALE8_STAGED1(res); // Q10
861 }
862
863
864 /* Compute the samples */
865 for(idx = 0, i = 0; i < 8; i++) {
866 k = (i + 8) & 0xf;
867 SBC_FIXED_0(res);
868 for(j = 0; j < 10; idx++) {
869 MULA(res, state->V[ch][state->offset[ch][i]+j++], sbc_proto_8_80m0[idx]);
870 MULA(res, state->V[ch][state->offset[ch][k]+j++], sbc_proto_8_80m1[idx]);
871 }
872 /* Store in output */
873 frame->pcm_sample[ch][blk * 8 + i] = (short)SCALE8_STAGED2(res); // Q0
874
875 }
876 }
877
878 static int sbc_synthesize_audio(struct sbc_decoder_state *state, struct sbc_frame *frame)
879 {
880 int ch, blk;
881
882 switch (frame->subbands) {
883 case 4:
884 for (ch = 0; ch < frame->channels; ch++) {
885 for (blk = 0; blk < frame->blocks; blk++)
886 sbc_synthesize_four(state, frame, ch, blk);
887 }
888 return frame->blocks * 4;
889
890 case 8:
891 for (ch = 0; ch < frame->channels; ch++) {
892 for (blk = 0; blk < frame->blocks; blk++)
893 sbc_synthesize_eight(state, frame, ch, blk);
894 }
895 return frame->blocks * 8;
896
897 default:
898 return -EIO;
899 }
900 }
901
902 static void sbc_encoder_init(struct sbc_encoder_state *state, const struct sbc_frame *frame)
903 {
904 memset(&state->X, 0, sizeof(state->X));
905 state->subbands = frame->subbands;
906 }
907
908 static __inline void _sbc_analyze_four(const int32_t *in, int32_t *out)
909 {
910
911 sbc_extended_t res;
912 sbc_extended_t t[8];
913
914 out[0] = out[1] = out[2] = out[3] = 0;
915
916 MUL(res, _sbc_proto_4[0], (in[8] - in[32])); // Q18
917 MULA(res, _sbc_proto_4[1], (in[16] - in[24]));
918 t[0] = SCALE4_STAGE1(res); // Q8
919
920 MUL(res, _sbc_proto_4[2], in[1]);
921 MULA(res, _sbc_proto_4[3], in[9]);
922 MULA(res, _sbc_proto_4[4], in[17]);
923 MULA(res, _sbc_proto_4[5], in[25]);
924 MULA(res, _sbc_proto_4[6], in[33]);
925 t[1] = SCALE4_STAGE1(res);
926
927 MUL(res, _sbc_proto_4[7], in[2]);
928 MULA(res, _sbc_proto_4[8], in[10]);
929 MULA(res, _sbc_proto_4[9], in[18]);
930 MULA(res, _sbc_proto_4[10], in[26]);
931 MULA(res, _sbc_proto_4[11], in[34]);
932 t[2] = SCALE4_STAGE1(res);
933
934 MUL(res, _sbc_proto_4[12], in[3]);
935 MULA(res, _sbc_proto_4[13], in[11]);
936 MULA(res, _sbc_proto_4[14], in[19]);
937 MULA(res, _sbc_proto_4[15], in[27]);
938 MULA(res, _sbc_proto_4[16], in[35]);
939 t[3] = SCALE4_STAGE1(res);
940
941 MUL(res, _sbc_proto_4[17], in[4]);
942 MULA(res, _sbc_proto_4[18], (in[12] + in[28]));
943 MULA(res, _sbc_proto_4[19], in[20]);
944 MULA(res, _sbc_proto_4[17], in[36]);
945 t[4] = SCALE4_STAGE1(res);
946
947 MUL(res, _sbc_proto_4[16], in[5]);
948 MULA(res, _sbc_proto_4[15], in[13]);
949 MULA(res, _sbc_proto_4[14], in[21]);
950 MULA(res, _sbc_proto_4[13], in[29]);
951 MULA(res, _sbc_proto_4[12], in[37]);
952 t[5] = SCALE4_STAGE1(res);
953
954 MUL(res, _sbc_proto_4[11], in[6]);
955 MULA(res, _sbc_proto_4[10], in[14]);
956 MULA(res, _sbc_proto_4[9], in[22]);
957 MULA(res, _sbc_proto_4[8], in[30]);
958 MULA(res, _sbc_proto_4[7], in[38]);
959 t[6] = SCALE4_STAGE1(res);
960
961 MUL(res, _sbc_proto_4[6], in[7]);
962 MULA(res, _sbc_proto_4[5], in[15]);
963 MULA(res, _sbc_proto_4[4], in[23]);
964 MULA(res, _sbc_proto_4[3], in[31]);
965 MULA(res, _sbc_proto_4[2], in[39]);
966 t[7] = SCALE4_STAGE1(res);
967
968 MUL(res, _anamatrix4[0], t[0]);
969 MULA(res, _anamatrix4[1], t[1]);
970 MULA(res, _anamatrix4[2], t[2]);
971 MULA(res, _anamatrix4[1], t[3]);
972 MULA(res, _anamatrix4[0], t[4]);
973 MULA(res, _anamatrix4[3], t[5]);
974 MULA(res, -_anamatrix4[3], t[7]);
975 out[0] = (int)SCALE4_STAGE2(res); // Q0
976
977 MUL(res, -_anamatrix4[0], t[0]);
978 MULA(res, _anamatrix4[3], t[1]);
979 MULA(res, _anamatrix4[2], t[2]);
980 MULA(res, _anamatrix4[3], t[3]);
981 MULA(res, -_anamatrix4[0], t[4]);
982 MULA(res, -_anamatrix4[1], t[5]);
983 MULA(res, _anamatrix4[1], t[7]);
984 out[1] = (int)SCALE4_STAGE2(res);
985
986
987 MUL(res, -_anamatrix4[0], t[0]);
988 MULA(res, -_anamatrix4[3], t[1]);
989 MULA(res, _anamatrix4[2], t[2]);
990 MULA(res, -_anamatrix4[3], t[3]);
991 MULA(res, -_anamatrix4[0], t[4]);
992 MULA(res, _anamatrix4[1], t[5]);
993 MULA(res, -_anamatrix4[1], t[7]);
994 out[2] = (int)SCALE4_STAGE2(res);
995
996 MUL(res, _anamatrix4[0], t[0]);
997 MULA(res, -_anamatrix4[1], t[1]);
998 MULA(res, _anamatrix4[2], t[2]);
999 MULA(res, -_anamatrix4[1], t[3]);
1000 MULA(res, _anamatrix4[0], t[4]);
1001 MULA(res, -_anamatrix4[3], t[5]);
1002 MULA(res, _anamatrix4[3], t[7]);
1003 out[3] = (int)SCALE4_STAGE2(res);
1004 }
1005 static __inline void sbc_analyze_four(struct sbc_encoder_state *state,
1006 struct sbc_frame *frame, int ch, int blk)
1007 {
1008 int i;
1009 /* Input 4 New Audio Samples */
1010 for (i = 39; i >= 4; i--)
1011 state->X[ch][i] = state->X[ch][i - 4];
1012 for (i = 3; i >= 0; i--)
1013 state->X[ch][i] = frame->pcm_sample[ch][blk * 4 + (3 - i)];
1014 _sbc_analyze_four(state->X[ch], frame->sb_sample_f[blk][ch]);
1015 }
1016
1017 static __inline void _sbc_analyze_eight(const int32_t *in, int32_t *out)
1018 {
1019 sbc_extended_t res;
1020 sbc_extended_t t[8];
1021
1022 out[0] = out[1] = out[2] = out[3] = out[4] = out[5] = out[6] = out[7] = 0;
1023
1024 MUL(res, _sbc_proto_8[0], (in[16] - in[64])); // Q18 = Q18 * Q0
1025 MULA(res, _sbc_proto_8[1], (in[32] - in[48]));
1026 MULA(res, _sbc_proto_8[2], in[4]);
1027 MULA(res, _sbc_proto_8[3], in[20]);
1028 MULA(res, _sbc_proto_8[4], in[36]);
1029 MULA(res, _sbc_proto_8[5], in[52]);
1030 t[0] = SCALE8_STAGE1(res); // Q10
1031
1032 MUL(res, _sbc_proto_8[6], in[2]);
1033 MULA(res, _sbc_proto_8[7], in[18]);
1034 MULA(res, _sbc_proto_8[8], in[34]);
1035 MULA(res, _sbc_proto_8[9], in[50]);
1036 MULA(res, _sbc_proto_8[10], in[66]);
1037 t[1] = SCALE8_STAGE1(res);
1038
1039 MUL(res, _sbc_proto_8[11], in[1]);
1040 MULA(res, _sbc_proto_8[12], in[17]);
1041 MULA(res, _sbc_proto_8[13], in[33]);
1042 MULA(res, _sbc_proto_8[14], in[49]);
1043 MULA(res, _sbc_proto_8[15], in[65]);
1044 MULA(res, _sbc_proto_8[16], in[3]);
1045 MULA(res, _sbc_proto_8[17], in[19]);
1046 MULA(res, _sbc_proto_8[18], in[35]);
1047 MULA(res, _sbc_proto_8[19], in[51]);
1048 MULA(res, _sbc_proto_8[20], in[67]);
1049 t[2] = SCALE8_STAGE1(res);
1050
1051 MUL(res, _sbc_proto_8[21], in[5]);
1052 MULA(res, _sbc_proto_8[22], in[21]);
1053 MULA(res, _sbc_proto_8[23], in[37]);
1054 MULA(res, _sbc_proto_8[24], in[53]);
1055 MULA(res, _sbc_proto_8[25], in[69]);
1056 MULA(res, -_sbc_proto_8[15], in[15]);
1057 MULA(res, -_sbc_proto_8[14], in[31]);
1058 MULA(res, -_sbc_proto_8[13], in[47]);
1059 MULA(res, -_sbc_proto_8[12], in[63]);
1060 MULA(res, -_sbc_proto_8[11], in[79]);
1061 t[3] = SCALE8_STAGE1(res);
1062
1063 MUL(res, _sbc_proto_8[26], in[6]);
1064 MULA(res, _sbc_proto_8[27], in[22]);
1065 MULA(res, _sbc_proto_8[28], in[38]);
1066 MULA(res, _sbc_proto_8[29], in[54]);
1067 MULA(res, _sbc_proto_8[30], in[70]);
1068 MULA(res, -_sbc_proto_8[10], in[14]);
1069 MULA(res, -_sbc_proto_8[9], in[30]);
1070 MULA(res, -_sbc_proto_8[8], in[46]);
1071 MULA(res, -_sbc_proto_8[7], in[62]);
1072 MULA(res, -_sbc_proto_8[6], in[78]);
1073 t[4] = SCALE8_STAGE1(res);
1074
1075 MUL(res, _sbc_proto_8[31], in[7]);
1076 MULA(res, _sbc_proto_8[32], in[23]);
1077 MULA(res, _sbc_proto_8[33], in[39]);
1078 MULA(res, _sbc_proto_8[34], in[55]);
1079 MULA(res, _sbc_proto_8[35], in[71]);
1080 MULA(res, -_sbc_proto_8[20], in[13]);
1081 MULA(res, -_sbc_proto_8[19], in[29]);
1082 MULA(res, -_sbc_proto_8[18], in[45]);
1083 MULA(res, -_sbc_proto_8[17], in[61]);
1084 MULA(res, -_sbc_proto_8[16], in[77]);
1085 t[5] = SCALE8_STAGE1(res);
1086
1087 MUL(res, _sbc_proto_8[36], (in[8] + in[72]));
1088 MULA(res, _sbc_proto_8[37], in[24]);
1089 MULA(res, _sbc_proto_8[38], in[40]);
1090 MULA(res, _sbc_proto_8[37], in[56]);
1091 MULA(res, -_sbc_proto_8[39], in[12]);
1092 MULA(res, -_sbc_proto_8[5], in[28]);
1093 MULA(res, -_sbc_proto_8[4], in[44]);
1094 MULA(res, -_sbc_proto_8[3], in[60]);
1095 MULA(res, -_sbc_proto_8[2], in[76]);
1096 t[6] = SCALE8_STAGE1(res);
1097
1098 MUL(res, _sbc_proto_8[35], in[9]);
1099 MULA(res, _sbc_proto_8[34], in[25]);
1100 MULA(res, _sbc_proto_8[33], in[41]);
1101 MULA(res, _sbc_proto_8[32], in[57]);
1102 MULA(res, _sbc_proto_8[31], in[73]);
1103 MULA(res, -_sbc_proto_8[25], in[11]);
1104 MULA(res, -_sbc_proto_8[24], in[27]);
1105 MULA(res, -_sbc_proto_8[23], in[43]);
1106 MULA(res, -_sbc_proto_8[22], in[59]);
1107 MULA(res, -_sbc_proto_8[21], in[75]);
1108 t[7] = SCALE8_STAGE1(res);
1109
1110 MUL(res, _anamatrix8[0], t[0]); // = Q14 * Q10
1111 MULA(res, _anamatrix8[7], t[1]);
1112 MULA(res, _anamatrix8[2], t[2]);
1113 MULA(res, _anamatrix8[3], t[3]);
1114 MULA(res, _anamatrix8[6], t[4]);
1115 MULA(res, _anamatrix8[4], t[5]);
1116 MULA(res, _anamatrix8[1], t[6]);
1117 MULA(res, _anamatrix8[5], t[7]);
1118 out[0] = (int)SCALE8_STAGE2(res); // Q0
1119
1120 MUL(res, _anamatrix8[1], t[0]);
1121 MULA(res, _anamatrix8[7], t[1]);
1122 MULA(res, _anamatrix8[3], t[2]);
1123 MULA(res, -_anamatrix8[5], t[3]);
1124 MULA(res, -_anamatrix8[6], t[4]);
1125 MULA(res, -_anamatrix8[2], t[5]);
1126 MULA(res, -_anamatrix8[0], t[6]);
1127 MULA(res, -_anamatrix8[4], t[7]);
1128 out[1] = (int)SCALE8_STAGE2(res);
1129
1130 MUL(res, -_anamatrix8[1], t[0]);
1131 MULA(res, _anamatrix8[7], t[1]);
1132 MULA(res, _anamatrix8[4], t[2]);
1133 MULA(res, -_anamatrix8[2], t[3]);
1134 MULA(res, -_anamatrix8[6], t[4]);
1135 MULA(res, _anamatrix8[5], t[5]);
1136 MULA(res, _anamatrix8[0], t[6]);
1137 MULA(res, _anamatrix8[3], t[7]);
1138 out[2] = (int)SCALE8_STAGE2(res);
1139
1140 MUL(res, -_anamatrix8[0], t[0]);
1141 MULA(res, _anamatrix8[7], t[1]);
1142 MULA(res, _anamatrix8[5], t[2]);
1143 MULA(res, -_anamatrix8[4], t[3]);
1144 MULA(res, _anamatrix8[6], t[4]);
1145 MULA(res, _anamatrix8[3], t[5]);
1146 MULA(res, -_anamatrix8[1], t[6]);
1147 MULA(res, -_anamatrix8[2], t[7]);
1148 out[3] = (int)SCALE8_STAGE2(res);
1149
1150 MUL(res, -_anamatrix8[0], t[0]);
1151 MULA(res, _anamatrix8[7], t[1]);
1152 MULA(res, -_anamatrix8[5], t[2]);
1153 MULA(res, _anamatrix8[4], t[3]);
1154 MULA(res, _anamatrix8[6], t[4]);
1155 MULA(res, -_anamatrix8[3], t[5]);
1156 MULA(res, -_anamatrix8[1], t[6]);
1157 MULA(res, _anamatrix8[2], t[7]);
1158 out[4] = (int)SCALE8_STAGE2(res);
1159
1160 MUL(res, -_anamatrix8[1], t[0]);
1161 MULA(res, _anamatrix8[7], t[1]);
1162 MULA(res, -_anamatrix8[4], t[2]);
1163 MULA(res, _anamatrix8[2], t[3]);
1164 MULA(res, -_anamatrix8[6], t[4]);
1165 MULA(res, -_anamatrix8[5], t[5]);
1166 MULA(res, _anamatrix8[0], t[6]);
1167 MULA(res, -_anamatrix8[3], t[7]);
1168 out[5] = (int)SCALE8_STAGE2(res);
1169
1170 MUL(res, _anamatrix8[1], t[0]);
1171 MULA(res, _anamatrix8[7], t[1]);
1172 MULA(res, -_anamatrix8[3], t[2]);
1173 MULA(res, _anamatrix8[5], t[3]);
1174 MULA(res, -_anamatrix8[6], t[4]);
1175 MULA(res, _anamatrix8[2], t[5]);
1176 MULA(res, -_anamatrix8[0], t[6]);
1177 MULA(res, _anamatrix8[4], t[7]);
1178 out[6] = (int)SCALE8_STAGE2(res);
1179
1180 MUL(res, _anamatrix8[0], t[0]);
1181 MULA(res, _anamatrix8[7], t[1]);
1182 MULA(res, -_anamatrix8[2], t[2]);
1183 MULA(res, -_anamatrix8[3], t[3]);
1184 MULA(res, _anamatrix8[6], t[4]);
1185 MULA(res, -_anamatrix8[4], t[5]);
1186 MULA(res, _anamatrix8[1], t[6]);
1187 MULA(res, -_anamatrix8[5], t[7]);
1188 out[7] = (int)SCALE8_STAGE2(res);
1189 }
1190
1191 static __inline void sbc_analyze_eight(struct sbc_encoder_state *state,
1192 struct sbc_frame *frame, int ch, int blk)
1193 {
1194 int i;
1195
1196 /* Input 8 Audio Samples */
1197 for (i = 79; i >= 8; i--)
1198 state->X[ch][i] = state->X[ch][i - 8];
1199 for (i = 7; i >= 0; i--)
1200 state->X[ch][i] = frame->pcm_sample[ch][blk * 8 + (7 - i)];
1201 _sbc_analyze_eight(state->X[ch], frame->sb_sample_f[blk][ch]);
1202 }
1203
1204 static int sbc_analyze_audio(struct sbc_encoder_state *state, struct sbc_frame *frame)
1205 {
1206 int ch, blk;
1207
1208 switch (frame->subbands)
1209 {
1210 case 4:
1211 for (ch = 0; ch < frame->channels; ch++)
1212 for (blk = 0; blk < frame->blocks; blk++) {
1213 sbc_analyze_four(state, frame, ch, blk);
1214 }
1215 return frame->blocks * 4;
1216
1217 case 8:
1218 for (ch = 0; ch < frame->channels; ch++)
1219 for (blk = 0; blk < frame->blocks; blk++) {
1220 sbc_analyze_eight(state, frame, ch, blk);
1221 }
1222 return frame->blocks * 8;
1223
1224 default:
1225 return -EIO;
1226 }
1227 }
1228
1229 /*
1230 * Packs the SBC frame from frame into the memory at data. At most len
1231 * bytes will be used, should more memory be needed an appropriate
1232 * error code will be returned. Returns the length of the packed frame
1233 * on success or a negative value on error.
1234 *
1235 * The error codes are:
1236 * -1 Not enough memory reserved
1237 * -2 Unsupported sampling rate
1238 * -3 Unsupported number of blocks
1239 * -4 Unsupported number of subbands
1240 * -5 Bitpool value out of bounds
1241 * -99 not implemented
1242 */
1243
1244 static int sbc_pack_frame(uint8_t * data, struct sbc_frame *frame, size_t len)
1245 {
1246 size_t produced;
1247 /* Will copy the header parts for CRC-8 calculation here */
1248 uint8_t crc_header[11] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
1249 int crc_pos = 0;
1250
1251 uint8_t sf; /* Sampling frequency as temporary value for table lookup */
1252
1253 int ch, sb, blk, bit; /* channel, subband, block and bit counters */
1254 int bits[2][8]; /* bits distribution */
1255 int levels[2][8]; /* levels are derived from that */
1256
1257 u_int32_t scalefactor[2][8]; /* derived from frame->scale_factor */
1258
1259 if (len < 4)
1260 {
1261 return -1;
1262 }
1263
1264 /* Clear first 4 bytes of data (that's the constant length part of the SBC header) */
1265 memset(data, 0, 4);
1266
1267 data[0] = SBC_SYNCWORD;
1268
1269 if (frame->sampling_frequency == 16000)
1270 {
1271 data[1] |= (SBC_FS_16 & 0x03) << 6;
1272 sf = SBC_FS_16;
1273 }
1274 else if (frame->sampling_frequency == 32000)
1275 {
1276 data[1] |= (SBC_FS_32 & 0x03) << 6;
1277 sf = SBC_FS_32;
1278 }
1279 else if (frame->sampling_frequency == 44100)
1280 {
1281 data[1] |= (SBC_FS_44 & 0x03) << 6;
1282 sf = SBC_FS_44;
1283 }
1284 else if (frame->sampling_frequency == 48000)
1285 {
1286 data[1] |= (SBC_FS_48 & 0x03) << 6;
1287 sf = SBC_FS_48;
1288 }
1289 else
1290 {
1291 return -2;
1292 }
1293
1294 switch (frame->blocks)
1295 {
1296 case 4:
1297 data[1] |= (SBC_NB_4 & 0x03) << 4;
1298 break;
1299 case 8:
1300 data[1] |= (SBC_NB_8 & 0x03) << 4;
1301 break;
1302 case 12:
1303 data[1] |= (SBC_NB_12 & 0x03) << 4;
1304 break;
1305 case 15:
1306 data[1] |= (SBC_NB_16 & 0x03) << 4;
1307 break;
1308 default:
1309 return -3;
1310 break;
1311 }
1312
1313 data[1] |= (frame->channel_mode & 0x03) << 2;
1314
1315 data[1] |= (frame->allocation_method & 0x01) << 1;
1316
1317 switch (frame->subbands) {
1318 case 4:
1319 /* Nothing to do */
1320 break;
1321 case 8:
1322 data[1] |= 0x01;
1323 break;
1324 default:
1325 return -4;
1326 break;
1327 }
1328
1329 data[2] = frame->bitpool;
1330 if (((frame->channel_mode == MONO || frame->channel_mode == DUAL_CHANNEL)
1331 && frame->bitpool > 16 * frame->subbands)
1332 || ((frame->channel_mode == STEREO || frame->channel_mode == JOINT_STEREO)
1333 && frame->bitpool > 32 * frame->subbands)) {
1334 return -5;
1335 }
1336
1337 /* Can't fill in crc yet */
1338
1339 produced = 32;
1340
1341 // evan.
1342 data[1] = 0x00;
1343 data[2] = 0x00;
1344
1345 crc_header[0] = data[1];
1346 crc_header[1] = data[2];
1347 crc_pos = 16;
1348
1349
1350 for (ch = 0; ch < frame->channels; ch++) {
1351 for (sb = 0; sb < frame->subbands; sb++) {
1352 frame->scale_factor[ch][sb] = 0;
1353 scalefactor[ch][sb] = 2;
1354 for (blk = 0; blk < frame->blocks; blk++) {
1355 while (scalefactor[ch][sb] < (u_int32_t)fabs(frame->sb_sample_f[blk][ch][sb])) {
1356 frame->scale_factor[ch][sb]++;
1357 scalefactor[ch][sb] *= 2;
1358 }
1359 }
1360 }
1361 }
1362
1363 if (frame->channel_mode == JOINT_STEREO) {
1364 int32_t sb_sample_j[16][2][7]; /* like frame->sb_sample but joint stereo */
1365 u_int32_t scalefactor_j[2][7], scale_factor_j[2][7]; /* scalefactor and scale_factor in joint case */
1366
1367 /* Calculate joint stereo signal */
1368 for (sb = 0; sb < frame->subbands - 1; sb++) {
1369 for (blk = 0; blk < frame->blocks; blk++) {
1370 sb_sample_j[blk][0][sb] = (frame->sb_sample_f[blk][0][sb] + frame->sb_sample_f[blk][1][sb]) >> 1;
1371 sb_sample_j[blk][1][sb] = (frame->sb_sample_f[blk][0][sb] - frame->sb_sample_f[blk][1][sb]) >> 1;
1372 }
1373 }
1374
1375 /* calculate scale_factor_j and scalefactor_j for joint case */
1376 for (ch = 0; ch < 2; ch++) {
1377 for (sb = 0; sb < frame->subbands - 1; sb++) {
1378 scale_factor_j[ch][sb] = 0;
1379 scalefactor_j[ch][sb] = 2;
1380 for (blk = 0; blk < frame->blocks; blk++) {
1381 while (scalefactor_j[ch][sb] < (u_int32_t)fabs(sb_sample_j[blk][ch][sb])) {
1382 scale_factor_j[ch][sb]++;
1383 scalefactor_j[ch][sb] *= 2;
1384 }
1385 }
1386 }
1387 }
1388
1389 /* decide which subbands to join */
1390 frame->join = 0;
1391 for (sb = 0; sb < frame->subbands - 1; sb++) {
1392 if ((scalefactor[0][sb] + scalefactor[1][sb]) >
1393 (scalefactor_j[0][sb] + scalefactor_j[1][sb]) ) {
1394 /* use joint stereo for this subband */
1395 frame->join |= 1 << sb;
1396 frame->scale_factor[0][sb] = scale_factor_j[0][sb];
1397 frame->scale_factor[1][sb] = scale_factor_j[1][sb];
1398 scalefactor[0][sb] = scalefactor_j[0][sb];
1399 scalefactor[1][sb] = scalefactor_j[1][sb];
1400 for (blk = 0; blk < frame->blocks; blk++) {
1401 frame->sb_sample_f[blk][0][sb] = sb_sample_j[blk][0][sb];
1402 frame->sb_sample_f[blk][1][sb] = sb_sample_j[blk][1][sb];
1403 }
1404 }
1405 }
1406
1407 if (len * 8 < produced + frame->subbands)
1408 return -1;
1409
1410 data[4] = 0;
1411 for (sb = 0; sb < frame->subbands - 1; sb++) {
1412 data[4] |= ((frame->join >> sb) & 0x01) << (7 - sb);
1413 }
1414 if (frame->subbands == 4) {
1415 crc_header[crc_pos / 8] = data[4] & 0xf0;
1416 } else {
1417 crc_header[crc_pos / 8] = data[4];
1418 }
1419
1420 produced += frame->subbands;
1421 crc_pos += frame->subbands;
1422 }
1423
1424 if (len * 8 < produced + (4 * frame->subbands * frame->channels))
1425 return -1;
1426
1427 for (ch = 0; ch < frame->channels; ch++) {
1428 for (sb = 0; sb < frame->subbands; sb++) {
1429 if (produced % 8 == 0)
1430 data[produced / 8] = 0;
1431 data[produced / 8] |= ((frame->scale_factor[ch][sb] & 0x0F) << (4 - (produced % 8)));
1432 crc_header[crc_pos / 8] |= ((frame->scale_factor[ch][sb] & 0x0F) << (4 - (crc_pos % 8)));
1433
1434 produced += 4;
1435 crc_pos += 4;
1436 }
1437 }
1438
1439 data[3] = sbc_crc8(crc_header, crc_pos);
1440
1441
1442
1443 sbc_calculate_bits(frame, bits, sf);
1444
1445 for (ch = 0; ch < frame->channels; ch++) {
1446 for (sb = 0; sb < frame->subbands; sb++) {
1447 levels[ch][sb] = (1 << bits[ch][sb]) - 1;
1448 }
1449 }
1450
1451 for (blk = 0; blk < frame->blocks; blk++) {
1452 for (ch = 0; ch < frame->channels; ch++) {
1453 for (sb = 0; sb < frame->subbands; sb++) {
1454 if (levels[ch][sb] > 0) {
1455 frame->audio_sample[blk][ch][sb] =
1456 (uint16_t) ((((frame->sb_sample_f[blk][ch][sb]*levels[ch][sb]) >> (frame->scale_factor[ch][sb] + 1)) +
1457 levels[ch][sb]) >> 1);
1458 } else {
1459 frame->audio_sample[blk][ch][sb] = 0;
1460 }
1461 }
1462 }
1463 }
1464
1465 for (blk = 0; blk < frame->blocks; blk++) {
1466 for (ch = 0; ch < frame->channels; ch++) {
1467 for (sb = 0; sb < frame->subbands; sb++) {
1468 if (bits[ch][sb] != 0) {
1469 for (bit = 0; bit < bits[ch][sb]; bit++) {
1470 int b; /* A bit */
1471 if (produced > len * 8) {
1472 return -1;
1473 }
1474 if (produced % 8 == 0) {
1475 data[produced / 8] = 0;
1476 }
1477 b = ((frame->audio_sample[blk][ch][sb]) >> (bits[ch][sb] - bit -
1478 1)) & 0x01;
1479 data[produced / 8] |= b << (7 - (produced % 8));
1480 produced++;
1481 }
1482 }
1483 }
1484 }
1485 }
1486
1487 if (produced % 8 != 0) {
1488 produced += 8 - (produced % 8);
1489 }
1490
1491 return (int)(produced / 8);
1492 }
1493
1494 struct sbc_priv {
1495 int init;
1496 struct sbc_frame frame;
1497 struct sbc_decoder_state dec_state;
1498 struct sbc_encoder_state enc_state;
1499 };
1500
1501 int sbc_init(sbc_t *sbc)//int sbc_init(sbc_t *sbc, unsigned long flags)
1502 {
1503 if (!sbc)
1504 return -EIO;
1505
1506 //flags = flags;
1507
1508 memset(sbc, 0, sizeof(sbc_t));
1509
1510 sbc->priv = malloc(sizeof(struct sbc_priv) + SBC_ALIGN_MASK);
1511 if (!sbc->priv)
1512 return -ENOMEM;
1513 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1514
1515 sbc->rate = 16000;
1516 sbc->channels = 1;
1517 sbc->joint = 0;
1518 sbc->subbands = 8;
1519 sbc->blocks = 15;
1520 sbc->bitpool = 26;
1521
1522 return 0;
1523 }
1524
1525 int sbc_reinit(sbc_t *sbc)//int sbc_reinit(sbc_t *sbc, unsigned long flags)
1526 {
1527 struct sbc_priv *priv;
1528
1529 if (!sbc || !sbc->priv)
1530 return -EIO;
1531
1532 //flags = flags;
1533 priv = sbc->priv;
1534
1535 if (priv->init == 1)
1536 memset(sbc->priv, 0, sizeof(struct sbc_priv));
1537
1538 return 0;
1539 }
1540
1541
1542 int sbc_decode(sbc_t *sbc, void *input, int input_len, void *output,
1543 int output_len, int *written)
1544 {
1545 struct sbc_priv *priv;
1546 char *ptr;
1547 int i, ch, framelen, samples;
1548
1549 if (!sbc)
1550 {
1551 //DbgPrint("Exit.when sbc is NULL.\n");
1552 return -EIO;
1553 }
1554
1555 if (!sbc && !input)
1556 {
1557 //DbgPrint("!sbc && !input\n");
1558 return -EIO;
1559 }
1560
1561 priv = sbc->priv;
1562 if(!priv)
1563 return -99;
1564
1565 framelen = sbc_unpack_frame(input, &priv->frame, input_len);
1566
1567 if (!priv->init)
1568 {
1569 sbc_decoder_init(&priv->dec_state, &priv->frame);
1570 priv->init = 1;
1571
1572 sbc->rate = priv->frame.sampling_frequency;
1573 sbc->channels = priv->frame.channels;
1574 sbc->subbands = priv->frame.subbands;
1575 sbc->blocks = priv->frame.blocks;
1576 sbc->bitpool = priv->frame.bitpool;
1577 }
1578
1579 if (!output)
1580 {
1581 //DbgPrint("!output\n");
1582 return framelen;
1583 }
1584
1585 if (written)
1586 *written = 0;
1587
1588 if (framelen <= 0)
1589 {
1590 //DbgPrint("Exit when framelen <= 0\n");
1591 return framelen;
1592 }
1593
1594 samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame);
1595
1596 ptr = output;
1597 if (output_len < samples * priv->frame.channels * 2)
1598 samples = output_len / (priv->frame.channels * 2);
1599
1600 /*
1601 if (!sbc->data)
1602 {
1603 sbc->size = samples * priv->frame.channels * 2;
1604 sbc->data = malloc(sbc->size);
1605 }
1606
1607
1608 if (sbc->size < samples * priv->frame.channels * 2)
1609 {
1610 sbc->size = samples * priv->frame.channels * 2;
1611 sbc->data = realloc(sbc->data, sbc->size);
1612 }
1613
1614 if (!sbc->data)
1615 {
1616 sbc->size = 0;
1617 return -ENOMEM;
1618 }
1619 */
1620
1621 for (i = 0; i < samples; i++)
1622 {
1623 for (ch = 0; ch < priv->frame.channels; ch++)
1624 {
1625 int16_t s;
1626 s = priv->frame.pcm_sample[ch][i];
1627 /*
1628 *ptr++ = (s & 0xff00) >> 8;
1629 *ptr++ = (s & 0x00ff);
1630 */
1631 *ptr++ = (s & 0x00ff);
1632 *ptr++ = (s & 0xff00) >> 8;
1633 }
1634 }
1635
1636 if (written)
1637 *written = samples * priv->frame.channels * 2;
1638
1639 return framelen;
1640 }
1641
1642 int sbc_encode(sbc_t *sbc, void *input, int input_len, void *output,
1643 int output_len, int *written)
1644 {
1645 struct sbc_priv *priv;
1646 char *ptr;
1647 int i, ch, framelen, samples;
1648
1649 if (!sbc)
1650 {
1651 //DbgPrint("Exit, when !sbc.\n");
1652 return -EIO;
1653 }
1654
1655
1656 if (!sbc && !input)
1657 {
1658 //DbgPrint("Exit, when !sbc && !input.\n");
1659 return -EIO;
1660 }
1661
1662 /// make sure sbc has been initialized
1663 priv = sbc->priv;
1664 if(priv == NULL){
1665 //DbgPrint("priv == NULL\n");
1666 return -EIO;
1667 }
1668
1669 if (written)
1670 *written = 0;
1671
1672 if (!priv->init)
1673 {
1674 //DbgPrint("Initial priv->frame ,when priv->init is null\n");
1675 priv->frame.sampling_frequency = sbc->rate;
1676 priv->frame.channels = sbc->channels;
1677 priv->frame.channel_mode = MONO;
1678 priv->frame.allocation_method = LOUDNESS;
1679 priv->frame.subbands = sbc->subbands;
1680 priv->frame.blocks = sbc->blocks;
1681 priv->frame.bitpool = sbc->bitpool;
1682
1683 sbc_encoder_init(&priv->enc_state, &priv->frame);
1684 priv->init = 1;
1685 }
1686
1687 /* input must be large enough to encode a complete frame */
1688 if (input_len < 240)
1689 {
1690 //DbgPrint("Exit, when input_len < priv->frame.codesize..\n");
1691 return 0;
1692 }
1693
1694 /* output must be large enough to receive the encoded frame */
1695 if (!output || output_len < 57)
1696 {
1697 //DbgPrint("Exit, when !output || output_len < priv->frame.length\n");
1698 return -ENOSPC;
1699 }
1700
1701 ptr = input;
1702 for (i = 0; i < priv->frame.subbands * priv->frame.blocks; i++)
1703 {
1704 for (ch = 0; ch < sbc->channels; ch++)
1705 {
1706 // int16_t s = (ptr[0] & 0xff) << 8 | (ptr[1] & 0xff);
1707 int16_t s = (ptr[1] & 0xff) << 8 | (ptr[0] & 0xff);
1708 ptr += 2;
1709 priv->frame.pcm_sample[ch][i] = s;
1710 }
1711 }
1712
1713 samples = 0;
1714 samples = sbc_analyze_audio(&priv->enc_state, &priv->frame);
1715
1716 if (!sbc->data)
1717 {
1718 sbc->size = 1024;
1719 // sbc->data = malloc(sbc->size);
1720 sbc->data = malloc(sbc->size);
1721 if (!sbc->data)
1722 {
1723 //DbgPrint("sbc->data allocate failed!!!\n");
1724 return -ENOMEM;
1725 }
1726 memset(sbc->data, 0, sbc->size);
1727 }
1728
1729 if (!sbc->data)
1730 {
1731 sbc->size = 0;
1732 //DbgPrint("sbc->data is null, so exit!!!\n");
1733 return -ENOMEM;
1734 }
1735
1736 framelen = sbc_pack_frame(output, &priv->frame, output_len);
1737 if (written)
1738 {
1739 *written = (int)framelen;//in 64 bit os, it should be okay.
1740 }
1741
1742 sbc->len = framelen;
1743 sbc->duration = (1000000 * priv->frame.subbands * priv->frame.blocks) / sbc->rate;
1744
1745 return samples * sbc->channels * 2;
1746 }
1747
1748
1749 void sbc_finish(sbc_t *sbc)
1750 {
1751 if (!sbc)
1752 return;
1753
1754 if (sbc->data)
1755 free(sbc->data);
1756
1757
1758 if (sbc->priv)
1759 {
1760 free(sbc->priv);
1761 }
1762 memset(sbc, 0, sizeof(sbc_t));
1763 }