moyuhai
311 lines
11 KiB
C
311 lines
11 KiB
C
#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;
|
|
}
|
|
|