xref: /btstack/src/classic/btstack_cvsd_plc.c (revision 9491a7bfdd257d229f79a73b1abc3f041c4445f8) 
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 
44 #include <stdint.h>
45 #include <stdio.h>
46 #include <stdlib.h>
47 #include <string.h>
48 #include <math.h>
49 
50 #include "btstack_cvsd_plc.h"
51 #include "btstack_debug.h"
52 
53 static float rcos[CVSD_OLAL] = {
54     0.99148655,0.96623611,0.92510857,0.86950446,
55     0.80131732,0.72286918,0.63683150,0.54613418,
56     0.45386582,0.36316850,0.27713082,0.19868268,
57     0.13049554,0.07489143,0.03376389,0.00851345};
58 
59 static float CrossCorrelation(int8_t *x, int8_t *y){
60     float num = 0;
61     float den = 0;
62     float x2 = 0;
63     float y2 = 0;
64     int   m;
65     for (m=0;m<CVSD_M;m++){
66         num+=((float)x[m])*y[m];
67         x2+=((float)x[m])*x[m];
68         y2+=((float)y[m])*y[m];
69     }
70     den = (float)sqrt(x2*y2);
71     return num/den;
72 }
73 
74 static int PatternMatch(int8_t *y){
75     float maxCn = -999999.0;  // large negative number
76     int   bestmatch = 0;
77     float Cn;
78     int   n;
79     for (n=0;n<CVSD_N;n++){
80         Cn = CrossCorrelation(&y[CVSD_LHIST-CVSD_M], &y[n]);
81         if (Cn>maxCn){
82             bestmatch=n;
83             maxCn = Cn;
84         }
85     }
86     return bestmatch;
87 }
88 
89 static float AmplitudeMatch(int8_t *y, int8_t bestmatch) {
90     int   i;
91     float sumx = 0;
92     float sumy = 0.000001f;
93     float sf;
94 
95     for (i=0;i<CVSD_FS;i++){
96         sumx += abs(y[CVSD_LHIST-CVSD_FS+i]);
97         sumy += abs(y[bestmatch+i]);
98     }
99     sf = sumx/sumy;
100     // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts
101     if (sf<0.75f) sf=0.75f;
102     if (sf>1.2f) sf=1.2f;
103     return sf;
104 }
105 
106 static int8_t crop_to_int8(float val){
107     float croped_val = val;
108     if (croped_val > 127.0)  croped_val= 127.0;
109     if (croped_val < -128.0) croped_val=-128.0;
110     return (int8_t) croped_val;
111 }
112 
113 
114 void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){
115     memset(plc_state, 0, sizeof(btstack_cvsd_plc_state_t));
116 }
117 
118 void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *out){
119     float val;
120     int   i = 0;
121     float sf = 1;
122     plc_state->nbf++;
123 
124     if (plc_state->nbf==1){
125         // Perform pattern matching to find where to replicate
126         plc_state->bestlag = PatternMatch(plc_state->hist);
127         // the replication begins after the template match
128         plc_state->bestlag += CVSD_M;
129 
130         // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet
131         sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);
132         for (i=0;i<CVSD_OLAL;i++){
133             val = sf*plc_state->hist[plc_state->bestlag+i];
134             plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val);
135         }
136 
137         for (;i<CVSD_FS;i++){
138             val = sf*plc_state->hist[plc_state->bestlag+i];
139             plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val);
140         }
141 
142         for (;i<CVSD_FS+CVSD_OLAL;i++){
143             float left  = sf*plc_state->hist[plc_state->bestlag+i];
144             float right = plc_state->hist[plc_state->bestlag+i];
145             val = left*rcos[i-CVSD_FS] + right*rcos[CVSD_OLAL-1-i+CVSD_FS];
146             plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val);
147         }
148 
149         for (;i<CVSD_FS+CVSD_RT+CVSD_OLAL;i++){
150             plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
151         }
152     } else {
153         for (;i<CVSD_FS+CVSD_RT+CVSD_OLAL;i++){
154             plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
155         }
156     }
157     for (i=0;i<CVSD_FS;i++){
158         out[i] = plc_state->hist[CVSD_LHIST+i];
159     }
160 
161    // shift the history buffer
162    for (i=0;i<CVSD_LHIST+CVSD_RT+CVSD_OLAL;i++){
163         plc_state->hist[i] = plc_state->hist[i+CVSD_FS];
164     }
165 }
166 
167 void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *in, int8_t *out){
168     float val;
169     int i = 0;
170     if (plc_state->nbf>0){
171         for (i=0;i<CVSD_RT;i++){
172             out[i] = plc_state->hist[CVSD_LHIST+i];
173         }
174 
175         for (i=CVSD_RT;i<CVSD_RT+CVSD_OLAL;i++){
176             float left  = plc_state->hist[CVSD_LHIST+i];
177             float right = in[i];
178             val = left * rcos[i-CVSD_RT] + right *rcos[CVSD_OLAL+CVSD_RT-1-i];
179             out[i] = (int8_t)val;
180         }
181     }
182 
183     for (;i<CVSD_FS;i++){
184         out[i] = in[i];
185     }
186     // Copy the output to the history buffer
187     for (i=0;i<CVSD_FS;i++){
188         plc_state->hist[CVSD_LHIST+i] = out[i];
189     }
190     // shift the history buffer
191     for (i=0;i<CVSD_LHIST;i++){
192         plc_state->hist[i] = plc_state->hist[i+CVSD_FS];
193     }
194     plc_state->nbf=0;
195 }
196 
197 static int count_equal_bytes(int8_t * packet, uint16_t size){
198     int count = 0;
199     int temp_count = 1;
200     int i;
201     for (i = 0; i < size-1; i++){
202         if (packet[i] == packet[i+1]){
203             temp_count++;
204             continue;
205         }
206         if (count < temp_count){
207             count = temp_count;
208         }
209         temp_count = 1;
210     }
211     if (temp_count > count + 1){
212         count = temp_count;
213     }
214     return count;
215 }
216 
217 static int bad_frame(int8_t * frame, uint16_t size){
218     return count_equal_bytes(frame, size) > 20;
219 }
220 
221 void btstack_cvsd_plc_process_data(btstack_cvsd_plc_state_t * state, int8_t * in, uint16_t size, int8_t * out){
222     if (size != 24){
223         log_error("btstack_cvsd_plc_process_data: audio frame size is incorrect. Expected %d, got %d", CVSD_FS, size);
224     }
225     state->frame_count++;
226     if (bad_frame(in,size)){
227         memcpy(out, in, size);
228         if (state->good_frames_nr > CVSD_LHIST/CVSD_FS){
229             btstack_cvsd_plc_bad_frame(state, out);
230             state->bad_frames_nr++;
231         } else {
232             memset(out, 0, CVSD_FS);
233         }
234     } else {
235         btstack_cvsd_plc_good_frame(state, in, out);
236         state->good_frames_nr++;
237         if (state->good_frames_nr == 1){
238             printf("First good frame at index %d\n", state->frame_count-1);
239         }
240     }
241 }
242 
243 void btstack_cvsd_plc_mark_bad_frame(btstack_cvsd_plc_state_t * state, int8_t * in, uint16_t size, int8_t * out){
244     if (size != 24){
245         log_error("btstack_cvsd_plc_mark_bad_frame: audio frame size is incorrect. Expected %d, got %d", CVSD_FS, size);
246     }
247     state->frame_count++;
248 
249     if (bad_frame(in,size)){
250         memcpy(out, in, size);
251         if (state->good_frames_nr > CVSD_LHIST/CVSD_FS){
252             memset(out, 50, size);
253             state->bad_frames_nr++;
254         }
255     } else {
256         memcpy(out, in, size);
257         state->good_frames_nr++;
258         if (state->good_frames_nr == 1){
259             printf("First good frame at index %d\n", state->frame_count-1);
260         }
261     }
262 }
263 
264 void btstack_cvsd_dump_statistics(btstack_cvsd_plc_state_t * state){
265     printf("Good frames: %d\n", state->good_frames_nr);
266     printf("Bad frames: %d\n", state->bad_frames_nr);
267 }
268