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 * 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_sbc_plc.h" 52 53 static uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d, 54 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 55 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 56 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 57 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c}; 58 59 /* Raised COSine table for OLA */ 60 static float rcos[OLAL] = { 61 0.99148655f,0.96623611f,0.92510857f,0.86950446f, 62 0.80131732f,0.72286918f,0.63683150f,0.54613418f, 63 0.45386582f,0.36316850f,0.27713082f,0.19868268f, 64 0.13049554f,0.07489143f,0.03376389f,0.00851345f}; 65 66 static float CrossCorrelation(int16_t *x, int16_t *y); 67 static int PatternMatch(int16_t *y); 68 static float AmplitudeMatch(int16_t *y, int16_t bestmatch); 69 70 uint8_t * sbc_plc_zero_signal_frame(void){ 71 return (uint8_t *)&indices0; 72 } 73 74 void sbc_plc_init(sbc_plc_state_t *plc_state){ 75 int i; 76 plc_state->nbf=0; 77 plc_state->bestlag=0; 78 for (i=0;i<LHIST+SBCRT;i++){ 79 plc_state->hist[i] = 0; 80 } 81 82 } 83 84 void sbc_plc_bad_frame(sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out){ 85 int i; 86 float val; 87 float sf; 88 plc_state->nbf++; 89 sf=1.0f; 90 91 i=0; 92 if (plc_state->nbf==1){ 93 /* Perform pattern matching to find where to replicate */ 94 plc_state->bestlag = PatternMatch(plc_state->hist); 95 /* the replication begins after the template match */ 96 plc_state->bestlag += M; 97 98 /* Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet */ 99 sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag); 100 for (i=0;i<OLAL;i++){ 101 val = ZIRbuf[i]*rcos[i] + sf*plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-i-1]; 102 if (val > 32767.0) val= 32767.0; 103 if (val < -32768.0) val=-32768.0; 104 plc_state->hist[LHIST+i] = (int16_t)val; 105 } 106 107 for (;i<FS;i++){ 108 val = sf*plc_state->hist[plc_state->bestlag+i]; if (val > 32767.0) val= 32767.0; 109 if (val < -32768.0) val=-32768.0; plc_state->hist[LHIST+i] = (int16_t)val; 110 } 111 112 for (;i<FS+OLAL;i++){ 113 val = sf*plc_state->hist[plc_state->bestlag+i]*rcos[i-FS]+plc_state->hist[plc_state->bestlag+i]*rcos[OLAL-1-i+FS]; 114 if (val > 32767.0) val= 32767.0; 115 if (val < -32768.0) val=-32768.0; 116 plc_state->hist[LHIST+i] = (int16_t)val; 117 } 118 119 for (;i<FS+SBCRT+OLAL;i++) 120 plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 121 } else { 122 for (;i<FS;i++) 123 plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 124 for (;i<FS+SBCRT+OLAL;i++) 125 plc_state->hist[LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 126 } 127 for (i=0;i<FS;i++) 128 out[i] = plc_state->hist[LHIST+i]; 129 130 /* shift the history buffer */ 131 for (i=0;i<LHIST+SBCRT+OLAL;i++) 132 plc_state->hist[i] = plc_state->hist[i+FS]; 133 134 } 135 136 void sbc_plc_good_frame(sbc_plc_state_t *plc_state, int16_t *in, int16_t *out){ 137 int i; 138 i=0; 139 if (plc_state->nbf>0){ 140 for (i=0;i<SBCRT;i++) 141 out[i] = plc_state->hist[LHIST+i]; 142 for (;i<SBCRT+OLAL;i++) 143 out[i] = (int16_t)(plc_state->hist[LHIST+i]*rcos[i-SBCRT] + in[i]*rcos[OLAL-1-i+SBCRT]); 144 } 145 for (;i<FS;i++) 146 out[i] = in[i]; 147 /*Copy the output to the history buffer */ 148 for (i=0;i<FS;i++) 149 plc_state->hist[LHIST+i] = out[i]; 150 /* shift the history buffer */ 151 for (i=0;i<LHIST;i++) 152 plc_state->hist[i] = plc_state->hist[i+FS]; 153 plc_state->nbf=0; 154 } 155 156 157 float CrossCorrelation(int16_t *x, int16_t *y){ 158 int m; 159 float num; 160 float den; 161 float Cn; 162 float x2, y2; 163 num=0; 164 den=0; 165 x2=0.0; 166 y2=0.0; 167 for (m=0;m<M;m++){ 168 num+=((float)x[m])*y[m]; 169 x2+=((float)x[m])*x[m]; 170 y2+=((float)y[m])*y[m]; 171 } 172 den = (float)sqrt(x2*y2); 173 Cn = num/den; 174 return(Cn); 175 } 176 177 int PatternMatch(int16_t *y){ 178 int n; 179 float maxCn; 180 float Cn; 181 int bestmatch; 182 maxCn=-999999.0; /* large negative number */ 183 bestmatch=0; 184 for (n=0;n<N;n++){ 185 Cn = CrossCorrelation(&y[LHIST-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(int16_t *y, int16_t bestmatch) { 196 int i; 197 float sumx; 198 float sumy; 199 float sf; 200 sumx = 0.0; 201 sumy = 0.000001f; 202 for (i=0;i<FS;i++){ 203 sumx += abs(y[LHIST-FS+i]); 204 sumy += abs(y[bestmatch+i]); 205 } 206 sf = sumx/sumy; 207 /* This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts */ 208 if (sf<0.75f) sf=0.75f; 209 if (sf>1.2f) sf=1.2f; 210 return(sf); 211 }