From 1ff8adc53aa857a377f3cc83a750b1df606b69ea Mon Sep 17 00:00:00 2001 From: Milanka Ringwald Date: Thu, 14 Jul 2016 11:21:39 +0200 Subject: [PATCH] sbc plc: add and compile files --- example/Makefile.inc | 1 + src/classic/sbc_decoder.h | 2 + src/classic/sbc_decoder_bludroid.c | 9 +- src/classic/sbc_plc.c | 211 +++++++++++++++++++++++++++++ src/classic/sbc_plc.h | 75 ++++++++++ test/sbc/Makefile | 6 +- test/sbc/sbc_decoder_test.c | 6 +- 7 files changed, 306 insertions(+), 4 deletions(-) create mode 100644 src/classic/sbc_plc.c create mode 100644 src/classic/sbc_plc.h diff --git a/example/Makefile.inc b/example/Makefile.inc index 9ae6e77f3..f14f0a005 100644 --- a/example/Makefile.inc +++ b/example/Makefile.inc @@ -73,6 +73,7 @@ include ${SBC_DECODER_ROOT}/Makefile.inc include ${SBC_ENCODER_ROOT}/Makefile.inc SBC_DECODER += \ + ${BTSTACK_ROOT}/src/classic/sbc_plc.c \ ${BTSTACK_ROOT}/src/classic/sbc_decoder_bludroid.c \ EXAMPLES = \ diff --git a/src/classic/sbc_decoder.h b/src/classic/sbc_decoder.h index 37509a384..d9bd82d87 100644 --- a/src/classic/sbc_decoder.h +++ b/src/classic/sbc_decoder.h @@ -44,6 +44,7 @@ #define __SBC_DECODER_H #include +#include "sbc_plc.h" #if defined __cplusplus extern "C" { @@ -59,6 +60,7 @@ typedef struct { void (*handle_pcm_data)(int16_t * data, int num_samples, int num_channels, int sample_rate, void * context); // private void * decoder_state; + sbc_plc_state_t plc_state; sbc_mode_t mode; } sbc_decoder_state_t; diff --git a/src/classic/sbc_decoder_bludroid.c b/src/classic/sbc_decoder_bludroid.c index 327f6ee2f..ec7af3b56 100644 --- a/src/classic/sbc_decoder_bludroid.c +++ b/src/classic/sbc_decoder_bludroid.c @@ -49,7 +49,10 @@ #include #include #include + #include "sbc_decoder.h" +#include "sbc_plc.h" + #include "oi_codec_sbc.h" #include "oi_assert.h" @@ -116,6 +119,7 @@ void sbc_decoder_init(sbc_decoder_state_t * state, sbc_mode_t mode, void (*callb state->mode = mode; state->context = context; state->decoder_state = &bd_state; + sbc_plc_init(&state->plc_state); } static void append_received_sbc_data(bludroid_decoder_state_t * state, uint8_t * buffer, int size){ @@ -160,6 +164,7 @@ void sbc_decoder_process_data(sbc_decoder_state_t * state, uint8_t * buffer, int 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), @@ -170,7 +175,9 @@ void sbc_decoder_process_data(sbc_decoder_state_t * state, uint8_t * buffer, int case OI_CODEC_SBC_NO_SYNCWORD: break; case OI_CODEC_SBC_CHECKSUM_MISMATCH: - printf("Frame decode error: OI_CODEC_SBC_CHECKSUM_MISMATCH\n"); + // 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); diff --git a/src/classic/sbc_plc.c b/src/classic/sbc_plc.c new file mode 100644 index 000000000..6e83767a6 --- /dev/null +++ b/src/classic/sbc_plc.c @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS + * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +/* + * sbc_plc.c + * + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "sbc_plc.h" + +static int16_t indices0[] = {0xad, 0x0, 0x0, 0xc5, 0x0, 0x0, 0x0, 0x0, 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, +0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c}; + +/* Raised COSine table for OLA */ +static float rcos[OLAL] = { + 0.99148655f,0.96623611f,0.92510857f,0.86950446f, + 0.80131732f,0.72286918f,0.63683150f,0.54613418f, + 0.45386582f,0.36316850f,0.27713082f,0.19868268f, + 0.13049554f,0.07489143f,0.03376389f,0.00851345f}; + +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){ + return (int16_t *)&indices0; +} + +void sbc_plc_init(sbc_plc_state_t *plc_state){ + int i; + plc_state->nbf=0; + plc_state->bestlag=0; + for (i=0;ihist[i] = 0; + } + +} + +void sbc_plc_bad_frame(sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out){ + int i; + float val; + float sf; + plc_state->nbf++; + sf=1.0f; + + i=0; + if (plc_state->nbf==1){ + /* Perform pattern matching to find where to replicate */ + plc_state->bestlag = PatternMatch(plc_state->hist); + /* the replication begins after the template match */ + plc_state->bestlag += M; + + /* Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet */ + sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag); + for (i=0;ihist[plc_state->bestlag+i]*rcos[OLAL-i-1]; + if (val > 32767.0) val= 32767.0; + if (val < -32768.0) val=-32768.0; + plc_state->hist[LHIST+i] = (int16_t)val; + } + + for (;ihist[plc_state->bestlag+i]; if (val > 32767.0) val= 32767.0; + if (val < -32768.0) val=-32768.0; plc_state->hist[LHIST+i] = (int16_t)val; + } + + for (;ihist[plc_state->bestlag+i]*rcos[i-FS]+plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-1-i+FS]; + if (val > 32767.0) val= 32767.0; + if (val < -32768.0) val=-32768.0; + plc_state->hist[LHIST+i] = (int16_t)val; + } + + for (;ihist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; + } else { + for (;ihist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; + for (;ihist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; + } + for (i=0;ihist[LHIST+i]; + + /* shift the history buffer */ + for (i=0;ihist[i] = plc_state->hist[i+FS]; + +} + +void sbc_plc_good_frame(sbc_plc_state_t *plc_state, int16_t *in, int16_t *out){ + int i; + i=0; + if (plc_state->nbf>0){ + for (i=0;ihist[LHIST+i]; + for (;ihist[LHIST+i]*rcos[i-SBCRT] + in[i]*rcos[OLAL-1-i+SBCRT]); + } + for (;ihist[LHIST+i] = out[i]; + /* shift the history buffer */ + for (i=0;ihist[i] = plc_state->hist[i+FS]; + plc_state->nbf=0; +} + + +float CrossCorrelation(int16_t *x, int16_t *y){ + int m; + float num; + float den; + float Cn; + float x2, y2; + num=0; + den=0; + x2=0.0; + y2=0.0; + for (m=0;mmaxCn){ + bestmatch=n; + maxCn = Cn; + } + } + return(bestmatch); +} + + +float AmplitudeMatch(int16_t *y, int16_t bestmatch) { + int i; + float sumx; + float sumy; + float sf; + sumx = 0.0; + sumy = 0.000001f; + for (i=0;i1.2f) sf=1.2f; + return(sf); +} \ No newline at end of file diff --git a/src/classic/sbc_plc.h b/src/classic/sbc_plc.h new file mode 100644 index 000000000..55da85feb --- /dev/null +++ b/src/classic/sbc_plc.h @@ -0,0 +1,75 @@ +/* + * Copyright (C) 2016 BlueKitchen GmbH + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * 3. Neither the name of the copyright holders nor the names of + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * 4. Any redistribution, use, or modification is done solely for + * personal benefit and not for any commercial purpose or for + * monetary gain. + * + * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS + * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS + * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + * + * Please inquire about commercial licensing options at + * contact@bluekitchen-gmbh.com + * + */ + +/* + * sbc_plc.h + * + */ + +#ifndef __SBC_PLC_H +#define __SBC_PLC_H + +#include + +#if defined __cplusplus +extern "C" { +#endif + +#define FS 120 /* Frame Size */ +#define N 256 /* 16ms - Window Length for pattern matching */ +#define M 64 /* 4ms - Template for matching */ +#define LHIST (N+FS-1) /* Length of history buffer required */ +#define SBCRT 36 /* SBC Reconvergence Time (samples) */ +#define OLAL 16 /* OverLap-Add Length (samples) */ + +/* PLC State Information */ +typedef struct sbc_plc_state { + int16_t hist[LHIST+FS+SBCRT+OLAL]; + int16_t bestlag; + int nbf; +} sbc_plc_state_t; + +#if defined __cplusplus +} +#endif + +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); + +#endif // __SBC_PLC_H \ No newline at end of file diff --git a/test/sbc/Makefile b/test/sbc/Makefile index b00692967..12c914321 100644 --- a/test/sbc/Makefile +++ b/test/sbc/Makefile @@ -7,6 +7,10 @@ SBC_ENCODER_ROOT = ${BTSTACK_ROOT}/3rd-party/bluedroid/encoder include ${SBC_DECODER_ROOT}/Makefile.inc include ${SBC_ENCODER_ROOT}/Makefile.inc +SBC_DECODER += \ + ${BTSTACK_ROOT}/src/classic/sbc_plc.c \ + ${BTSTACK_ROOT}/src/classic/sbc_decoder_bludroid.c \ + SBC_DECODER_OBJ = $(SBC_DECODER:.c=.o) SBC_ENCODER_OBJ = $(SBC_ENCODER:.c=.o) @@ -30,7 +34,7 @@ SBC_TESTS = sbc_decoder_test sbc_encoder_test all: ${SBC_TESTS} -sbc_decoder_test: ${SBC_DECODER_OBJ} ${COMMON_OBJ} ${BTSTACK_ROOT}/src/classic/sbc_decoder_bludroid.o sbc_decoder_test.o +sbc_decoder_test: ${SBC_DECODER_OBJ} ${COMMON_OBJ} sbc_decoder_test.o ${CC} $^ ${CFLAGS} ${LDFLAGS} -o $@ sbc_encoder_test: ${SBC_ENCODER_OBJ} sbc_encoder_test.o diff --git a/test/sbc/sbc_decoder_test.c b/test/sbc/sbc_decoder_test.c index 4a87bff0d..ad417f01c 100644 --- a/test/sbc/sbc_decoder_test.c +++ b/test/sbc/sbc_decoder_test.c @@ -49,13 +49,13 @@ #include #include #include -#include "sbc_decoder.h" + #include "oi_assert.h" #include "btstack.h" +#include "sbc_decoder.h" static uint8_t read_buffer[24]; -static uint8_t buf[4]; typedef struct wav_writer_state { FILE * wav_file; @@ -82,11 +82,13 @@ static ssize_t __read(int fd, void *buf, size_t count){ } void little_endian_fstore_16(FILE *wav_file, uint16_t value){ + uint8_t buf[2]; little_endian_store_32(buf, 0, value); fwrite(&buf, 1, 2, wav_file); } void little_endian_fstore_32(FILE *wav_file, uint32_t value){ + uint8_t buf[4]; little_endian_store_32(buf, 0, value); fwrite(&buf, 1, 4, wav_file); }