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 "functions.h"
11 :
12 : /*************************************************************************/
13 :
14 :
15 0 : void process_olpa_fx(Word16 *mem_s6k4_exp, Word16 mem_s12k8[], Word16 mem_s6k4[], Word16 *pitch, Word16 *s12k8,
16 : Word16 len, Word16 *normcorr, Word16 *mem_pitch,
17 : Word16 *pitch_flag,
18 : Word16 s12k8_exp, Word16 frame_dms, Word8 *scratchBuffer)
19 : {
20 : Word32 sum, sum0, sum1, sum2, prod, inv;
21 : Word16 shift, s6k4_exp, prod_exp, min_pitch, max_pitch;
22 : Word16 scale0, scale1, scale2, pitch2, normcorr2, len2, acflen, mem_in_len;
23 : Word32 max32;
24 : Word32 *ac;
25 : Word16 *s6k4;
26 : Counter n;
27 :
28 : Counter m;
29 : Word32 L_tmp, L_tmp2;
30 :
31 :
32 : #ifdef DYNMEM_COUNT
33 : Dyn_Mem_In("process_olpa_fx", sizeof(struct {
34 : Word32 sum, sum0, sum1, sum2, prod, inv;
35 : Word16 shift, s6k4_exp, prod_exp, min_pitch, max_pitch;
36 : Word16 scale0, scale1, scale2, pitch2, normcorr2, len2, acflen, mem_in_len;
37 : Word32 max32;
38 : Word32 *ac;
39 : Word16 *s6k4;
40 : Counter n;
41 : Word32 sums[3];
42 : Counter m;
43 : Word32 L_tmp, L_tmp2;
44 : }));
45 : #endif
46 :
47 : /* Buffer alignment */
48 0 : ac = (Word32 *)scratchAlign(scratchBuffer, 0); /* Size = 4 * RANGE_PITCH_6K4 = 392 bytes */
49 :
50 : /* Downsample input signal by a factor of 2 (12.8kHz -> 6.4kHz) */
51 0 : mem_in_len = MAX_PITCH_6K4; move16();
52 0 : len2 = shr(len, 1);
53 0 : acflen = len2; move16();
54 :
55 0 : SWITCH(frame_dms)
56 : {
57 0 : case 50:
58 0 : mem_in_len = add(mem_in_len, 32);
59 0 : acflen = add(acflen, 32);
60 0 : break;
61 :
62 0 : case 25:
63 0 : mem_in_len = add(mem_in_len, 48);
64 0 : acflen = add(acflen, 48);
65 0 : break;
66 : }
67 :
68 0 : s6k4 = mem_s6k4 + mem_in_len;
69 0 : sum = L_mac(L_mac(L_mult(mem_s12k8[0], 4053), mem_s12k8[1], 7712), mem_s12k8[2], 9239);
70 0 : *s6k4++ = round_fx(L_mac_sat(L_mac(sum, s12k8[0], 7712), s12k8[1], 4053)); move16();
71 0 : sum = L_mac(L_mac(L_mult(mem_s12k8[2], 4053), s12k8[0], 7712), s12k8[1], 9239);
72 0 : *s6k4++ = round_fx(L_mac_sat(L_mac(sum, s12k8[2], 7712), s12k8[3], 4053)); move16();
73 :
74 0 : FOR (n = 5; n < len; n += 2)
75 : {
76 0 : sum = L_mac(L_mac(L_mult(s12k8[n - 4], 4053), s12k8[n - 3], 7712), s12k8[n - 2], 9239);
77 0 : *s6k4++ = round_fx_sat(L_mac_sat(L_mac(sum, s12k8[n - 1], 7712), s12k8[n], 4053)); move16();
78 : }
79 :
80 0 : mem_s12k8[0] = s12k8[len - 3]; move16();
81 0 : mem_s12k8[1] = s12k8[len - 2]; move16();
82 0 : mem_s12k8[2] = s12k8[len - 1]; move16();
83 :
84 : /* Scale downsampled signal */
85 0 : s6k4 = mem_s6k4 + mem_in_len;
86 0 : scale0 = sub(getScaleFactor16_0(mem_s6k4, mem_in_len), 3);
87 0 : *mem_s6k4_exp = sub(*mem_s6k4_exp, scale0); move16();
88 0 : scale1 = sub(getScaleFactor16_0(s6k4, len2), 3);
89 0 : s6k4_exp = sub(s12k8_exp, scale1);
90 0 : scale2 = sub(*mem_s6k4_exp, s6k4_exp);
91 0 : IF (scale2 > 0)
92 : {
93 0 : Scale_sig(s6k4, len2, sub(scale1, scale2));
94 0 : shift = scale0; move16();
95 0 : s6k4_exp = *mem_s6k4_exp; move16();
96 : }
97 : ELSE
98 : {
99 0 : Scale_sig(s6k4, len2, scale1);
100 0 : shift = add(scale0, scale2);
101 0 : *mem_s6k4_exp = s6k4_exp; move16();
102 : }
103 :
104 0 : SWITCH(frame_dms)
105 : {
106 0 : case 50:
107 0 : s6k4 = s6k4 - 32;
108 0 : break;
109 :
110 0 : case 25:
111 0 : s6k4 = s6k4 - 48;
112 0 : break;
113 : }
114 :
115 0 : Scale_sig(mem_s6k4, mem_in_len, shift);
116 :
117 : /* Compute autocorrelation */
118 0 : FOR (n = MIN_PITCH_6K4; n <= MAX_PITCH_6K4; n++)
119 : {
120 0 : sum = L_mult0(s6k4[0], s6k4[0 - n]);
121 0 : FOR (m = 1; m < acflen; m++)
122 : {
123 0 : sum = L_mac0(sum, s6k4[m], s6k4[m - n]);
124 : }
125 0 : ac[n - MIN_PITCH_6K4] = sum; move32();
126 : }
127 :
128 : /* Weight autocorrelation and find maximum */
129 0 : max32 = Mpy_32_16_lc3plus(ac[0], olpa_ac_weighting[0]); move32();
130 0 : *pitch = MIN_PITCH_6K4; move16();
131 0 : FOR (n = MIN_PITCH_6K4 + 1; n <= MAX_PITCH_6K4; n++)
132 : {
133 0 : L_tmp = Mpy_32_16_lc3plus(ac[n - MIN_PITCH_6K4], olpa_ac_weighting[n - MIN_PITCH_6K4]);
134 0 : L_tmp2 = L_sub_sat(L_tmp, max32);
135 0 : if (L_tmp2 > 0)
136 : {
137 0 : *pitch = n; move16();
138 : }
139 0 : max32 = L_max(L_tmp, max32);
140 : }
141 :
142 : /* Compute normalized correlation */
143 0 : sum0 = L_mult0(s6k4[0], s6k4[0 - *pitch]);
144 0 : sum1 = L_mac0(1, s6k4[0 - *pitch], s6k4[0 - *pitch]);
145 0 : sum2 = L_mac0(1, s6k4[0], s6k4[0]);
146 0 : for (m = 1; m < acflen; m++)
147 : {
148 0 : sum0 = L_mac0(sum0, s6k4[m], s6k4[m - *pitch]);
149 0 : sum1 = L_mac0(sum1, s6k4[m - *pitch], s6k4[m - *pitch]);
150 0 : sum2 = L_mac0(sum2, s6k4[m], s6k4[m]);
151 : }
152 0 : scale1 = norm_l(sum1);
153 0 : scale2 = norm_l(sum2);
154 0 : sum1 = L_shl_pos(sum1, scale1);
155 0 : sum2 = L_shl_pos(sum2, scale2);
156 0 : prod = Mpy_32_32_lc3plus(sum1, sum2);
157 0 : shift = norm_l(prod);
158 0 : prod = L_shl_pos(prod, shift);
159 0 : prod_exp = sub(62, add(add(scale1, scale2), shift));
160 0 : inv = Isqrt_lc3plus(prod, &prod_exp);
161 0 : scale0 = norm_l(sum0);
162 0 : sum0 = L_shl_pos(sum0, scale0);
163 0 : prod = Mpy_32_32_lc3plus(sum0, inv);
164 0 : prod_exp = add(sub(31, scale0), prod_exp);
165 0 : test();
166 0 : IF (prod == 0 || sub(norm_l(prod), prod_exp) >= 0)
167 : {
168 0 : *normcorr = s_max(0, round_fx_sat(L_shl_sat(prod, prod_exp))); move16();
169 : }
170 : ELSE
171 : {
172 0 : *normcorr = 32767; move16();
173 : }
174 :
175 : /* Second try in the neighborhood of the previous pitch */
176 0 : min_pitch = s_max(MIN_PITCH_6K4, sub(*mem_pitch, 4));
177 0 : max_pitch = s_min(MAX_PITCH_6K4, add(*mem_pitch, 4));
178 :
179 0 : max32 = ac[min_pitch - MIN_PITCH_6K4]; move32();
180 0 : pitch2 = min_pitch; move16();
181 0 : FOR (n = min_pitch + 1; n <= max_pitch; n++)
182 : {
183 0 : L_tmp = L_sub_sat(ac[n - MIN_PITCH_6K4], max32);
184 0 : if (L_tmp > 0)
185 : {
186 0 : pitch2 = n; move16();
187 : }
188 0 : max32 = L_max(ac[n - MIN_PITCH_6K4], max32);
189 : }
190 0 : IF (sub(*pitch, pitch2) != 0)
191 : {
192 0 : sum0 = L_mult0(s6k4[0], s6k4[0 - pitch2]);
193 0 : sum1 = L_mac0(1, s6k4[0 - pitch2], s6k4[0 - pitch2]);
194 0 : sum2 = L_mac0(1, s6k4[0], s6k4[0]);
195 0 : for (m = 1; m < acflen; m++)
196 : {
197 0 : sum0 = L_mac0(sum0, s6k4[m], s6k4[m - pitch2]);
198 0 : sum1 = L_mac0(sum1, s6k4[m - pitch2], s6k4[m - pitch2]);
199 0 : sum2 = L_mac0(sum2, s6k4[m], s6k4[m]);
200 : }
201 0 : scale1 = norm_l(sum1);
202 0 : scale2 = norm_l(sum2);
203 0 : sum1 = L_shl_pos(sum1, scale1);
204 0 : sum2 = L_shl_pos(sum2, scale2);
205 0 : prod = Mpy_32_32_lc3plus(sum1, sum2);
206 0 : shift = norm_l(prod);
207 0 : prod = L_shl_pos(prod, shift);
208 0 : prod_exp = sub(62, add(add(scale1, scale2), shift));
209 0 : inv = Isqrt_lc3plus(prod, &prod_exp);
210 0 : scale0 = norm_l(sum0);
211 0 : sum0 = L_shl_pos(sum0, scale0);
212 0 : prod = Mpy_32_32_lc3plus(sum0, inv);
213 0 : prod_exp = add(sub(31, scale0), prod_exp);
214 0 : test();
215 0 : IF (prod == 0 || sub(norm_l(prod), prod_exp) >= 0)
216 : {
217 0 : normcorr2 = s_max(0, round_fx_sat(L_shl_sat(prod, prod_exp))); move16();
218 : }
219 : ELSE
220 : {
221 0 : normcorr2 = 32767; move16();
222 : }
223 0 : IF (sub(normcorr2, mult_r(*normcorr, 27853)) > 0)
224 : {
225 0 : *pitch = pitch2; move16();
226 0 : *normcorr = normcorr2; move16();
227 : }
228 : }
229 :
230 0 : SWITCH(frame_dms)
231 : {
232 0 : case 50:
233 0 : if(*pitch_flag == 1) {
234 0 : *mem_pitch = *pitch; move16();
235 0 : *pitch_flag = 0;
236 : }
237 : else {
238 0 : *pitch_flag += 1;
239 : }
240 0 : break;
241 :
242 0 : case 25:
243 0 : if (*pitch_flag == 3) {
244 0 : *mem_pitch = *pitch; move16();
245 0 : *pitch_flag = 0;
246 : }
247 : else {
248 0 : *pitch_flag += 1;
249 : }
250 0 : break;
251 :
252 0 : default:
253 :
254 0 : *mem_pitch = *pitch; move16();
255 : }
256 :
257 : /* Update memory */
258 0 : basop_memmove(mem_s6k4, &mem_s6k4[len2], mem_in_len * sizeof(Word16));
259 :
260 : /* Upsample pitch by a factor of 2 (6.4kHz -> 12.8kHz) */
261 0 : *pitch = shl_pos(*pitch, 1); move16();
262 :
263 : #ifdef DYNMEM_COUNT
264 : Dyn_Mem_Out();
265 : #endif
266 0 : }
267 :
|
