sbc plc: add support in bludroiod decoder impl

3rd-party/bluedroid/decoder/srce/decoder-sbc.c

@@ -84,12 +84,14 @@ PRIVATE OI_STATUS FindSyncword(OI_CODEC_SBC_DECODER_CONTEXT *context,
     if (context->common.frameInfo.mSBCEnabled){
         syncword = OI_mSBC_SYNCWORD;
     }
+    //printf("search %x\n", syncword);
     /* BK4BTSTACK_CHANGE END */
     while (*frameBytes && (**frameData != syncword)) {
+      //  printf("%c ", **frameData);
         (*frameBytes)--;
         (*frameData)++;
     }
-        
+    //printf("\n\n");
     if (*frameBytes) {
         /* Syncword found, *frameData points to it, and *frameBytes correctly
          * reflects the number of bytes available to read, including the
@@ -267,7 +269,7 @@ OI_STATUS OI_CODEC_SBC_DecodeFrame(OI_CODEC_SBC_DECODER_CONTEXT *context,
     TRACE(("+OI_CODEC_SBC_DecodeFrame"));
 
     TRACE(("Finding syncword"));
-    
+    //printf("OI_CODEC_SBC_DecodeFrame frameBytes %d\n", frameBytes);
     status = FindSyncword(context, frameData, frameBytes);
     if (!OI_SUCCESS(status)) {
         return status;

src/classic/sbc_decoder_bludroid.c

@@ -60,11 +60,13 @@
 
 #define SBC_MAX_CHANNELS 2
 #define DECODER_DATA_SIZE (SBC_MAX_CHANNELS*SBC_MAX_BLOCKS*SBC_MAX_BANDS * 2 + SBC_CODEC_MIN_FILTER_BUFFERS*SBC_MAX_BANDS*SBC_MAX_CHANNELS * 2)
+
 typedef struct {
     OI_UINT32 bytes_in_frame_buffer;
     OI_CODEC_SBC_DECODER_CONTEXT decoder_context;
     
     uint8_t frame_buffer[SBC_MAX_FRAME_LEN];
+    int16_t pcm_plc_data[SBC_MAX_CHANNELS * SBC_MAX_BANDS * SBC_MAX_BLOCKS];
     int16_t pcm_data[SBC_MAX_CHANNELS * SBC_MAX_BANDS * SBC_MAX_BLOCKS];
     uint32_t pcm_bytes;
     OI_UINT32 decoder_data[(DECODER_DATA_SIZE+3)/4]; 
@@ -133,60 +135,93 @@ static void append_received_sbc_data(bludroid_decoder_state_t * state, uint8_t *
     state->bytes_in_frame_buffer += size;
 }
 
+
 void sbc_decoder_process_data(sbc_decoder_state_t * state, uint8_t * buffer, int size){
-    int bytes_to_process = size;
     bludroid_decoder_state_t * bd_decoder_state = (bludroid_decoder_state_t*)state->decoder_state;
+    int bytes_to_process = size;
+    int msbc_frame_size = 57;
+
+    // printf("<<-- enter -->>\n");
+
+    // printf("Process data: in buffer %u, new %u\n", bd_decoder_state->bytes_in_frame_buffer, size);
 
     while (bytes_to_process > 0){
-        int space_in_frame_buffer = sizeof(bd_decoder_state->frame_buffer) - bd_decoder_state->bytes_in_frame_buffer;
+
-        int bytes_to_append = space_in_frame_buffer > bytes_to_process ? bytes_to_process : space_in_frame_buffer;
+        int bytes_missing_for_complete_msbc_frame = msbc_frame_size - bd_decoder_state->bytes_in_frame_buffer;
-        // fill frame buffer to max capacity
+        int bytes_to_append = btstack_min(bytes_to_process, bytes_missing_for_complete_msbc_frame);
+        
         append_received_sbc_data(bd_decoder_state, buffer, bytes_to_append);
+        // printf("Append %u bytes, now %u in buffer \n", bytes_to_append, bd_decoder_state->bytes_in_frame_buffer);
+        buffer           += bytes_to_append;
+        bytes_to_process -= bytes_to_append;
         
-        // process whole buffer, possibly more then one frame
+        if (bd_decoder_state->bytes_in_frame_buffer < msbc_frame_size){
-        while (1){
+            // printf("not enough data %d > %d\n", msbc_frame_size, bd_decoder_state->bytes_in_frame_buffer);
-            uint16_t bytes_in_buffer_before = bd_decoder_state->bytes_in_frame_buffer;
+            if (bytes_to_process){
-            const OI_BYTE *frame_data = bd_decoder_state->frame_buffer;
+                printf("SHOULD NOT HAPPEN... not enough bytes, but bytes left to process\n");
-            OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(bd_decoder_state->decoder_context), 
-                                                        &frame_data, 
-                                                        &(bd_decoder_state->bytes_in_frame_buffer), 
-                                                        bd_decoder_state->pcm_data, 
-                                                        &(bd_decoder_state->pcm_bytes));
-
-            if (status == OI_CODEC_SBC_CHECKSUM_MISMATCH){
-                // advance at least one byte
-                bd_decoder_state->bytes_in_frame_buffer--;
-            }
-
-            uint16_t bytes_processed = bytes_in_buffer_before - bd_decoder_state->bytes_in_frame_buffer;
-            // log_info("sbc_decoder_process_data: decode status %u, processed %u, left %u", status, bytes_processed, bd_decoder_state->bytes_in_frame_buffer);
-            memmove(bd_decoder_state->frame_buffer, bd_decoder_state->frame_buffer + bytes_processed, bd_decoder_state->bytes_in_frame_buffer);
-
-            switch(status){
-                case 0:
-                    sbc_plc_good_frame(&state->plc_state, bd_decoder_state->pcm_data, bd_decoder_state->pcm_data);
-                    state->handle_pcm_data(bd_decoder_state->pcm_data, 
-                                        sbc_decoder_num_samples_per_frame(state), 
-                                        sbc_decoder_num_channels(state), 
-                                        sbc_decoder_sample_rate(state), state->context);
-                    continue;
-                case OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA:
-                case OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA:
-                case OI_CODEC_SBC_NO_SYNCWORD:
-                    break;
-                case OI_CODEC_SBC_CHECKSUM_MISMATCH:
-                    // call decoder with indices0 as frame_data
-                    // then call:
-                    // sbc_plc_bad_frame(&state->plc_state, bd_decoder_state->pcm_data, bd_decoder_state->pcm_data);
-                    break;
-                default:
-                    printf("Frame decode error: %d\n", status);
-                    break;
             }
             break;
         }
         
-        buffer     += bytes_to_append;
+        uint16_t bytes_in_buffer_before = bd_decoder_state->bytes_in_frame_buffer;
-        bytes_to_process -= bytes_to_append;
+        uint16_t bytes_processed = 0;
+        const OI_BYTE *frame_data = bd_decoder_state->frame_buffer;
+
+        OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(bd_decoder_state->decoder_context), 
+                                                    &frame_data, 
+                                                    &(bd_decoder_state->bytes_in_frame_buffer), 
+                                                    bd_decoder_state->pcm_plc_data, 
+                                                    &(bd_decoder_state->pcm_bytes));
+
+        bytes_processed = bytes_in_buffer_before - bd_decoder_state->bytes_in_frame_buffer;
+
+        // printf("Decode status %u, processed %u, left %u\n", status, bytes_processed, bd_decoder_state->bytes_in_frame_buffer);
+
+        memmove(bd_decoder_state->frame_buffer, bd_decoder_state->frame_buffer + bytes_processed, bd_decoder_state->bytes_in_frame_buffer);
+
+        //printf("frame_count %d, expected frame len %d, bytes in frame %d \n", frame_count, OI_CODEC_SBC_CalculateFramelen(&bd_decoder_state->decoder_context.common.frameInfo), bytes_in_buffer_before);
+        switch(status){
+            case 0:
+                // printf("OK: \n");
+                sbc_plc_good_frame(&state->plc_state, bd_decoder_state->pcm_plc_data, bd_decoder_state->pcm_data);
+                state->handle_pcm_data(bd_decoder_state->pcm_data, 
+                                    sbc_decoder_num_samples_per_frame(state), 
+                                    sbc_decoder_num_channels(state), 
+                                    sbc_decoder_sample_rate(state), state->context);
+                continue;
+            case OI_CODEC_SBC_NOT_ENOUGH_HEADER_DATA:
+            case OI_CODEC_SBC_NOT_ENOUGH_BODY_DATA:
+                // printf("NOT_ENOUGH_DATA: \n");
+                // do nothing
+                break;
+
+            case OI_CODEC_SBC_NO_SYNCWORD:
+            case OI_CODEC_SBC_CHECKSUM_MISMATCH:
+                // printf("NO_SYNCWORD or CHECKSUM_MISMATCH\n");
+                bd_decoder_state->bytes_in_frame_buffer = 0;
+
+                frame_data = sbc_plc_zero_signal_frame();
+                OI_UINT32 bytes_in_frame_buffer = msbc_frame_size;
+                
+                status = OI_CODEC_SBC_DecodeFrame(&(bd_decoder_state->decoder_context), 
+                                                    &frame_data, 
+                                                    &bytes_in_frame_buffer, 
+                                                    bd_decoder_state->pcm_plc_data, 
+                                                    &(bd_decoder_state->pcm_bytes));
+                // printf("after bad frame, new status: %d, bytes in frame %d\n", status, bytes_in_frame_buffer);
+
+                if (status != 0) exit(10);
+                sbc_plc_bad_frame(&state->plc_state, bd_decoder_state->pcm_plc_data, bd_decoder_state->pcm_data);
+                state->handle_pcm_data(bd_decoder_state->pcm_data, 
+                                    sbc_decoder_num_samples_per_frame(state), 
+                                    sbc_decoder_num_channels(state), 
+                                    sbc_decoder_sample_rate(state), state->context);
+                
+                break;
+            default:
+                printf("Frame decode error: %d\n", status);
+                break;
+        }
     }
+    // printf ("<<-- exit -->>\n");
 }

src/classic/sbc_plc.c

@@ -50,7 +50,7 @@
 
 #include "sbc_plc.h"
 
-static int16_t indices0[] = {0xad, 0x0, 0x0, 0xc5, 0x0, 0x0, 0x0, 0x0, 0x77, 0x6d,
+static uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d,
 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
@@ -67,8 +67,8 @@ static float CrossCorrelation(int16_t *x, int16_t *y);
 static int PatternMatch(int16_t *y);
 static float AmplitudeMatch(int16_t *y, int16_t bestmatch);
 
-int16_t * sbc_plc_zero_signal_indices(void){
+uint8_t * sbc_plc_zero_signal_frame(void){
-    return (int16_t *)&indices0;
+    return (uint8_t *)&indices0;
 }
 
 void sbc_plc_init(sbc_plc_state_t *plc_state){

src/classic/sbc_plc.h

@@ -70,6 +70,6 @@ typedef struct sbc_plc_state {
 void sbc_plc_init(sbc_plc_state_t *plc_state);
 void sbc_plc_bad_frame(sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out); 
 void sbc_plc_good_frame(sbc_plc_state_t *plc_state, int16_t *in, int16_t *out);
-int16_t * sbc_plc_zero_signal_indices(void);
+uint8_t * sbc_plc_zero_signal_frame(void);
 
 #endif // __SBC_PLC_H