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