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