1 /* 2 * Copyright (C) 2014 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 MATTHIAS 24 * RINGWALD 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_decoder_bluedroid.c" 39 40 // ***************************************************************************** 41 // 42 // SBC decoder based on Bluedroid library 43 // 44 // ***************************************************************************** 45 46 #include "btstack_config.h" 47 48 #include <stdint.h> 49 50 #include <stdlib.h> 51 #include <string.h> 52 53 #ifdef LOG_FRAME_STATUS 54 #include <stdio.h> 55 #endif 56 57 #include "btstack_sbc.h" 58 #include "btstack_sbc_plc.h" 59 60 #include "oi_codec_sbc.h" 61 #include "oi_assert.h" 62 #include "btstack.h" 63 64 #define mSBC_SYNCWORD 0xad 65 #define SBC_SYNCWORD 0x9c 66 #define SBC_MAX_CHANNELS 2 67 // #define LOG_FRAME_STATUS 68 69 #define DECODER_DATA_SIZE (SBC_MAX_CHANNELS*SBC_MAX_BLOCKS*SBC_MAX_BANDS * 4 + SBC_CODEC_MIN_FILTER_BUFFERS*SBC_MAX_BANDS*SBC_MAX_CHANNELS * 2) 70 71 typedef struct { 72 OI_UINT32 bytes_in_frame_buffer; 73 OI_CODEC_SBC_DECODER_CONTEXT decoder_context; 74 75 uint8_t frame_buffer[SBC_MAX_FRAME_LEN]; 76 int16_t pcm_plc_data[SBC_MAX_CHANNELS * SBC_MAX_BANDS * SBC_MAX_BLOCKS]; 77 int16_t pcm_data[SBC_MAX_CHANNELS * SBC_MAX_BANDS * SBC_MAX_BLOCKS]; 78 uint32_t pcm_bytes; 79 OI_UINT32 decoder_data[(DECODER_DATA_SIZE+3)/4]; 80 int first_good_frame_found; 81 int h2_sequence_nr; 82 uint16_t msbc_bad_bytes; 83 } bludroid_decoder_state_t; 84 85 static btstack_sbc_decoder_state_t * sbc_decoder_state_singleton = NULL; 86 static bludroid_decoder_state_t bd_decoder_state; 87 88 // Testing only - START 89 static int plc_enabled = 1; 90 static int corrupt_frame_period = -1; 91 // Testing - STOP 92 93 void btstack_sbc_decoder_test_set_plc_enabled(int enabled){ 94 plc_enabled = enabled; 95 } 96 97 void btstack_sbc_decoder_test_simulate_corrupt_frames(int period){ 98 corrupt_frame_period = period; 99 } 100 101 static int find_sequence_of_zeros(const OI_BYTE *frame_data, OI_UINT32 frame_bytes, int seq_length){ 102 int zero_seq_count = 0; 103 unsigned int i; 104 for (i=0; i<frame_bytes; i++){ 105 if (frame_data[i] == 0) { 106 zero_seq_count++; 107 if (zero_seq_count >= seq_length) return zero_seq_count; 108 } else { 109 zero_seq_count = 0; 110 } 111 } 112 return 0; 113 } 114 115 // returns position of mSBC sync word 116 static int find_h2_sync(const OI_BYTE *frame_data, OI_UINT32 frame_bytes, int * sync_word_nr){ 117 int syncword = mSBC_SYNCWORD; 118 uint8_t h2_first_byte = 0; 119 uint8_t h2_second_byte = 0; 120 121 unsigned int i; 122 for (i=0; i<frame_bytes; i++){ 123 if (frame_data[i] == syncword) { 124 // check: first byte == 1 125 if (h2_first_byte == 1) { 126 // check lower nibble of second byte == 0x08 127 uint8_t ln = h2_second_byte & 0x0F; 128 if (ln == 8) { 129 // check if bits 0+2 == bits 1+3 130 uint8_t hn = h2_second_byte >> 4; 131 if ( ((hn>>1) & 0x05) == (hn & 0x05) ) { 132 *sync_word_nr = ((hn & 0x04) >> 1) | (hn & 0x01); 133 return i; 134 } 135 } 136 } 137 } 138 h2_first_byte = h2_second_byte; 139 h2_second_byte = frame_data[i]; 140 } 141 return -1; 142 } 143 144 int btstack_sbc_decoder_num_samples_per_frame(btstack_sbc_decoder_state_t * state){ 145 bludroid_decoder_state_t * decoder_state = (bludroid_decoder_state_t *) state->decoder_state; 146 return decoder_state->decoder_context.common.frameInfo.nrof_blocks * decoder_state->decoder_context.common.frameInfo.nrof_subbands; 147 } 148 149 int btstack_sbc_decoder_num_channels(btstack_sbc_decoder_state_t * state){ 150 bludroid_decoder_state_t * decoder_state = (bludroid_decoder_state_t *) state->decoder_state; 151 return decoder_state->decoder_context.common.frameInfo.nrof_channels; 152 } 153 154 int btstack_sbc_decoder_sample_rate(btstack_sbc_decoder_state_t * state){ 155 bludroid_decoder_state_t * decoder_state = (bludroid_decoder_state_t *) state->decoder_state; 156 return decoder_state->decoder_context.common.frameInfo.frequency; 157 } 158 159 #ifdef OI_DEBUG 160 void OI_AssertFail(const char* file, int line, const char* reason){ 161 log_error("AssertFail file %s, line %d, reason %s", file, line, reason); 162 } 163 #endif 164 165 void btstack_sbc_decoder_init(btstack_sbc_decoder_state_t * state, btstack_sbc_mode_t mode, void (*callback)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context), void * context){ 166 if (sbc_decoder_state_singleton && (sbc_decoder_state_singleton != state) ){ 167 log_error("SBC decoder: different sbc decoder state already registered"); 168 } 169 OI_STATUS status = OI_STATUS_SUCCESS; 170 switch (mode){ 171 case SBC_MODE_STANDARD: 172 // note: we always request stereo output, even for mono input 173 status = OI_CODEC_SBC_DecoderReset(&(bd_decoder_state.decoder_context), bd_decoder_state.decoder_data, sizeof(bd_decoder_state.decoder_data), 2, 2, FALSE); 174 break; 175 case SBC_MODE_mSBC: 176 status = OI_CODEC_mSBC_DecoderReset(&(bd_decoder_state.decoder_context), bd_decoder_state.decoder_data, sizeof(bd_decoder_state.decoder_data)); 177 break; 178 default: 179 break; 180 } 181 182 if (status != OI_STATUS_SUCCESS){ 183 log_error("SBC decoder: error during reset %d\n", status); 184 } 185 186 sbc_decoder_state_singleton = state; 187 188 bd_decoder_state.bytes_in_frame_buffer = 0; 189 bd_decoder_state.pcm_bytes = sizeof(bd_decoder_state.pcm_data); 190 bd_decoder_state.h2_sequence_nr = -1; 191 bd_decoder_state.first_good_frame_found = 0; 192 193 memset(state, 0, sizeof(btstack_sbc_decoder_state_t)); 194 state->handle_pcm_data = callback; 195 state->mode = mode; 196 state->context = context; 197 state->decoder_state = &bd_decoder_state; 198 btstack_sbc_plc_init(&state->plc_state); 199 } 200 201 static void append_received_sbc_data(bludroid_decoder_state_t * state, uint8_t * buffer, int size){ 202 int numFreeBytes = sizeof(state->frame_buffer) - state->bytes_in_frame_buffer; 203 204 if (size > numFreeBytes){ 205 log_error("SBC data: more bytes read %u than free bytes in buffer %u", size, numFreeBytes); 206 } 207 208 (void)memcpy(state->frame_buffer + state->bytes_in_frame_buffer, buffer, 209 size); 210 state->bytes_in_frame_buffer += size; 211 } 212 213 static void btstack_sbc_decoder_bluedroid_simulate_error(const OI_BYTE *frame_data) { 214 static int frame_count = 0; 215 if (corrupt_frame_period > 0){ 216 frame_count++; 217 218 if ((frame_count % corrupt_frame_period) == 0){ 219 *(uint8_t*)&frame_data[5] = 0; 220 frame_count = 0; 221 } 222 } 223 } 224 225 static void btstack_sbc_decoder_process_sbc_data(btstack_sbc_decoder_state_t * state, uint8_t * buffer, int size){ 226 bludroid_decoder_state_t * decoder_state = (bludroid_decoder_state_t*)state->decoder_state; 227 int input_bytes_to_process = size; 228 int keep_decoding = 1; 229 230 while (keep_decoding) { 231 // Fill decoder_state->frame_buffer as much as possible. 232 int bytes_free_in_frame_buffer = SBC_MAX_FRAME_LEN - decoder_state->bytes_in_frame_buffer; 233 int bytes_to_append = btstack_min(input_bytes_to_process, bytes_free_in_frame_buffer); 234 if (bytes_to_append){ 235 append_received_sbc_data(decoder_state, buffer, bytes_to_append); 236 buffer += bytes_to_append; 237 input_bytes_to_process -= bytes_to_append; 238 } 239 240 // Decode the next frame in decoder_state->frame_buffer. 241 int bytes_in_frame_buffer_before_decoding = decoder_state->bytes_in_frame_buffer; 242 const OI_BYTE *frame_data = decoder_state->frame_buffer; 243 OI_UINT32 frame_data_len = decoder_state->bytes_in_frame_buffer; 244 OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(decoder_state->decoder_context), 245 &frame_data, 246 &frame_data_len, 247 decoder_state->pcm_plc_data, 248 &(decoder_state->pcm_bytes)); 249 uint16_t bytes_processed = bytes_in_frame_buffer_before_decoding - frame_data_len; 250 251 // testing only - corrupt frame periodically 252 btstack_sbc_decoder_bluedroid_simulate_error(frame_data); 253 254 // Handle decoding result. 255 switch(status){ 256 case OI_STATUS_SUCCESS: 257 case OI_CODEC_SBC_PARTIAL_DECODE: 258 state->handle_pcm_data(decoder_state->pcm_plc_data, 259 btstack_sbc_decoder_num_samples_per_frame(state), 260 btstack_sbc_decoder_num_channels(state), 261 btstack_sbc_decoder_sample_rate(state), state->context); 262 state->good_frames_nr++; 263 break; 264 265 case OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA: 266 case OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA: 267 case OI_CODEC_SBC_NOT_ENOUGH_AUDIO_DATA: 268 if (input_bytes_to_process > 0){ 269 // Should never occur: The SBC codec claims there is not enough bytes in the frame_buffer, 270 // but the frame_buffer was full. (The frame_buffer is always full before decoding when input_bytes_to_process > 0.) 271 // Clear frame_buffer. 272 log_info("SBC decode: frame_buffer too small for frame"); 273 bytes_processed = bytes_in_frame_buffer_before_decoding; 274 } else { 275 // Exit decode loop, because there is not enough data in frame_buffer to decode the next frame. 276 keep_decoding = 0; 277 } 278 break; 279 280 case OI_CODEC_SBC_NO_SYNCWORD: 281 // This means the entire frame_buffer did not contain the syncword. 282 // Discard the frame_buffer contents. 283 log_info("SBC decode: no syncword found"); 284 bytes_processed = bytes_in_frame_buffer_before_decoding; 285 break; 286 287 case OI_CODEC_SBC_CHECKSUM_MISMATCH: 288 // The next frame is somehow corrupt. 289 log_info("SBC decode: checksum error"); 290 // Did the codec consume any bytes? 291 if (bytes_processed > 0){ 292 // Good. Nothing to do. 293 } else { 294 // Skip the bogus frame by skipping the header. 295 bytes_processed = 1; 296 } 297 break; 298 299 case OI_STATUS_INVALID_PARAMETERS: 300 // This caused by corrupt frames. 301 // The codec apparently does not recover from this. 302 // Re-initialize the codec. 303 log_info("SBC decode: invalid parameters: resetting codec"); 304 if (OI_CODEC_SBC_DecoderReset(&(bd_decoder_state.decoder_context), bd_decoder_state.decoder_data, sizeof(bd_decoder_state.decoder_data), 2, 2, FALSE) != OI_STATUS_SUCCESS){ 305 log_info("SBC decode: resetting codec failed"); 306 307 } 308 break; 309 default: 310 // Anything else went wrong. 311 // Skip a few bytes and try again. 312 bytes_processed = 1; 313 log_info("SBC decode: unknown status %d", status); 314 break; 315 } 316 317 // Remove decoded frame from decoder_state->frame_buffer. 318 if (bytes_processed > bytes_in_frame_buffer_before_decoding) { 319 bytes_processed = bytes_in_frame_buffer_before_decoding; 320 } 321 memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed, bytes_in_frame_buffer_before_decoding - bytes_processed); 322 decoder_state->bytes_in_frame_buffer -= bytes_processed; 323 } 324 } 325 326 327 static void btstack_sbc_decoder_insert_missing_frames(btstack_sbc_decoder_state_t *state) { 328 bludroid_decoder_state_t * decoder_state = (bludroid_decoder_state_t*)state->decoder_state; 329 const unsigned int MSBC_FRAME_SIZE = 60; 330 331 while (decoder_state->first_good_frame_found && (decoder_state->msbc_bad_bytes >= MSBC_FRAME_SIZE)){ 332 333 decoder_state->msbc_bad_bytes -= MSBC_FRAME_SIZE; 334 state->bad_frames_nr++; 335 336 // prepare zero signal frame 337 const OI_BYTE * frame_data = btstack_sbc_plc_zero_signal_frame(); 338 OI_UINT32 bytes_in_frame_buffer = 57; 339 340 // log_info("Trace bad frame generator, bad bytes %u", decoder_state->msbc_bad_bytes); 341 OI_STATUS status = status = OI_CODEC_SBC_DecodeFrame(&(decoder_state->decoder_context), 342 &frame_data, 343 &bytes_in_frame_buffer, 344 decoder_state->pcm_plc_data, 345 &(decoder_state->pcm_bytes)); 346 347 if (status) { 348 log_error("SBC decoder for ZIR frame: error %d\n", status); 349 } 350 351 if (bytes_in_frame_buffer){ 352 log_error("PLC: not all bytes of zero frame processed, left %u\n", (unsigned int) bytes_in_frame_buffer); 353 } 354 355 if (plc_enabled) { 356 btstack_sbc_plc_bad_frame(&state->plc_state, decoder_state->pcm_plc_data, decoder_state->pcm_data); 357 } else { 358 (void)memcpy(decoder_state->pcm_data, 359 decoder_state->pcm_plc_data, 360 decoder_state->pcm_bytes); 361 } 362 363 state->handle_pcm_data(decoder_state->pcm_data, 364 btstack_sbc_decoder_num_samples_per_frame(state), 365 btstack_sbc_decoder_num_channels(state), 366 btstack_sbc_decoder_sample_rate(state), state->context); 367 } 368 } 369 370 static void btstack_sbc_decoder_process_msbc_data(btstack_sbc_decoder_state_t * state, int packet_status_flag, uint8_t * buffer, int size){ 371 bludroid_decoder_state_t * decoder_state = (bludroid_decoder_state_t*)state->decoder_state; 372 int input_bytes_to_process = size; 373 const unsigned int MSBC_FRAME_SIZE = 60; 374 375 while (input_bytes_to_process > 0){ 376 377 // Use PLC to insert missing frames (after first sync found) 378 btstack_sbc_decoder_insert_missing_frames(state); 379 380 // fill buffer with new data 381 int bytes_missing_for_complete_msbc_frame = MSBC_FRAME_SIZE - decoder_state->bytes_in_frame_buffer; 382 int bytes_to_append = btstack_min(input_bytes_to_process, bytes_missing_for_complete_msbc_frame); 383 if (bytes_to_append) { 384 append_received_sbc_data(decoder_state, buffer, bytes_to_append); 385 buffer += bytes_to_append; 386 input_bytes_to_process -= bytes_to_append; 387 } 388 389 // complete frame in buffer? 390 if (decoder_state->bytes_in_frame_buffer < MSBC_FRAME_SIZE) break; 391 392 uint16_t bytes_in_frame_buffer_before_decoding = decoder_state->bytes_in_frame_buffer; 393 uint16_t bytes_processed = 0; 394 const OI_BYTE *frame_data = decoder_state->frame_buffer; 395 396 // testing only - corrupt frame periodically 397 btstack_sbc_decoder_bluedroid_simulate_error(frame_data); 398 399 // assert frame looks like this: 01 x8 AD [rest of frame 56 bytes] 00 400 int h2_syncword = 0; 401 int h2_sync_pos = find_h2_sync(frame_data, decoder_state->bytes_in_frame_buffer, &h2_syncword); 402 if (h2_sync_pos < 0){ 403 // no sync found, discard all but last 2 bytes 404 bytes_processed = decoder_state->bytes_in_frame_buffer - 2; 405 memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed, decoder_state->bytes_in_frame_buffer); 406 decoder_state->bytes_in_frame_buffer -= bytes_processed; // == 2 407 // don't try PLC without at least a single good frame 408 if (decoder_state->first_good_frame_found){ 409 decoder_state->msbc_bad_bytes += bytes_processed; 410 } 411 continue; 412 } 413 414 decoder_state->h2_sequence_nr = h2_syncword; 415 416 // drop data before it 417 bytes_processed = h2_sync_pos - 2; 418 if (bytes_processed > 2){ 419 memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed, decoder_state->bytes_in_frame_buffer); 420 decoder_state->bytes_in_frame_buffer -= bytes_processed; 421 // don't try PLC without at least a single good frame 422 if (decoder_state->first_good_frame_found){ 423 decoder_state->msbc_bad_bytes += bytes_processed; 424 } 425 continue; 426 } 427 428 int bad_frame = 0; 429 int zero_seq_found = find_sequence_of_zeros(frame_data, decoder_state->bytes_in_frame_buffer, 20); 430 431 // after first valid frame, zero sequences count as bad frames 432 if (decoder_state->first_good_frame_found){ 433 bad_frame = zero_seq_found || packet_status_flag; 434 } 435 436 if (bad_frame){ 437 // stats 438 if (zero_seq_found){ 439 state->zero_frames_nr++; 440 } else { 441 state->bad_frames_nr++; 442 } 443 #ifdef LOG_FRAME_STATUS 444 if (zero_seq_found){ 445 printf("%d : ZERO FRAME\n", decoder_state->h2_sequence_nr); 446 } else { 447 printf("%d : BAD FRAME\n", decoder_state->h2_sequence_nr); 448 } 449 #endif 450 // retry after dropoing 3 byte sync 451 bytes_processed = 3; 452 memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed, decoder_state->bytes_in_frame_buffer); 453 decoder_state->bytes_in_frame_buffer -= bytes_processed; 454 decoder_state->msbc_bad_bytes += bytes_processed; 455 // log_info("Trace bad frame"); 456 continue; 457 } 458 459 // ready to decode frame 460 OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(decoder_state->decoder_context), 461 &frame_data, 462 &(decoder_state->bytes_in_frame_buffer), 463 decoder_state->pcm_plc_data, 464 &(decoder_state->pcm_bytes)); 465 466 bytes_processed = bytes_in_frame_buffer_before_decoding - decoder_state->bytes_in_frame_buffer; 467 // 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); 468 469 switch(status){ 470 case 0: 471 // synced 472 decoder_state->first_good_frame_found = 1; 473 474 // get rid of padding byte, not processed by SBC decoder 475 decoder_state->bytes_in_frame_buffer = 0; 476 477 // restart counting bad bytes 478 decoder_state->msbc_bad_bytes = 0; 479 480 // feed good frame into PLC history 481 btstack_sbc_plc_good_frame(&state->plc_state, decoder_state->pcm_plc_data, decoder_state->pcm_data); 482 483 // deliver PCM data 484 state->handle_pcm_data(decoder_state->pcm_data, 485 btstack_sbc_decoder_num_samples_per_frame(state), 486 btstack_sbc_decoder_num_channels(state), 487 btstack_sbc_decoder_sample_rate(state), state->context); 488 489 // stats 490 state->good_frames_nr++; 491 continue; 492 493 case OI_CODEC_SBC_CHECKSUM_MISMATCH: 494 // The next frame is somehow corrupt. 495 log_debug("OI_CODEC_SBC_CHECKSUM_MISMATCH"); 496 // Did the codec consume any bytes? 497 if (bytes_processed > 0){ 498 // Good. Nothing to do. 499 } else { 500 // Skip the bogus frame by skipping the header. 501 bytes_processed = 1; 502 } 503 break; 504 505 case OI_STATUS_INVALID_PARAMETERS: 506 // This caused by corrupt frames. 507 // The codec apparently does not recover from this. 508 // Re-initialize the codec. 509 log_info("SBC decode: invalid parameters: resetting codec"); 510 if (OI_CODEC_mSBC_DecoderReset(&(bd_decoder_state.decoder_context), bd_decoder_state.decoder_data, sizeof(bd_decoder_state.decoder_data)) != OI_STATUS_SUCCESS){ 511 log_info("SBC decode: resetting codec failed"); 512 } 513 break; 514 default: 515 log_info("Frame decode error: %d", status); 516 break; 517 } 518 519 // on success, while loop was restarted, so all processed bytes have been "bad" 520 decoder_state->msbc_bad_bytes += bytes_processed; 521 522 // drop processed bytes from frame buffer 523 memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed, decoder_state->bytes_in_frame_buffer); 524 } 525 } 526 527 void btstack_sbc_decoder_process_data(btstack_sbc_decoder_state_t * state, int packet_status_flag, uint8_t * buffer, int size){ 528 if (state->mode == SBC_MODE_mSBC){ 529 btstack_sbc_decoder_process_msbc_data(state, packet_status_flag, buffer, size); 530 } else { 531 btstack_sbc_decoder_process_sbc_data(state, buffer, size); 532 } 533 } 534