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 #include <stdint.h> 44 #include <stdio.h> 45 #include <stdlib.h> 46 #include <string.h> 47 #include <fcntl.h> 48 #include <unistd.h> 49 #include <math.h> 50 51 #include "btstack_cvsd_plc.h" 52 53 static float rcos[CVSD_OLAL] = { 54 0.99148655,0.96623611,0.92510857,0.86950446, 55 0.80131732,0.72286918,0.63683150,0.54613418, 56 0.45386582,0.36316850,0.27713082,0.19868268, 57 0.13049554,0.07489143,0.03376389,0.00851345}; 58 59 static float CrossCorrelation(int8_t *x, int8_t *y); 60 static int PatternMatch(int8_t *y); 61 static float AmplitudeMatch(int8_t *y, int8_t bestmatch); 62 63 static int8_t crop_to_int8(float val){ 64 float croped_val = 0; 65 if (val > 127.0) croped_val= 127.0; 66 if (val < -128.0) croped_val=-128.0; 67 return (int8_t) croped_val; 68 } 69 70 void btstack_cvsd_plc_init(btstack_cvsd_plc_state_t *plc_state){ 71 plc_state->nbf = 0; 72 plc_state->bestlag = 0; 73 memset(plc_state->hist, 0, sizeof(plc_state->hist)); 74 } 75 76 void btstack_cvsd_plc_bad_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *out){ 77 float val; 78 float sf = 1; 79 int i = 0; 80 81 plc_state->nbf++; 82 83 if (plc_state->nbf==1){ 84 /* Perform pattern matching to find where to replicate */ 85 plc_state->bestlag = PatternMatch(plc_state->hist); 86 87 /* the replication begins after the template match */ 88 plc_state->bestlag += CVSD_M; 89 90 /* Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet */ 91 sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag); 92 93 for (i=0;i<CVSD_OLAL;i++){ 94 float left = plc_state->hist[CVSD_LHIST-1]; 95 float right = sf*plc_state->hist[plc_state->bestlag+i]; 96 val = left * rcos[i] + right *rcos[CVSD_OLAL-1-i]; 97 plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val); 98 } 99 100 for (i=CVSD_OLAL;i<CVSD_FS;i++){ 101 val = sf*plc_state->hist[plc_state->bestlag+i]; 102 plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val); 103 } 104 105 for (i=CVSD_FS;i<CVSD_FS+CVSD_OLAL;i++){ 106 float left = sf*plc_state->hist[plc_state->bestlag+i]; 107 float right = plc_state->hist[plc_state->bestlag+i]; 108 val = left * rcos[i-CVSD_FS] + right *rcos[CVSD_OLAL+CVSD_FS-1-i]; 109 plc_state->hist[CVSD_LHIST+i] = crop_to_int8(val); 110 } 111 112 for (i=CVSD_FS+CVSD_OLAL;i<CVSD_FS+CVSD_OLAL+CVSD_RT;i++) 113 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 114 115 } else { 116 for (i=0;i<CVSD_FS+CVSD_RT+CVSD_OLAL;i++) 117 plc_state->hist[CVSD_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 118 } 119 120 for (i=0;i<CVSD_FS;i++){ 121 out[i] = plc_state->hist[CVSD_LHIST+i]; 122 } 123 124 /* shift the history buffer */ 125 for (i=0;i<CVSD_LHIST+CVSD_RT+CVSD_OLAL;i++){ 126 plc_state->hist[i] = plc_state->hist[i+CVSD_FS]; 127 } 128 } 129 130 void btstack_cvsd_plc_good_frame(btstack_cvsd_plc_state_t *plc_state, int8_t *in, int8_t *out){ 131 float val; 132 int i = 0; 133 if (plc_state->nbf>0){ 134 for (i=0;i<CVSD_RT;i++){ 135 out[i] = plc_state->hist[CVSD_LHIST+i]; 136 } 137 138 for (i=CVSD_RT;i<CVSD_RT+CVSD_OLAL;i++){ 139 float left = plc_state->hist[CVSD_LHIST+i]; 140 float right = in[i]; 141 val = left * rcos[i-CVSD_RT] + right *rcos[CVSD_OLAL+CVSD_RT-1-i]; 142 out[i] = crop_to_int8(val); 143 } 144 } 145 146 for (;i<CVSD_FS;i++){ 147 out[i] = in[i]; 148 } 149 150 /*Copy the output to the history buffer */ 151 for (i=0;i<CVSD_FS;i++){ 152 plc_state->hist[CVSD_LHIST+i] = out[i]; 153 } 154 155 /* shift the history buffer */ 156 for (i=0;i<CVSD_LHIST;i++){ 157 plc_state->hist[i] = plc_state->hist[i+CVSD_FS]; 158 } 159 160 plc_state->nbf=0; 161 } 162 163 164 float CrossCorrelation(int8_t *x, int8_t *y){ 165 float num = 0; 166 float den = 0; 167 float x2 = 0; 168 float y2 = 0; 169 int m; 170 for (m=0;m<CVSD_M;m++){ 171 num+=((float)x[m])*y[m]; 172 x2+=((float)x[m])*x[m]; 173 y2+=((float)y[m])*y[m]; 174 } 175 den = (float)sqrt(x2*y2); 176 return num/den; 177 } 178 179 int PatternMatch(int8_t *y){ 180 float maxCn = -999999.0; /* large negative number */ 181 int bestmatch = 0; 182 float Cn; 183 int n; 184 for (n=0;n<CVSD_N;n++){ 185 Cn = CrossCorrelation(&y[CVSD_LHIST-CVSD_M] /* x */, &y[n]); 186 if (Cn>maxCn){ 187 bestmatch=n; 188 maxCn = Cn; 189 } 190 } 191 return bestmatch; 192 } 193 194 195 float AmplitudeMatch(int8_t *y, int8_t bestmatch) { 196 int i; 197 float sumx = 0; 198 float sumy = 0.000001f; 199 float sf; 200 201 for (i=0;i<CVSD_FS;i++){ 202 sumx += abs(y[CVSD_LHIST-CVSD_FS+i]); 203 sumy += abs(y[bestmatch+i]); 204 } 205 sf = sumx/sumy; 206 /* This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts */ 207 if (sf<0.75f) sf=0.75f; 208 if (sf>1.2f) sf=1.2f; 209 return sf; 210 }