1 /* 2 * 3 * Bluetooth low-complexity, subband codec (SBC) library 4 * 5 * Copyright (C) 2004-2009 Marcel Holtmann <marcel (at) holtmann.org> 6 * Copyright (C) 2004-2005 Henryk Ploetz <henryk (at) ploetzli.ch> 7 * Copyright (C) 2005-2008 Brad Midgley <bmidgley (at) 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 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 int sbc_pack_frame_internal(uint8_t *data, 746 struct sbc_frame *frame, size_t len, 747 int frame_subbands, int frame_channels) 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 /* like frame->sb_sample but joint stereo */ 805 int32_t sb_sample_j[16][2]; 806 /* scalefactor and scale_factor in joint case */ 807 uint32_t scalefactor_j[2]; 808 uint8_t scale_factor_j[2]; 809 810 uint8_t joint = 0; 811 frame->joint = 0; 812 813 for (sb = 0; sb < frame_subbands - 1; sb++) { 814 scale_factor_j[0] = 0; 815 scalefactor_j[0] = 2 << SCALE_OUT_BITS; 816 scale_factor_j[1] = 0; 817 scalefactor_j[1] = 2 << SCALE_OUT_BITS; 818 819 for (blk = 0; blk < frame->blocks; blk++) { 820 uint32_t tmp; 821 /* Calculate joint stereo signal */ 822 sb_sample_j[blk][0] = 823 ASR(frame->sb_sample_f[blk][0][sb], 1) + 824 ASR(frame->sb_sample_f[blk][1][sb], 1); 825 sb_sample_j[blk][1] = 826 ASR(frame->sb_sample_f[blk][0][sb], 1) - 827 ASR(frame->sb_sample_f[blk][1][sb], 1); 828 829 /* calculate scale_factor_j and scalefactor_j for joint case */ 830 tmp = fabs(sb_sample_j[blk][0]); 831 while (scalefactor_j[0] < tmp) { 832 scale_factor_j[0]++; 833 scalefactor_j[0] *= 2; 834 } 835 tmp = fabs(sb_sample_j[blk][1]); 836 while (scalefactor_j[1] < tmp) { 837 scale_factor_j[1]++; 838 scalefactor_j[1] *= 2; 839 } 840 } 841 842 /* decide whether to join this subband */ 843 if ((frame->scale_factor[0][sb] + 844 frame->scale_factor[1][sb]) > 845 (scale_factor_j[0] + 846 scale_factor_j[1])) { 847 /* use joint stereo for this subband */ 848 joint |= 1 << (frame_subbands - 1 - sb); 849 frame->joint |= 1 << sb; 850 frame->scale_factor[0][sb] = scale_factor_j[0]; 851 frame->scale_factor[1][sb] = scale_factor_j[1]; 852 for (blk = 0; blk < frame->blocks; blk++) { 853 frame->sb_sample_f[blk][0][sb] = 854 sb_sample_j[blk][0]; 855 frame->sb_sample_f[blk][1][sb] = 856 sb_sample_j[blk][1]; 857 } 858 } 859 } 860 861 PUT_BITS(data_ptr, bits_cache, bits_count, 862 joint, frame_subbands); 863 crc_header[crc_pos >> 3] = joint; 864 crc_pos += frame_subbands; 865 } 866 867 for (ch = 0; ch < frame_channels; ch++) { 868 for (sb = 0; sb < frame_subbands; sb++) { 869 PUT_BITS(data_ptr, bits_cache, bits_count, 870 frame->scale_factor[ch][sb] & 0x0F, 4); 871 crc_header[crc_pos >> 3] <<= 4; 872 crc_header[crc_pos >> 3] |= frame->scale_factor[ch][sb] & 0x0F; 873 crc_pos += 4; 874 } 875 } 876 877 /* align the last crc byte */ 878 if (crc_pos % 8) 879 crc_header[crc_pos >> 3] <<= 8 - (crc_pos % 8); 880 881 data[3] = sbc_crc8(crc_header, crc_pos); 882 883 sbc_calculate_bits(frame, bits); 884 885 for (ch = 0; ch < frame_channels; ch++) { 886 for (sb = 0; sb < frame_subbands; sb++) { 887 levels[ch][sb] = ((1 << bits[ch][sb]) - 1) << 888 (32 - (frame->scale_factor[ch][sb] + 889 SCALE_OUT_BITS + 2)); 890 sb_sample_delta[ch][sb] = (uint32_t) 1 << 891 (frame->scale_factor[ch][sb] + 892 SCALE_OUT_BITS + 1); 893 } 894 } 895 896 for (blk = 0; blk < frame->blocks; blk++) { 897 for (ch = 0; ch < frame_channels; ch++) { 898 for (sb = 0; sb < frame_subbands; sb++) { 899 900 if (bits[ch][sb] == 0) 901 continue; 902 903 audio_sample = ((uint64_t) levels[ch][sb] * 904 (sb_sample_delta[ch][sb] + 905 frame->sb_sample_f[blk][ch][sb])) >> 32; 906 907 PUT_BITS(data_ptr, bits_cache, bits_count, 908 audio_sample, bits[ch][sb]); 909 } 910 } 911 } 912 913 FLUSH_BITS(data_ptr, bits_cache, bits_count); 914 915 return data_ptr - data; 916 } 917 918 static int sbc_pack_frame(uint8_t *data, struct sbc_frame *frame, size_t len) 919 { 920 if (frame->subbands == 4) { 921 if (frame->channels == 1) 922 return sbc_pack_frame_internal(data, frame, len, 4, 1); 923 else 924 return sbc_pack_frame_internal(data, frame, len, 4, 2); 925 } else { 926 if (frame->channels == 1) 927 return sbc_pack_frame_internal(data, frame, len, 8, 1); 928 else 929 return sbc_pack_frame_internal(data, frame, len, 8, 2); 930 } 931 } 932 933 static void sbc_encoder_init(struct sbc_encoder_state *state, 934 const struct sbc_frame *frame) 935 { 936 memset(&state->X, 0, sizeof(state->X)); 937 state->position = (SBC_X_BUFFER_SIZE - frame->subbands * 9) & ~7; 938 939 sbc_init_primitives(state); 940 } 941 942 struct sbc_priv { 943 int init; 944 struct SBC_ALIGNED sbc_frame frame; 945 struct SBC_ALIGNED sbc_decoder_state dec_state; 946 struct SBC_ALIGNED sbc_encoder_state enc_state; 947 }; 948 949 static void sbc_set_defaults(sbc_t *sbc, unsigned long flags) 950 { 951 sbc->frequency = SBC_FREQ_44100; 952 sbc->mode = SBC_MODE_STEREO; 953 sbc->subbands = SBC_SB_8; 954 sbc->blocks = SBC_BLK_16; 955 sbc->bitpool = 32; 956 #if __BYTE_ORDER == __LITTLE_ENDIAN 957 sbc->endian = SBC_LE; 958 #elif __BYTE_ORDER == __BIG_ENDIAN 959 sbc->endian = SBC_BE; 960 #else 961 #error "Unknown byte order" 962 #endif 963 } 964 965 int sbc_init(sbc_t *sbc, unsigned long flags) 966 { 967 if (!sbc) 968 return -EIO; 969 970 memset(sbc, 0, sizeof(sbc_t)); 971 972 sbc->priv_alloc_base = malloc(sizeof(struct sbc_priv) + SBC_ALIGN_MASK); 973 if (!sbc->priv_alloc_base) 974 return -ENOMEM; 975 976 sbc->priv = (void *) (((uintptr_t) sbc->priv_alloc_base + 977 SBC_ALIGN_MASK) & ~((uintptr_t) SBC_ALIGN_MASK)); 978 979 memset(sbc->priv, 0, sizeof(struct sbc_priv)); 980 981 sbc_set_defaults(sbc, flags); 982 983 return 0; 984 } 985 986 ssize_t sbc_parse(sbc_t *sbc, const void *input, size_t input_len) 987 { 988 return sbc_decode(sbc, input, input_len, NULL, 0, NULL); 989 } 990 991 ssize_t sbc_decode(sbc_t *sbc, const void *input, size_t input_len, 992 void *output, size_t output_len, size_t *written) 993 { 994 struct sbc_priv *priv; 995 char *ptr; 996 int i, ch, framelen, samples; 997 998 if (!sbc || !input) 999 return -EIO; 1000 1001 priv = sbc->priv; 1002 1003 framelen = sbc_unpack_frame(input, &priv->frame, input_len); 1004 1005 if (!priv->init) { 1006 sbc_decoder_init(&priv->dec_state, &priv->frame); 1007 priv->init = 1; 1008 1009 sbc->frequency = priv->frame.frequency; 1010 sbc->mode = priv->frame.mode; 1011 sbc->subbands = priv->frame.subband_mode; 1012 sbc->blocks = priv->frame.block_mode; 1013 sbc->allocation = priv->frame.allocation; 1014 sbc->bitpool = priv->frame.bitpool; 1015 1016 priv->frame.codesize = sbc_get_codesize(sbc); 1017 priv->frame.length = framelen; 1018 } 1019 1020 if (!output) 1021 return framelen; 1022 1023 if (written) 1024 *written = 0; 1025 1026 if (framelen <= 0) 1027 return framelen; 1028 1029 samples = sbc_synthesize_audio(&priv->dec_state, &priv->frame); 1030 1031 ptr = output; 1032 1033 if (output_len < (size_t) (samples * priv->frame.channels * 2)) 1034 samples = output_len / (priv->frame.channels * 2); 1035 1036 for (i = 0; i < samples; i++) { 1037 for (ch = 0; ch < priv->frame.channels; ch++) { 1038 int16_t s; 1039 s = priv->frame.pcm_sample[ch][i]; 1040 1041 if (sbc->endian == SBC_BE) { 1042 *ptr++ = (s & 0xff00) >> 8; 1043 *ptr++ = (s & 0x00ff); 1044 } else { 1045 *ptr++ = (s & 0x00ff); 1046 *ptr++ = (s & 0xff00) >> 8; 1047 } 1048 } 1049 } 1050 1051 if (written) 1052 *written = samples * priv->frame.channels * 2; 1053 1054 return framelen; 1055 } 1056 1057 ssize_t sbc_encode(sbc_t *sbc, const void *input, size_t input_len, 1058 void *output, size_t output_len, size_t *written) 1059 { 1060 struct sbc_priv *priv; 1061 int framelen, samples; 1062 int (*sbc_enc_process_input)(int position, 1063 const uint8_t *pcm, int16_t X[2][SBC_X_BUFFER_SIZE], 1064 int nsamples, int nchannels); 1065 1066 if (!sbc || !input) 1067 return -EIO; 1068 1069 priv = sbc->priv; 1070 1071 if (written) 1072 *written = 0; 1073 1074 if (!priv->init) { 1075 priv->frame.frequency = sbc->frequency; 1076 priv->frame.mode = sbc->mode; 1077 priv->frame.channels = sbc->mode == SBC_MODE_MONO ? 1 : 2; 1078 priv->frame.allocation = sbc->allocation; 1079 priv->frame.subband_mode = sbc->subbands; 1080 priv->frame.subbands = sbc->subbands ? 8 : 4; 1081 priv->frame.block_mode = sbc->blocks; 1082 priv->frame.blocks = 4 + (sbc->blocks * 4); 1083 priv->frame.bitpool = sbc->bitpool; 1084 priv->frame.codesize = sbc_get_codesize(sbc); 1085 priv->frame.length = sbc_get_frame_length(sbc); 1086 1087 sbc_encoder_init(&priv->enc_state, &priv->frame); 1088 priv->init = 1; 1089 } 1090 1091 /* input must be large enough to encode a complete frame */ 1092 if (input_len < priv->frame.codesize) 1093 return 0; 1094 1095 /* output must be large enough to receive the encoded frame */ 1096 if (!output || output_len < priv->frame.length) 1097 return -ENOSPC; 1098 1099 /* Select the needed input data processing function and call it */ 1100 if (priv->frame.subbands == 8) { 1101 if (sbc->endian == SBC_BE) 1102 sbc_enc_process_input = 1103 priv->enc_state.sbc_enc_process_input_8s_be; 1104 else 1105 sbc_enc_process_input = 1106 priv->enc_state.sbc_enc_process_input_8s_le; 1107 } else { 1108 if (sbc->endian == SBC_BE) 1109 sbc_enc_process_input = 1110 priv->enc_state.sbc_enc_process_input_4s_be; 1111 else 1112 sbc_enc_process_input = 1113 priv->enc_state.sbc_enc_process_input_4s_le; 1114 } 1115 1116 priv->enc_state.position = sbc_enc_process_input( 1117 priv->enc_state.position, (const uint8_t *) input, 1118 priv->enc_state.X, priv->frame.subbands * priv->frame.blocks, 1119 priv->frame.channels); 1120 1121 samples = sbc_analyze_audio(&priv->enc_state, &priv->frame); 1122 1123 priv->enc_state.sbc_calc_scalefactors( 1124 priv->frame.sb_sample_f, priv->frame.scale_factor, 1125 priv->frame.blocks, priv->frame.channels, priv->frame.subbands); 1126 1127 framelen = sbc_pack_frame(output, &priv->frame, output_len); 1128 1129 if (written) 1130 *written = framelen; 1131 1132 return samples * priv->frame.channels * 2; 1133 } 1134 1135 void sbc_finish(sbc_t *sbc) 1136 { 1137 if (!sbc) 1138 return; 1139 1140 if (sbc->priv_alloc_base) 1141 free(sbc->priv_alloc_base); 1142 1143 memset(sbc, 0, sizeof(sbc_t)); 1144 } 1145 1146 size_t sbc_get_frame_length(sbc_t *sbc) 1147 { 1148 int ret; 1149 uint8_t subbands, channels, blocks, joint, bitpool; 1150 struct sbc_priv *priv; 1151 1152 priv = sbc->priv; 1153 if (priv->init) 1154 return priv->frame.length; 1155 1156 subbands = sbc->subbands ? 8 : 4; 1157 blocks = 4 + (sbc->blocks * 4); 1158 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2; 1159 joint = sbc->mode == SBC_MODE_JOINT_STEREO ? 1 : 0; 1160 bitpool = sbc->bitpool; 1161 1162 ret = 4 + (4 * subbands * channels) / 8; 1163 /* This term is not always evenly divide so we round it up */ 1164 if (channels == 1) 1165 ret += ((blocks * channels * bitpool) + 7) / 8; 1166 else 1167 ret += (((joint ? subbands : 0) + blocks * bitpool) + 7) / 8; 1168 1169 return ret; 1170 } 1171 1172 unsigned sbc_get_frame_duration(sbc_t *sbc) 1173 { 1174 uint8_t subbands, blocks; 1175 uint16_t frequency; 1176 struct sbc_priv *priv; 1177 1178 priv = sbc->priv; 1179 if (!priv->init) { 1180 subbands = sbc->subbands ? 8 : 4; 1181 blocks = 4 + (sbc->blocks * 4); 1182 } else { 1183 subbands = priv->frame.subbands; 1184 blocks = priv->frame.blocks; 1185 } 1186 1187 switch (sbc->frequency) { 1188 case SBC_FREQ_16000: 1189 frequency = 16000; 1190 break; 1191 1192 case SBC_FREQ_32000: 1193 frequency = 32000; 1194 break; 1195 1196 case SBC_FREQ_44100: 1197 frequency = 44100; 1198 break; 1199 1200 case SBC_FREQ_48000: 1201 frequency = 48000; 1202 break; 1203 default: 1204 return 0; 1205 } 1206 1207 return (1000000 * blocks * subbands) / frequency; 1208 } 1209 1210 size_t sbc_get_codesize(sbc_t *sbc) 1211 { 1212 uint16_t subbands, channels, blocks; 1213 struct sbc_priv *priv; 1214 1215 priv = sbc->priv; 1216 if (!priv->init) { 1217 subbands = sbc->subbands ? 8 : 4; 1218 blocks = 4 + (sbc->blocks * 4); 1219 channels = sbc->mode == SBC_MODE_MONO ? 1 : 2; 1220 } else { 1221 subbands = priv->frame.subbands; 1222 blocks = priv->frame.blocks; 1223 channels = priv->frame.channels; 1224 } 1225 1226 return subbands * blocks * channels * 2; 1227 } 1228 1229 const char *sbc_get_implementation_info(sbc_t *sbc) 1230 { 1231 struct sbc_priv *priv; 1232 1233 if (!sbc) 1234 return NULL; 1235 1236 priv = sbc->priv; 1237 if (!priv) 1238 return NULL; 1239 1240 return priv->enc_state.implementation_info; 1241 } 1242 1243 int sbc_reinit(sbc_t *sbc, unsigned long flags) 1244 { 1245 struct sbc_priv *priv; 1246 1247 if (!sbc || !sbc->priv) 1248 return -EIO; 1249 1250 priv = sbc->priv; 1251 1252 if (priv->init == 1) 1253 memset(sbc->priv, 0, sizeof(struct sbc_priv)); 1254 1255 sbc_set_defaults(sbc, flags); 1256 1257 return 0; 1258 } 1259
