Line data Source code
1 : /******************************************************************************
2 : * ETSI TS 103 634 V1.5.1 *
3 : * Low Complexity Communication Codec Plus (LC3plus) *
4 : * *
5 : * Copyright licence is solely granted through ETSI Intellectual Property *
6 : * Rights Policy, 3rd April 2019. No patent licence is granted by implication, *
7 : * estoppel or otherwise. *
8 : ******************************************************************************/
9 :
10 : #include "defines.h"
11 : #include "constants.h"
12 : #include "functions.h"
13 :
14 : #ifdef ENABLE_HR_MODE
15 : static Word16 getScaleFactor32_withNegativeScaling(Word32 *data32, Word16 dataLen);
16 : #else
17 : static Word16 getScaleFactor16_withNegativeScaling(Word16 *data16, Word16 dataLen);
18 : #endif
19 :
20 :
21 0 : void processPCapply_fx(Word16 yLen, Word16 q_old_res_fx[], Word16 *q_old_res_fx_exp,
22 : #ifdef ENABLE_HR_MODE
23 : Word32 q_res_fx[],
24 : #else
25 : Word16 q_res_fx[],
26 : #endif
27 : Word16 q_old_d_fx[], Word16 spec_inv_idx, Word16 *fac, Word16 *fac_e, Word32 q_d_fx[],
28 : Word16 *q_fx_exp, Word16 gg_idx, Word16 gg_idx_off, Word16 prev_gg, Word16 prev_gg_e,
29 : Word16 *pc_nbLostFramesInRow)
30 : {
31 : Counter i;
32 : Word16 s, s2, s3, c, tmp16, tmp16_2, inv_gain, thr;
33 : Word32 ener_curr, ener_prev, mean_nrg_high, mean_nrg_low;
34 : Word16 global_gain, global_gain_e, gg2, gg2_e, prev_gg2, prev_gg2_e;
35 : Word32 tmp32, ener_curr_gg2, ener_prev_gg2;
36 : Word16 fac_local, fac_local_e;
37 :
38 : #ifdef DYNMEM_COUNT
39 : struct _dynmem
40 : {
41 : Counter i;
42 : Word16 s, s2, s3, c, tmp16, tmp16_2, inv_gain, thr;
43 : Word32 ener_curr, ener_prev, mean_nrg_high, mean_nrg_low;
44 : Word16 global_gain, global_gain_e, gg2, gg2_e, prev_gg2, prev_gg2_e;
45 : Word32 tmp32, ener_curr_gg2, ener_prev_gg2;
46 : Word16 fac_local, fac_local_e;
47 : };
48 : Dyn_Mem_In("processPCapply_fx", sizeof(struct _dynmem));
49 : #endif
50 :
51 0 : assert(spec_inv_idx >= 0);
52 :
53 0 : *pc_nbLostFramesInRow = add(*pc_nbLostFramesInRow, 1);
54 :
55 0 : tmp32 = L_shl_pos(L_mult0(add(gg_idx, gg_idx_off), 0x797D), 7);
56 0 : global_gain_e = add(extract_l(L_shr_pos(tmp32, 25)), 1);
57 0 : global_gain = round_fx(BASOP_Util_InvLog2_lc3plus(L_or(tmp32, 0xFE000000)));
58 :
59 : /** Calculate rescaling factor **/
60 :
61 : /* mean_nrg_low = mean(q_d_prev(1:spec_inv_idx-1).^2);
62 : mean_nrg_high = mean(q_d_prev(spec_inv_idx:end).^2); */
63 0 : s = getScaleFactor16(q_old_d_fx, yLen);
64 :
65 0 : mean_nrg_low = 0;
66 0 : move32();
67 0 : FOR (i = 0; i < spec_inv_idx; i++)
68 : {
69 0 : tmp16 = shl_sat(q_old_d_fx[i], sub(s, 4));
70 0 : mean_nrg_low = L_mac0(mean_nrg_low, tmp16, tmp16); /* exp = 2s - 8 */
71 : }
72 :
73 0 : mean_nrg_high = 0;
74 0 : move32();
75 0 : FOR (i = spec_inv_idx; i < yLen; i++)
76 : {
77 0 : tmp16 = shl_sat(q_old_d_fx[i], sub(s, 4));
78 0 : mean_nrg_high = L_mac0(mean_nrg_high, tmp16, tmp16); /* exp = 2s - 8 */
79 : }
80 :
81 0 : IF (sub(spec_inv_idx, sub(yLen, spec_inv_idx)) < 0)
82 : {
83 0 : c = div_s(spec_inv_idx, sub(yLen, spec_inv_idx));
84 0 : mean_nrg_high = Mpy_32_16_lc3plus(mean_nrg_high, c); /* exp = 2s - 8 */
85 : }
86 : ELSE
87 : {
88 0 : c = div_s(sub(yLen, spec_inv_idx), spec_inv_idx);
89 0 : mean_nrg_low = Mpy_32_16_lc3plus(mean_nrg_low, c); /* exp = 2s - 8 */
90 : }
91 :
92 : /* ener_prev = sum(q_old_res(1:spec_inv_idx-1).^2);
93 : ener_curr = sum( q_res(1:spec_inv_idx-1).^2); */
94 0 : s = getScaleFactor16(q_old_res_fx, spec_inv_idx);
95 0 : ener_prev = 0; move32();
96 0 : FOR (i = 0; i < spec_inv_idx; i++)
97 : {
98 0 : tmp16 = shl_sat(q_old_res_fx[i], sub(s, 4));
99 0 : ener_prev = L_mac0(ener_prev, tmp16, tmp16); /* exp = - (2s - 8 - 2**q_old_res_fx_exp) */
100 : }
101 :
102 0 : ener_curr = 0;
103 0 : move32();
104 :
105 : #ifdef ENABLE_HR_MODE
106 0 : s2 = getScaleFactor32_lc3plus(q_res_fx, spec_inv_idx);
107 0 : FOR (i = 0; i < spec_inv_idx; i++)
108 : {
109 0 : tmp16 = extract_h(L_shl_sat(q_res_fx[i], sub(s2, 4)));
110 0 : ener_curr = L_mac0(ener_curr, tmp16, tmp16); /* exp = - (2s2 - 8) */
111 : }
112 0 : s2 = s2 - 16;
113 : #else
114 : s2 = getScaleFactor16(q_res_fx, spec_inv_idx);
115 : FOR (i = 0; i < spec_inv_idx; i++)
116 : {
117 : tmp16 = shl_sat(q_res_fx[i], sub(s2, 4));
118 : ener_curr = L_mac0(ener_curr, tmp16, tmp16); /* exp = - (2s2 - 8) */
119 : }
120 : #endif
121 :
122 :
123 0 : s = shl(sub(s, *q_old_res_fx_exp), 1);
124 0 : s2 = shl(s2, 1);
125 0 : s3 = s_max(s, s2);
126 0 : ener_prev = L_shr_sat(ener_prev, sub(s3, s2));
127 0 : ener_curr = L_shr_sat(ener_curr, sub(s3, s));
128 :
129 : /* fac = 1; */
130 0 : *fac = 1;
131 : /* if ener_prev > 0 */
132 0 : IF ( ener_prev > 0 )
133 : {
134 : /* fac = sqrt(ener_curr/ener_prev); */
135 0 : s = getScaleFactor32_lc3plus(&ener_prev, 1);
136 0 : s2 = getScaleFactor32_lc3plus(&ener_curr, 1);
137 0 : s3 = s_min(s, s2);
138 0 : tmp16 = extract_h(L_shl_sat(ener_curr, s3));
139 0 : tmp16_2 = extract_h(L_shl_sat(ener_prev, s3));
140 :
141 0 : *fac_e = 0; move16();
142 0 : if ( tmp16_2 == 0) {
143 0 : tmp16_2 = 32767; move16();
144 0 : *fac_e = 15; move16();
145 : } else {
146 0 : tmp16_2 = Inv16_lc3plus(tmp16_2, fac_e);
147 : }
148 :
149 0 : *fac = mult(tmp16, tmp16_2);
150 :
151 0 : IF (sub(*fac, 32767) < 0)
152 : {
153 0 : *fac = Sqrt16_lc3plus(*fac, fac_e); move16();
154 : }
155 : }
156 :
157 : /* fac_local = fac; */
158 0 : fac_local = *fac;
159 0 : fac_local_e = *fac_e;
160 :
161 : /* if (mean_nrg_low > mean_nrg_high) && (ener_prev * prev_gg^2 > ener_curr * gg^2) */
162 0 : prev_gg2 = mult(prev_gg, prev_gg);
163 0 : prev_gg2_e = shl(prev_gg_e, 1);
164 0 : ener_prev_gg2 = Mpy_32_16_lc3plus(ener_prev, prev_gg2); /* exp = prev_gg2_e */
165 :
166 0 : gg2 = mult(global_gain, global_gain);
167 0 : gg2_e = shl(global_gain_e, 1);
168 0 : ener_curr_gg2 = Mpy_32_16_lc3plus(ener_curr, gg2); /* exp = gg2_e */
169 :
170 0 : s3 = s_max(prev_gg2_e, gg2_e);
171 0 : ener_prev_gg2 = L_shr_sat(ener_prev_gg2, sub(s3, prev_gg2_e));
172 0 : ener_curr_gg2 = L_shr_sat(ener_curr_gg2, sub(s3, gg2_e));
173 :
174 :
175 0 : test();
176 0 : IF ( (L_sub(mean_nrg_low, mean_nrg_high) <= 0) || (L_sub(ener_prev_gg2, ener_curr_gg2) <= 0) )
177 : {
178 : /* fac = prev_gg/gg; */
179 0 : s = global_gain_e; move16();
180 0 : inv_gain = Inv16_lc3plus(global_gain, &s);
181 0 : fac_local = mult(prev_gg, inv_gain);
182 0 : fac_local_e = add(s, prev_gg_e);
183 : }
184 :
185 : /* write synthesized samples */
186 0 : *q_old_res_fx_exp = add(*q_old_res_fx_exp, fac_local_e);
187 0 : thr = shl_sat(20480, sub(-15, *q_old_res_fx_exp));
188 0 : FOR (i = spec_inv_idx; i < yLen; i++)
189 : {
190 0 : q_res_fx[i] = extract_h(L_mult(q_old_res_fx[i] /* exp = q_old_res_fx_exp' */, fac_local /* exp = fac_e */)); /* exp = q_old_res_fx_exp */
191 :
192 0 : IF (sub(abs_s(q_res_fx[i]), thr) < 0)
193 : {
194 0 : q_res_fx[i] = 0;
195 0 : move16();
196 : }
197 : }
198 :
199 : #ifdef ENABLE_HR_MODE
200 0 : s = getScaleFactor32_withNegativeScaling(&q_res_fx[0], spec_inv_idx) - 16; /* exp = 0 */
201 0 : s2 = getScaleFactor32_withNegativeScaling(&q_res_fx[spec_inv_idx],
202 0 : sub(yLen, spec_inv_idx)) - 16; /* exp = q_old_res_fx_exp */
203 : #else
204 : s = getScaleFactor16_withNegativeScaling(&q_res_fx[0], spec_inv_idx); /* exp = 0 */
205 : s2 = getScaleFactor16_withNegativeScaling(&q_res_fx[spec_inv_idx],
206 : sub(yLen, spec_inv_idx)); /* exp = q_old_res_fx_exp */
207 : #endif
208 :
209 0 : s3 = add(s, *q_old_res_fx_exp);
210 0 : IF (sub(s3, s2) > 0)
211 : {
212 0 : tmp16 = sub(s3, s2);
213 0 : s = sub(s, tmp16);
214 0 : s3 = sub(s3, tmp16);
215 : }
216 0 : *q_fx_exp = sub(15, s);
217 0 : move16();
218 :
219 : #ifdef ENABLE_HR_MODE
220 0 : s = add(s, 16);
221 0 : s3 = add(s3, 16);
222 : #endif
223 :
224 0 : s = s_max(s, -31);
225 0 : s = s_min(s, 31);
226 0 : s3 = s_max(s3, -31);
227 0 : s3 = s_min(s3, 31);
228 :
229 0 : FOR (i = 0; i < spec_inv_idx; i++)
230 : {
231 : #ifdef ENABLE_HR_MODE
232 0 : q_d_fx[i] = L_shl(q_res_fx[i], s);
233 : #else
234 : q_d_fx[i] = L_shl(L_deposit_h(q_res_fx[i]), s);
235 : #endif
236 0 : move32();
237 : }
238 0 : FOR (; i < yLen; i++)
239 : {
240 : #ifdef ENABLE_HR_MODE
241 0 : q_d_fx[i] = L_shl(q_res_fx[i], s3);
242 : #else
243 : q_d_fx[i] = L_shl(L_deposit_h(q_res_fx[i]), s3);
244 : #endif
245 0 : move32();
246 : }
247 :
248 :
249 : #ifdef DYNMEM_COUNT
250 : Dyn_Mem_Out();
251 : #endif
252 0 : }
253 :
254 : #ifndef ENABLE_HR_MODE
255 : static Word16 getScaleFactor16_withNegativeScaling(Word16 *data16, Word16 dataLen)
256 : {
257 : Counter i;
258 : Dyn_Mem_Deluxe_In(Word16 tmp, shift; Word16 x_min, x_max;);
259 :
260 : x_max = 0;
261 : move16();
262 : x_min = 0;
263 : move16();
264 :
265 : FOR (i = 0; i < dataLen; i++)
266 : {
267 : if (data16[i] > 0)
268 : x_max = s_max(x_max, data16[i]);
269 : if (data16[i] < 0)
270 : x_min = s_min(x_min, data16[i]);
271 : }
272 :
273 : tmp = s_max(x_max, negate(x_min));
274 : shift = norm_s(tmp);
275 : if (tmp == 0)
276 : {
277 : shift = 15;
278 : move16();
279 : }
280 :
281 : Dyn_Mem_Deluxe_Out();
282 :
283 : return shift;
284 : }
285 :
286 : #else
287 0 : static Word16 getScaleFactor32_withNegativeScaling(Word32 *data32, Word16 dataLen)
288 : {
289 : Counter i;
290 : Dyn_Mem_Deluxe_In(Word32 tmp, shift; Word32 x_min, x_max;);
291 :
292 0 : x_max = L_add(0, 0);
293 0 : x_min = L_add(0, 0);
294 :
295 0 : FOR (i = 0; i < dataLen; i++)
296 : {
297 0 : if (data32[i] >= 0)
298 0 : x_max = L_max(x_max, data32[i]);
299 0 : if (data32[i] < 0)
300 0 : x_min = L_min(x_min, data32[i]);
301 : }
302 :
303 0 : tmp = L_max(x_max, L_negate(x_min));
304 0 : shift = norm_l(tmp);
305 0 : if (tmp == 0)
306 : {
307 0 : shift = 31;
308 0 : move16();
309 : }
310 :
311 : Dyn_Mem_Deluxe_Out();
312 :
313 0 : return shift;
314 : }
315 : #endif
|
