xref: /btstack/src/classic/btstack_sbc_plc.c (revision 7e6b1e83d2aaaacbcb5e04ae26e1c08d6d0bb297) 
1 /*
2  * Copyright (C) 2016 BlueKitchen GmbH
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  *
8  * 1. Redistributions of source code must retain the above copyright
9  *    notice, this list of conditions and the following disclaimer.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 3. Neither the name of the copyright holders nor the names of
14  *    contributors may be used to endorse or promote products derived
15  *    from this software without specific prior written permission.
16  * 4. Any redistribution, use, or modification is done solely for
17  *    personal benefit and not for any commercial purpose or for
18  *    monetary gain.
19  *
20  * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21  * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS
24  * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30  * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31  * SUCH DAMAGE.
32  *
33  * Please inquire about commercial licensing options at
34  * [email protected]
35  *
36  */
37 
38 /*
39  * btstack_sbc_plc.c
40  *
41  */
42 
43 #include <stdint.h>
44 #include <stdio.h>
45 #include <stdlib.h>
46 #include <string.h>
47 #include <fcntl.h>
48 #include <unistd.h>
49 #include <math.h>
50 
51 #include "btstack_sbc_plc.h"
52 
53 static uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d,
54 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
55 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
56 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
57 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c};
58 
59 /* Raised COSine table for OLA */
60 static float rcos[SBC_OLAL] = {
61     0.99148655f,0.96623611f,0.92510857f,0.86950446f,
62     0.80131732f,0.72286918f,0.63683150f,0.54613418f,
63     0.45386582f,0.36316850f,0.27713082f,0.19868268f,
64     0.13049554f,0.07489143f,0.03376389f,0.00851345f};
65 
66 static float CrossCorrelation(int16_t *x, int16_t *y);
67 static int PatternMatch(int16_t *y);
68 static float AmplitudeMatch(int16_t *y, int16_t bestmatch);
69 
70 static int16_t crop_to_int16(float val){
71     float croped_val = 0;
72     if (val > 32767.0)  croped_val= 32767.0;
73     if (val < -32768.0) croped_val=-32768.0;
74     return (int16_t) croped_val;
75 }
76 
77 uint8_t * btstack_sbc_plc_zero_signal_frame(void){
78     return (uint8_t *)&indices0;
79 }
80 
81 void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){
82     plc_state->nbf=0;
83     plc_state->bestlag=0;
84     memset(plc_state->hist,0,sizeof(plc_state->hist));
85 }
86 
87 void btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out){
88     float val;
89     int   i = 0;
90     float sf = 1;
91 
92     plc_state->nbf++;
93 
94     if (plc_state->nbf==1){
95         /* Perform pattern matching to find where to replicate */
96         plc_state->bestlag = PatternMatch(plc_state->hist);
97         /* the replication begins after the template match */
98         plc_state->bestlag += SBC_M;
99 
100         /* Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet */
101         sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);
102         for (i=0;i<SBC_OLAL;i++){
103             float left  = ZIRbuf[i];
104             float right = sf*plc_state->hist[plc_state->bestlag+i];
105             val = left*rcos[i] + right*rcos[SBC_OLAL-i-1];
106             plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
107         }
108 
109         for (;i<SBC_FS;i++){
110             val = sf*plc_state->hist[plc_state->bestlag+i];
111             plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
112         }
113 
114         for (;i<SBC_FS+SBC_OLAL;i++){
115             float left  = sf*plc_state->hist[plc_state->bestlag+i];
116             float right = plc_state->hist[plc_state->bestlag+i];
117             val = left*rcos[i-SBC_FS]+right*rcos[SBC_OLAL-1-i+SBC_FS];
118             plc_state->hist[SBC_LHIST+i] = crop_to_int16(val);
119         }
120 
121         for (;i<SBC_FS+SBC_RT+SBC_OLAL;i++){
122             plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
123         }
124 
125     } else {
126         for (i=0;i<SBC_FS+SBC_RT+SBC_OLAL;i++)
127             plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
128     }
129     for (i=0;i<SBC_FS;i++){
130         out[i] = plc_state->hist[SBC_LHIST+i];
131     }
132 
133     /* shift the history buffer */
134     for (i=0;i<SBC_LHIST+SBC_RT+SBC_OLAL;i++){
135         plc_state->hist[i] = plc_state->hist[i+SBC_FS];
136     }
137 
138 }
139 
140 void btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t *plc_state, int16_t *in, int16_t *out){
141     float val;
142     int i = 0;
143     if (plc_state->nbf>0){
144         for (i=0;i<SBC_RT;i++){
145             out[i] = plc_state->hist[SBC_LHIST+i];
146         }
147 
148         for (;i<SBC_RT+SBC_OLAL;i++){
149             float left  = plc_state->hist[SBC_LHIST+i];
150             float right = in[i];
151             val = left*rcos[i-SBC_RT] + right*rcos[SBC_OLAL-1-i+SBC_RT];
152             out[i] = crop_to_int16(val);
153         }
154     }
155     for (;i<SBC_FS;i++){
156         out[i] = in[i];
157     }
158 
159     /*Copy the output to the history buffer */
160     for (i=0;i<SBC_FS;i++){
161         plc_state->hist[SBC_LHIST+i] = out[i];
162     }
163     /* shift the history buffer */
164     for (i=0;i<SBC_LHIST;i++){
165         plc_state->hist[i] = plc_state->hist[i+SBC_FS];
166     }
167 
168     plc_state->nbf=0;
169 }
170 
171 
172 float CrossCorrelation(int16_t *x, int16_t *y){
173     float num = 0;
174     float den = 0;
175     float x2 = 0;
176     float y2 = 0;
177     int   m;
178     for (m=0;m<SBC_M;m++){
179         num+=((float)x[m])*y[m];
180         x2+=((float)x[m])*x[m];
181         y2+=((float)y[m])*y[m];
182     }
183     den = (float)sqrt(x2*y2);
184     return num/den;
185 }
186 
187 int PatternMatch(int16_t *y){
188     float maxCn = -999999.0;  /* large negative number */
189     int   bestmatch = 0;
190     float Cn;
191     int   n;
192     for (n=0;n<SBC_N;n++){
193         Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M] /* x */, &y[n]);
194         if (Cn>maxCn){
195             bestmatch=n;
196             maxCn = Cn;
197         }
198     }
199     return bestmatch;
200 }
201 
202 
203 float AmplitudeMatch(int16_t *y, int16_t bestmatch) {
204     int   i;
205     float sumx = 0;
206     float sumy = 0.000001f;
207     float sf;
208 
209     for (i=0;i<SBC_FS;i++){
210         sumx += abs(y[SBC_LHIST-SBC_FS+i]);
211         sumy += abs(y[bestmatch+i]);
212     }
213     sf = sumx/sumy;
214     /* This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts */
215     if (sf<0.75f) sf=0.75f;
216     if (sf>1.2f) sf=1.2f;
217     return sf;
218 }