V1.0
This commit is contained in:
moyuhai
@@ -0,0 +1,310 @@
|
||||
#include "msbc_decoder_port.h"
|
||||
|
||||
static struct sbc_decoder_state sbc_internal_decoder_state = {0};
|
||||
|
||||
static void msbc_append_received_data(struct sbc_decoder_state * state, uint8_t * buffer, int size)
|
||||
{
|
||||
int numFreeBytes = sizeof(state->frame_buffer) - state->bytes_in_frame_buffer;
|
||||
|
||||
if (size > numFreeBytes) {
|
||||
printf("SBC data: more bytes read %u than free bytes in buffer %u", size, numFreeBytes);
|
||||
}
|
||||
|
||||
(void)memcpy(state->frame_buffer + state->bytes_in_frame_buffer, buffer,
|
||||
size);
|
||||
state->bytes_in_frame_buffer += size;
|
||||
}
|
||||
|
||||
// returns position of mSBC sync word
|
||||
static int msbc_find_h2_sync(const uint8_t *frame_data, OI_UINT32 frame_bytes, int * sync_word_nr)
|
||||
{
|
||||
int syncword = mSBC_SYNCWORD;
|
||||
uint8_t h2_first_byte = 0;
|
||||
uint8_t h2_second_byte = 0;
|
||||
|
||||
unsigned int i;
|
||||
for (i=0; i<frame_bytes; i++) {
|
||||
if (frame_data[i] == syncword) {
|
||||
// check: first byte == 1
|
||||
if (h2_first_byte == 1) {
|
||||
// check lower nibble of second byte == 0x08
|
||||
uint8_t ln = h2_second_byte & 0x0F;
|
||||
if (ln == 8) {
|
||||
// check if bits 0+2 == bits 1+3
|
||||
uint8_t hn = h2_second_byte >> 4;
|
||||
if ( ((hn>>1) & 0x05) == (hn & 0x05) ) {
|
||||
*sync_word_nr = ((hn & 0x04) >> 1) | (hn & 0x01);
|
||||
return i;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
h2_first_byte = h2_second_byte;
|
||||
h2_second_byte = frame_data[i];
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
static int msbc_find_sequence_of_zeros(const uint8_t *frame_data, OI_UINT32 frame_bytes,
|
||||
int seq_length)
|
||||
{
|
||||
int zero_seq_count = 0;
|
||||
unsigned int i;
|
||||
for (i=0; i<frame_bytes; i++) {
|
||||
if (frame_data[i] == 0) {
|
||||
zero_seq_count++;
|
||||
if (zero_seq_count >= seq_length)
|
||||
return zero_seq_count;
|
||||
} else {
|
||||
zero_seq_count = 0;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
static void msbc_decoder_drop_processed_bytes(struct sbc_decoder_state * decoder_state, uint16_t bytes_processed)
|
||||
{
|
||||
memmove(decoder_state->frame_buffer, decoder_state->frame_buffer + bytes_processed,
|
||||
decoder_state->bytes_in_frame_buffer-bytes_processed);
|
||||
|
||||
decoder_state->bytes_in_frame_buffer -= bytes_processed;
|
||||
}
|
||||
|
||||
static int16_t msbc_crop_sample(float val){
|
||||
float croped_val = val;
|
||||
if (croped_val > 32767.0) croped_val= 32767.0;
|
||||
if (croped_val < -32768.0) croped_val=-32768.0;
|
||||
return (int16_t) croped_val;
|
||||
}
|
||||
|
||||
void msbc_plc_good_frame(struct sbc_plc_state *plc_state, int16_t *in, int16_t *out){
|
||||
float val;
|
||||
int i = 0;
|
||||
plc_state->good_frames_nr++;
|
||||
plc_state->frame_count++;
|
||||
|
||||
#ifdef OCTAVE_OUTPUT
|
||||
FILE * oct_file = NULL;
|
||||
if (plc_state->nbf>0){
|
||||
oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD);
|
||||
if (oct_file){
|
||||
octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (plc_state->nbf>0) {
|
||||
for (i=0;i<SBC_RT;i++) {
|
||||
out[i] = plc_state->hist[SBC_LHIST+i];
|
||||
}
|
||||
|
||||
for (i = SBC_RT;i<(SBC_RT+SBC_OLAL);i++) {
|
||||
float left = plc_state->hist[SBC_LHIST+i];
|
||||
float right = in[i];
|
||||
val = (left*rcos[i-SBC_RT]) + (right*rcos[SBC_OLAL+SBC_RT-1-i]);
|
||||
out[i] = msbc_crop_sample(val);
|
||||
}
|
||||
}
|
||||
|
||||
for (;i<SBC_FS;i++) {
|
||||
out[i] = in[i];
|
||||
}
|
||||
// Copy the output to the history buffer
|
||||
for (i=0;i<SBC_FS;i++) {
|
||||
plc_state->hist[SBC_LHIST+i] = out[i];
|
||||
}
|
||||
|
||||
// shift the history buffer
|
||||
for (i=0;i<SBC_LHIST;i++) {
|
||||
plc_state->hist[i] = plc_state->hist[i+SBC_FS];
|
||||
}
|
||||
|
||||
#ifdef OCTAVE_OUTPUT
|
||||
if (oct_file){
|
||||
octave_fprintf_plot_output(plc_state, oct_file);
|
||||
fclose(oct_file);
|
||||
}
|
||||
#endif
|
||||
plc_state->nbf=0;
|
||||
}
|
||||
|
||||
static int msbc_decoder_get_num_samples_per_frame(struct sbc_decoder_state * state)
|
||||
{
|
||||
return state->decoder_context.common.frameInfo.nrof_blocks * state->decoder_context.common.frameInfo.nrof_subbands;
|
||||
}
|
||||
|
||||
static int msbc_decoder_get_num_channels(struct sbc_decoder_state * state)
|
||||
{
|
||||
return state->decoder_context.common.frameInfo.nrof_channels;
|
||||
}
|
||||
|
||||
static int msbc_decoder_get_sample_rate(struct sbc_decoder_state * state)
|
||||
{
|
||||
return state->decoder_context.common.frameInfo.frequency;
|
||||
}
|
||||
|
||||
void msbc_decoder_process_msbc_data(int packet_status_flag, uint8_t * buffer, int size)
|
||||
{
|
||||
int h2_syncword = 0,h2_sync_pos = 0,zero_seq_found = 0,bytes_missing = 0,bytes_to_append = 0;
|
||||
int input_bytes = size;
|
||||
int bad_frame = 0;
|
||||
uint16_t undecoding_bytes = 0,bytes_processed = 0;
|
||||
const uint8_t *frame_data;
|
||||
const unsigned int MSBC_FRAME_SIZE = 60;
|
||||
struct sbc_decoder_state * decoder_state = &sbc_internal_decoder_state;
|
||||
|
||||
while (input_bytes > 0) {
|
||||
bytes_missing = MSBC_FRAME_SIZE - decoder_state->bytes_in_frame_buffer;
|
||||
bytes_to_append = (input_bytes < bytes_missing ? input_bytes : bytes_missing);
|
||||
if (bytes_to_append) {
|
||||
msbc_append_received_data(decoder_state, buffer, bytes_to_append);
|
||||
buffer += bytes_to_append;
|
||||
input_bytes -= bytes_to_append;
|
||||
}
|
||||
if (decoder_state->bytes_in_frame_buffer < MSBC_FRAME_SIZE)
|
||||
break;
|
||||
undecoding_bytes = decoder_state->bytes_in_frame_buffer;
|
||||
|
||||
frame_data = decoder_state->frame_buffer;
|
||||
|
||||
// testing only - corrupt frame periodically
|
||||
//btstack_sbc_decoder_bluedroid_simulate_error(frame_data);
|
||||
|
||||
// assert frame looks like this: 01 x8 AD [rest of frame 56 bytes] 00
|
||||
h2_sync_pos = msbc_find_h2_sync(frame_data, decoder_state->bytes_in_frame_buffer, &h2_syncword);
|
||||
if (h2_sync_pos < 0) {
|
||||
// no sync found, discard all but last 2 bytes
|
||||
bytes_processed = decoder_state->bytes_in_frame_buffer - 2;
|
||||
msbc_decoder_drop_processed_bytes(decoder_state, bytes_processed);
|
||||
// don't try PLC without at least a single good frame
|
||||
if (decoder_state->first_good_frame_found){
|
||||
decoder_state->msbc_bad_bytes += bytes_processed;
|
||||
}
|
||||
continue;
|
||||
}
|
||||
|
||||
zero_seq_found = msbc_find_sequence_of_zeros(frame_data, decoder_state->bytes_in_frame_buffer, 20);
|
||||
// after first valid frame, zero sequences count as bad frames
|
||||
if (decoder_state->first_good_frame_found) {
|
||||
bad_frame = zero_seq_found || packet_status_flag;
|
||||
}
|
||||
|
||||
if (bad_frame) {
|
||||
// stats
|
||||
if (zero_seq_found) {
|
||||
decoder_state->zero_frames_nr++;
|
||||
} else {
|
||||
decoder_state->bad_frames_nr++;
|
||||
}
|
||||
#ifdef LOG_FRAME_STATUS
|
||||
if (zero_seq_found) {
|
||||
printf("%d : ZERO FRAME\n", decoder_state->h2_sequence_nr);
|
||||
} else {
|
||||
printf("%d : BAD FRAME\n", decoder_state->h2_sequence_nr);
|
||||
}
|
||||
#endif
|
||||
// retry after dropping 3 byte sync
|
||||
bytes_processed = 3;
|
||||
msbc_decoder_drop_processed_bytes(decoder_state, bytes_processed);
|
||||
decoder_state->msbc_bad_bytes += bytes_processed;
|
||||
// log_info("Trace bad frame");
|
||||
continue;
|
||||
}
|
||||
|
||||
//ready to decode frame
|
||||
OI_STATUS status = OI_CODEC_SBC_DecodeFrame(&(decoder_state->decoder_context),
|
||||
&frame_data,
|
||||
&(decoder_state->bytes_in_frame_buffer),
|
||||
decoder_state->pcm_plc_data,
|
||||
&(decoder_state->pcm_bytes));
|
||||
|
||||
bytes_processed = undecoding_bytes - decoder_state->bytes_in_frame_buffer;
|
||||
// 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);
|
||||
|
||||
switch(status) {
|
||||
case 0:
|
||||
// synced
|
||||
decoder_state->first_good_frame_found = 1;
|
||||
|
||||
// get rid of padding byte, not processed by SBC decoder
|
||||
decoder_state->bytes_in_frame_buffer = 0;
|
||||
|
||||
// restart counting bad bytes
|
||||
decoder_state->msbc_bad_bytes = 0;
|
||||
|
||||
// feed good frame into PLC history
|
||||
msbc_plc_good_frame(&decoder_state->plc_state,
|
||||
decoder_state->pcm_plc_data,
|
||||
decoder_state->pcm_data);
|
||||
|
||||
// deliver PCM data
|
||||
decoder_state->handle_pcm_data(decoder_state->pcm_data,
|
||||
msbc_decoder_get_num_samples_per_frame(decoder_state),
|
||||
msbc_decoder_get_num_channels(decoder_state),
|
||||
msbc_decoder_get_sample_rate(decoder_state),
|
||||
decoder_state->context);
|
||||
|
||||
// stats
|
||||
decoder_state->good_frames_nr++;
|
||||
continue;
|
||||
|
||||
case OI_CODEC_SBC_CHECKSUM_MISMATCH:
|
||||
// The next frame is somehow corrupt.
|
||||
printf("OI_CODEC_SBC_CHECKSUM_MISMATCH");
|
||||
// Did the codec consume any bytes?
|
||||
if (bytes_processed > 0){
|
||||
// Good. Nothing to do.
|
||||
} else {
|
||||
// Skip the bogus frame by skipping the header.
|
||||
bytes_processed = 1;
|
||||
}
|
||||
break;
|
||||
|
||||
case OI_STATUS_INVALID_PARAMETERS:
|
||||
// This caused by corrupt frames.
|
||||
// The codec apparently does not recover from this.
|
||||
// Re-initialize the codec.
|
||||
printf("SBC decode: invalid parameters: resetting codec");
|
||||
if (OI_CODEC_mSBC_DecoderReset(&(decoder_state->decoder_context),
|
||||
decoder_state->decoder_data,
|
||||
sizeof(decoder_state->decoder_data)) != OI_STATUS_SUCCESS) {
|
||||
printf("SBC decode: resetting codec failed");
|
||||
}
|
||||
break;
|
||||
default:
|
||||
printf("Frame decode error: %d", status);
|
||||
break;
|
||||
}
|
||||
|
||||
// on success, while loop was restarted, so all processed bytes have been "bad"
|
||||
decoder_state->msbc_bad_bytes += bytes_processed;
|
||||
|
||||
// drop processed bytes from frame buffer
|
||||
msbc_decoder_drop_processed_bytes(decoder_state, bytes_processed);
|
||||
}
|
||||
}
|
||||
|
||||
int msbc_decoder_init(msbc_decoder_data_cb cb)
|
||||
{
|
||||
struct sbc_decoder_state * decoder_state = &sbc_internal_decoder_state;
|
||||
OI_STATUS status = OI_STATUS_SUCCESS;
|
||||
|
||||
status = OI_CODEC_mSBC_DecoderReset(&(decoder_state->decoder_context),
|
||||
decoder_state->decoder_data,
|
||||
sizeof(decoder_state->decoder_data));
|
||||
|
||||
decoder_state->bytes_in_frame_buffer = 0;
|
||||
decoder_state->pcm_bytes = sizeof(decoder_state->pcm_data);
|
||||
decoder_state->h2_sequence_nr = -1;
|
||||
decoder_state->first_good_frame_found = 0;
|
||||
decoder_state->handle_pcm_data = cb;
|
||||
|
||||
decoder_state->context = NULL;
|
||||
decoder_state->plc_state.nbf = 0;
|
||||
decoder_state->plc_state.bestlag = 0;
|
||||
memset(decoder_state->plc_state.hist,0,sizeof(decoder_state->plc_state.hist));
|
||||
|
||||
return (int)status;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user