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 : /* guard against unindended includes */
11 : #ifndef INCLUDED_FROM_IISFFT_C
12 : #error "this file must not be included"
13 : #endif
14 :
15 : #define FFT_INTERNAL_TRIG_PREC double
16 : #define BORDER_FOR_SECOND_SCRATCH 100
17 :
18 : static const LC3_INT primeFactors[] = {2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53,
19 : 59, 61, 67, 71, 73, 79, 83, 89, 97, 101, 103, 107, 109, 113, 127, 131,
20 : 137, 139, 149, 151, 157, 163, 167, 173, 179, 181, 191, 193, 197, 199, 211, 223,
21 : 227, 229, 233, 239, 241, 251, 257, 263, 269, 271, 277, 281, 283, 293, 0};
22 :
23 : /* fft, returns 1 if length is supported and fft was applied */
24 0 : static LC3_INT fft_n(LC3_FLOAT* x, LC3_INT length)
25 : {
26 0 : switch (length) {
27 0 : case 2:
28 0 : fft2(x);
29 0 : return 1;
30 0 : case 3:
31 0 : fft3(x);
32 0 : return 1;
33 0 : case 4:
34 0 : fft4(x);
35 0 : return 1;
36 0 : case 5:
37 0 : fft5(x);
38 0 : return 1;
39 0 : case 8:
40 0 : fft8(x);
41 0 : return 1;
42 0 : case 9:
43 0 : fft9(x);
44 0 : return 1;
45 0 : case 15:
46 0 : fft15(x);
47 0 : return 1;
48 0 : case 16:
49 0 : fft16(x);
50 0 : return 1;
51 0 : case 32:
52 0 : fft32(x);
53 0 : return 1;
54 0 : case 60:
55 0 : fft60(x);
56 0 : return 1;
57 0 : case 64:
58 0 : fft64(x);
59 0 : return 1;
60 0 : case 128:
61 0 : fft128(x);
62 0 : return 1;
63 0 : case 240:
64 0 : fft240(x);
65 0 : return 1;
66 0 : case 256:
67 0 : LC3_cfft(x, x + 1, 256, 2, -1);
68 0 : return 1;
69 0 : case 384:
70 0 : fft384(x);
71 0 : return 1;
72 0 : case 480:
73 0 : fft480(x);
74 0 : return 1;
75 0 : case 512:
76 0 : LC3_cfft(x, x + 1, 512, 2, -1);
77 0 : return 1;
78 0 : case 1024:
79 0 : LC3_cfft(x, x + 1, 1024, 2, -1);
80 0 : return 1;
81 0 : default:
82 0 : return 0;
83 : }
84 : }
85 :
86 :
87 : /* returns 1 on success or 0 if IISFFT_MAXFACTORS is too small */
88 0 : static LC3_INT factorize(LC3_INT length, LC3_INT* restrict numFactors, LC3_INT* restrict factor,
89 : LC3_INT* restrict isPrime)
90 : {
91 0 : LC3_INT remainder = length;
92 0 : LC3_INT idx = 0, cnt = 0;
93 0 : LC3_INT actFac = primeFactors[idx];
94 0 : LC3_INT inc = 0;
95 :
96 0 : *numFactors = 0;
97 :
98 0 : while (remainder > 1 && actFac != 0) {
99 0 : if (remainder % actFac == 0) {
100 0 : if (inc == 0) {
101 0 : inc = 1;
102 0 : (*numFactors)++;
103 : }
104 0 : remainder /= actFac;
105 : } else {
106 0 : actFac = primeFactors[++idx];
107 0 : inc = 0;
108 : }
109 : }
110 0 : if (remainder > 1) {
111 0 : (*numFactors)++;
112 : }
113 :
114 0 : if (*numFactors > IISFFT_MAXFACTORS)
115 0 : return 0;
116 :
117 0 : idx = 0, cnt = 0, inc = 0;
118 0 : remainder = length;
119 0 : actFac = primeFactors[idx];
120 0 : (factor)[cnt] = 1;
121 0 : while (remainder > 1 && actFac != 0) {
122 0 : if (remainder % actFac == 0) {
123 0 : (factor)[cnt] *= actFac;
124 0 : remainder /= actFac;
125 0 : inc = 1;
126 0 : if (factor[cnt] == actFac) { /* first appearance of the factor */
127 0 : isPrime[cnt] = 1;
128 : } else {
129 0 : isPrime[cnt] = 0;
130 : }
131 : } else {
132 0 : actFac = primeFactors[++idx];
133 0 : if (inc == 1) {
134 0 : cnt++;
135 : }
136 0 : inc = 0;
137 0 : (factor)[cnt] = 1;
138 : }
139 : }
140 0 : if (remainder > 1) {
141 0 : factor[cnt] = remainder;
142 : }
143 0 : return 1;
144 : }
145 :
146 0 : static void oddFFT(LC3_FLOAT* restrict x, LC3_INT length, LC3_FLOAT* restrict scratch)
147 : {
148 : LC3_INT i, k, n;
149 : LC3_FLOAT * src1, *src2, *dest1, *dest2;
150 : FFT_INTERNAL_TRIG_PREC sinValOrg, cosValOrg;
151 :
152 0 : dest1 = scratch + 1;
153 0 : dest2 = scratch + length + 1;
154 0 : src1 = x + 2;
155 0 : src2 = x + 2 * length - 1;
156 :
157 0 : scratch[0] = x[0];
158 0 : scratch[length] = x[1];
159 :
160 0 : for (i = 2; i < length; i += 2) {
161 : LC3_FLOAT tmp1R, tmp1I, tmp2R, tmp2I;
162 0 : tmp1R = *src1++;
163 0 : tmp1I = *src1++;
164 0 : tmp2I = *src2--;
165 0 : tmp2R = *src2--;
166 0 : *dest1++ = tmp1R + tmp2R;
167 0 : *dest1++ = tmp1R - tmp2R;
168 0 : *dest2++ = tmp1I + tmp2I;
169 0 : *dest2++ = tmp1I - tmp2I;
170 :
171 0 : x[0] += tmp1R + tmp2R;
172 0 : x[1] += tmp1I + tmp2I;
173 : }
174 :
175 0 : dest1 = x + 2;
176 0 : dest2 = x + 2 * length - 2;
177 0 : for (k = 2; k < length; k += 2) {
178 0 : FFT_INTERNAL_TRIG_PREC sinVal = 0, cosVal = 1;
179 0 : cosValOrg = LC3_COS(-M_PIl * k / length);
180 0 : sinValOrg = LC3_SIN(-M_PIl * k / length);
181 :
182 0 : *dest1 = *dest2 = scratch[0];
183 0 : *(dest1 + 1) = *(dest2 + 1) = scratch[length];
184 :
185 0 : src1 = scratch + 1;
186 0 : src2 = scratch + length + 1;
187 :
188 0 : for (n = 2; n < length; n += 2) {
189 : LC3_FLOAT rePre, reMre, imPim, imMim;
190 : /*
191 : cos(x+y) = cox(x) cos(y) - sin(x) sin(y);
192 : sin(x+y) = sin(x) cos(y) + cos(x) sin(y);
193 : */
194 0 : FFT_INTERNAL_TRIG_PREC tmp = cosVal * cosValOrg - sinVal * sinValOrg;
195 0 : sinVal = sinVal * cosValOrg + cosVal * sinValOrg;
196 0 : cosVal = tmp;
197 :
198 0 : rePre = *src1++;
199 0 : reMre = *src1++;
200 0 : imPim = *src2++;
201 0 : imMim = *src2++;
202 :
203 0 : *dest1 += (LC3_FLOAT)cosVal * rePre - (LC3_FLOAT)sinVal * imMim;
204 0 : *(dest1 + 1) += (LC3_FLOAT)sinVal * reMre + (LC3_FLOAT)cosVal * imPim;
205 0 : *dest2 += (LC3_FLOAT)cosVal * rePre + (LC3_FLOAT)sinVal * imMim;
206 0 : *(dest2 + 1) += (LC3_FLOAT)-sinVal * reMre + (LC3_FLOAT)cosVal * imPim;
207 : }
208 0 : dest1 += 2;
209 0 : dest2 -= 2;
210 : }
211 0 : }
212 :
213 0 : static LC3_INT findInverse(LC3_INT a, LC3_INT b)
214 : {
215 0 : LC3_INT b0 = b, t, q;
216 0 : LC3_INT x0 = 0, x1 = 1;
217 :
218 0 : if (b == 1) {
219 0 : return 1;
220 : }
221 :
222 0 : while (a > 1) {
223 0 : q = a / b;
224 0 : t = b, b = a % b, a = t;
225 0 : t = x0, x0 = x1 - q * x0, x1 = t;
226 : }
227 :
228 0 : if (x1 < 0) {
229 0 : x1 += b0;
230 : }
231 :
232 0 : return x1;
233 : }
234 :
235 0 : static LC3_INT getGeneratorStupid(LC3_INT groupLength)
236 : {
237 0 : LC3_INT generator = 2; /* start value */
238 0 : LC3_INT count = 1, number = generator;
239 :
240 0 : while (generator < 100) { /* hopefully the generator is smaller than 100 */
241 0 : while (number != 1) {
242 0 : number = (number * generator) % groupLength;
243 0 : count++;
244 : }
245 0 : if (count == groupLength - 1) {
246 0 : return generator;
247 : } else {
248 0 : generator++;
249 0 : count = 1;
250 0 : number = generator;
251 : }
252 : }
253 :
254 0 : return -1;
255 : }
256 :
257 0 : static LC3_INT getGenerator(LC3_INT groupLength)
258 : {
259 0 : LC3_INT generator = 2; /* start value */
260 : LC3_INT count, number, factorCount, found, count2;
261 0 : LC3_INT factors[16] = {0};
262 :
263 : /* factorize: only for a group length with factors < 300 */
264 0 : factorCount = 0;
265 0 : number = groupLength - 1;
266 0 : found = 0;
267 0 : count = 0;
268 0 : while (number != 1) {
269 0 : if (primeFactors[count] == 0) {
270 : /* Not all factors listed */
271 0 : return getGeneratorStupid(groupLength);
272 : }
273 0 : if (number % primeFactors[count] == 0) {
274 0 : number /= primeFactors[count];
275 0 : if (found == 0) {
276 0 : factors[factorCount++] = primeFactors[count];
277 0 : found = 1;
278 : }
279 : } else {
280 0 : count++;
281 0 : found = 0;
282 : }
283 : }
284 :
285 0 : for (count = 0; factors[count] != 0; count++) {
286 0 : factors[count] = (groupLength - 1) / factors[count];
287 : }
288 :
289 : /* calculate generator */
290 0 : number = generator;
291 0 : count = 0;
292 0 : while (factors[count] != 0) {
293 0 : for (count2 = 0; count2 < factors[count] - 1; count2++) {
294 0 : number = (number * generator) % groupLength;
295 : }
296 0 : if (number != 1) {
297 0 : count++;
298 0 : number = generator;
299 0 : if (factors[count] == 0) {
300 0 : return generator;
301 : }
302 : } else {
303 0 : count = 0;
304 0 : generator++;
305 0 : number = generator;
306 : }
307 : }
308 :
309 0 : return -1;
310 : }
311 :
312 0 : static void primeFFT(LC3_FLOAT* restrict x, LC3_INT length, LC3_FLOAT* restrict scratch, LC3_INT* restrict scratch2)
313 : {
314 0 : LC3_INT i, k, middle = (length - 1) / 2;
315 : LC3_FLOAT *src1, *src2, *dest1, *dest2;
316 : LC3_INT * mapping, *map;
317 : LC3_INT generator;
318 :
319 0 : LC3_INT mappingTable[25][97] = {
320 : {0, 2},
321 : {0, 2, 4},
322 : {0, 2, 4, 8, 6},
323 : {0, 2, 6, 4, 12, 8, 10},
324 : {0, 2, 4, 8, 16, 10, 20, 18, 14, 6, 12},
325 : {0, 2, 4, 8, 16, 6, 12, 24, 22, 18, 10, 20, 14},
326 : {0, 2, 6, 18, 20, 26, 10, 30, 22, 32, 28, 16, 14, 8, 24, 4, 12},
327 : {0, 2, 4, 8, 16, 32, 26, 14, 28, 18, 36, 34, 30, 22, 6, 12, 24, 10, 20},
328 : {0, 2, 10, 4, 20, 8, 40, 16, 34, 32, 22, 18, 44, 36, 42, 26, 38, 6, 30, 12, 14, 24, 28},
329 : {0, 2, 4, 8, 16, 32, 6, 12, 24, 48, 38, 18, 36, 14, 28, 56, 54, 50, 42, 26, 52, 46, 34, 10, 20, 40, 22, 44, 30},
330 : {0, 2, 6, 18, 54, 38, 52, 32, 34, 40, 58, 50, 26, 16, 48, 20,
331 : 60, 56, 44, 8, 24, 10, 30, 28, 22, 4, 12, 36, 46, 14, 42},
332 : {0, 2, 4, 8, 16, 32, 64, 54, 34, 68, 62, 50, 26, 52, 30, 60, 46, 18, 36,
333 : 72, 70, 66, 58, 42, 10, 20, 40, 6, 12, 24, 48, 22, 44, 14, 28, 56, 38},
334 : {0, 2, 12, 72, 22, 50, 54, 78, 58, 20, 38, 64, 56, 8, 48, 42, 6, 36, 52, 66, 68,
335 : 80, 70, 10, 60, 32, 28, 4, 24, 62, 44, 18, 26, 74, 34, 40, 76, 46, 30, 16, 14},
336 : {0, 2, 6, 18, 54, 76, 56, 82, 74, 50, 64, 20, 60, 8, 24, 72, 44, 46, 52, 70, 38, 28,
337 : 84, 80, 68, 32, 10, 30, 4, 12, 36, 22, 66, 26, 78, 62, 14, 42, 40, 34, 16, 48, 58},
338 : {0, 2, 10, 50, 62, 28, 46, 42, 22, 16, 80, 24, 26, 36, 86, 54, 82, 34, 76, 4, 20, 6, 30, 56,
339 : 92, 84, 44, 32, 66, 48, 52, 72, 78, 14, 70, 68, 58, 8, 40, 12, 60, 18, 90, 74, 88, 64, 38},
340 : {0, 2, 4, 8, 16, 32, 64, 22, 44, 88, 70, 34, 68, 30, 60, 14, 28, 56, 6, 12, 24, 48, 96, 86, 66, 26, 52,
341 : 104, 102, 98, 90, 74, 42, 84, 62, 18, 36, 72, 38, 76, 46, 92, 78, 50, 100, 94, 82, 58, 10, 20, 40, 80, 54},
342 : {0, 2, 4, 8, 16, 32, 64, 10, 20, 40, 80, 42, 84, 50, 100, 82, 46, 92, 66, 14,
343 : 28, 56, 112, 106, 94, 70, 22, 44, 88, 58, 116, 114, 110, 102, 86, 54, 108, 98, 78, 38,
344 : 76, 34, 68, 18, 36, 72, 26, 52, 104, 90, 62, 6, 12, 24, 48, 96, 74, 30, 60},
345 : {0, 2, 4, 8, 16, 32, 64, 6, 12, 24, 48, 96, 70, 18, 36, 72, 22, 44, 88, 54, 108,
346 : 94, 66, 10, 20, 40, 80, 38, 76, 30, 60, 120, 118, 114, 106, 90, 58, 116, 110, 98, 74, 26,
347 : 52, 104, 86, 50, 100, 78, 34, 68, 14, 28, 56, 112, 102, 82, 42, 84, 46, 92, 62},
348 : {0, 2, 4, 8, 16, 32, 64, 128, 122, 110, 86, 38, 76, 18, 36, 72, 10, 20, 40, 80, 26, 52, 104,
349 : 74, 14, 28, 56, 112, 90, 46, 92, 50, 100, 66, 132, 130, 126, 118, 102, 70, 6, 12, 24, 48, 96, 58,
350 : 116, 98, 62, 124, 114, 94, 54, 108, 82, 30, 60, 120, 106, 78, 22, 44, 88, 42, 84, 34, 68},
351 : {0, 2, 14, 98, 118, 116, 102, 4, 28, 54, 94, 90, 62, 8, 56, 108, 46, 38, 124, 16, 112, 74, 92, 76,
352 : 106, 32, 82, 6, 42, 10, 70, 64, 22, 12, 84, 20, 140, 128, 44, 24, 26, 40, 138, 114, 88, 48, 52, 80,
353 : 134, 86, 34, 96, 104, 18, 126, 30, 68, 50, 66, 36, 110, 60, 136, 100, 132, 72, 78, 120, 130, 58, 122},
354 : {0, 2, 10, 50, 104, 82, 118, 6, 30, 4, 20, 100, 62, 18, 90, 12, 60, 8, 40,
355 : 54, 124, 36, 34, 24, 120, 16, 80, 108, 102, 72, 68, 48, 94, 32, 14, 70, 58, 144,
356 : 136, 96, 42, 64, 28, 140, 116, 142, 126, 46, 84, 128, 56, 134, 86, 138, 106, 92, 22,
357 : 110, 112, 122, 26, 130, 66, 38, 44, 74, 78, 98, 52, 114, 132, 76, 88},
358 : {0, 2, 6, 18, 54, 4, 12, 36, 108, 8, 24, 72, 58, 16, 48, 144, 116, 32, 96, 130,
359 : 74, 64, 34, 102, 148, 128, 68, 46, 138, 98, 136, 92, 118, 38, 114, 26, 78, 76, 70, 52,
360 : 156, 152, 140, 104, 154, 146, 122, 50, 150, 134, 86, 100, 142, 110, 14, 42, 126, 62, 28, 84,
361 : 94, 124, 56, 10, 30, 90, 112, 20, 60, 22, 66, 40, 120, 44, 132, 80, 82, 88, 106},
362 : {0, 2, 4, 8, 16, 32, 64, 128, 90, 14, 28, 56, 112, 58, 116, 66, 132, 98, 30, 60, 120,
363 : 74, 148, 130, 94, 22, 44, 88, 10, 20, 40, 80, 160, 154, 142, 118, 70, 140, 114, 62, 124, 82,
364 : 164, 162, 158, 150, 134, 102, 38, 76, 152, 138, 110, 54, 108, 50, 100, 34, 68, 136, 106, 46, 92,
365 : 18, 36, 72, 144, 122, 78, 156, 146, 126, 86, 6, 12, 24, 48, 96, 26, 52, 104, 42, 84},
366 : {0, 2, 6, 18, 54, 162, 130, 34, 102, 128, 28, 84, 74, 44, 132, 40, 120, 4,
367 : 12, 36, 108, 146, 82, 68, 26, 78, 56, 168, 148, 88, 86, 80, 62, 8, 24, 72,
368 : 38, 114, 164, 136, 52, 156, 112, 158, 118, 176, 172, 160, 124, 16, 48, 144, 76, 50,
369 : 150, 94, 104, 134, 46, 138, 58, 174, 166, 142, 70, 32, 96, 110, 152, 100, 122, 10,
370 : 30, 90, 92, 98, 116, 170, 154, 106, 140, 64, 14, 42, 126, 22, 66, 20, 60},
371 : {0, 2, 10, 50, 56, 86, 42, 16, 80, 12, 60, 106, 142, 128, 58, 96, 92, 72, 166, 54,
372 : 76, 186, 154, 188, 164, 44, 26, 130, 68, 146, 148, 158, 14, 70, 156, 4, 20, 100, 112, 172,
373 : 84, 32, 160, 24, 120, 18, 90, 62, 116, 192, 184, 144, 138, 108, 152, 178, 114, 182, 134, 88,
374 : 52, 66, 136, 98, 102, 122, 28, 140, 118, 8, 40, 6, 30, 150, 168, 64, 126, 48, 46, 36,
375 : 180, 124, 38, 190, 174, 94, 82, 22, 110, 162, 34, 170, 74, 176, 104, 132, 78}};
376 :
377 0 : if (length < BORDER_FOR_SECOND_SCRATCH) {
378 0 : for (i = 1;; i++) {
379 0 : if (primeFactors[i] == length) {
380 0 : mapping = mappingTable[i];
381 0 : break;
382 : }
383 0 : assert(primeFactors[i] != 0);
384 : }
385 : } else {
386 0 : mapping = scratch2;
387 :
388 : /* get primitive root */
389 0 : generator = getGenerator(length);
390 0 : assert(generator != -1);
391 :
392 : /* init mapping */
393 0 : mapping[0] = 0;
394 0 : mapping[1] = 1;
395 0 : for (i = 2; i < length; i++) {
396 0 : mapping[i] = mapping[i - 1] * generator;
397 0 : if (mapping[i] > length - 1) {
398 0 : mapping[i] = (mapping[i] % length - 1) + 1;
399 : }
400 : }
401 :
402 : /* double mapping value */
403 0 : for (i = 1; i < length; i++) {
404 0 : mapping[i] *= 2;
405 : }
406 : }
407 :
408 : /* remap input to scratch */
409 0 : scratch[0] = x[0];
410 0 : scratch[1] = x[1];
411 0 : scratch[2] = x[2];
412 0 : scratch[3] = x[3];
413 0 : map = mapping + length - 1;
414 0 : for (i = 4; i < 2 * length; map--) {
415 0 : scratch[i++] = x[(*map)];
416 0 : scratch[i++] = x[(*map) + 1];
417 : }
418 :
419 : /* print sums and diffs into scratch by using symmetry */
420 0 : x[length] = x[1]; /* imaginary und real part */
421 0 : dest1 = x + 1;
422 0 : dest2 = x + length + 1;
423 0 : src1 = scratch + 2;
424 0 : src2 = scratch + length + 1;
425 :
426 0 : for (i = 2; i < length; i += 2) {
427 : LC3_FLOAT tmp1R, tmp1I, tmp2R, tmp2I;
428 0 : tmp1R = *src1++;
429 0 : tmp1I = *src1++;
430 0 : tmp2R = *src2++;
431 0 : tmp2I = *src2++;
432 0 : *dest1++ = tmp1R + tmp2R;
433 0 : *dest1++ = tmp1R - tmp2R;
434 0 : *dest2++ = tmp1I + tmp2I;
435 0 : *dest2++ = tmp1I - tmp2I;
436 :
437 0 : scratch[0] += tmp1R + tmp2R;
438 0 : scratch[1] += tmp1I + tmp2I;
439 : }
440 :
441 : /* init with values from the first column */
442 0 : dest1 = scratch + 2;
443 0 : for (i = 1; i < length; i++) {
444 0 : *dest1++ = x[0]; /* add real part of x(0)(factor = 1) */
445 0 : *dest1++ = x[length]; /* add imaginary part of x(0)(factor = 1) */
446 : }
447 :
448 0 : for (k = 1; k <= middle; k++) {
449 : /* loop through all cos/sin values */
450 : LC3_FLOAT sinVal, cosVal;
451 : LC3_INT length1, length2;
452 : LC3_FLOAT rePre, reMre, imPim, imMim;
453 :
454 0 : cosVal = (LC3_FLOAT)LC3_COS(-M_PIl * mapping[k] / length);
455 0 : sinVal = (LC3_FLOAT)LC3_SIN(-M_PIl * mapping[k] / length);
456 :
457 : /* compute in two parts (length1, length2) to avoid if() in for loop */
458 0 : length1 = middle - k + 1;
459 0 : length2 = middle - length1;
460 0 : src1 = x + 1;
461 0 : src2 = x + length + 1;
462 0 : dest1 = scratch + 2 * k;
463 0 : dest2 = scratch + 2 * (middle + k);
464 :
465 0 : for (i = 0; i < length1; i++) {
466 0 : rePre = *src1++;
467 0 : reMre = *src1++;
468 0 : imPim = *src2++;
469 0 : imMim = *src2++;
470 :
471 0 : *dest1++ += cosVal * rePre - sinVal * imMim;
472 0 : *dest1++ += cosVal * imPim + sinVal * reMre;
473 :
474 0 : *dest2++ += cosVal * rePre + sinVal * imMim;
475 0 : *dest2++ += cosVal * imPim - sinVal * reMre;
476 : }
477 0 : if (dest2 == scratch + 2 * length) {
478 0 : dest2 = scratch + 2;
479 : }
480 0 : for (i = 0; i < length2; i++) {
481 0 : rePre = *src1++;
482 0 : reMre = *src1++;
483 0 : imPim = *src2++;
484 0 : imMim = *src2++;
485 :
486 0 : *dest1++ += cosVal * rePre - sinVal * imMim;
487 0 : *dest1++ += cosVal * imPim + sinVal * reMre;
488 :
489 0 : *dest2++ += cosVal * rePre + sinVal * imMim;
490 0 : *dest2++ += cosVal * imPim - sinVal * reMre;
491 : }
492 : }
493 :
494 : /* remap output to x */
495 0 : x[0] = scratch[0];
496 0 : x[1] = scratch[1];
497 0 : map = mapping + 1;
498 0 : for (i = 2; i < 2 * length; map++) {
499 0 : x[(*map)] = scratch[i++];
500 0 : x[(*map) + 1] = scratch[i++];
501 : }
502 0 : }
503 :
504 0 : static void nextFFT(LC3_FLOAT* x, LC3_INT length, LC3_FLOAT* scratch)
505 : {
506 0 : if (fft_n(x, length)) /* have function for length */
507 0 : return;
508 :
509 0 : assert(length % 2 != 0);
510 0 : oddFFT(x, length, scratch);
511 : }
512 :
513 0 : static inline LC3_INT findFactor(const LC3_INT length)
514 : {
515 : static const LC3_INT factors[] = {16, 9, 8, 7, 5, 4, 3, 2, 0};
516 0 : LC3_INT i = 0, factor = 0;
517 0 : for (i = 0; factors[i] != 0; i++) {
518 0 : if (length % factors[i] == 0) {
519 0 : factor = factors[i];
520 0 : break;
521 : }
522 : }
523 0 : return factor;
524 : }
525 :
526 0 : static inline void twiddle(LC3_FLOAT* x, const LC3_INT length, const LC3_INT n1, const LC3_INT n2)
527 : {
528 : LC3_INT i, ii;
529 : FFT_INTERNAL_TRIG_PREC sinValOrg, cosValOrg;
530 0 : FFT_INTERNAL_TRIG_PREC sinVal = 0, cosVal = 1;
531 0 : FFT_INTERNAL_TRIG_PREC twReal = 0, twImag = 1;
532 :
533 0 : cosValOrg = LC3_COS(-2 * (LC3_FLOAT)M_PIl / length);
534 0 : sinValOrg = LC3_SIN(-2 * (LC3_FLOAT)M_PIl / length);
535 :
536 0 : for (i = 1; i < n1; i++) {
537 0 : FFT_INTERNAL_TRIG_PREC tmp = 0.;
538 0 : twReal = 1;
539 0 : twImag = 0;
540 :
541 0 : tmp = cosVal * cosValOrg - sinVal * sinValOrg;
542 0 : sinVal = sinVal * cosValOrg + cosVal * sinValOrg;
543 0 : cosVal = tmp;
544 :
545 0 : for (ii = 1; ii < n2; ii++) {
546 : LC3_FLOAT xRe, xIm;
547 : FFT_INTERNAL_TRIG_PREC tmpReal;
548 :
549 0 : tmpReal = twReal * cosVal - twImag * sinVal;
550 0 : twImag = twImag * cosVal + sinVal * twReal;
551 0 : twReal = tmpReal;
552 :
553 0 : xRe = x[2 * (i * n2 + ii)];
554 0 : xIm = x[2 * (i * n2 + ii) + 1];
555 :
556 0 : x[2 * (i * n2 + ii)] = (LC3_FLOAT)twReal * xRe - (LC3_FLOAT)twImag * xIm;
557 0 : x[2 * (i * n2 + ii) + 1] = (LC3_FLOAT)twImag * xRe + (LC3_FLOAT)twReal * xIm;
558 : }
559 : }
560 0 : }
561 :
562 0 : static void cooleyTukeyFFT(LC3_FLOAT* restrict x, const LC3_INT length, LC3_FLOAT* restrict scratch,
563 : LC3_INT* restrict scratch2, LC3_INT isPrime)
564 : {
565 : LC3_INT factor;
566 : LC3_INT i, ii;
567 : LC3_INT n1, n2;
568 0 : LC3_INT cnt = 0;
569 : LC3_FLOAT *src, *dest;
570 :
571 0 : if (fft_n(x, length))
572 0 : return;
573 :
574 0 : factor = findFactor(length);
575 0 : if (factor > 0 && (length / factor > 1)) {
576 0 : n1 = factor;
577 0 : n2 = length / factor;
578 :
579 : /* DATA Resorting for stage1 */
580 0 : dest = scratch;
581 0 : for (i = 0; i < 2 * n1; i += 2) {
582 0 : src = x + i;
583 0 : for (ii = 0; ii < n2; ii++) {
584 : /* *dest++ = x[2*(i+ii*n1)]; */
585 : /* *dest++ = x[2*(i+ii*n1)+1]; */
586 0 : *dest++ = *src;
587 0 : *dest++ = *(src + 1);
588 0 : src += 2 * n1;
589 : }
590 : }
591 0 : src = scratch;
592 0 : dest = x;
593 0 : for (i = 0; i < length; i++) {
594 0 : *dest++ = *src++;
595 0 : *dest++ = *src++;
596 : }
597 : /* perform n1 ffts of length n2 */
598 0 : for (i = 0; i < n1; i++) {
599 0 : cooleyTukeyFFT(x + 2 * i * n2, n2, scratch + 2 * i * n2, scratch2, isPrime);
600 : }
601 : /*data twiddeling */
602 0 : twiddle(x, length, n1, n2);
603 : /* DATA Resorting for stage2 */
604 0 : cnt = 0;
605 0 : for (i = 0; i < n2; i++) {
606 0 : for (ii = 0; ii < n1; ii++) {
607 0 : scratch[2 * cnt] = x[2 * (i + ii * n2)];
608 0 : scratch[2 * cnt + 1] = x[2 * (i + ii * n2) + 1];
609 0 : cnt++;
610 : }
611 : }
612 : /* perform n2 ffts of length n1 */
613 0 : for (i = 0; i < n2; i++) {
614 0 : nextFFT(scratch + 2 * i * n1, n1, x + 2 * i * n1);
615 : }
616 0 : cnt = 0;
617 0 : for (i = 0; i < n1; i++) {
618 0 : for (ii = 0; ii < n2; ii++) {
619 0 : x[2 * cnt] = scratch[2 * (i + ii * n1)];
620 0 : x[2 * cnt + 1] = scratch[2 * (i + ii * n1) + 1];
621 0 : cnt++;
622 : }
623 : }
624 : } else {
625 0 : if (isPrime == 1 && length > 23) {
626 0 : primeFFT(x, length, scratch, scratch2);
627 : } else {
628 0 : oddFFT(x, length, scratch);
629 : }
630 : }
631 : }
632 :
633 0 : static void pfaDFT(LC3_FLOAT* restrict x, const LC3_INT length, LC3_FLOAT* restrict scratch1, const LC3_INT numFactors,
634 : const LC3_INT* const factor, LC3_INT* restrict scratch2, const LC3_INT* const isPrime)
635 : {
636 0 : LC3_FLOAT* tmp = scratch1;
637 : LC3_INT i, ii, n1, n2, idx, incr, cnt;
638 0 : LC3_INT n1_inv = 1;
639 :
640 0 : if (numFactors <= 1) {
641 0 : cooleyTukeyFFT(x, length, scratch1, scratch2, isPrime[0]);
642 0 : return;
643 : }
644 :
645 0 : n2 = factor[0];
646 0 : n1 = length / n2;
647 :
648 0 : n1_inv = findInverse(n1, n2);
649 :
650 0 : idx = 0;
651 0 : incr = n1 * n1_inv;
652 0 : cnt = 0;
653 0 : for (i = 0; i < n1; i++) {
654 0 : for (ii = 0; ii < n2 - 1; ii++) {
655 0 : tmp[cnt++] = x[2 * idx];
656 0 : tmp[cnt++] = x[2 * idx + 1];
657 :
658 0 : idx += incr;
659 0 : if (idx > length) {
660 0 : idx -= length;
661 : }
662 : }
663 0 : tmp[cnt++] = x[2 * idx];
664 0 : tmp[cnt++] = x[2 * idx + 1];
665 0 : idx++;
666 : }
667 :
668 0 : for (cnt = 0; cnt < length; cnt += n2) {
669 0 : cooleyTukeyFFT(tmp + 2 * cnt, n2, x + 2 * cnt, scratch2, isPrime[0]);
670 : }
671 0 : for (cnt = 0; cnt < n1; cnt++) {
672 0 : for (i = 0; i < n2; i++) {
673 0 : x[2 * (cnt + i * n1)] = tmp[2 * (cnt * n2 + i)];
674 0 : x[2 * (cnt + i * n1) + 1] = tmp[2 * (cnt * n2 + i) + 1];
675 : }
676 : }
677 0 : for (cnt = 0; cnt < length; cnt += n1) {
678 0 : pfaDFT(x + 2 * cnt, n1, tmp, numFactors - 1, &factor[1], scratch2, &isPrime[1]);
679 : }
680 :
681 0 : cnt = 0;
682 0 : for (i = 0; i < n2; i++) {
683 0 : idx = i * n1;
684 0 : for (ii = 0; ii < n1; ii++) {
685 0 : tmp[2 * idx] = x[cnt++];
686 0 : tmp[2 * idx + 1] = x[cnt++];
687 0 : idx += n2;
688 0 : if (idx > length) {
689 0 : idx -= length;
690 : }
691 : }
692 : }
693 :
694 0 : for (cnt = 0; cnt < length; cnt++) {
695 0 : x[2 * cnt] = tmp[2 * cnt];
696 0 : x[2 * cnt + 1] = tmp[2 * cnt + 1];
697 : }
698 : }
|
