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