1 /* Copyright 2019 The Chromium OS Authors. All rights reserved.
2 * Use of this source code is governed by a BSD-style license that can be
3 * found in the LICENSE file.
4 */
5
6 #include <float.h>
7 #include <math.h>
8 #include <stdlib.h>
9 #include <string.h>
10 #include <stdio.h>
11
12 #include "cras_plc.h"
13
14 #define MSBC_SAMPLE_SIZE 2 /* 2 bytes*/
15 #define MSBC_PKT_LEN 57 /* Packet length without the header */
16 #define MSBC_FS 120 /* Frame Size */
17 #define MSBC_CODE_SIZE 240 /* MSBC_SAMPLE_SIZE * MSBC_FS */
18
19 #define PLC_WL 256 /* 16ms - Window Length for pattern matching */
20 #define PLC_TL 64 /* 4ms - Template Length for matching */
21 #define PLC_HL (PLC_WL + MSBC_FS - 1) /* Length of History buffer required */
22 #define PLC_SBCRL 36 /* SBC Reconvergence sample Length */
23 #define PLC_OLAL 16 /* OverLap-Add Length */
24
25 #define PLC_WINDOW_SIZE 5
26 #define PLC_PL_THRESHOLD 2
27
28 /* The pre-computed zero input bit stream of mSBC codec, per HFP 1.7 spec.
29 * This mSBC frame will be decoded into all-zero input PCM. */
30 static const uint8_t msbc_zero_frame[] = {
31 0xad, 0x00, 0x00, 0xc5, 0x00, 0x00, 0x00, 0x00, 0x77, 0x6d, 0xb6, 0xdd,
32 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d, 0xb6,
33 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77, 0x6d,
34 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6d, 0xdd, 0xb6, 0xdb, 0x77,
35 0x6d, 0xb6, 0xdd, 0xdb, 0x6d, 0xb7, 0x76, 0xdb, 0x6c
36 };
37
38 /* Raised Cosine table for OLA */
39 static const float rcos[PLC_OLAL] = { 0.99148655f, 0.96623611f, 0.92510857f,
40 0.86950446f, 0.80131732f, 0.72286918f,
41 0.63683150f, 0.54613418f, 0.45386582f,
42 0.36316850f, 0.27713082f, 0.19868268f,
43 0.13049554f, 0.07489143f, 0.03376389f,
44 0.00851345f };
45
46 /* This structure tracks the packet loss information for last PLC_WINDOW_SIZE
47 * of packets:
48 * loss_hist - The packet loss history of receiving packets. 1 means lost.
49 * ptr - The index of the to be updated packet loss status.
50 * count - The count of lost packets in the window.
51 */
52 struct packet_window {
53 uint8_t loss_hist[PLC_WINDOW_SIZE];
54 unsigned int ptr;
55 unsigned int count;
56 };
57
58 /* The PLC is specifically designed for mSBC. The algorithm searches the
59 * history of receiving samples to find the best match samples and constructs
60 * substitutions for the lost samples. The selection is based on pattern
61 * matching a template, composed of a length of samples preceding to the lost
62 * samples. It then uses the following samples after the best match as the
63 * replacement samples and applies Overlap-Add to reduce the audible
64 * distortion.
65 *
66 * This structure holds related info needed to conduct the PLC algorithm.
67 * Members:
68 * hist - The history buffer for receiving samples, we also use it to
69 * buffer the processed replacement samples.
70 * best_lag - The index of the best substitution samples in sample history.
71 * handled_bad_frames - Number of bad frames handled since the last good
72 * frame.
73 * zero_frame - A buffer used for storing the samples from decoding the
74 * mSBC zero frame packet.
75 * pl_window - A window monitoring how many packets are bad within the recent
76 * PLC_WINDOW_SIZE of packets. This is used to determine if we
77 * want to disable the PLC temporarily.
78 */
79 struct cras_msbc_plc {
80 int16_t hist[PLC_HL + MSBC_FS + PLC_SBCRL + PLC_OLAL];
81 unsigned int best_lag;
82 int handled_bad_frames;
83 int16_t zero_frame[MSBC_FS];
84 struct packet_window *pl_window;
85 };
86
cras_msbc_plc_create()87 struct cras_msbc_plc *cras_msbc_plc_create()
88 {
89 struct cras_msbc_plc *plc =
90 (struct cras_msbc_plc *)calloc(1, sizeof(*plc));
91 plc->pl_window =
92 (struct packet_window *)calloc(1, sizeof(*plc->pl_window));
93 return plc;
94 }
95
cras_msbc_plc_destroy(struct cras_msbc_plc * plc)96 void cras_msbc_plc_destroy(struct cras_msbc_plc *plc)
97 {
98 free(plc->pl_window);
99 free(plc);
100 }
101
f_to_s16(float input)102 static int16_t f_to_s16(float input)
103 {
104 return input > INT16_MAX ?
105 INT16_MAX :
106 input < INT16_MIN ? INT16_MIN : (int16_t)input;
107 }
108
overlap_add(int16_t * output,float scaler_d,const int16_t * desc,float scaler_a,const int16_t * asc)109 void overlap_add(int16_t *output, float scaler_d, const int16_t *desc,
110 float scaler_a, const int16_t *asc)
111 {
112 for (int i = 0; i < PLC_OLAL; i++) {
113 output[i] =
114 f_to_s16(scaler_d * desc[i] * rcos[i] +
115 scaler_a * asc[i] * rcos[PLC_OLAL - 1 - i]);
116 }
117 }
118
update_plc_state(struct packet_window * w,uint8_t is_packet_loss)119 void update_plc_state(struct packet_window *w, uint8_t is_packet_loss)
120 {
121 uint8_t *curr = &w->loss_hist[w->ptr];
122 if (is_packet_loss != *curr) {
123 w->count += (is_packet_loss - *curr);
124 *curr = is_packet_loss;
125 }
126 w->ptr = (w->ptr + 1) % PLC_WINDOW_SIZE;
127 }
128
possibly_pause_plc(struct packet_window * w)129 int possibly_pause_plc(struct packet_window *w)
130 {
131 /* The packet loss count comes from a time window and we use it as an
132 * indicator of our confidence of the PLC algorithm. It is known to
133 * generate poorer and robotic feeling sounds, when the majority of
134 * samples in the PLC history buffer are from the concealment results.
135 */
136 return w->count >= PLC_PL_THRESHOLD;
137 }
138
cras_msbc_plc_handle_good_frames(struct cras_msbc_plc * state,const uint8_t * input,uint8_t * output)139 int cras_msbc_plc_handle_good_frames(struct cras_msbc_plc *state,
140 const uint8_t *input, uint8_t *output)
141 {
142 int16_t *frame_head, *input_samples, *output_samples;
143 if (state->handled_bad_frames == 0) {
144 /* If there was no packet concealment before this good frame,
145 * we just simply copy the input to output without reconverge.
146 */
147 memmove(output, input, MSBC_FS * MSBC_SAMPLE_SIZE);
148 } else {
149 frame_head = &state->hist[PLC_HL];
150 input_samples = (int16_t *)input;
151 output_samples = (int16_t *)output;
152
153 /* For the first good frame after packet loss, we need to
154 * conceal the received samples to have it reconverge with the
155 * true output.
156 */
157 memcpy(output_samples, frame_head,
158 PLC_SBCRL * MSBC_SAMPLE_SIZE);
159 overlap_add(&output_samples[PLC_SBCRL], 1.0,
160 &frame_head[PLC_SBCRL], 1.0,
161 &input_samples[PLC_SBCRL]);
162 memmove(&output_samples[PLC_SBCRL + PLC_OLAL],
163 &input_samples[PLC_SBCRL + PLC_OLAL],
164 (MSBC_FS - PLC_SBCRL - PLC_OLAL) * MSBC_SAMPLE_SIZE);
165 state->handled_bad_frames = 0;
166 }
167
168 /* Shift the history and update the good frame to the end of it. */
169 memmove(state->hist, &state->hist[MSBC_FS],
170 (PLC_HL - MSBC_FS) * MSBC_SAMPLE_SIZE);
171 memcpy(&state->hist[PLC_HL - MSBC_FS], output,
172 MSBC_FS * MSBC_SAMPLE_SIZE);
173 update_plc_state(state->pl_window, 0);
174 return MSBC_CODE_SIZE;
175 }
176
cross_correlation(int16_t * x,int16_t * y)177 float cross_correlation(int16_t *x, int16_t *y)
178 {
179 float sum = 0, x2 = 0, y2 = 0;
180
181 for (int i = 0; i < PLC_TL; i++) {
182 sum += ((float)x[i]) * y[i];
183 x2 += ((float)x[i]) * x[i];
184 y2 += ((float)y[i]) * y[i];
185 }
186 return sum / sqrtf(x2 * y2);
187 }
188
pattern_match(int16_t * hist)189 int pattern_match(int16_t *hist)
190 {
191 int best = 0;
192 float cn, max_cn = FLT_MIN;
193
194 for (int i = 0; i < PLC_WL; i++) {
195 cn = cross_correlation(&hist[PLC_HL - PLC_TL], &hist[i]);
196 if (cn > max_cn) {
197 best = i;
198 max_cn = cn;
199 }
200 }
201 return best;
202 }
203
amplitude_match(int16_t * x,int16_t * y)204 float amplitude_match(int16_t *x, int16_t *y)
205 {
206 uint32_t sum_x = 0, sum_y = 0;
207 float scaler;
208 for (int i = 0; i < MSBC_FS; i++) {
209 sum_x += abs(x[i]);
210 sum_y += abs(y[i]);
211 }
212
213 if (sum_y == 0)
214 return 1.2f;
215
216 scaler = (float)sum_x / sum_y;
217 return scaler > 1.2f ? 1.2f : scaler < 0.75f ? 0.75f : scaler;
218 }
219
cras_msbc_plc_handle_bad_frames(struct cras_msbc_plc * state,struct cras_audio_codec * codec,uint8_t * output)220 int cras_msbc_plc_handle_bad_frames(struct cras_msbc_plc *state,
221 struct cras_audio_codec *codec,
222 uint8_t *output)
223 {
224 float scaler;
225 int16_t *best_match_hist;
226 int16_t *frame_head = &state->hist[PLC_HL];
227 size_t pcm_decoded = 0;
228
229 /* mSBC codec is stateful, the history of signal would contribute to the
230 * decode result state->zero_frame.
231 */
232 codec->decode(codec, msbc_zero_frame, MSBC_PKT_LEN, state->zero_frame,
233 MSBC_FS, &pcm_decoded);
234
235 /* The PLC algorithm is more likely to generate bad results that sound
236 * robotic after severe packet losses happened. Only applying it when
237 * we are confident.
238 */
239 if (!possibly_pause_plc(state->pl_window)) {
240 if (state->handled_bad_frames == 0) {
241 /* Finds the best matching samples and amplitude */
242 state->best_lag = pattern_match(state->hist) + PLC_TL;
243 best_match_hist = &state->hist[state->best_lag];
244 scaler = amplitude_match(&state->hist[PLC_HL - MSBC_FS],
245 best_match_hist);
246
247 /* Constructs the substitution samples */
248 overlap_add(frame_head, 1.0, state->zero_frame, scaler,
249 best_match_hist);
250 for (int i = PLC_OLAL; i < MSBC_FS; i++)
251 state->hist[PLC_HL + i] =
252 f_to_s16(scaler * best_match_hist[i]);
253 overlap_add(&frame_head[MSBC_FS], scaler,
254 &best_match_hist[MSBC_FS], 1.0,
255 &best_match_hist[MSBC_FS]);
256
257 memmove(&frame_head[MSBC_FS + PLC_OLAL],
258 &best_match_hist[MSBC_FS + PLC_OLAL],
259 PLC_SBCRL * MSBC_SAMPLE_SIZE);
260 } else {
261 memmove(frame_head, &state->hist[state->best_lag],
262 (MSBC_FS + PLC_SBCRL + PLC_OLAL) *
263 MSBC_SAMPLE_SIZE);
264 }
265 state->handled_bad_frames++;
266 } else {
267 /* This is a case similar to receiving a good frame with all
268 * zeros, we set handled_bad_frames to zero to prevent the
269 * following good frame from being concealed to reconverge with
270 * the zero frames we fill in. The concealment result sounds
271 * more artificial and weird than simply writing zeros and
272 * following samples.
273 */
274 memmove(frame_head, state->zero_frame, MSBC_CODE_SIZE);
275 memset(frame_head + MSBC_CODE_SIZE, 0,
276 (PLC_SBCRL + PLC_OLAL) * MSBC_SAMPLE_SIZE);
277 state->handled_bad_frames = 0;
278 }
279
280 memcpy(output, frame_head, MSBC_CODE_SIZE);
281 memmove(state->hist, &state->hist[MSBC_FS],
282 (PLC_HL + PLC_SBCRL + PLC_OLAL) * MSBC_SAMPLE_SIZE);
283 update_plc_state(state->pl_window, 1);
284 return MSBC_CODE_SIZE;
285 }
286