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