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