mirror/btstack
1
0
Fork 0
mirror of https://github.com/bluekitchen/btstack.git synced 2025-01-18 19:21:54 +00:00
Code Issues Packages Projects Releases Wiki Activity

sbc plc: add and compile files

Browse Source
This commit is contained in:
Milanka Ringwald 2016-07-14 11:21:39 +02:00
parent 613518d19a
commit 1ff8adc53a
7 changed files with 306 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
example/Makefile.inc
View File

@ -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 = \

2
src/classic/sbc_decoder.h
View File

@ -44,6 +44,7 @@
#define __SBC_DECODER_H
#include <stdint.h>
#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;

9
src/classic/sbc_decoder_bludroid.c
View File

@ -49,7 +49,10 @@
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#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);

211
src/classic/sbc_plc.c Normal file
View File

@ -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 <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#include <math.h>
#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;i<LHIST+SBCRT;i++){
plc_state->hist[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;i<OLAL;i++){
val = ZIRbuf[i]*rcos[i] + sf*plc_state->hist[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 (;i<FS;i++){
val = sf*plc_state->hist[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 (;i<FS+OLAL;i++){
val = sf*plc_state->hist[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 (;i<FS+SBCRT+OLAL;i++)
plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
} else {
for (;i<FS;i++)
plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
for (;i<FS+SBCRT+OLAL;i++)
plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i];
}
for (i=0;i<FS;i++)
out[i] = plc_state->hist[LHIST+i];
/* shift the history buffer */
for (i=0;i<LHIST+SBCRT+OLAL;i++)
plc_state->hist[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;i<SBCRT;i++)
out[i] = plc_state->hist[LHIST+i];
for (;i<SBCRT+OLAL;i++)
out[i] = (int16_t)(plc_state->hist[LHIST+i]*rcos[i-SBCRT] + in[i]*rcos[OLAL-1-i+SBCRT]);
}
for (;i<FS;i++)
out[i] = in[i];
/*Copy the output to the history buffer */
for (i=0;i<FS;i++)
plc_state->hist[LHIST+i] = out[i];
/* shift the history buffer */
for (i=0;i<LHIST;i++)
plc_state->hist[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;m<M;m++){
num+=((float)x[m])*y[m];
x2+=((float)x[m])*x[m];
y2+=((float)y[m])*y[m];
}
den = (float)sqrt(x2*y2);
Cn = num/den;
return(Cn);
}
int PatternMatch(int16_t *y){
int n;
float maxCn;
float Cn;
int bestmatch;
maxCn=-999999.0; /* large negative number */
bestmatch=0;
for (n=0;n<N;n++){
Cn = CrossCorrelation(&y[LHIST-M] /* x */, &y[n]);
if (Cn>maxCn){
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;i<FS;i++){
sumx += abs(y[LHIST-FS+i]);
sumy += abs(y[bestmatch+i]);
}
sf = sumx/sumy;
/* This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts */
if (sf<0.75f) sf=0.75f;
if (sf>1.2f) sf=1.2f;
return(sf);
}

75
src/classic/sbc_plc.h Normal file
View File

@ -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 <stdint.h>
#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

6
test/sbc/Makefile
View File

@ -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

6
test/sbc/sbc_decoder_test.c
View File

@ -49,13 +49,13 @@
#include <string.h>
#include <fcntl.h>
#include <unistd.h>
#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);
}