1 /*
2 * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdint.h>
8 #include <stdio.h>
9 #include <stdlib.h>
10 #include <string.h>
11 #include "common/bt_target.h"
12 #include "sbc_plc.h"
13
14 #if (PLC_INCLUDED == TRUE)
15 /* msbc */
16 static const uint8_t indices0[] = { 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d,
17 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
18 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
19 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
20 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c};
21
22
23 /* 8 kHZ */
24 static const int16_t indices0_pcm[] = {
25 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
26 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
27 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
28 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
29 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
30 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
31
32 /* Raised COSine table for OLA */
33 /* 16 kHZ */
34 static float rcos[SBC_OLAL] = {
35 0.99148655f,0.96623611f,0.92510857f,0.86950446f,
36 0.80131732f,0.72286918f,0.63683150f,0.54613418f,
37 0.45386582f,0.36316850f,0.27713082f,0.19868268f,
38 0.13049554f,0.07489143f,0.03376389f,0.00851345f};
39
40 // /* 8 kHZ */
41 // static float rcos[SBC_OLAL] = {
42 // 0.96984631f,0.88302222f, 0.75f,0.58682409f,
43 // 0.41317591f, 0.25f,0.11697778f,0.09015369f};
44
45
SqrtByCarmack(const float x)46 static float SqrtByCarmack(const float x){
47 union {
48 int i;
49 float y;
50 } float_int;
51
52 float x2;
53 const float threehalfs = 1.5f;
54
55 x2 = x * 0.5f;
56 float_int.y = x;
57
58 float_int.i = 0x5f375a86 - (float_int.i >> 1);
59 float_int.y = float_int.y * (threehalfs - (x2 * float_int.y * float_int.y));
60 // float_int.y = float_int.y * (threehalfs - (x2 * float_int.y * float_int.y));
61 // float_int.y = float_int.y * (threehalfs - (x2 * float_int.y * float_int.y));
62
63 return (x * float_int.y);
64 }
65
absolute(float x)66 static float absolute(float x){
67 if (x < 0) {
68 x = -x;
69 }
70
71 return x;
72 }
73
74 /**
75 * Compute the cross correlation according to Eq. (4) of Goodman
76 * paper, except that the true correlation is used. His formula
77 * seems to be incorrect.
78 *
79 * @param x pointer to x input vector
80 * @param y pointer to y input vector
81 *
82 * @return value containing the cross-correlation of x and y
83 */
CrossCorrelation(int16_t * x,int16_t * y)84 static float CrossCorrelation(int16_t *x, int16_t *y){
85 int m;
86 float num = 0;
87 float den = 0;
88 float x2 = 0;
89 float y2 = 0;
90
91 for (m = 0; m < SBC_M; m++) {
92 num += ((float)x[m]) * y[m];
93 x2 += ((float)x[m]) * x[m];
94 y2 += ((float)y[m]) * y[m];
95 }
96 den = (float)SqrtByCarmack(x2 * y2);
97 return num / den;
98 }
99
100 /**
101 * Perform pattern matching to find the match of template with the
102 * history buffer according to Section B of Goodman paper.
103 *
104 * @param y pointer to history buffer
105 *
106 * @return the lag corresponding to the best match. The lag is
107 * with respect to the beginning of the history buffer.
108 *
109 */
PatternMatch(int16_t * y)110 static int PatternMatch(int16_t *y){
111 int n;
112 float maxCn = -999999.0; // large negative number
113 float Cn;
114 int bestmatch = 0;
115
116 for (n = 0; n < SBC_N; n++){
117 Cn = CrossCorrelation(&y[SBC_LHIST-SBC_M], &y[n]);
118 if (Cn > maxCn){
119 bestmatch = n;
120 maxCn = Cn;
121 }
122 }
123 return bestmatch;
124 }
125
126 /**
127 * Perform amplitude matching using mean-absolute-value according
128 * to Goodman paper.
129 *
130 * @param y pointer to history buffer
131 * @param bestmatch value of the lag to the best match
132 *
133 * @return scale factor
134 */
AmplitudeMatch(int16_t * y,int16_t bestmatch)135 static float AmplitudeMatch(int16_t *y, int16_t bestmatch) {
136 int i;
137 float sumx = 0;
138 float sumy = 0.000001f;
139 float sf;
140
141 for (i = 0; i < SBC_FS; i++){
142 sumx += absolute(y[SBC_LHIST - SBC_FS + i]);
143 sumy += absolute(y[bestmatch + i]);
144 }
145 sf = sumx / sumy;
146 // This is not in the paper, but limit the scaling factor to something reasonable to avoid creating artifacts
147 if (sf < 0.75f) {
148 sf = 0.75f;
149 }
150 if (sf > 1.2f) {
151 sf = 1.2f;
152 }
153 return sf;
154 }
155
crop_sample(float val)156 static int16_t crop_sample(float val){
157 float croped_val = val;
158 if (croped_val > 32767.0) croped_val= 32767.0;
159 if (croped_val < -32768.0) croped_val=-32768.0;
160 return (int16_t) croped_val;
161 }
162
163 /**
164 * Get a zero signal eSCO frame
165 * @return pointer to data buffer
166 */
sbc_plc_zero_signal_frame(void)167 uint8_t * sbc_plc_zero_signal_frame(void){
168 return (uint8_t *)&indices0;
169 }
170
171 /**
172 * Get a zero signal eSCO pcm frame
173 * @return pointer to data buffer
174 */
sbc_plc_zero_signal_frame_pcm(void)175 int16_t * sbc_plc_zero_signal_frame_pcm(void){
176 return (int16_t *)&indices0_pcm;
177 }
178
179 /**
180 * Perform PLC initialization of memory vectors.
181 *
182 * @param plc_state pointer to PLC state memory
183 */
sbc_plc_init(sbc_plc_state_t * plc_state)184 void sbc_plc_init(sbc_plc_state_t *plc_state){
185 plc_state->nbf=0;
186 plc_state->bestlag=0;
187 memset(plc_state->hist, 0, sizeof(plc_state->hist));
188 }
189
190 /**
191 * Perform PLC deinitialization of memory vectors.
192 *
193 * @param plc_state pointer to PLC state memory
194 */
sbc_plc_deinit(sbc_plc_state_t * plc_state)195 void sbc_plc_deinit(sbc_plc_state_t *plc_state){
196 plc_state->nbf=0;
197 plc_state->bestlag=0;
198 memset(plc_state->hist, 0, sizeof(plc_state->hist));
199 }
200
201 /**
202 * Perform bad frame processing.
203 *
204 * @param plc_state pointer to PLC state memory
205 * @param ZIRbuf pointer to the ZIR response of the SBC decoder
206 * @param out pointer to the output samples
207 */
sbc_plc_bad_frame(sbc_plc_state_t * plc_state,int16_t * ZIRbuf,int16_t * out)208 void sbc_plc_bad_frame(sbc_plc_state_t *plc_state, int16_t *ZIRbuf, int16_t *out){
209 int i = 0;
210 float val;
211 float sf = 1;
212
213 plc_state->nbf++;
214
215 if (plc_state->nbf == 1){
216 // Perform pattern matching to find where to replicate
217 plc_state->bestlag = PatternMatch(plc_state->hist);
218 // the replication begins after the template match
219 plc_state->bestlag += SBC_M;
220
221 // Compute Scale Factor to Match Amplitude of Substitution Packet to that of Preceding Packet
222 sf = AmplitudeMatch(plc_state->hist, plc_state->bestlag);
223
224 for (i = 0; i < SBC_OLAL; i++){
225 val = ZIRbuf[i] * rcos[i]
226 + sf * plc_state->hist[plc_state->bestlag + i] * rcos[SBC_OLAL - i - 1];
227 plc_state->hist[SBC_LHIST + i] = crop_sample(val);
228 }
229
230 for (; i < SBC_FS; i++){
231 val = sf*plc_state->hist[plc_state->bestlag + i];
232 plc_state->hist[SBC_LHIST + i] = crop_sample(val);
233 }
234
235 for (; i < SBC_FS + SBC_OLAL; i++){
236 val = sf * plc_state->hist[plc_state->bestlag + i] * rcos[i-SBC_FS]
237 + plc_state->hist[plc_state->bestlag + i] * rcos[SBC_OLAL - 1 - i + SBC_FS];
238 plc_state->hist[SBC_LHIST + i] = crop_sample(val);
239 }
240
241 for (; i < SBC_FS + SBC_RT + SBC_OLAL; i++){
242 plc_state->hist[SBC_LHIST + i] = plc_state->hist[plc_state->bestlag + i];
243 }
244 } else {
245 for ( ;i < SBC_FS + SBC_RT + SBC_OLAL; i++){
246 plc_state->hist[SBC_LHIST + i] = plc_state->hist[plc_state->bestlag + i];
247 }
248 }
249
250 for (i = 0; i < SBC_FS; i++){
251 out[i] = plc_state->hist[SBC_LHIST + i];
252 }
253
254 // shift the history buffer
255 for (i = 0; i < SBC_LHIST + SBC_RT + SBC_OLAL; i++){
256 plc_state->hist[i] = plc_state->hist[i + SBC_FS];
257 }
258 }
259
260 /**
261 * Perform good frame processing. Most of the time, this function
262 * just updates history buffers and passes the input to the output,
263 * but in the first good frame after frame loss, it must conceal the
264 * received signal as it reconverges with the true output.
265 *
266 * @param plc_state pointer to PLC state memory
267 * @param in pointer to the input vector
268 * @param out pointer to the output samples
269 */
sbc_plc_good_frame(sbc_plc_state_t * plc_state,int16_t * in,int16_t * out)270 void sbc_plc_good_frame(sbc_plc_state_t *plc_state, int16_t *in, int16_t *out){
271 int i = 0;
272
273 if (plc_state->nbf > 0){
274 for (i = 0; i < SBC_RT; i++){
275 out[i] = plc_state->hist[SBC_LHIST + i];
276 }
277
278 for (i = SBC_RT; i < SBC_RT + SBC_OLAL; i++){
279 out[i] = (int16_t)(plc_state->hist[SBC_LHIST + i] * rcos[i - SBC_RT] + in[i] * rcos[SBC_OLAL - 1 - i + SBC_RT]);
280 }
281 }
282
283 for (; i < SBC_FS; i++){
284 out[i] = in[i];
285 }
286 // Copy the output to the history buffer
287 for (i = 0; i < SBC_FS; i++){
288 plc_state->hist[SBC_LHIST + i] = out[i];
289 }
290 // shift the history buffer
291 for (i = 0; i < SBC_LHIST; i++){
292 plc_state->hist[i] = plc_state->hist[i + SBC_FS];
293 }
294
295 plc_state->nbf = 0;
296 }
297
298 #endif ///(PLC_INCLUDED == TRUE)
299
