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