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