1 /*
2 * Copyright (C) 2023 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * 3. Neither the name of the copyright holders nor the names of
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 * 4. Any redistribution, use, or modification is done solely for
17 * personal benefit and not for any commercial purpose or for
18 * monetary gain.
19 *
20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 * SUCH DAMAGE.
32 *
33 * Please inquire about commercial licensing options at
34 * [email protected]
35 *
36 */
37
38 #define BTSTACK_FILE__ "btstack_sbc_bluedroid.c"
39
40 #include "btstack_sbc_bluedroid.h"
41
42 #include "btstack_bool.h"
43 #include "btstack_config.h"
44 #include "btstack_debug.h"
45 #include "btstack_util.h"
46
47 #include <stdint.h>
48 #include <string.h>
49 #include "bluetooth.h"
50
51 // ENCODER
52
btstack_sbc_encoder_bluedroid_configure(void * context,btstack_sbc_mode_t mode,uint8_t blocks,uint8_t subbands,btstack_sbc_allocation_method_t allocation_method,uint16_t sample_rate,uint8_t bitpool,btstack_sbc_channel_mode_t channel_mode)53 static uint8_t btstack_sbc_encoder_bluedroid_configure(void * context, btstack_sbc_mode_t mode,
54 uint8_t blocks, uint8_t subbands, btstack_sbc_allocation_method_t allocation_method,
55 uint16_t sample_rate, uint8_t bitpool, btstack_sbc_channel_mode_t channel_mode){
56
57 btstack_sbc_encoder_bluedroid_t * instance = (btstack_sbc_encoder_bluedroid_t *) context;
58
59 instance->mode = mode;
60
61 switch (instance->mode){
62 case SBC_MODE_STANDARD:
63 instance->params.s16NumOfBlocks = blocks;
64 instance->params.s16NumOfSubBands = subbands;
65 instance->params.s16AllocationMethod = (uint8_t)allocation_method;
66 instance->params.s16BitPool = bitpool;
67 instance->params.mSBCEnabled = 0;
68 instance->params.s16ChannelMode = (uint8_t)channel_mode;
69 instance->params.s16NumOfChannels = 2;
70 if (instance->params.s16ChannelMode == SBC_MONO){
71 instance->params.s16NumOfChannels = 1;
72 }
73 switch(sample_rate){
74 case 16000: instance->params.s16SamplingFreq = SBC_sf16000; break;
75 case 32000: instance->params.s16SamplingFreq = SBC_sf32000; break;
76 case 44100: instance->params.s16SamplingFreq = SBC_sf44100; break;
77 case 48000: instance->params.s16SamplingFreq = SBC_sf48000; break;
78 default: instance->params.s16SamplingFreq = 0; break;
79 }
80 break;
81 case SBC_MODE_mSBC:
82 instance->params.s16NumOfBlocks = 15;
83 instance->params.s16NumOfSubBands = 8;
84 instance->params.s16AllocationMethod = SBC_LOUDNESS;
85 instance->params.s16BitPool = 26;
86 instance->params.s16ChannelMode = SBC_MONO;
87 instance->params.s16NumOfChannels = 1;
88 instance->params.mSBCEnabled = 1;
89 instance->params.s16SamplingFreq = SBC_sf16000;
90 break;
91 default:
92 return ERROR_CODE_INVALID_HCI_COMMAND_PARAMETERS;
93 }
94
95 SBC_Encoder_Init(&instance->params);
96
97 return ERROR_CODE_SUCCESS;
98 }
99
100 /**
101 * @brief Return number of audio frames required for one SBC packet
102 * @param context
103 * @note each audio frame contains 2 sample values in stereo modes
104 */
btstack_sbc_encoder_bluedroid_num_audio_frames(void * context)105 static uint16_t btstack_sbc_encoder_bluedroid_num_audio_frames(void * context){
106 btstack_sbc_encoder_bluedroid_t * instance = (btstack_sbc_encoder_bluedroid_t *) context;
107 return instance->params.s16NumOfSubBands * instance->params.s16NumOfBlocks;
108 }
109
btstack_sbc_encoder_bluedroid_sbc_buffer_length(void * context)110 static uint16_t btstack_sbc_encoder_bluedroid_sbc_buffer_length(void * context){
111 btstack_sbc_encoder_bluedroid_t * instance = (btstack_sbc_encoder_bluedroid_t *) context;
112 return instance->params.u16PacketLength;
113 }
114
115 /**
116 * @brief Encode PCM data
117 * @param context
118 * @param pcm_in with samples in host endianess
119 * @param sbc_out
120 * @return status
121 */
btstack_sbc_encoder_bluedroid_encode_signed_16(void * context,const int16_t * pcm_in,uint8_t * sbc_out)122 static uint8_t btstack_sbc_encoder_bluedroid_encode_signed_16(void * context, const int16_t* pcm_in, uint8_t * sbc_out){
123 btstack_sbc_encoder_bluedroid_t * instance = (btstack_sbc_encoder_bluedroid_t *) context;
124
125 instance->params.ps16PcmBuffer = (int16_t *) pcm_in;
126 instance->params.pu8Packet = sbc_out;
127 if (instance->params.mSBCEnabled){
128 instance->params.pu8Packet[0] = 0xad;
129 }
130 SBC_Encoder(&instance->params);
131 return ERROR_CODE_SUCCESS;
132 }
133
134 static const btstack_sbc_encoder_t btstack_sbc_encoder_bluedroid = {
135 .configure = btstack_sbc_encoder_bluedroid_configure,
136 .sbc_buffer_length = btstack_sbc_encoder_bluedroid_sbc_buffer_length,
137 .num_audio_frames = btstack_sbc_encoder_bluedroid_num_audio_frames,
138 .encode_signed_16 = btstack_sbc_encoder_bluedroid_encode_signed_16
139 };
140
btstack_sbc_encoder_bluedroid_init_instance(btstack_sbc_encoder_bluedroid_t * context)141 const btstack_sbc_encoder_t * btstack_sbc_encoder_bluedroid_init_instance(btstack_sbc_encoder_bluedroid_t * context){
142 memset(context, 0, sizeof(btstack_sbc_encoder_bluedroid_t));
143 return &btstack_sbc_encoder_bluedroid;
144 }
145
146 // DECODER
147
148 #include "btstack_config.h"
149
150 #include <stdint.h>
151
152 #include <stdlib.h>
153 #include <string.h>
154 #include "btstack_sbc_plc.h"
155
156 #include "oi_codec_sbc.h"
157 #include "oi_assert.h"
158
159 #ifdef LOG_FRAME_STATUS
160 #include <stdio.h>
161 #endif
162
163 #define mSBC_SYNCWORD 0xad
164 #define SBC_SYNCWORD 0x9c
165 #define SBC_MAX_CHANNELS 2
166 // #define LOG_FRAME_STATUS
167
168 // Testing only - START
169 static int plc_enabled = 1;
170 static int corrupt_frame_period = -1;
171 // Testing - STOP
172
btstack_sbc_decoder_bluedroid_test_set_plc_enabled(int enabled)173 void btstack_sbc_decoder_bluedroid_test_set_plc_enabled(int enabled){
174 plc_enabled = enabled;
175 }
176
btstack_sbc_decoder_bluedroid_test_simulate_corrupt_frames(int period)177 void btstack_sbc_decoder_bluedroid_test_simulate_corrupt_frames(int period){
178 corrupt_frame_period = period;
179 }
180
find_sequence_of_zeros(const OI_BYTE * frame_data,OI_UINT32 frame_bytes,int seq_length)181 static int find_sequence_of_zeros(const OI_BYTE *frame_data, OI_UINT32 frame_bytes, int seq_length){
182 int zero_seq_count = 0;
183 unsigned int i;
184 for (i=0; i<frame_bytes; i++){
185 if (frame_data[i] == 0) {
186 zero_seq_count++;
187 if (zero_seq_count >= seq_length) return zero_seq_count;
188 } else {
189 zero_seq_count = 0;
190 }
191 }
192 return 0;
193 }
194
195 // returns position of mSBC sync word
find_h2_sync(const OI_BYTE * frame_data,OI_UINT32 frame_bytes,int * sync_word_nr)196 static int find_h2_sync(const OI_BYTE *frame_data, OI_UINT32 frame_bytes, int * sync_word_nr){
197 int syncword = mSBC_SYNCWORD;
198 uint8_t h2_first_byte = 0;
199 uint8_t h2_second_byte = 0;
200
201 unsigned int i;
202 for (i=0; i<frame_bytes; i++){
203 if (frame_data[i] == syncword) {
204 // check: first byte == 1
205 if (h2_first_byte == 1) {
206 // check lower nibble of second byte == 0x08
207 uint8_t ln = h2_second_byte & 0x0F;
208 if (ln == 8) {
209 // check if bits 0+2 == bits 1+3
210 uint8_t hn = h2_second_byte >> 4;
211 if ( ((hn>>1) & 0x05) == (hn & 0x05) ) {
212 *sync_word_nr = ((hn & 0x04) >> 1) | (hn & 0x01);
213 return i;
214 }
215 }
216 }
217 }
218 h2_first_byte = h2_second_byte;
219 h2_second_byte = frame_data[i];
220 }
221 return -1;
222 }
223
224 #ifdef OI_DEBUG
OI_AssertFail(const char * file,int line,const char * reason)225 void OI_AssertFail(const char* file, int line, const char* reason){
226 log_error("AssertFail file %s, line %d, reason %s", file, line, reason);
227 }
228 #endif
229
append_received_sbc_data(btstack_sbc_decoder_bluedroid_t * state,const uint8_t * buffer,int size)230 static void append_received_sbc_data(btstack_sbc_decoder_bluedroid_t * state, const uint8_t * buffer, int size){
231 int numFreeBytes = sizeof(state->frame_buffer) - state->bytes_in_frame_buffer;
232
233 if (size > numFreeBytes){
234 log_error("SBC data: more bytes read %u than free bytes in buffer %u", size, numFreeBytes);
235 }
236
237 (void)memcpy(state->frame_buffer + state->bytes_in_frame_buffer, buffer,
238 size);
239 state->bytes_in_frame_buffer += size;
240 }
241
btstack_sbc_decoder_bluedroid_simulate_error(const OI_BYTE * frame_data)242 static void btstack_sbc_decoder_bluedroid_simulate_error(const OI_BYTE *frame_data) {
243 static int frame_count = 0;
244 if (corrupt_frame_period > 0){
245 frame_count++;
246
247 if ((frame_count % corrupt_frame_period) == 0){
248 *(uint8_t*)&frame_data[5] = 0;
249 frame_count = 0;
250 }
251 }
252 }
253
btstack_sbc_decoder_deliver_pcm_data(btstack_sbc_decoder_bluedroid_t * state)254 static void btstack_sbc_decoder_deliver_pcm_data(btstack_sbc_decoder_bluedroid_t *state) {
255 // deliver PCM data
256 uint16_t num_samples_per_frame = state->decoder_context.common.frameInfo.nrof_blocks *
257 state->decoder_context.common.frameInfo.nrof_subbands;
258 uint8_t num_channels = state->decoder_context.common.frameInfo.nrof_channels;
259 uint16_t sample_rate = state->decoder_context.common.frameInfo.frequency;
260 state->handle_pcm_data(state->pcm_data, num_samples_per_frame, num_channels, sample_rate, state->callback_context);
261 }
262
btstack_sbc_decoder_bluedroid_process_sbc_data(btstack_sbc_decoder_bluedroid_t * state,const uint8_t * buffer,int size)263 static void btstack_sbc_decoder_bluedroid_process_sbc_data(btstack_sbc_decoder_bluedroid_t * state, const uint8_t * buffer, int size){
264 int input_bytes_to_process = size;
265 int keep_decoding = 1;
266
267 while (keep_decoding) {
268 // Fill decoder_state->frame_buffer as much as possible.
269 int bytes_free_in_frame_buffer = SBC_MAX_FRAME_LEN - state->bytes_in_frame_buffer;
270 int bytes_to_append = btstack_min(input_bytes_to_process, bytes_free_in_frame_buffer);
271 if (bytes_to_append){
272 append_received_sbc_data(state, buffer, bytes_to_append);
273 buffer += bytes_to_append;
274 input_bytes_to_process -= bytes_to_append;
275 }
276
277 // Decode the next frame in decoder_state->frame_buffer.
278 int bytes_in_frame_buffer_before_decoding = state->bytes_in_frame_buffer;
279 const OI_BYTE *frame_data = state->frame_buffer;
280 OI_UINT32 frame_data_len = state->bytes_in_frame_buffer;
281 OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(state->decoder_context),
282 &frame_data,
283 &frame_data_len,
284 state->pcm_data,
285 &(state->pcm_bytes));
286 uint16_t bytes_processed = bytes_in_frame_buffer_before_decoding - frame_data_len;
287
288 // testing only - corrupt frame periodically
289 btstack_sbc_decoder_bluedroid_simulate_error(frame_data);
290
291 // Handle decoding result.
292 switch(status){
293 case OI_STATUS_SUCCESS:
294 case OI_CODEC_SBC_PARTIAL_DECODE:
295 // deliver PCM data
296 btstack_sbc_decoder_deliver_pcm_data(state);
297
298 state->good_frames_nr++;
299 break;
300
301 case OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA:
302 case OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA:
303 case OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA:
304 if (input_bytes_to_process > 0){
305 // Should never occur: The SBC codec claims there is not enough bytes in the frame_buffer,
306 // but the frame_buffer was full. (The frame_buffer is always full before decoding when input_bytes_to_process > 0.)
307 // Clear frame_buffer.
308 log_info("SBC decode: frame_buffer too small for frame");
309 bytes_processed = bytes_in_frame_buffer_before_decoding;
310 } else {
311 // Exit decode loop, because there is not enough data in frame_buffer to decode the next frame.
312 keep_decoding = 0;
313 }
314 break;
315
316 case OI_CODEC_SBC_NO_SYNCWORD:
317 // This means the entire frame_buffer did not contain the syncword.
318 // Discard the frame_buffer contents.
319 log_info("SBC decode: no syncword found");
320 bytes_processed = bytes_in_frame_buffer_before_decoding;
321 break;
322
323 case OI_CODEC_SBC_CHECKSUM_MISMATCH:
324 // The next frame is somehow corrupt.
325 log_info("SBC decode: checksum error");
326 // Did the codec consume any bytes?
327 if (bytes_processed > 0){
328 // Good. Nothing to do.
329 } else {
330 // Skip the bogus frame by skipping the header.
331 bytes_processed = 1;
332 }
333 break;
334
335 case OI_STATUS_INVALID_PARAMETERS:
336 // This caused by corrupt frames.
337 // The codec apparently does not recover from this.
338 // Re-initialize the codec.
339 log_info("SBC decode: invalid parameters: resetting codec");
340 if (OI_CODEC_SBC_DecoderReset(&(state->decoder_context), state->decoder_data, sizeof(state->decoder_data), 2, 2, FALSE) != OI_STATUS_SUCCESS){
341 log_info("SBC decode: resetting codec failed");
342
343 }
344 break;
345 default:
346 // Anything else went wrong.
347 // Skip a few bytes and try again.
348 bytes_processed = 1;
349 log_info("SBC decode: unknown status %d", status);
350 break;
351 }
352
353 // Remove decoded frame from decoder_state->frame_buffer.
354 if (bytes_processed > bytes_in_frame_buffer_before_decoding) {
355 bytes_processed = bytes_in_frame_buffer_before_decoding;
356 }
357 memmove(state->frame_buffer, state->frame_buffer + bytes_processed, bytes_in_frame_buffer_before_decoding - bytes_processed);
358 state->bytes_in_frame_buffer -= bytes_processed;
359 }
360 }
361
362
btstack_sbc_decoder_insert_missing_frames(btstack_sbc_decoder_bluedroid_t * state)363 static void btstack_sbc_decoder_insert_missing_frames(btstack_sbc_decoder_bluedroid_t *state) {
364 const unsigned int MSBC_FRAME_SIZE = 60;
365
366 while (state->first_good_frame_found && (state->msbc_bad_bytes >= MSBC_FRAME_SIZE)){
367
368 state->msbc_bad_bytes -= MSBC_FRAME_SIZE;
369 state->bad_frames_nr++;
370
371 // prepare zero signal frame
372 const OI_BYTE * frame_data = btstack_sbc_plc_zero_signal_frame();
373 OI_UINT32 bytes_in_frame_buffer = 57;
374
375 // log_info("Trace bad frame generator, bad bytes %u", decoder_state->msbc_bad_bytes);
376 OI_STATUS status = status = OI_CODEC_SBC_DecodeFrame(&(state->decoder_context),
377 &frame_data,
378 &bytes_in_frame_buffer,
379 state->pcm_plc_data,
380 &(state->pcm_bytes));
381
382 if (status) {
383 log_error("SBC decoder for ZIR frame: error %d\n", status);
384 }
385
386 if (bytes_in_frame_buffer){
387 log_error("PLC: not all bytes of zero frame processed, left %u\n", (unsigned int) bytes_in_frame_buffer);
388 }
389
390 if (plc_enabled) {
391 btstack_sbc_plc_bad_frame(&state->plc_state, state->pcm_plc_data, state->pcm_data);
392 } else {
393 (void)memcpy(state->pcm_data,
394 state->pcm_plc_data,
395 state->pcm_bytes);
396 }
397
398 // deliver PCM data
399 btstack_sbc_decoder_deliver_pcm_data(state);
400 }
401 }
402
btstack_sbc_decoder_drop_processed_bytes(btstack_sbc_decoder_bluedroid_t * state,uint16_t bytes_processed)403 static void btstack_sbc_decoder_drop_processed_bytes(btstack_sbc_decoder_bluedroid_t * state, uint16_t bytes_processed){
404 memmove(state->frame_buffer, state->frame_buffer + bytes_processed, state->bytes_in_frame_buffer - bytes_processed);
405 state->bytes_in_frame_buffer -= bytes_processed;
406 }
407
btstack_sbc_decoder_bluedroid_process_msbc_data(btstack_sbc_decoder_bluedroid_t * state,int packet_status_flag,const uint8_t * buffer,int size)408 static void btstack_sbc_decoder_bluedroid_process_msbc_data(btstack_sbc_decoder_bluedroid_t * state, int packet_status_flag, const uint8_t * buffer, int size){
409 int input_bytes_to_process = size;
410 const unsigned int MSBC_FRAME_SIZE = 60;
411
412 while (input_bytes_to_process > 0){
413
414 // Use PLC to insert missing frames (after first sync found)
415 btstack_sbc_decoder_insert_missing_frames(state);
416 // fill buffer with new data
417 int bytes_missing_for_complete_msbc_frame = MSBC_FRAME_SIZE - state->bytes_in_frame_buffer;
418 int bytes_to_append = btstack_min(input_bytes_to_process, bytes_missing_for_complete_msbc_frame);
419 if (bytes_to_append) {
420 append_received_sbc_data(state, buffer, bytes_to_append);
421 buffer += bytes_to_append;
422 input_bytes_to_process -= bytes_to_append;
423 }
424 // complete frame in buffer?
425 if (state->bytes_in_frame_buffer < MSBC_FRAME_SIZE) break;
426
427 uint16_t bytes_in_frame_buffer_before_decoding = state->bytes_in_frame_buffer;
428 uint16_t bytes_processed = 0;
429 const OI_BYTE *frame_data = state->frame_buffer;
430
431 // testing only - corrupt frame periodically
432 btstack_sbc_decoder_bluedroid_simulate_error(frame_data);
433
434 // assert frame looks like this: 01 x8 AD [rest of frame 56 bytes] 00
435 int h2_syncword = 0;
436 int h2_sync_pos = find_h2_sync(frame_data, state->bytes_in_frame_buffer, &h2_syncword);
437 if (h2_sync_pos < 0){
438 // no sync found, discard all but last 2 bytes
439 bytes_processed = state->bytes_in_frame_buffer - 2;
440 btstack_sbc_decoder_drop_processed_bytes(state, bytes_processed);
441 // don't try PLC without at least a single good frame
442 if (state->first_good_frame_found){
443 state->msbc_bad_bytes += bytes_processed;
444 }
445 continue;
446 }
447
448 state->h2_sequence_nr = h2_syncword;
449
450 // drop data before it
451 bytes_processed = h2_sync_pos - 2;
452 if (bytes_processed > 0){
453 memmove(state->frame_buffer, state->frame_buffer + bytes_processed, state->bytes_in_frame_buffer - bytes_processed);
454 state->bytes_in_frame_buffer -= bytes_processed;
455 // don't try PLC without at least a single good frame
456 if (state->first_good_frame_found){
457 state->msbc_bad_bytes += bytes_processed;
458 }
459 continue;
460 }
461
462 int bad_frame = 0;
463 int zero_seq_found = find_sequence_of_zeros(frame_data, state->bytes_in_frame_buffer, 20);
464
465 // after first valid frame, zero sequences count as bad frames
466 if (state->first_good_frame_found){
467 bad_frame = zero_seq_found || packet_status_flag;
468 }
469
470 if (bad_frame){
471 // stats
472 if (zero_seq_found){
473 state->zero_frames_nr++;
474 } else {
475 state->bad_frames_nr++;
476 }
477 #ifdef LOG_FRAME_STATUS
478 if (zero_seq_found){
479 printf("%d : ZERO FRAME\n", instance->h2_sequence_nr);
480 } else {
481 printf("%d : BAD FRAME\n", instance->h2_sequence_nr);
482 }
483 #endif
484 // retry after dropping 3 byte sync
485 bytes_processed = 3;
486 btstack_sbc_decoder_drop_processed_bytes(state, bytes_processed);
487 state->msbc_bad_bytes += bytes_processed;
488 // log_info("Trace bad frame");
489 continue;
490 }
491
492 //ready to decode frame
493 OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(state->decoder_context),
494 &frame_data,
495 &(state->bytes_in_frame_buffer),
496 state->pcm_plc_data,
497 &(state->pcm_bytes));
498
499 bytes_processed = bytes_in_frame_buffer_before_decoding - state->bytes_in_frame_buffer;
500 // log_info("Trace decode status %u, processed %u (bad bytes %u), bytes in buffer %u", (int) status, bytes_processed, decoder_state->msbc_bad_bytes, decoder_state->bytes_in_frame_buffer);
501
502 switch(status){
503 case 0:
504 // synced
505 state->first_good_frame_found = 1;
506
507 // get rid of padding byte, not processed by SBC decoder
508 state->bytes_in_frame_buffer = 0;
509
510 // restart counting bad bytes
511 state->msbc_bad_bytes = 0;
512
513 // feed good frame into PLC history
514 btstack_sbc_plc_good_frame(&state->plc_state, state->pcm_plc_data, state->pcm_data);
515
516 // deliver PCM data
517 btstack_sbc_decoder_deliver_pcm_data(state);
518
519 // stats
520 state->good_frames_nr++;
521 continue;
522
523 case OI_CODEC_SBC_CHECKSUM_MISMATCH:
524 // The next frame is somehow corrupt.
525 log_debug("OI_CODEC_SBC_CHECKSUM_MISMATCH");
526 // Did the codec consume any bytes?
527 if (bytes_processed > 0){
528 // Good. Nothing to do.
529 } else {
530 // Skip the bogus frame by skipping the header.
531 bytes_processed = 1;
532 }
533 break;
534
535 case OI_STATUS_INVALID_PARAMETERS:
536 // This caused by corrupt frames.
537 // The codec apparently does not recover from this.
538 // Re-initialize the codec.
539 log_info("SBC decode: invalid parameters: resetting codec");
540 if (OI_CODEC_mSBC_DecoderReset(&(state->decoder_context), state->decoder_data, sizeof(state->decoder_data)) != OI_STATUS_SUCCESS){
541 log_info("SBC decode: resetting codec failed");
542 }
543 break;
544 default:
545 log_info("Frame decode error: %d", status);
546 break;
547 }
548
549 // on success, while loop was restarted, so all processed bytes have been "bad"
550 state->msbc_bad_bytes += bytes_processed;
551
552 // drop processed bytes from frame buffer
553 btstack_sbc_decoder_drop_processed_bytes(state, bytes_processed);
554 }
555 }
556
557
btstack_sbc_decoder_bluedroid_configure(void * context,btstack_sbc_mode_t mode,void (* callback)(int16_t * data,int num_samples,int num_channels,int sample_rate,void * context),void * callback_context)558 static void btstack_sbc_decoder_bluedroid_configure(void * context, btstack_sbc_mode_t mode, void (*callback)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context), void * callback_context){
559 btstack_sbc_decoder_bluedroid_t * state = (btstack_sbc_decoder_bluedroid_t*) context;
560 OI_STATUS status = OI_STATUS_SUCCESS;
561 switch (mode){
562 case SBC_MODE_STANDARD:
563 // note: we always request stereo output, even for mono input
564 status = OI_CODEC_SBC_DecoderReset(&state->decoder_context, state->decoder_data, sizeof(state->decoder_data), 2, 2, FALSE);
565 break;
566 case SBC_MODE_mSBC:
567 status = OI_CODEC_mSBC_DecoderReset(&(state->decoder_context), state->decoder_data, sizeof(state->decoder_data));
568 break;
569 default:
570 btstack_unreachable();
571 break;
572 }
573
574 if (status != OI_STATUS_SUCCESS){
575 log_error("SBC decoder: error during reset %d\n", status);
576 }
577
578 state->bytes_in_frame_buffer = 0;
579 state->pcm_bytes = sizeof(state->pcm_data);
580 state->h2_sequence_nr = -1;
581 state->first_good_frame_found = 0;
582
583 state->handle_pcm_data = callback;
584 state->callback_context = callback_context;
585 state->mode = mode;
586 btstack_sbc_plc_init(&state->plc_state);
587 }
588
btstack_sbc_decoder_bluedroid_decode_signed_16(void * context,uint8_t packet_status_flag,const uint8_t * buffer,uint16_t size)589 static void btstack_sbc_decoder_bluedroid_decode_signed_16(void * context, uint8_t packet_status_flag, const uint8_t * buffer, uint16_t size){
590 btstack_sbc_decoder_bluedroid_t * state = (btstack_sbc_decoder_bluedroid_t*) context;
591 if (state->mode == SBC_MODE_mSBC){
592 btstack_sbc_decoder_bluedroid_process_msbc_data(state, packet_status_flag, buffer, size);
593 } else {
594 btstack_sbc_decoder_bluedroid_process_sbc_data(state, buffer, size);
595 }
596 }
597
598 static const btstack_sbc_decoder_t btstack_sbc_decoder_bluedroid = {
599 .configure = btstack_sbc_decoder_bluedroid_configure,
600 .decode_signed_16 = btstack_sbc_decoder_bluedroid_decode_signed_16
601 };
602
btstack_sbc_decoder_bluedroid_init_instance(btstack_sbc_decoder_bluedroid_t * context)603 const btstack_sbc_decoder_t * btstack_sbc_decoder_bluedroid_init_instance(btstack_sbc_decoder_bluedroid_t * context){
604 memset(context, 0, sizeof(btstack_sbc_encoder_bluedroid_t));
605 return &btstack_sbc_decoder_bluedroid;
606 }
607