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 BLUEKITCHEN
24 * GMBH 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 #define BTSTACK_FILE__ "btstack_sbc_plc.c"
39
40 /*
41 * btstack_sbc_plc.c
42 *
43 */
44
45 #include <stdint.h>
46 #include <stdlib.h>
47 #include <string.h>
48
49 #ifdef OCTAVE_OUTPUT
50 #include <stdio.h>
51 #endif
52
53 #include "btstack_sbc_plc.h"
54 #include "btstack_debug.h"
55
56 #define SAMPLE_FORMAT int16_t
57
58 // Zero Frame (57 bytes) with padding zeros to avoid out of bound reads
59 static uint8_t indices0[] = {
60 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d,
61 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6,
62 0xdb, 0x77, 0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb,
63 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6, 0xdd, 0xdb, 0x6d,
64 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6,
65 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c,
66 /* padding */ 0x00, 0x00, 0x00
67 };
68
69 /* Raised COSine table for OLA */
70 static float rcos[SBC_OLAL] = {
71 0.99148655f,0.96623611f,0.92510857f,0.86950446f,
72 0.80131732f,0.72286918f,0.63683150f,0.54613418f,
73 0.45386582f,0.36316850f,0.27713082f,0.19868268f,
74 0.13049554f,0.07489143f,0.03376389f,0.00851345f
75 };
76
77 // taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi
78 // Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation
sqrt3(const float x)79 static float sqrt3(const float x){
80 union {
81 int i;
82 float x;
83 } u;
84 u.x = x;
85 u.i = (1<<29) + (u.i >> 1) - (1<<22);
86
87 // Two Babylonian Steps (simplified from:)
88 // u.x = 0.5f * (u.x + x/u.x);
89 // u.x = 0.5f * (u.x + x/u.x);
90 u.x = u.x + (x/u.x);
91 u.x = (0.25f*u.x) + (x/u.x);
92
93 return u.x;
94 }
95
absolute(float x)96 static float absolute(float x){
97 if (x < 0) x = -x;
98 return x;
99 }
100
CrossCorrelation(SAMPLE_FORMAT * x,SAMPLE_FORMAT * y)101 static float CrossCorrelation(SAMPLE_FORMAT *x, SAMPLE_FORMAT *y){
102 float num = 0;
103 float den = 0;
104 float x2 = 0;
105 float y2 = 0;
106 int m;
107 for (m=0;m<SBC_M;m++){
108 num+=((float)x[m])*y[m];
109 x2+=((float)x[m])*x[m];
110 y2+=((float)y[m])*y[m];
111 }
112 den = (float)sqrt3(x2*y2);
113 return num/den;
114 }
115
PatternMatch(SAMPLE_FORMAT * y)116 static int PatternMatch(SAMPLE_FORMAT *y){
117 float maxCn = -999999.f; // large negative number
118 int bestmatch = 0;
119 float Cn;
120 int n;
121 for (n=0;n<SBC_N;n++){
122 Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M], &y[n]);
123 if (Cn>maxCn){
124 bestmatch=n;
125 maxCn = Cn;
126 }
127 }
128 return bestmatch;
129 }
130
AmplitudeMatch(SAMPLE_FORMAT * y,SAMPLE_FORMAT bestmatch)131 static float AmplitudeMatch(SAMPLE_FORMAT *y, SAMPLE_FORMAT bestmatch) {
132 int i;
133 float sumx = 0;
134 float sumy = 0.000001f;
135 float sf;
136
137 for (i=0;i<SBC_FS;i++){
138 sumx += absolute(y[SBC_LHIST-SBC_FS+i]);
139 sumy += absolute(y[bestmatch+i]);
140 }
141 sf = sumx/sumy;
142 // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts
143 if (sf<0.75f) sf=0.75f;
144 if (sf>1.0f) sf=1.0f;
145 return sf;
146 }
147
crop_sample(float val)148 static SAMPLE_FORMAT crop_sample(float val){
149 float croped_val = val;
150 if (croped_val > 32767.f) croped_val= 32767.f;
151 if (croped_val < -32768.f) croped_val=-32768.f;
152 return (SAMPLE_FORMAT) croped_val;
153 }
154
btstack_sbc_plc_zero_signal_frame(void)155 uint8_t * btstack_sbc_plc_zero_signal_frame(void){
156 return (uint8_t *)&indices0;
157 }
158
btstack_sbc_plc_init(btstack_sbc_plc_state_t * plc_state)159 void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){
160 plc_state->nbf=0;
161 plc_state->bestlag=0;
162 memset(plc_state->hist,0,sizeof(plc_state->hist));
163 }
164
165 #ifdef OCTAVE_OUTPUT
166 typedef enum {
167 OCTAVE_FRAME_TYPE_UNKNOWN = 0,
168 OCTAVE_FRAME_TYPE_GOOD,
169 OCTAVE_FRAME_TYPE_BAD
170 } octave_frame_type_t;
171
172 static const char * octave_frame_type_name[] = {
173 "unknown",
174 "good",
175 "bad"
176 };
177
178 static octave_frame_type_t octave_frame_type;
179 static char octave_base_name[1000];
180
octave_frame_type2str(int index)181 const char * octave_frame_type2str(int index){
182 if (index <= 0 || index >= sizeof(octave_frame_type_t)) return octave_frame_type_name[0];
183 return octave_frame_type_name[index];
184 }
185
btstack_sbc_plc_octave_set_base_name(const char * base_name)186 void btstack_sbc_plc_octave_set_base_name(const char * base_name){
187 strcpy(octave_base_name, base_name);
188 printf("OCTAVE: base name set to %s\n", octave_base_name);
189 }
190
octave_fprintf_array_int16(FILE * oct_file,char * name,int data_len,int16_t * data)191 static void octave_fprintf_array_int16(FILE * oct_file, char * name, int data_len, int16_t * data){
192 fprintf(oct_file, "%s = [", name);
193 int i;
194 for (i = 0; i < data_len - 1; i++){
195 fprintf(oct_file, "%d, ", data[i]);
196 }
197 fprintf(oct_file, "%d", data[i]);
198 fprintf(oct_file, "%s", "];\n");
199 }
200
open_octave_file(btstack_sbc_plc_state_t * plc_state,octave_frame_type_t frame_type)201 static FILE * open_octave_file(btstack_sbc_plc_state_t *plc_state, octave_frame_type_t frame_type){
202 char oct_file_name[1200];
203 octave_frame_type = frame_type;
204 snprintf(oct_file_name, sizeof(oct_file_name), "%s_octave_plc_%d_%s.m",
205 octave_base_name, plc_state->frame_count,
206 octave_frame_type2str(octave_frame_type));
207 oct_file_name[sizeof(oct_file_name) - 1] = 0;
208
209 FILE * oct_file = fopen(oct_file_name, "wb");
210 if (oct_file == NULL){
211 printf("OCTAVE: could not open file %s\n", oct_file_name);
212 return NULL;
213 }
214 printf("OCTAVE: opened file %s\n", oct_file_name);
215 return oct_file;
216 }
217
octave_fprintf_plot_history_frame(btstack_sbc_plc_state_t * plc_state,FILE * oct_file,int frame_nr)218 static void octave_fprintf_plot_history_frame(btstack_sbc_plc_state_t *plc_state, FILE * oct_file, int frame_nr){
219 char title[100];
220 char hist_name[10];
221 snprintf(hist_name, sizeof(hist_name), "hist%d", plc_state->nbf);
222 hist_name[sizeof(hist_name) - 1] = 0;
223
224 octave_fprintf_array_int16(oct_file, hist_name, SBC_LHIST, plc_state->hist);
225
226 fprintf(oct_file, "y = [min(%s):1000:max(%s)];\n", hist_name, hist_name);
227 fprintf(oct_file, "x = zeros(1, size(y,2));\n");
228 fprintf(oct_file, "b = [0: %d];\n", SBC_LHIST+SBC_FS+SBC_RT+SBC_OLAL);
229
230 int pos = SBC_FS;
231 fprintf(oct_file, "shift_x = x + %d;\n", pos);
232
233 pos = SBC_LHIST - 1;
234 fprintf(oct_file, "lhist_x = x + %d;\n", pos);
235 pos += SBC_OLAL;
236 fprintf(oct_file, "lhist_olal1_x = x + %d;\n", pos);
237 pos += SBC_FS - SBC_OLAL;
238 fprintf(oct_file, "lhist_fs_x = x + %d;\n", pos);
239 pos += SBC_OLAL;
240 fprintf(oct_file, "lhist_olal2_x = x + %d;\n", pos);
241 pos += SBC_RT;
242 fprintf(oct_file, "lhist_rt_x = x + %d;\n", pos);
243
244 fprintf(oct_file, "pattern_window_x = x + %d;\n", SBC_LHIST - SBC_M);
245
246 fprintf(oct_file, "hf = figure();\n");
247 snprintf(title, sizeof(title), "PLC %s frame %d",
248 octave_frame_type2str(octave_frame_type), frame_nr);
249 title[sizeof(title) - 1] = 0;
250
251 fprintf(oct_file, "hold on;\n");
252 fprintf(oct_file, "h1 = plot(%s); \n", hist_name);
253
254 fprintf(oct_file, "title(\"%s\");\n", title);
255
256 fprintf(oct_file, "plot(lhist_x, y, 'k'); \n");
257 fprintf(oct_file, "text(max(lhist_x) - 10, max(y)+1000, 'lhist'); \n");
258
259 fprintf(oct_file, "plot(lhist_olal1_x, y, 'k'); \n");
260 fprintf(oct_file, "text(max(lhist_olal1_x) - 10, max(y)+1000, 'OLAL'); \n");
261
262 fprintf(oct_file, "plot(lhist_fs_x, y, 'k'); \n");
263 fprintf(oct_file, "text(max(lhist_fs_x) - 10, max(y)+1000, 'FS'); \n");
264
265 fprintf(oct_file, "plot(lhist_olal2_x, y, 'k'); \n");
266 fprintf(oct_file, "text(max(lhist_olal2_x) - 10, max(y)+1000, 'OLAL'); \n");
267
268 fprintf(oct_file, "plot(lhist_rt_x, y, 'k');\n");
269 fprintf(oct_file, "text(max(lhist_rt_x) - 10, max(y)+1000, 'RT'); \n");
270
271 if (octave_frame_type == OCTAVE_FRAME_TYPE_GOOD) return;
272
273 int x0 = plc_state->bestlag;
274 int x1 = plc_state->bestlag + SBC_M - 1;
275 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1);
276 fprintf(oct_file, "text(%d - 10, -10, 'bestlag'); \n", x0);
277
278 x0 = plc_state->bestlag + SBC_M ;
279 x1 = plc_state->bestlag + SBC_M + SBC_FS - 1;
280 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'kd'); \n", x0, x1, hist_name, x0, x1);
281
282 x0 = SBC_LHIST - SBC_M;
283 x1 = SBC_LHIST - 1;
284 fprintf(oct_file, "plot(b(%d:%d), %s(%d:%d), 'rd'); \n", x0, x1, hist_name, x0, x1);
285 fprintf(oct_file, "plot(pattern_window_x, y, 'g'); \n");
286 fprintf(oct_file, "text(max(pattern_window_x) - 10, max(y)+1000, 'M'); \n");
287 }
288
octave_fprintf_plot_output(btstack_sbc_plc_state_t * plc_state,FILE * oct_file)289 static void octave_fprintf_plot_output(btstack_sbc_plc_state_t *plc_state, FILE * oct_file){
290 if (!oct_file) return;
291 char out_name[10];
292 snprintf(out_name, sizeof(out_name), "out%d", plc_state->nbf);
293 out_name[sizeof(out_name) - 1] = 0;
294 int x0 = SBC_LHIST;
295 int x1 = x0 + SBC_FS - 1;
296 octave_fprintf_array_int16(oct_file, out_name, SBC_FS, plc_state->hist+x0);
297 fprintf(oct_file, "h2 = plot(b(%d:%d), %s, 'cd'); \n", x0, x1, out_name);
298
299 char rest_hist_name[10];
300 snprintf(rest_hist_name, sizeof(rest_hist_name), "rest%d", plc_state->nbf);
301 rest_hist_name[sizeof(rest_hist_name) - 1] = 0;
302 x0 = SBC_LHIST + SBC_FS;
303 x1 = x0 + SBC_OLAL + SBC_RT - 1;
304 octave_fprintf_array_int16(oct_file, rest_hist_name, SBC_OLAL + SBC_RT, plc_state->hist+x0);
305 fprintf(oct_file, "h3 = plot(b(%d:%d), %s, 'kd'); \n", x0, x1, rest_hist_name);
306
307 char new_hist_name[10];
308 snprintf(new_hist_name, sizeof(new_hist_name), "hist%d", plc_state->nbf);
309 new_hist_name[sizeof(new_hist_name) - 1] = 0;
310 octave_fprintf_array_int16(oct_file, new_hist_name, SBC_LHIST, plc_state->hist);
311 fprintf(oct_file, "h4 = plot(%s, 'r--'); \n", new_hist_name);
312
313 fprintf(oct_file, "legend ([h1, h2, h3, h4], {\"hist\", \"out\", \"rest\", \"new hist\"}, \"location\", \"northeast\");\n ");
314
315 char fig_name[1200];
316 snprintf(fig_name, sizeof(fig_name), "../%s_octave_plc_%d_%s",
317 octave_base_name, plc_state->frame_count,
318 octave_frame_type2str(octave_frame_type));
319 fig_name[sizeof(fig_name) - 1] = 0;
320 fprintf(oct_file, "print(hf, \"%s.jpg\", \"-djpg\");", fig_name);
321 }
322 #endif
323
324
btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t * plc_state,SAMPLE_FORMAT * ZIRbuf,SAMPLE_FORMAT * out)325 void btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t *plc_state, SAMPLE_FORMAT *ZIRbuf, SAMPLE_FORMAT *out){
326 float val;
327 int i;
328 float sf = 1;
329
330 plc_state->nbf++;
331 plc_state->bad_frames_nr++;
332 plc_state->frame_count++;
333
334 if (plc_state->max_consecutive_bad_frames_nr < plc_state->nbf){
335 plc_state->max_consecutive_bad_frames_nr = plc_state->nbf;
336 }
337
338 if (plc_state->nbf==1){
339 // printf("first bad frame\n");
340 // Perform pattern matching to find where to replicate
341 plc_state->bestlag = PatternMatch(plc_state->hist);
342 }
343
344 #ifdef OCTAVE_OUTPUT
345 FILE * oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_BAD);
346 if (oct_file){
347 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count);
348 }
349 #endif
350
351 if (plc_state->nbf==1){
352 // the replication begins after the template match
353 plc_state->bestlag += SBC_M;
354
355 // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet
356 sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);
357 // printf("sf Apmlitude Match %f, new data %d, bestlag+M %d\n", sf, ZIRbuf[0], plc_state->hist[plc_state->bestlag]);
358 for (i=0; i<SBC_OLAL; i++){
359 float left = ZIRbuf[i];
360 float right = sf*plc_state->hist[plc_state->bestlag+i];
361 val = (left*rcos[i]) + (right*rcos[SBC_OLAL-1-i]);
362 // val = sf*plc_state->hist[plc_state->bestlag+i];
363 plc_state->hist[SBC_LHIST+i] = crop_sample(val);
364 }
365
366 for (i=SBC_OLAL; i<SBC_FS; i++){
367 val = sf*plc_state->hist[plc_state->bestlag+i];
368 plc_state->hist[SBC_LHIST+i] = crop_sample(val);
369 }
370
371 for (i=SBC_FS; i<(SBC_FS+SBC_OLAL); i++){
372 float left = sf*plc_state->hist[plc_state->bestlag+i];
373 float right = plc_state->hist[plc_state->bestlag+i];
374 val = (left*rcos[i-SBC_FS])+(right*rcos[SBC_OLAL-1-i+SBC_FS]);
375 plc_state->hist[SBC_LHIST+i] = crop_sample(val);
376 }
377
378 for (i=(SBC_FS+SBC_OLAL); i<(SBC_FS+SBC_RT+SBC_OLAL); i++){
379 plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
380 }
381 } else {
382 // printf("succesive bad frame nr %d\n", plc_state->nbf);
383 for (i=0; i<(SBC_FS+SBC_RT+SBC_OLAL); i++){
384 plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i];
385 }
386 }
387 for (i=0; i<SBC_FS; i++){
388 out[i] = plc_state->hist[SBC_LHIST+i];
389 }
390
391 // shift the history buffer
392 for (i=0; i<(SBC_LHIST+SBC_RT+SBC_OLAL); i++){
393 plc_state->hist[i] = plc_state->hist[i+SBC_FS];
394 }
395
396 #ifdef OCTAVE_OUTPUT
397 if (oct_file){
398 octave_fprintf_plot_output(plc_state, oct_file);
399 fclose(oct_file);
400 }
401 #endif
402 }
403
btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t * plc_state,SAMPLE_FORMAT * in,SAMPLE_FORMAT * out)404 void btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t *plc_state, SAMPLE_FORMAT *in, SAMPLE_FORMAT *out){
405 float val;
406 int i = 0;
407 plc_state->good_frames_nr++;
408 plc_state->frame_count++;
409
410 #ifdef OCTAVE_OUTPUT
411 FILE * oct_file = NULL;
412 if (plc_state->nbf>0){
413 oct_file = open_octave_file(plc_state, OCTAVE_FRAME_TYPE_GOOD);
414 if (oct_file){
415 octave_fprintf_plot_history_frame(plc_state, oct_file, plc_state->frame_count);
416 }
417 }
418 #endif
419
420 if (plc_state->nbf>0){
421 for (i=0;i<SBC_RT;i++){
422 out[i] = plc_state->hist[SBC_LHIST+i];
423 }
424
425 for (i = SBC_RT;i<(SBC_RT+SBC_OLAL);i++){
426 float left = plc_state->hist[SBC_LHIST+i];
427 float right = in[i];
428 val = (left*rcos[i-SBC_RT]) + (right*rcos[SBC_OLAL+SBC_RT-1-i]);
429 out[i] = crop_sample(val);
430 }
431 }
432
433 for (;i<SBC_FS;i++){
434 out[i] = in[i];
435 }
436 // Copy the output to the history buffer
437 for (i=0;i<SBC_FS;i++){
438 plc_state->hist[SBC_LHIST+i] = out[i];
439 }
440
441 // shift the history buffer
442 for (i=0;i<SBC_LHIST;i++){
443 plc_state->hist[i] = plc_state->hist[i+SBC_FS];
444 }
445
446 #ifdef OCTAVE_OUTPUT
447 if (oct_file){
448 octave_fprintf_plot_output(plc_state, oct_file);
449 fclose(oct_file);
450 }
451 #endif
452 plc_state->nbf=0;
453 }
454
btstack_sbc_dump_statistics(btstack_sbc_plc_state_t * state)455 void btstack_sbc_dump_statistics(btstack_sbc_plc_state_t * state){
456 UNUSED(state);
457 log_info("Good frames: %d\n", state->good_frames_nr);
458 log_info("Bad frames: %d\n", state->bad_frames_nr);
459 log_info("Max Consecutive bad frames: %d\n", state->max_consecutive_bad_frames_nr);
460 }
461