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@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
FLS(uint8_t x)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
calc_scalefactors_joint(int32_t sb_sample[16][2][8])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
calc_scalefactors(int32_t samples[16][2][8])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
calc_bitneed(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
get_bits(uint8_t * data,int numbits,uint32_t * sample)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
move_bits(uint8_t * data,int numbits,uint32_t sample)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
move_bits_crc(uint8_t * data,int numbits,uint32_t sample)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
sbc_encode(int16_t * input,int32_t * samples)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
sbc_decode(int32_t * samples,int16_t * pcm)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
Crc8(uint8_t inCrc,uint8_t * inData,size_t numbits,ssize_t inBytes)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
make_crc(uint8_t config)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
make_frame(uint8_t * frame,int16_t * input)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
readloop(int fd,void * buf,size_t nbytes)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
stream(int in,int outfd,uint8_t mode,uint8_t freq,uint8_t bands,uint8_t blocks,uint8_t alloc_method,uint8_t bitpool,size_t mtu,int volume)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
recvstream(int in,int outfd)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
parseFrame(uint8_t * myFrame,int16_t * pcm)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