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