1 /* $NetBSD: sbc_encode.c,v 1.13 2023/07/21 02:11:18 nat Exp $ */ 2 3 /*- 4 * Copyright (c) 2015 - 2016 Nathanial Sloss <nathanialsloss (at) yahoo.com.au> 5 * All rights reserved. 6 * 7 * This software is dedicated to the memory of - 8 * Baron James Anlezark (Barry) - 1 Jan 1949 - 13 May 2012. 9 * 10 * Barry was a man who loved his music. 11 * 12 * Redistribution and use in source and binary forms, with or without 13 * modification, are permitted provided that the following conditions 14 * are met: 15 * 1. Redistributions of source code must retain the above copyright 16 * notice, this list of conditions and the following disclaimer. 17 * 2. Redistributions in binary form must reproduce the above copyright 18 * notice, this list of conditions and the following disclaimer in the 19 * documentation and/or other materials provided with the distribution. 20 * 21 * THIS SOFTWARE IS PROVIDED BY THE NETBSD FOUNDATION, INC. AND CONTRIBUTORS 22 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 23 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 24 * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR CONTRIBUTORS 25 * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 * POSSIBILITY OF SUCH DAMAGE. 32 */ 33 34 #include <sys/cdefs.h> 35 #include <sys/time.h> 36 #include <sys/types.h> 37 #include <sys/param.h> 38 #include <errno.h> 39 #include <stdbool.h> 40 #include <stdlib.h> 41 #include <string.h> 42 #include <unistd.h> 43 #include <inttypes.h> 44 45 #include <sbc_coeffs.h> 46 #include <sbc_crc.h> 47 #include "sbc_encode.h" 48 49 static uint8_t make_crc(uint8_t); 50 uint8_t Crc8(uint8_t, uint8_t *, size_t, ssize_t); 51 static ssize_t make_frame(uint8_t *, int16_t *); 52 static ssize_t parseFrame(uint8_t *, int16_t *); 53 static void calc_scalefactors(int32_t samples[16][2][8]); 54 static uint8_t calc_scalefactors_joint(int32_t sb_sample[16][2][8]); 55 static size_t sbc_encode(int16_t *, int32_t *); 56 static void calc_bitneed(void); 57 static ssize_t get_bits(uint8_t *, int, uint32_t *); 58 static ssize_t move_bits(uint8_t *, int, uint32_t); 59 static ssize_t move_bits_crc(uint8_t *, int, uint32_t); 60 static size_t sbc_decode(int32_t *, int16_t *); 61 62 uint32_t scalefactor[2][8]; 63 int bits[2][8]; 64 int global_chan = 2; 65 int global_bands = 8; 66 int global_blocks = 16; 67 int global_volume = 0; 68 uint8_t global_bitpool = 32; 69 uint8_t global_mode = MODE_STEREO; 70 uint8_t global_alloc = ALLOC_LOUDNESS; 71 uint8_t global_freq = FREQ_44_1K; 72 uint8_t global_bands_config = BANDS_8; 73 uint8_t global_block_config = BLOCKS_16; 74 uint8_t join = 0; 75 76 #define SYNCWORD 0x9c 77 78 struct a2dp_frame_header { 79 uint8_t syncword; 80 uint8_t config; 81 uint8_t bitpool; 82 uint8_t crc; 83 }; 84 85 struct a2dp_frame_header_joint { 86 uint8_t syncword; 87 uint8_t config; 88 uint8_t bitpool; 89 uint8_t crc; 90 uint8_t joint; 91 }; 92 93 94 struct a2dp_frame_mono { 95 struct a2dp_frame_header header; 96 uint8_t scale[4]; 97 uint8_t samples[256]; 98 }; 99 100 struct a2dp_frame_joint { 101 struct a2dp_frame_header_joint header; 102 uint8_t scale[8]; 103 uint8_t samples[256]; 104 }; 105 106 struct a2dp_frame { 107 struct a2dp_frame_header header; 108 uint8_t scale[8]; 109 uint8_t samples[256]; 110 }; 111 112 struct rtpHeader { 113 uint8_t id; /* Just random number. */ 114 uint8_t id2; 115 uint8_t seqnumMSB; /* Packet seq. number most significant byte. */ 116 uint8_t seqnumLSB; 117 uint8_t ts3; /* Timestamp most significant byte. */ 118 uint8_t ts2; 119 uint8_t ts1; 120 uint8_t ts0; /* Timestamp least significant byte. */ 121 uint8_t reserved3; 122 uint8_t reserved2; 123 uint8_t reserved1; 124 uint8_t reserved0; /* Reseverd least significant byte set to 1. */ 125 uint8_t numFrames; /* Number of sbc frames in this packet. */ 126 }; 127 128 /* Loudness offset allocations. */ 129 int loudnessoffset8[4][8] = { 130 { -2, 0, 0, 0, 0, 0, 0, 1 }, 131 { -3, 0, 0, 0, 0, 0, 1, 2 }, 132 { -4, 0, 0, 0, 0, 0, 1, 2 }, 133 { -4, 0, 0, 0, 0, 0, 1, 2 }, 134 }; 135 136 int loudnessoffset4[4][4] = { 137 { -1, 0, 0, 0 }, 138 { -2, 0, 0, 1 }, 139 { -2, 0, 0, 1 }, 140 { -2, 0, 0, 1 } 141 }; 142 143 u_int 144 FLS(uint8_t x) 145 { 146 u_int numset = 0; 147 while (x) { 148 if (x & 1) 149 break; 150 x >>= 1; 151 numset++; 152 } 153 return numset; 154 } 155 156 uint8_t 157 calc_scalefactors_joint(int32_t sb_sample[16][2][8]) 158 { 159 int64_t sb_j[16][2]; 160 uint32_t lz, x, y; 161 int32_t ax; 162 int block, sb; 163 unsigned int joint; 164 165 joint = 0; 166 for (sb = 0; sb < global_bands - 1; sb++) { 167 for (block = 0; block < global_blocks; block++) { 168 sb_j[block][0] = (sb_sample[block][0][sb]) + 169 (sb_sample[block][1][sb]); 170 sb_j[block][1] = (sb_sample[block][0][sb]) - 171 (sb_sample[block][1][sb]); 172 } 173 174 x = 1 << 15; 175 y = 1 << 15; 176 for (block = 0; block < global_blocks; block++) { 177 ax = abs((int32_t)(sb_j[block][0] / 2)); 178 if (ax) 179 x |= (uint32_t)ax; 180 ax = abs((int32_t)(sb_j[block][1] / 2)); 181 if (ax) 182 y |= (uint32_t)ax; 183 } 184 185 lz = 1; 186 while (!(x & __BIT(30))) { 187 lz++; 188 x <<= 1; 189 } 190 x = 16 - lz; 191 192 lz = 1; 193 while (!(y & __BIT(30))) { 194 lz++; 195 y <<= 1; 196 } 197 y = 16 - lz; 198 199 if ((scalefactor[0][sb] + scalefactor[1][sb]) > x + y) { 200 joint |= (unsigned int)(1 << (global_bands - sb - 1)); 201 scalefactor[0][sb] = x; 202 scalefactor[1][sb] = y; 203 for (block = 0; block < global_blocks; block++) { 204 sb_sample[block][0][sb] = (int32_t) 205 (sb_j[block][0] / 2); 206 sb_sample[block][1][sb] = (int32_t) 207 (sb_j[block][1] / 2); 208 } 209 } 210 } 211 212 return (uint8_t)joint; 213 } 214 215 void 216 calc_scalefactors(int32_t samples[16][2][8]) 217 { 218 uint32_t lz, x; 219 int32_t ax; 220 int ch, sb, block; 221 222 for (ch = 0; ch < global_chan; ch++) { 223 for (sb = 0; sb < global_bands; sb++) { 224 x = 1 << 16; 225 for (block = 0; block < global_blocks; block++) { 226 ax = abs((int32_t)samples[block][ch][sb]); 227 if (ax) 228 x |= (uint32_t)ax; 229 } 230 231 lz = 1; 232 while (!(x & __BIT(30))) { 233 lz++; 234 x <<= 1; 235 } 236 scalefactor[ch][sb] = 16 - lz; 237 } 238 } 239 } 240 241 void 242 calc_bitneed(void) 243 { 244 int32_t bitneed[2][8]; 245 int32_t max_bitneed, bitcount; 246 int32_t slicecount, bitslice; 247 int32_t loudness; 248 int ch, sb,start_chan = 0; 249 250 if (global_mode == MODE_DUAL) 251 global_chan = 1; 252 next_chan: 253 max_bitneed=0; 254 bitcount=0; 255 slicecount=0; 256 257 if (global_alloc == ALLOC_SNR) { 258 for (ch = start_chan; ch < global_chan; ch++) { 259 for (sb = 0; sb < global_bands; sb++) { 260 bitneed[ch][sb] = (int32_t)scalefactor[ch][sb]; 261 262 if (bitneed[ch][sb] > max_bitneed) 263 max_bitneed = bitneed[ch][sb]; 264 } 265 } 266 } else { 267 for (ch = start_chan; ch < global_chan; ch++) { 268 for (sb = 0; sb < global_bands; sb++) { 269 if (scalefactor[ch][sb] == 0) 270 bitneed[ch][sb] = -5; 271 else { 272 if (global_bands == 8) { 273 loudness = (int32_t) 274 ((int)scalefactor[ch][sb] 275 - loudnessoffset8 276 [3 - FLS(global_freq)][sb]); 277 } else { 278 loudness = (int32_t) 279 ((int)scalefactor[ch][sb] 280 - loudnessoffset4 281 [3 - FLS(global_freq)][sb]); 282 } 283 if (loudness > 0) 284 bitneed[ch][sb] = loudness / 2; 285 else 286 bitneed[ch][sb] = loudness; 287 } 288 if (bitneed[ch][sb] > max_bitneed) 289 max_bitneed = bitneed[ch][sb]; 290 } 291 } 292 } 293 294 slicecount = bitcount = 0; 295 bitslice = max_bitneed+1; 296 do { 297 bitslice--; 298 bitcount += slicecount; 299 slicecount = 0; 300 for (ch = start_chan; ch < global_chan; ch++) { 301 for (sb = 0; sb < global_bands; sb++) { 302 if((bitneed[ch][sb] > bitslice + 1)&& 303 (bitneed[ch][sb] < bitslice + 16)) 304 slicecount++; 305 else if(bitneed[ch][sb] == bitslice + 1) 306 slicecount += 2; 307 } 308 } 309 } while (bitcount + slicecount < global_bitpool); 310 if (bitcount + slicecount == global_bitpool) { 311 bitcount += slicecount; 312 bitslice--; 313 } 314 315 for (ch = start_chan; ch < global_chan; ch++) { 316 for (sb = 0; sb < global_bands; sb++) { 317 if (bitneed[ch][sb] < bitslice + 2) 318 bits[ch][sb] = 0; 319 else { 320 bits[ch][sb] = bitneed[ch][sb] - bitslice; 321 if (bits[ch][sb] > 16) 322 bits[ch][sb] = 16; 323 } 324 } 325 } 326 327 if (global_mode == MODE_DUAL) 328 ch = start_chan; 329 else 330 ch = 0; 331 sb = 0; 332 while (bitcount < global_bitpool && sb < global_bands) { 333 if ((bits[ch][sb] >= 2) && (bits[ch][sb] < 16)) { 334 bits[ch][sb]++; 335 bitcount++; 336 } else if ((bitneed[ch][sb] == bitslice + 1) && 337 (global_bitpool > bitcount + 1)) { 338 bits[ch][sb] = 2; 339 bitcount += 2; 340 } 341 if (global_chan == 1 || start_chan == 1) 342 sb++; 343 else if (ch == 1) { 344 ch = 0; 345 sb++; 346 } else 347 ch = 1; 348 } 349 350 if (global_mode == MODE_DUAL) 351 ch = start_chan; 352 else 353 ch = 0; 354 sb = 0; 355 while (bitcount < global_bitpool && sb < global_bands) { 356 if (bits[ch][sb] < 16) { 357 bits[ch][sb]++; 358 bitcount++; 359 } 360 if (global_chan == 1 || start_chan == 1) 361 sb++; 362 else if (ch == 1) { 363 ch = 0; 364 sb++; 365 } else 366 ch = 1; 367 } 368 369 if (global_mode == MODE_DUAL && start_chan == 0) { 370 start_chan = 1; 371 global_chan = 2; 372 goto next_chan; 373 } 374 } 375 376 ssize_t 377 get_bits(uint8_t *data, int numbits, uint32_t *sample) 378 { 379 static uint64_t cache = 0; 380 static int cache_pos = 0; 381 uint64_t tmp_cache; 382 ssize_t written = 0; 383 384 while (cache_pos < numbits) { 385 cache <<= 8; 386 cache |= *data & 0xff; 387 data++; 388 written++; 389 cache_pos += 8; 390 } 391 392 if (numbits == 0) { 393 if (cache_pos >= 8) { 394 cache_pos -= 8; 395 tmp_cache = cache >> cache_pos; 396 *sample = (uint32_t)tmp_cache; 397 written--; 398 } 399 if (cache_pos) { 400 *sample = (uint32_t)cache; 401 written--; 402 } 403 cache = 0; 404 cache_pos = 0; 405 } else { 406 cache_pos -= numbits; 407 tmp_cache = cache & __BITS((uintmax_t)(numbits + cache_pos), 408 (uintmax_t)cache_pos); 409 cache &= ~tmp_cache; 410 tmp_cache >>= cache_pos; 411 *sample = (uint32_t)tmp_cache; 412 } 413 return written; 414 } 415 416 ssize_t 417 move_bits(uint8_t *data, int numbits, uint32_t sample) 418 { 419 static uint64_t cache = 0; 420 static int cache_pos = 0; 421 uint8_t tmp_cache; 422 ssize_t written = 0; 423 424 if (numbits == 0) { 425 while (cache_pos >= 8) { 426 cache_pos -= 8; 427 tmp_cache = (uint8_t)(cache >> cache_pos); 428 *data++ = tmp_cache; 429 written++; 430 } 431 if (cache_pos) 432 *data = (uint8_t)cache; 433 cache = 0; 434 cache_pos = 0; 435 } else { 436 cache_pos += numbits; 437 cache <<= numbits; 438 cache |= sample & __BITS((uintmax_t)numbits, 0); 439 while (cache_pos >= 8) { 440 cache_pos -= 8; 441 tmp_cache = (uint8_t)(cache >> cache_pos); 442 *data++ = tmp_cache; 443 written++; 444 } 445 } 446 return written; 447 } 448 449 ssize_t 450 move_bits_crc(uint8_t *data, int numbits, uint32_t sample) 451 { 452 static uint64_t cache = 0; 453 static int cache_pos = 0; 454 uint8_t tmp_cache; 455 ssize_t written = 0; 456 457 if (numbits > 8 || numbits < 0) 458 return 0; 459 460 if (numbits == 0) { 461 while (cache_pos >= 8) { 462 cache_pos -= 8; 463 tmp_cache = (uint8_t)(cache >> cache_pos); 464 *data++ = tmp_cache; 465 written++; 466 } 467 if (cache_pos) 468 *data = (uint8_t)cache; 469 cache = 0; 470 cache_pos = 0; 471 } else { 472 cache_pos += numbits; 473 cache <<= numbits; 474 cache |= sample & __BITS((uintmax_t)numbits, 0); 475 if (cache_pos >= 8) { 476 cache_pos -= 8; 477 tmp_cache = (uint8_t)(cache >> cache_pos); 478 *data = tmp_cache; 479 written++; 480 } 481 } 482 return written; 483 } 484 485 size_t 486 sbc_encode(int16_t *input, int32_t *samples) 487 { 488 int64_t delta[2][8], levels[2][8], S[80]; 489 static int32_t L[80], R[80]; 490 int32_t *X, Z[80], Y[80]; 491 int32_t output[16][2][8]; 492 int32_t audioout; 493 int16_t left[8], right[8], *data; 494 size_t numsamples; 495 int i, k, block, chan, sb; 496 497 for (block = 0;block < global_blocks; block++) { 498 499 k = 0; 500 for (i = 0;i < global_bands;i++) { 501 left[i] = input[k++]; 502 if (global_chan == 2) 503 right[i] = input[k++]; 504 } 505 input += k; 506 507 for (chan = 0; chan < global_chan; chan++) { 508 if (chan == 0) { 509 X = L; 510 data = left; 511 } else { 512 X = R; 513 data = right; 514 } 515 516 for (i = (global_bands * 10) - 1;i > global_bands -1; 517 i--) 518 X[i] = X[i - global_bands]; 519 k = 0; 520 for (i = global_bands - 1; i >= 0; i--) 521 X[i] = (int16_t)le16toh(data[k++]); 522 for (i = 0; i < global_bands * 10; i++) { 523 if (global_bands == 8) { 524 Z[i] = (sbc_coeffs8[i] * (X[i] << 525 global_volume)); 526 } else { 527 Z[i] = (sbc_coeffs4[i] * (X[i] << 528 global_volume)); 529 } 530 } 531 for (i = 0; i < global_bands * 2; i++) { 532 Y[i] = 0; 533 for (k = 0;k < 5;k++) 534 Y[i] += Z[i + k * global_bands * 2]; 535 } 536 for (i = 0; i < global_bands; i++) { 537 S[i] = 0; 538 for (k = 0; k < global_bands * 2; k++) { 539 if (global_bands == 8) { 540 S[i] += (int64_t)cosdata8[i][k] 541 * (int64_t)Y[k]; 542 } else { 543 S[i] += (int64_t)cosdata4[i][k] 544 * (int64_t)Y[k]; 545 } 546 } 547 output[block][chan][i] = (int32_t)(S[i] / 548 SIMULTI); 549 } 550 } 551 } 552 553 calc_scalefactors(output); 554 if (global_mode == MODE_JOINT) 555 join = calc_scalefactors_joint(output); 556 557 calc_bitneed(); 558 559 for(chan = 0; chan < global_chan; chan++) { 560 for (sb = 0; sb < global_bands; sb++) { 561 levels[chan][sb] = ((1 << bits[chan][sb]) - 1) << 562 (15 - scalefactor[chan][sb]); 563 delta[chan][sb] = 1 << (scalefactor[chan][sb] + 16); 564 } 565 } 566 567 numsamples = 0; 568 for (block = 0; block < global_blocks; block++) { 569 for (chan = 0; chan < global_chan; chan++) { 570 for (sb = 0; sb < global_bands; sb++) { 571 if (bits[chan][sb] == 0) 572 continue; 573 574 audioout = (int32_t)((levels[chan][sb] * 575 (delta[chan][sb] + (int32_t)output[block] 576 [chan][sb])) >> 32); 577 578 samples[numsamples++] = audioout; 579 } 580 } 581 } 582 return numsamples; 583 } 584 585 size_t 586 sbc_decode(int32_t *samples, int16_t *pcm) 587 { 588 static int64_t levels[2][8], delta[2][8]; 589 static int64_t S[8], V[2][160], L[160], R[160]; 590 int64_t *X; 591 static int64_t U[2][160], W[2][160]; 592 int64_t audioout; 593 int chan, block, sb, position, i, k; 594 size_t numsamples; 595 596 for(chan = 0; chan < global_chan; chan++) { 597 for (sb = 0; sb < global_bands; sb++) { 598 levels[chan][sb] = (1 << bits[chan][sb]) - 1; 599 delta[chan][sb] = 1 << (scalefactor[chan][sb] + 1); 600 } 601 } 602 603 numsamples = 0; 604 for (block = 0; block < global_blocks; block++) { 605 for (chan = 0; chan < global_chan; chan++) { 606 for (sb = 0; sb < global_bands; sb++) { 607 if (bits[chan][sb] == 0) 608 audioout = 0; 609 else { 610 audioout = ((((samples[numsamples] 611 * 2) + 1) * delta[chan][sb]) / 612 levels[chan][sb]) - 613 delta[chan][sb]; 614 } 615 samples[numsamples] = (int32_t)audioout; 616 numsamples++; 617 } 618 } 619 } 620 621 if (global_mode == MODE_JOINT) { 622 k = 0; 623 while (k < (global_blocks * global_bands * global_chan)) { 624 for (sb = 0; sb < global_bands; sb++) { 625 if (join & 1 << (global_bands - sb - 1)) { 626 audioout = samples[k]; 627 samples[k] = (2 * samples[k]) + (2 * 628 samples[k + global_bands]); 629 samples[k + global_bands] = 630 (int32_t)(2 * audioout) - (2 * 631 samples[k + global_bands]); 632 samples[k] /= 2; 633 samples[k + global_bands] /= 2; 634 } 635 k++; 636 } 637 k += global_bands; 638 } 639 } 640 641 642 position = 0; 643 for (block = 0; block < global_blocks; block++) { 644 for (chan = 0; chan < global_chan; chan++) { 645 if (chan == 0) 646 X = L; 647 else 648 X = R; 649 650 for (i = 0; i < global_bands; i++) 651 S[i] = samples[position++]; 652 for (i = ((global_bands * 20) - 1); i >= (global_bands 653 * 2); i--) 654 V[chan][i] = V[chan][i - (global_bands * 2)]; 655 656 for (k = 0; k < global_bands * 2; k++) { 657 V[chan][k] = 0; 658 for (i = 0; i < global_bands; i++) { 659 if (global_bands == 8) { 660 V[chan][k] += cosdecdata8[i][k] 661 * S[i]; 662 } else { 663 V[chan][k] += cosdecdata4[i][k] 664 * S[i]; 665 } 666 } 667 V[chan][k] /= SIMULTI; 668 } 669 670 for (i = 0; i <= 4; i++) { 671 for (k = 0; k < global_bands; k++) { 672 U[chan][(i * global_bands * 2) + k] = 673 V[chan][(i * global_bands * 4) + k]; 674 U[chan][(i * global_bands 675 * 2) + global_bands + k] = 676 V[chan][(i * global_bands * 4) + 677 (global_bands * 3) + k]; 678 } 679 } 680 681 for (i = 0; i < global_bands * 10; i++) { 682 if (global_bands == 4) { 683 W[chan][i] = U[chan][i] * 684 (sbc_coeffs4[i] * -4); 685 } else if (global_bands == 8) { 686 W[chan][i] = U[chan][i] * 687 (sbc_coeffs8[i] * -8); 688 } 689 } 690 691 for (k = 0; k < global_bands; k++) { 692 int offset = k + (block * global_bands); 693 X[offset] = 0; 694 for (i = 0; i < 10; i++) { 695 X[offset] += W[chan][k + (i * 696 global_bands)]; 697 } 698 X[offset] /= COEFFSMULTI; 699 } 700 } 701 } 702 703 k = 0; 704 i = 0; 705 while (k < (global_blocks * global_bands)) { 706 pcm[i++] = (int16_t)L[k]; 707 if (global_chan == 2) 708 pcm[i++] = (int16_t)R[k]; 709 k++; 710 } 711 712 return numsamples; 713 } 714 715 uint8_t 716 Crc8(uint8_t inCrc, uint8_t *inData, size_t numbits, ssize_t inBytes) 717 { 718 uint8_t data; 719 int i; 720 721 for (i = 0; i < (int)inBytes; i++) { 722 data = inCrc ^ inData[i]; 723 724 if (numbits == 8) 725 data = sbc_crc8[data]; 726 else if (numbits == 4) 727 data = sbc_crc4[data]; 728 729 inCrc = data; 730 } 731 return inCrc; 732 } 733 734 uint8_t 735 make_crc(uint8_t config) 736 { 737 uint8_t crc, data[11]; 738 int i, j; 739 uint8_t *dataStart = data; 740 uint8_t *crcData = data; 741 742 743 crcData += move_bits_crc(crcData, 8, config); 744 crcData += move_bits_crc(crcData, 8, global_bitpool); 745 if (global_mode == MODE_JOINT) { 746 if (global_bands == 8) 747 crcData += move_bits_crc(crcData, 8, join); 748 else 749 crcData += move_bits_crc(crcData, 4, join); 750 } 751 752 for(i = 0; i < global_chan; i++) { 753 for (j = 0; j < global_bands; j++) 754 crcData += move_bits_crc(crcData, 4, scalefactor[i][j]); 755 } 756 757 crc = Crc8(0xf, data, 8, (crcData - dataStart)); 758 759 if (global_mode == MODE_JOINT && global_bands == 4) { 760 move_bits_crc(crcData, 0, 0); 761 crc = Crc8(crc, crcData, 4, 1); 762 } 763 764 return crc; 765 } 766 767 ssize_t 768 make_frame(uint8_t *frame, int16_t *input) 769 { 770 static int32_t samples[256 * 2]; 771 uint8_t config, crc; 772 int block, chan, sb, j, i; 773 774 uint8_t *frameStart = frame; 775 776 config = (uint8_t)(((3 - FLS(global_freq)) << 6) | 777 ((3 - FLS(global_block_config)) << 4) | ((3 - FLS(global_mode)) 778 << 2) | ((FLS(global_alloc)) << 1) | 779 (1 - FLS(global_bands_config))); 780 781 sbc_encode(input,samples); 782 783 crc = make_crc(config); 784 785 frame += move_bits(frame, 8, SYNCWORD); 786 frame += move_bits(frame, 8, config); 787 frame += move_bits(frame, 8, global_bitpool); 788 frame += move_bits(frame, 8, crc); 789 790 if (global_mode == MODE_JOINT && global_bands == 8) 791 frame += move_bits(frame, 8, join); 792 else if (global_mode == MODE_JOINT && global_bands == 4) 793 frame += move_bits(frame, 4, join); 794 795 for(i = 0; i < global_chan; i++) { 796 for (j = 0; j < global_bands; j++) 797 frame += move_bits(frame, 4, scalefactor[i][j]); 798 } 799 800 i = 0; 801 for (block = 0; block < global_blocks; block++) { 802 for (chan = 0; chan < global_chan; chan++) { 803 for (sb = 0; sb < global_bands; sb++) { 804 if (bits[chan][sb] == 0) 805 continue; 806 807 frame += move_bits(frame, bits[chan][sb], 808 (uint32_t)samples[i++]); 809 } 810 } 811 } 812 frame += move_bits(frame, 0, 0); 813 814 return frame - frameStart; 815 } 816 817 static ssize_t 818 readloop(int fd, void *buf, size_t nbytes) 819 { 820 size_t count; 821 ssize_t ret; 822 823 count = 0; 824 while (nbytes > 0) { 825 ret = read(fd, ((char *)buf) + count, nbytes); 826 if (ret < 0) { 827 if (count == 0) 828 return ret; 829 break; 830 } 831 if (ret == 0) 832 break; 833 count += (size_t)ret; 834 nbytes -= (size_t)ret; 835 } 836 837 return (ssize_t) count; 838 } 839 840 ssize_t 841 stream(int in, int outfd, uint8_t mode, uint8_t freq, uint8_t bands, uint8_t 842 blocks, uint8_t alloc_method, uint8_t bitpool, size_t mtu, int volume) 843 { 844 struct rtpHeader myHeader; 845 struct timeval myTime; 846 uint8_t *whole, *frameData; 847 int16_t music[2048]; 848 ssize_t len, mySize[16], offset, next_pkt; 849 ssize_t pkt_len; 850 size_t readsize, totalSize; 851 size_t frequency; 852 static size_t ts = 0; 853 static uint16_t seqnumber = 0; 854 static time_t prevTime, readTime, sleepTime, timeNow; 855 int numpkts, tries; 856 857 global_mode = mode; 858 global_bitpool = bitpool; 859 global_alloc = alloc_method; 860 global_freq = freq; 861 global_volume = volume; 862 863 global_bands_config = bands; 864 if (bands == BANDS_8) 865 global_bands = 8; 866 else 867 global_bands = 4; 868 869 if (blocks == BLOCKS_4) 870 global_blocks = 4; 871 else if (blocks == BLOCKS_8) 872 global_blocks = 8; 873 else if (blocks == BLOCKS_12) 874 global_blocks = 12; 875 else { 876 blocks = BLOCKS_16; 877 global_blocks = 16; 878 } 879 880 global_block_config = blocks; 881 882 global_chan = 2; 883 if (global_mode == MODE_MONO) 884 global_chan = 1; 885 886 if (global_freq == FREQ_16K) 887 frequency = 16000; 888 else if (global_freq == FREQ_32K) 889 frequency = 32000; 890 else if (global_freq == FREQ_48K) 891 frequency = 48000; 892 else 893 frequency = 44100; 894 895 memset(&myHeader, 0, sizeof(myHeader)); 896 myHeader.id = 0x80; /* RTP v2 */ 897 myHeader.id2 = 0x60; /* payload type 96. */ 898 myHeader.seqnumMSB = (uint8_t)(seqnumber >> 8); 899 myHeader.seqnumLSB = (uint8_t)seqnumber; 900 myHeader.ts3 = (uint8_t)(ts >> 24); 901 myHeader.ts2 = (uint8_t)(ts >> 16); 902 myHeader.ts1 = (uint8_t)(ts >> 8); 903 myHeader.ts0 = (uint8_t)ts; 904 myHeader.reserved0 = 0x01; 905 906 totalSize = sizeof(myHeader); 907 908 frameData = malloc(mtu); 909 if (frameData == NULL) 910 return -1; 911 912 readsize = (size_t)((global_blocks * global_bands * global_chan) * 2); 913 914 numpkts = 0; 915 next_pkt = 0; 916 len = 0; 917 pkt_len = 80; 918 readTime = 0; 919 while (totalSize + ((size_t)pkt_len * 2) <= mtu) { 920 921 len = readloop(in, music, readsize); 922 readTime += (time_t)readsize; 923 if (len < (int)readsize) 924 break; 925 926 pkt_len = make_frame(frameData + next_pkt, music); 927 928 mySize[numpkts] = pkt_len; 929 next_pkt += pkt_len; 930 totalSize += (size_t)mySize[numpkts]; 931 numpkts++; 932 933 if (numpkts > 12) 934 break; 935 } 936 937 if (len < (int)readsize) { 938 free(frameData); 939 return -1; 940 } 941 942 readTime = readTime * 1000000 / 2 / global_chan / (time_t)frequency; 943 944 myHeader.numFrames = (uint8_t)numpkts; 945 whole = malloc(totalSize); 946 if (whole == NULL) 947 return -1; 948 949 memcpy(whole, &myHeader, sizeof(myHeader)); 950 offset = sizeof(myHeader); 951 952 memcpy(whole + offset, frameData, (size_t)next_pkt); 953 free(frameData); 954 955 /* Wait if necessary to avoid rapid playback. */ 956 gettimeofday(&myTime, NULL); 957 timeNow = myTime.tv_sec * 1000000 + myTime.tv_usec; 958 if (prevTime == 0) 959 prevTime = timeNow; 960 else 961 sleepTime += readTime - (timeNow - prevTime); 962 if (sleepTime >= 1000) { 963 usleep(500); 964 sleepTime -= 1000; 965 } 966 prevTime = timeNow; 967 968 tries = 1; 969 send_again: 970 len = write(outfd, whole, totalSize); 971 972 if (len == -1 && errno == EAGAIN) { 973 tries --; 974 if (tries >= 0) { 975 usleep(1); 976 goto send_again; 977 } else 978 len = (ssize_t)totalSize; 979 } else if (len == -1 && (errno == EINPROGRESS || 980 errno == EWOULDBLOCK)) { 981 usleep(1); 982 len = (ssize_t)totalSize; 983 } 984 985 seqnumber++; 986 ts += (1000000 * (size_t)(global_blocks * global_bands) 987 / frequency) * (size_t)numpkts; 988 989 free(whole); 990 991 return len; 992 } 993 994 ssize_t 995 recvstream(int in, int outfd) 996 { 997 struct rtpHeader myHeader; 998 static uint8_t frameData[8192]; 999 uint8_t *myFrame; 1000 static int16_t music[4096]; 1001 size_t decsize, totalSize, offset; 1002 int numpkts, cur_pkt; 1003 ssize_t len, res, pkt_len, next_pkt; 1004 static ssize_t readlen = 0; 1005 totalSize = 0; 1006 1007 len = read(in, frameData + readlen, (size_t)(1000 - readlen)); 1008 readlen += len; 1009 if (readlen <= 0) { 1010 readlen = 0; 1011 return -1; 1012 } 1013 1014 if (readlen < (int)sizeof(myHeader)) { 1015 readlen = 0; 1016 return -1; 1017 } 1018 1019 memcpy(&myHeader, frameData, sizeof(myHeader)); 1020 if (myHeader.id != 0x80) { 1021 return -1; 1022 } 1023 1024 numpkts = myHeader.numFrames; 1025 if (numpkts < 1 || numpkts > 13) { 1026 return -1; 1027 } 1028 1029 myFrame = frameData + sizeof(myHeader); 1030 next_pkt = 0; 1031 pkt_len = 0; 1032 cur_pkt = 0; 1033 offset = 0; 1034 while (cur_pkt < numpkts) { 1035 pkt_len = parseFrame(myFrame + next_pkt, &music[offset]); 1036 decsize = (size_t)(global_blocks * global_bands * global_chan); 1037 1038 next_pkt += pkt_len; 1039 totalSize += 2 * decsize; 1040 offset += decsize; 1041 cur_pkt++; 1042 } 1043 1044 res = (ssize_t)(sizeof(myHeader)) + next_pkt; 1045 readlen -= res; 1046 if (readlen > 0) 1047 memcpy(frameData, frameData + res, (size_t)readlen); 1048 1049 send_again: 1050 len = write(outfd, music, totalSize); 1051 1052 if (len == -1 && errno == EAGAIN) 1053 goto send_again; 1054 1055 return len; 1056 } 1057 1058 ssize_t 1059 parseFrame(uint8_t *myFrame, int16_t *pcm) 1060 { 1061 uint8_t mode, bitpool, alloc_method, freq, bands, config, myCrc, blkCrc; 1062 int sb, chan, block, blocks, i, j; 1063 int32_t samples[768]; 1064 1065 uint8_t *myFrame_start = myFrame; 1066 if (*myFrame++ != SYNCWORD) 1067 return -1; 1068 1069 config = *myFrame++; 1070 bitpool = *myFrame++; 1071 myCrc = *myFrame++; 1072 1073 freq = (uint8_t)(1 << (3 - ((config & 0xc0) >> 6))); 1074 blocks = 1 << (3 - ((config & 0x30) >> 4)); 1075 mode = (uint8_t)(1 << (3 - ((config & 0x0c) >> 2))); 1076 alloc_method = (uint8_t)(1 << (((config & 0x02) >> 1))); 1077 bands = (uint8_t)(1 << (1 - (config & 0x01))); 1078 1079 global_mode = mode; 1080 global_bitpool = bitpool; 1081 global_alloc = alloc_method; 1082 global_freq = freq; 1083 1084 global_bands_config = bands; 1085 if (bands == BANDS_8) 1086 global_bands = 8; 1087 else 1088 global_bands = 4; 1089 1090 if (blocks == BLOCKS_4) 1091 global_blocks = 4; 1092 else if (blocks == BLOCKS_8) 1093 global_blocks = 8; 1094 else if (blocks == BLOCKS_12) 1095 global_blocks = 12; 1096 else { 1097 blocks = BLOCKS_16; 1098 global_blocks = 16; 1099 } 1100 1101 global_block_config = (uint8_t)blocks; 1102 1103 global_chan = 2; 1104 if (global_mode == MODE_MONO) 1105 global_chan = 1; 1106 1107 if (global_mode == MODE_JOINT && global_bands == 8) 1108 myFrame += get_bits(myFrame, 8, (uint32_t *)&join); 1109 else if (global_mode == MODE_JOINT && global_bands == 4) 1110 myFrame += get_bits(myFrame, 4, (uint32_t *)&join); 1111 else 1112 join = 0; 1113 1114 for(i = 0; i < global_chan; i++) { 1115 for (j = 0; j < global_bands; j++) { 1116 myFrame += get_bits(myFrame, 4, 1117 (uint32_t *)&scalefactor[i][j]); 1118 } 1119 } 1120 1121 blkCrc = make_crc(config); 1122 if (blkCrc != myCrc) 1123 return 0; 1124 1125 calc_bitneed(); 1126 i = 0; 1127 for (block = 0; block < global_blocks; block++) { 1128 for (chan = 0; chan < global_chan; chan++) { 1129 for (sb = 0; sb < global_bands; sb++) { 1130 samples[i] = 0; 1131 if (bits[chan][sb] == 0) { 1132 i++; 1133 continue; 1134 } 1135 1136 myFrame += get_bits(myFrame, bits[chan][sb], 1137 (uint32_t *)&samples[i++]); 1138 } 1139 } 1140 } 1141 myFrame += get_bits(myFrame, 0, (uint32_t *)&samples[i]); 1142 sbc_decode(samples, pcm); 1143 1144 return myFrame - myFrame_start; 1145 } 1146
Indexes created Sat Sep 20 22:09:52 GMT 2025