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_sbc_plc.c" 39 40 /* 41 * btstack_sbc_plc.c 42 * 43 */ 44 45 #include <stdint.h> 46 #include <stdio.h> 47 #include <stdlib.h> 48 #include <string.h> 49 50 #include "btstack_sbc_plc.h" 51 52 #define SAMPLE_FORMAT int16_t 53 54 static uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 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, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 58 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c}; 59 60 /* Raised COSine table for OLA */ 61 static float rcos[SBC_OLAL] = { 62 0.99148655f,0.96623611f,0.92510857f,0.86950446f, 63 0.80131732f,0.72286918f,0.63683150f,0.54613418f, 64 0.45386582f,0.36316850f,0.27713082f,0.19868268f, 65 0.13049554f,0.07489143f,0.03376389f,0.00851345f}; 66 67 // taken from http://www.codeproject.com/Articles/69941/Best-Square-Root-Method-Algorithm-Function-Precisi 68 // Algorithm: Babylonian Method + some manipulations on IEEE 32 bit floating point representation 69 static float sqrt3(const float x){ 70 union { 71 int i; 72 float x; 73 } u; 74 u.x = x; 75 u.i = (1<<29) + (u.i >> 1) - (1<<22); 76 77 // Two Babylonian Steps (simplified from:) 78 // u.x = 0.5f * (u.x + x/u.x); 79 // u.x = 0.5f * (u.x + x/u.x); 80 u.x = u.x + x/u.x; 81 u.x = 0.25f*u.x + x/u.x; 82 83 return u.x; 84 } 85 86 static float absolute(float x){ 87 if (x < 0) x = -x; 88 return x; 89 } 90 91 static float CrossCorrelation(SAMPLE_FORMAT *x, SAMPLE_FORMAT *y){ 92 float num = 0; 93 float den = 0; 94 float x2 = 0; 95 float y2 = 0; 96 int m; 97 for (m=0;m<SBC_M;m++){ 98 num+=((float)x[m])*y[m]; 99 x2+=((float)x[m])*x[m]; 100 y2+=((float)y[m])*y[m]; 101 } 102 den = (float)sqrt3(x2*y2); 103 return num/den; 104 } 105 106 static int PatternMatch(SAMPLE_FORMAT *y){ 107 float maxCn = -999999.0; // large negative number 108 int bestmatch = 0; 109 float Cn; 110 int n; 111 for (n=0;n<SBC_N;n++){ 112 Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M], &y[n]); 113 if (Cn>maxCn){ 114 bestmatch=n; 115 maxCn = Cn; 116 } 117 } 118 return bestmatch; 119 } 120 121 static float AmplitudeMatch(SAMPLE_FORMAT *y, SAMPLE_FORMAT bestmatch) { 122 int i; 123 float sumx = 0; 124 float sumy = 0.000001f; 125 float sf; 126 127 for (i=0;i<SBC_FS;i++){ 128 sumx += absolute(y[SBC_LHIST-SBC_FS+i]); 129 sumy += absolute(y[bestmatch+i]); 130 } 131 sf = sumx/sumy; 132 // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts 133 if (sf<0.75f) sf=0.75f; 134 if (sf>1.2f) sf=1.2f; 135 return sf; 136 } 137 138 static SAMPLE_FORMAT crop_sample(float val){ 139 float croped_val = val; 140 if (croped_val > 32767.0) croped_val= 32767.0; 141 if (croped_val < -32768.0) croped_val=-32768.0; 142 return (SAMPLE_FORMAT) croped_val; 143 } 144 145 uint8_t * btstack_sbc_plc_zero_signal_frame(void){ 146 return (uint8_t *)&indices0; 147 } 148 149 void btstack_sbc_plc_init(btstack_sbc_plc_state_t *plc_state){ 150 plc_state->nbf=0; 151 plc_state->bestlag=0; 152 memset(plc_state->hist,0,sizeof(plc_state->hist)); 153 } 154 155 void btstack_sbc_plc_bad_frame(btstack_sbc_plc_state_t *plc_state, SAMPLE_FORMAT *ZIRbuf, SAMPLE_FORMAT *out){ 156 float val; 157 int i = 0; 158 float sf = 1; 159 plc_state->nbf++; 160 161 if (plc_state->nbf==1){ 162 // Perform pattern matching to find where to replicate 163 plc_state->bestlag = PatternMatch(plc_state->hist); 164 // the replication begins after the template match 165 plc_state->bestlag += SBC_M; 166 167 // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet 168 sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag); 169 for (i=0;i<SBC_OLAL;i++){ 170 float left = ZIRbuf[i]; 171 float right = sf*plc_state->hist[plc_state->bestlag+i]; 172 val = left*rcos[i] + right*rcos[SBC_OLAL-1-i]; 173 plc_state->hist[SBC_LHIST+i] = crop_sample(val); 174 } 175 176 for (;i<SBC_FS;i++){ 177 val = sf*plc_state->hist[plc_state->bestlag+i]; 178 plc_state->hist[SBC_LHIST+i] = crop_sample(val); 179 } 180 181 for (;i<SBC_FS+SBC_OLAL;i++){ 182 float left = sf*plc_state->hist[plc_state->bestlag+i]; 183 float right = plc_state->hist[plc_state->bestlag+i]; 184 val = left*rcos[i-SBC_FS]+right*rcos[SBC_OLAL-1-i+SBC_FS]; 185 plc_state->hist[SBC_LHIST+i] = crop_sample(val); 186 } 187 188 for (;i<SBC_FS+SBC_RT+SBC_OLAL;i++){ 189 plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 190 } 191 } else { 192 for (;i<SBC_FS+SBC_RT+SBC_OLAL;i++){ 193 plc_state->hist[SBC_LHIST+i] = plc_state->hist[plc_state->bestlag+i]; 194 } 195 } 196 for (i=0;i<SBC_FS;i++){ 197 out[i] = plc_state->hist[SBC_LHIST+i]; 198 } 199 200 // shift the history buffer 201 for (i=0;i<SBC_LHIST+SBC_RT+SBC_OLAL;i++){ 202 plc_state->hist[i] = plc_state->hist[i+SBC_FS]; 203 } 204 } 205 206 void btstack_sbc_plc_good_frame(btstack_sbc_plc_state_t *plc_state, SAMPLE_FORMAT *in, SAMPLE_FORMAT *out){ 207 float val; 208 int i = 0; 209 if (plc_state->nbf>0){ 210 for (i=0;i<SBC_RT;i++){ 211 out[i] = plc_state->hist[SBC_LHIST+i]; 212 } 213 214 for (i = SBC_RT;i<SBC_RT+SBC_OLAL;i++){ 215 float left = plc_state->hist[SBC_LHIST+i]; 216 float right = in[i]; 217 val = left*rcos[i-SBC_RT] + right*rcos[SBC_OLAL+SBC_RT-1-i]; 218 out[i] = (SAMPLE_FORMAT)val; 219 } 220 } 221 222 for (;i<SBC_FS;i++){ 223 out[i] = in[i]; 224 } 225 // Copy the output to the history buffer 226 for (i=0;i<SBC_FS;i++){ 227 plc_state->hist[SBC_LHIST+i] = out[i]; 228 } 229 // shift the history buffer 230 for (i=0;i<SBC_LHIST;i++){ 231 plc_state->hist[i] = plc_state->hist[i+SBC_FS]; 232 } 233 234 plc_state->nbf=0; 235 } 236