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 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 96 static float absolute(float x){ 97 if (x < 0) x = -x; 98 return x; 99 } 100 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 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 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 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 155 uint8_t * btstack_sbc_plc_zero_signal_frame(void){ 156 return (uint8_t *)&indices0; 157 } 158 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 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 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 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 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 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 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 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 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 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