1 /*- 2 * Copyright (c) 2015 Nathanial Sloss <nathanialsloss@yahoo.com.au> 3 * 4 * This software is dedicated to the memory of - 5 * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012. 6 * 7 * Barry was a man who loved his music. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 1. Redistributions of source code must retain the above copyright 13 * notice, this list of conditions and the following disclaimer. 14 * 2. Redistributions in binary form must reproduce the above copyright 15 * notice, this list of conditions and the following disclaimer in the 16 * documentation and/or other materials provided with the distribution. 17 * 18 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS 19 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 20 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 21 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS 22 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 * POSSIBILITY OF SUCH DAMAGE. 29 */ 30 31 #include <sys/cdefs.h> 32 #include <sys/types.h> 33 #include <sys/param.h> 34 #include <sys/endian.h> 35 #include <sys/uio.h> 36 37 #include <stdio.h> 38 #include <errno.h> 39 #include <stdbool.h> 40 #include <stdlib.h> 41 #include <string.h> 42 #include <unistd.h> 43 #include <math.h> 44 45 #include "sbc_coeffs.h" 46 #include "bt.h" 47 48 #define SYNCWORD 0x9c 49 #define ABS(x) (((x) < 0) ? -(x) : (x)) 50 #define BIT30 (1U << 30) 51 #define BM(x) ((1LL << (x)) - 1LL) 52 53 /* Loudness offset allocations. */ 54 static const int loudnessoffset8[4][8] = { 55 {-2, 0, 0, 0, 0, 0, 0, 1}, 56 {-3, 0, 0, 0, 0, 0, 1, 2}, 57 {-4, 0, 0, 0, 0, 0, 1, 2}, 58 {-4, 0, 0, 0, 0, 0, 1, 2}, 59 }; 60 61 static const int loudnessoffset4[4][4] = { 62 {-1, 0, 0, 0}, 63 {-2, 0, 0, 1}, 64 {-2, 0, 0, 1}, 65 {-2, 0, 0, 1}, 66 }; 67 68 static uint8_t 69 calc_scalefactors_joint(struct sbc_encode *sbc) 70 { 71 float sb_j[16][2]; 72 uint32_t x; 73 uint32_t y; 74 uint8_t block; 75 uint8_t joint; 76 uint8_t sb; 77 uint8_t lz; 78 79 joint = 0; 80 for (sb = 0; sb != sbc->bands - 1; sb++) { 81 for (block = 0; block < sbc->blocks; block++) { 82 sb_j[block][0] = (sbc->samples[block][0][sb] + 83 sbc->samples[block][1][sb]) / 2.0f; 84 sb_j[block][1] = (sbc->samples[block][0][sb] - 85 sbc->samples[block][1][sb]) / 2.0f; 86 } 87 88 x = 1 << 15; 89 y = 1 << 15; 90 for (block = 0; block < sbc->blocks; block++) { 91 x |= (uint32_t)ABS(sb_j[block][0]); 92 y |= (uint32_t)ABS(sb_j[block][1]); 93 } 94 95 lz = 1; 96 while (!(x & BIT30)) { 97 lz++; 98 x <<= 1; 99 } 100 x = 16 - lz; 101 102 lz = 1; 103 while (!(y & BIT30)) { 104 lz++; 105 y <<= 1; 106 } 107 y = 16 - lz; 108 109 if ((sbc->scalefactor[0][sb] + sbc->scalefactor[1][sb]) > x + y) { 110 joint |= 1 << (sbc->bands - sb - 1); 111 sbc->scalefactor[0][sb] = x; 112 sbc->scalefactor[1][sb] = y; 113 for (block = 0; block < sbc->blocks; block++) { 114 sbc->samples[block][0][sb] = sb_j[block][0]; 115 sbc->samples[block][1][sb] = sb_j[block][1]; 116 } 117 } 118 } 119 return (joint); 120 } 121 122 static void 123 calc_scalefactors(struct sbc_encode *sbc) 124 { 125 uint8_t block; 126 uint8_t ch; 127 uint8_t sb; 128 129 for (ch = 0; ch != sbc->channels; ch++) { 130 for (sb = 0; sb != sbc->bands; sb++) { 131 uint32_t x = 1 << 15; 132 uint8_t lx = 1; 133 134 for (block = 0; block != sbc->blocks; block++) 135 x |= (uint32_t)ABS(sbc->samples[block][ch][sb]); 136 137 while (!(x & BIT30)) { 138 lx++; 139 x <<= 1; 140 } 141 sbc->scalefactor[ch][sb] = 16 - lx; 142 } 143 } 144 } 145 146 static void 147 calc_bitneed(struct bt_config *cfg) 148 { 149 struct sbc_encode *sbc = cfg->handle.sbc_enc; 150 int32_t bitneed[2][8]; 151 int32_t max_bitneed, bitcount; 152 int32_t slicecount, bitslice; 153 int32_t loudness; 154 int ch, sb, start_chan = 0; 155 156 if (cfg->chmode == MODE_DUAL) 157 sbc->channels = 1; 158 159 next_chan: 160 max_bitneed = 0; 161 bitcount = 0; 162 slicecount = 0; 163 164 if (cfg->allocm == ALLOC_SNR) { 165 for (ch = start_chan; ch < sbc->channels; ch++) { 166 for (sb = 0; sb < sbc->bands; sb++) { 167 bitneed[ch][sb] = sbc->scalefactor[ch][sb]; 168 169 if (bitneed[ch][sb] > max_bitneed) 170 max_bitneed = bitneed[ch][sb]; 171 } 172 } 173 } else { 174 for (ch = start_chan; ch < sbc->channels; ch++) { 175 for (sb = 0; sb < sbc->bands; sb++) { 176 if (sbc->scalefactor[ch][sb] == 0) { 177 bitneed[ch][sb] = -5; 178 } else { 179 if (sbc->bands == 8) { 180 loudness = sbc->scalefactor[ch][sb] - 181 loudnessoffset8[cfg->freq][sb]; 182 } else { 183 loudness = sbc->scalefactor[ch][sb] - 184 loudnessoffset4[cfg->freq][sb]; 185 } 186 if (loudness > 0) 187 bitneed[ch][sb] = loudness / 2; 188 else 189 bitneed[ch][sb] = loudness; 190 } 191 if (bitneed[ch][sb] > max_bitneed) 192 max_bitneed = bitneed[ch][sb]; 193 } 194 } 195 } 196 197 slicecount = bitcount = 0; 198 bitslice = max_bitneed + 1; 199 do { 200 bitslice--; 201 bitcount += slicecount; 202 slicecount = 0; 203 for (ch = start_chan; ch < sbc->channels; ch++) { 204 for (sb = 0; sb < sbc->bands; sb++) { 205 if ((bitneed[ch][sb] > bitslice + 1) && 206 (bitneed[ch][sb] < bitslice + 16)) 207 slicecount++; 208 else if (bitneed[ch][sb] == bitslice + 1) 209 slicecount += 2; 210 } 211 } 212 } while (bitcount + slicecount < cfg->bitpool); 213 214 /* check if exactly one more fits */ 215 if (bitcount + slicecount == cfg->bitpool) { 216 bitcount += slicecount; 217 bitslice--; 218 } 219 for (ch = start_chan; ch < sbc->channels; ch++) { 220 for (sb = 0; sb < sbc->bands; sb++) { 221 if (bitneed[ch][sb] < bitslice + 2) { 222 sbc->bits[ch][sb] = 0; 223 } else { 224 sbc->bits[ch][sb] = bitneed[ch][sb] - bitslice; 225 if (sbc->bits[ch][sb] > 16) 226 sbc->bits[ch][sb] = 16; 227 } 228 } 229 } 230 231 if (cfg->chmode == MODE_DUAL) 232 ch = start_chan; 233 else 234 ch = 0; 235 sb = 0; 236 while (bitcount < cfg->bitpool && sb < sbc->bands) { 237 if ((sbc->bits[ch][sb] >= 2) && (sbc->bits[ch][sb] < 16)) { 238 sbc->bits[ch][sb]++; 239 bitcount++; 240 } else if ((bitneed[ch][sb] == bitslice + 1) && 241 (cfg->bitpool > bitcount + 1)) { 242 sbc->bits[ch][sb] = 2; 243 bitcount += 2; 244 } 245 if (sbc->channels == 1 || start_chan == 1) 246 sb++; 247 else if (ch == 1) { 248 ch = 0; 249 sb++; 250 } else 251 ch = 1; 252 } 253 254 if (cfg->chmode == MODE_DUAL) 255 ch = start_chan; 256 else 257 ch = 0; 258 sb = 0; 259 while (bitcount < cfg->bitpool && sb < sbc->bands) { 260 if (sbc->bits[ch][sb] < 16) { 261 sbc->bits[ch][sb]++; 262 bitcount++; 263 } 264 if (sbc->channels == 1 || start_chan == 1) 265 sb++; 266 else if (ch == 1) { 267 ch = 0; 268 sb++; 269 } else 270 ch = 1; 271 } 272 273 if (cfg->chmode == MODE_DUAL && start_chan == 0) { 274 start_chan = 1; 275 sbc->channels = 2; 276 goto next_chan; 277 } 278 } 279 280 static void 281 sbc_store_bits_crc(struct sbc_encode *sbc, uint32_t numbits, uint32_t value) 282 { 283 uint32_t off = sbc->bitoffset; 284 285 while (numbits-- && off != sbc->maxoffset) { 286 if (value & (1 << numbits)) { 287 sbc->data[off / 8] |= 1 << ((7 - off) & 7); 288 sbc->crc ^= 0x80; 289 } 290 sbc->crc *= 2; 291 if (sbc->crc & 0x100) 292 sbc->crc ^= 0x11d; /* CRC-8 polynomial */ 293 294 off++; 295 } 296 sbc->bitoffset = off; 297 } 298 299 static int 300 sbc_encode(struct bt_config *cfg) 301 { 302 struct sbc_encode *sbc = cfg->handle.sbc_enc; 303 const int16_t *input = sbc->music_data; 304 float delta[2][8]; 305 float levels[2][8]; 306 float mask[2][8]; 307 float S; 308 float *X; 309 float Z[80]; 310 float Y[80]; 311 float audioout; 312 int16_t left[8]; 313 int16_t right[8]; 314 int16_t *data; 315 int numsamples; 316 int i; 317 int k; 318 int block; 319 int chan; 320 int sb; 321 322 for (block = 0; block < sbc->blocks; block++) { 323 324 for (i = 0; i < sbc->bands; i++) { 325 left[i] = *input++; 326 if (sbc->channels == 2) 327 right[i] = *input++; 328 } 329 330 for (chan = 0; chan < sbc->channels; chan++) { 331 332 /* select right or left channel */ 333 if (chan == 0) { 334 X = sbc->left; 335 data = left; 336 } else { 337 X = sbc->right; 338 data = right; 339 } 340 341 /* shift up old data */ 342 for (i = (sbc->bands * 10) - 1; i > sbc->bands - 1; i--) 343 X[i] = X[i - sbc->bands]; 344 k = 0; 345 for (i = sbc->bands - 1; i >= 0; i--) 346 X[i] = data[k++]; 347 for (i = 0; i < sbc->bands * 10; i++) { 348 if (sbc->bands == 8) 349 Z[i] = sbc_coeffs8[i] * X[i]; 350 else 351 Z[i] = sbc_coeffs4[i] * X[i]; 352 } 353 for (i = 0; i < sbc->bands * 2; i++) { 354 Y[i] = 0; 355 for (k = 0; k < 5; k++) 356 Y[i] += Z[i + k * sbc->bands * 2]; 357 } 358 for (i = 0; i < sbc->bands; i++) { 359 S = 0; 360 for (k = 0; k < sbc->bands * 2; k++) { 361 if (sbc->bands == 8) { 362 S += cosdata8[i][k] * Y[k]; 363 } else { 364 S += cosdata4[i][k] * Y[k]; 365 } 366 } 367 sbc->samples[block][chan][i] = S * (1 << 15); 368 } 369 } 370 } 371 372 calc_scalefactors(sbc); 373 374 if (cfg->chmode == MODE_JOINT) 375 sbc->join = calc_scalefactors_joint(sbc); 376 else 377 sbc->join = 0; 378 379 calc_bitneed(cfg); 380 381 for (chan = 0; chan < sbc->channels; chan++) { 382 for (sb = 0; sb < sbc->bands; sb++) { 383 if (sbc->bits[chan][sb] == 0) 384 continue; 385 mask[chan][sb] = BM(sbc->bits[chan][sb]); 386 levels[chan][sb] = mask[chan][sb] * 387 (1LL << (15 - sbc->scalefactor[chan][sb])); 388 delta[chan][sb] = 389 (1LL << (sbc->scalefactor[chan][sb] + 16)); 390 } 391 } 392 393 numsamples = 0; 394 for (block = 0; block < sbc->blocks; block++) { 395 for (chan = 0; chan < sbc->channels; chan++) { 396 for (sb = 0; sb < sbc->bands; sb++) { 397 if (sbc->bits[chan][sb] == 0) 398 continue; 399 audioout = (levels[chan][sb] * 400 (delta[chan][sb] + sbc->samples[block][chan][sb])); 401 audioout /= (1LL << 32); 402 403 audioout = roundf(audioout); 404 405 /* range check */ 406 if (audioout > mask[chan][sb]) 407 audioout = mask[chan][sb]; 408 409 sbc->output[numsamples++] = audioout; 410 } 411 } 412 } 413 return (numsamples); 414 } 415 416 static void 417 sbc_decode(struct bt_config *cfg) 418 { 419 struct sbc_encode *sbc = cfg->handle.sbc_enc; 420 float delta[2][8]; 421 float levels[2][8]; 422 float audioout; 423 float *X; 424 float *V; 425 float left[160]; 426 float right[160]; 427 float U[160]; 428 float W[160]; 429 float S[8]; 430 int position; 431 int block; 432 int chan; 433 int sb; 434 int i; 435 int k; 436 437 for (chan = 0; chan < sbc->channels; chan++) { 438 for (sb = 0; sb < sbc->bands; sb++) { 439 levels[chan][sb] = (1 << sbc->bits[chan][sb]) - 1; 440 delta[chan][sb] = (1 << sbc->scalefactor[chan][sb]); 441 } 442 } 443 444 i = 0; 445 for (block = 0; block < sbc->blocks; block++) { 446 for (chan = 0; chan < sbc->channels; chan++) { 447 for (sb = 0; sb < sbc->bands; sb++) { 448 if (sbc->bits[chan][sb] == 0) { 449 audioout = 0; 450 } else { 451 audioout = 452 ((((sbc->output[i] * 2.0f) + 1.0f) * delta[chan][sb]) / 453 levels[chan][sb]) - delta[chan][sb]; 454 } 455 sbc->output[i++] = audioout; 456 } 457 } 458 } 459 460 if (cfg->chmode == MODE_JOINT) { 461 i = 0; 462 while (i < (sbc->blocks * sbc->bands * sbc->channels)) { 463 for (sb = 0; sb < sbc->bands; sb++) { 464 if (sbc->join & (1 << (sbc->bands - sb - 1))) { 465 audioout = sbc->output[i]; 466 sbc->output[i] = (2.0f * sbc->output[i]) + 467 (2.0f * sbc->output[i + sbc->bands]); 468 sbc->output[i + sbc->bands] = 469 (2.0f * audioout) - 470 (2.0f * sbc->output[i + sbc->bands]); 471 sbc->output[i] /= 2.0f; 472 sbc->output[i + sbc->bands] /= 2.0f; 473 } 474 i++; 475 } 476 i += sbc->bands; 477 } 478 } 479 position = 0; 480 for (block = 0; block < sbc->blocks; block++) { 481 for (chan = 0; chan < sbc->channels; chan++) { 482 /* select right or left channel */ 483 if (chan == 0) { 484 X = left; 485 V = sbc->left; 486 } else { 487 X = right; 488 V = sbc->right; 489 } 490 for (i = 0; i < sbc->bands; i++) 491 S[i] = sbc->output[position++]; 492 493 for (i = (sbc->bands * 20) - 1; i >= (sbc->bands * 2); i--) 494 V[i] = V[i - (sbc->bands * 2)]; 495 for (k = 0; k < sbc->bands * 2; k++) { 496 float vk = 0; 497 for (i = 0; i < sbc->bands; i++) { 498 if (sbc->bands == 8) { 499 vk += cosdecdata8[i][k] * S[i]; 500 } else { 501 vk += cosdecdata4[i][k] * S[i]; 502 } 503 } 504 V[k] = vk; 505 } 506 for (i = 0; i <= 4; i++) { 507 for (k = 0; k < sbc->bands; k++) { 508 U[(i * sbc->bands * 2) + k] = 509 V[(i * sbc->bands * 4) + k]; 510 U[(i * sbc->bands 511 * 2) + sbc->bands + k] = 512 V[(i * sbc->bands * 4) + 513 (sbc->bands * 3) + k]; 514 } 515 } 516 for (i = 0; i < sbc->bands * 10; i++) { 517 if (sbc->bands == 4) { 518 W[i] = U[i] * (sbc_coeffs4[i] * -4.0f); 519 } else if (sbc->bands == 8) { 520 W[i] = U[i] * (sbc_coeffs8[i] * -8.0f); 521 } else { 522 W[i] = 0; 523 } 524 } 525 526 for (k = 0; k < sbc->bands; k++) { 527 unsigned int offset = k + (block * sbc->bands); 528 529 X[offset] = 0; 530 for (i = 0; i < 10; i++) { 531 X[offset] += W[k + (i * sbc->bands)]; 532 } 533 534 if (X[offset] > 32767.0) 535 X[offset] = 32767.0; 536 else if (X[offset] < -32767.0) 537 X[offset] = -32767.0; 538 } 539 } 540 } 541 542 for (i = 0, k = 0; k != (sbc->blocks * sbc->bands); k++) { 543 sbc->music_data[i++] = left[k]; 544 if (sbc->channels == 2) 545 sbc->music_data[i++] = right[k]; 546 } 547 } 548 549 size_t 550 sbc_encode_frame(struct bt_config *cfg) 551 { 552 struct sbc_encode *sbc = cfg->handle.sbc_enc; 553 uint8_t config; 554 uint8_t block; 555 uint8_t chan; 556 uint8_t sb; 557 uint8_t j; 558 uint8_t i; 559 560 config = (cfg->freq << 6) | (cfg->blocks << 4) | 561 (cfg->chmode << 2) | (cfg->allocm << 1) | cfg->bands; 562 563 sbc_encode(cfg); 564 565 /* set initial CRC */ 566 sbc->crc = 0x5e; 567 568 /* reset data position and size */ 569 sbc->bitoffset = 0; 570 sbc->maxoffset = sizeof(sbc->data) * 8; 571 572 sbc_store_bits_crc(sbc, 8, SYNCWORD); 573 sbc_store_bits_crc(sbc, 8, config); 574 sbc_store_bits_crc(sbc, 8, cfg->bitpool); 575 576 /* skip 8-bit CRC */ 577 sbc->bitoffset += 8; 578 579 if (cfg->chmode == MODE_JOINT) { 580 if (sbc->bands == 8) 581 sbc_store_bits_crc(sbc, 8, sbc->join); 582 else if (sbc->bands == 4) 583 sbc_store_bits_crc(sbc, 4, sbc->join); 584 } 585 for (i = 0; i < sbc->channels; i++) { 586 for (j = 0; j < sbc->bands; j++) 587 sbc_store_bits_crc(sbc, 4, sbc->scalefactor[i][j]); 588 } 589 590 /* store 8-bit CRC */ 591 sbc->data[3] = (sbc->crc & 0xFF); 592 593 i = 0; 594 for (block = 0; block < sbc->blocks; block++) { 595 for (chan = 0; chan < sbc->channels; chan++) { 596 for (sb = 0; sb < sbc->bands; sb++) { 597 if (sbc->bits[chan][sb] == 0) 598 continue; 599 600 sbc_store_bits_crc(sbc, sbc->bits[chan][sb], sbc->output[i++]); 601 } 602 } 603 } 604 return ((sbc->bitoffset + 7) / 8); 605 } 606 607 static uint32_t 608 sbc_load_bits_crc(struct sbc_encode *sbc, uint32_t numbits) 609 { 610 uint32_t off = sbc->bitoffset; 611 uint32_t value = 0; 612 613 while (numbits-- && off != sbc->maxoffset) { 614 if (sbc->rem_data_ptr[off / 8] & (1 << ((7 - off) & 7))) { 615 value |= (1 << numbits); 616 sbc->crc ^= 0x80; 617 } 618 sbc->crc *= 2; 619 if (sbc->crc & 0x100) 620 sbc->crc ^= 0x11d; /* CRC-8 polynomial */ 621 622 off++; 623 } 624 sbc->bitoffset = off; 625 return (value); 626 } 627 628 size_t 629 sbc_decode_frame(struct bt_config *cfg, int bits) 630 { 631 struct sbc_encode *sbc = cfg->handle.sbc_enc; 632 uint8_t config; 633 uint8_t block; 634 uint8_t chan; 635 uint8_t sb; 636 uint8_t j; 637 uint8_t i; 638 639 sbc->rem_off = 0; 640 sbc->rem_len = 0; 641 642 config = (cfg->freq << 6) | (cfg->blocks << 4) | 643 (cfg->chmode << 2) | (cfg->allocm << 1) | cfg->bands; 644 645 /* set initial CRC */ 646 sbc->crc = 0x5e; 647 648 /* reset data position and size */ 649 sbc->bitoffset = 0; 650 sbc->maxoffset = bits; 651 652 /* verify SBC header */ 653 if (sbc->maxoffset < (8 * 4)) 654 return (0); 655 if (sbc_load_bits_crc(sbc, 8) != SYNCWORD) 656 return (0); 657 if (sbc_load_bits_crc(sbc, 8) != config) 658 return (0); 659 cfg->bitpool = sbc_load_bits_crc(sbc, 8); 660 661 (void)sbc_load_bits_crc(sbc, 8);/* CRC */ 662 663 if (cfg->chmode == MODE_JOINT) { 664 if (sbc->bands == 8) 665 sbc->join = sbc_load_bits_crc(sbc, 8); 666 else if (sbc->bands == 4) 667 sbc->join = sbc_load_bits_crc(sbc, 4); 668 else 669 sbc->join = 0; 670 } else { 671 sbc->join = 0; 672 } 673 674 for (i = 0; i < sbc->channels; i++) { 675 for (j = 0; j < sbc->bands; j++) 676 sbc->scalefactor[i][j] = sbc_load_bits_crc(sbc, 4); 677 } 678 679 calc_bitneed(cfg); 680 681 i = 0; 682 for (block = 0; block < sbc->blocks; block++) { 683 for (chan = 0; chan < sbc->channels; chan++) { 684 for (sb = 0; sb < sbc->bands; sb++) { 685 if (sbc->bits[chan][sb] == 0) { 686 i++; 687 continue; 688 } 689 sbc->output[i++] = 690 sbc_load_bits_crc(sbc, sbc->bits[chan][sb]); 691 } 692 } 693 } 694 695 sbc_decode(cfg); 696 697 sbc->rem_off = 0; 698 sbc->rem_len = sbc->blocks * sbc->channels * sbc->bands; 699 700 return ((sbc->bitoffset + 7) / 8); 701 } 702