1use crate::common::tables::{
2 BIT_REV_TABLE_16, BIT_REV_TABLE_32, BIT_REV_TABLE_64, BIT_REV_TABLE_128, BIT_REV_TABLE_256,
3 BIT_REV_TABLE_512, BIT_REV_TABLE_1024, BIT_REV_TABLE_2048, BIT_REV_TABLE_4096,
4};
5use crate::common::tables::{
6 TWIDDLE_TABLE_16_U16, TWIDDLE_TABLE_16_U32, TWIDDLE_TABLE_32_U16, TWIDDLE_TABLE_32_U32,
7 TWIDDLE_TABLE_64_U16, TWIDDLE_TABLE_64_U32, TWIDDLE_TABLE_128_U16, TWIDDLE_TABLE_128_U32,
8 TWIDDLE_TABLE_256_U16, TWIDDLE_TABLE_256_U32, TWIDDLE_TABLE_512_U16, TWIDDLE_TABLE_512_U32,
9 TWIDDLE_TABLE_1024_U16, TWIDDLE_TABLE_1024_U32, TWIDDLE_TABLE_2048_U16, TWIDDLE_TABLE_2048_U32,
10 TWIDDLE_TABLE_4096_U16, TWIDDLE_TABLE_4096_U32,
11};
12
13use super::{
14 bitreversal_i16, bitreversal_i32, radix4_butterfly_i16, radix4_butterfly_i32,
15 radix4_butterfly_inverse_i16, radix4_butterfly_inverse_i32,
16};
17
18const RADIX4_MODIFIER: u16 = 1;
19const RADIX4BY2_MODIFIER: u16 = 2;
20
21#[inline]
22fn i16_mul(a: i16, b: i16) -> i16 {
23 (((a as i32) * (b as i32)) >> 16) as i16
24}
25
26#[inline]
27fn i32_mul(a: i32, b: i32) -> i32 {
28 (((a as i64 * b as i64) + 0x8000_0000) >> 32) as i32
29}
30
31#[inline]
32fn upshift_quads_i16(data: &mut [i16]) {
33 for quad in data.chunks_exact_mut(4) {
34 quad[0] = quad[0].wrapping_shl(1);
35 quad[1] = quad[1].wrapping_shl(1);
36 quad[2] = quad[2].wrapping_shl(1);
37 quad[3] = quad[3].wrapping_shl(1);
38 }
39}
40
41#[inline]
42fn upshift_quads_i32(data: &mut [i32]) {
43 for quad in data.chunks_exact_mut(4) {
44 quad[0] = quad[0].wrapping_shl(1);
45 quad[1] = quad[1].wrapping_shl(1);
46 quad[2] = quad[2].wrapping_shl(1);
47 quad[3] = quad[3].wrapping_shl(1);
48 }
49}
50
51pub struct CfftI16 {
67 pub n_fft: usize,
68 pub ifft_flag: bool,
69 pub bit_reverse_flag: bool,
70 pub twiddle: &'static [u16],
71 pub bit_rev_table: &'static [u16],
72}
73
74impl CfftI16 {
75 pub const fn new(n_fft: usize, ifft_flag: bool, bit_reverse_flag: bool) -> Self {
76 assert!(n_fft.is_power_of_two(), "n_fft must be a power of two");
77 assert!(16 <= n_fft && n_fft <= 4096, "n_fft must be in [16, 4096]",);
78
79 let twiddle: &'static [u16] = match n_fft {
80 16 => &TWIDDLE_TABLE_16_U16,
81 32 => &TWIDDLE_TABLE_32_U16,
82 64 => &TWIDDLE_TABLE_64_U16,
83 128 => &TWIDDLE_TABLE_128_U16,
84 256 => &TWIDDLE_TABLE_256_U16,
85 512 => &TWIDDLE_TABLE_512_U16,
86 1024 => &TWIDDLE_TABLE_1024_U16,
87 2048 => &TWIDDLE_TABLE_2048_U16,
88 4096 => &TWIDDLE_TABLE_4096_U16,
89 _ => unreachable!(),
90 };
91
92 let bit_rev_table: &'static [u16] = match n_fft {
93 16 => &BIT_REV_TABLE_16,
94 32 => &BIT_REV_TABLE_32,
95 64 => &BIT_REV_TABLE_64,
96 128 => &BIT_REV_TABLE_128,
97 256 => &BIT_REV_TABLE_256,
98 512 => &BIT_REV_TABLE_512,
99 1024 => &BIT_REV_TABLE_1024,
100 2048 => &BIT_REV_TABLE_2048,
101 4096 => &BIT_REV_TABLE_4096,
102 _ => unreachable!(),
103 };
104
105 Self {
106 n_fft,
107 ifft_flag,
108 bit_reverse_flag,
109 twiddle,
110 bit_rev_table,
111 }
112 }
113
114 fn radix4by2_butterfly_i16(data: &mut [i16], n_fft: usize, twiddle: &[u16], modifier: u16) {
115 let n2 = n_fft >> 1;
116
117 for i in 0..n2 {
118 let cos_val = twiddle[2 * i] as i16;
119 let sin_val = twiddle[2 * i + 1] as i16;
120
121 let l = i + n2;
122
123 let t_re_half = (data[2 * i] as i32) >> 1;
124 let t_im_half = (data[2 * i + 1] as i32) >> 1;
125 let s_re_half = (data[2 * l] as i32) >> 1;
126 let s_im_half = (data[2 * l + 1] as i32) >> 1;
127
128 let xt = (t_re_half - s_re_half) as i16;
129 let yt = (t_im_half - s_im_half) as i16;
130
131 data[2 * i] = ((t_re_half + s_re_half) >> 1) as i16;
132 data[2 * i + 1] = ((t_im_half + s_im_half) >> 1) as i16;
133
134 let out_re = i16_mul(xt, cos_val).wrapping_add(i16_mul(yt, sin_val));
135 let out_im = i16_mul(yt, cos_val).wrapping_sub(i16_mul(xt, sin_val));
136
137 data[2 * l] = out_re;
138 data[2 * l + 1] = out_im;
139 }
140
141 radix4_butterfly_i16(&mut data[..n_fft], n2, twiddle, modifier);
142 radix4_butterfly_i16(&mut data[n_fft..], n2, twiddle, modifier);
143
144 upshift_quads_i16(data);
145 }
146
147 fn radix4by2_butterfly_inverse_i16(
148 data: &mut [i16],
149 n_fft: usize,
150 twiddle: &[u16],
151 modifier: u16,
152 ) {
153 let n2 = n_fft >> 1;
154
155 for i in 0..n2 {
156 let cos_val = twiddle[2 * i] as i16;
157 let sin_val = twiddle[2 * i + 1] as i16;
158
159 let l = i + n2;
160
161 let t_re_half = (data[2 * i] as i32) >> 1;
162 let t_im_half = (data[2 * i + 1] as i32) >> 1;
163 let s_re_half = (data[2 * l] as i32) >> 1;
164 let s_im_half = (data[2 * l + 1] as i32) >> 1;
165
166 let xt = (t_re_half - s_re_half) as i16;
167 let yt = (t_im_half - s_im_half) as i16;
168
169 data[2 * i] = ((t_re_half + s_re_half) >> 1) as i16;
170 data[2 * i + 1] = ((t_im_half + s_im_half) >> 1) as i16;
171
172 let out_re = i16_mul(xt, cos_val).wrapping_sub(i16_mul(yt, sin_val));
173 let out_im = i16_mul(yt, cos_val).wrapping_add(i16_mul(xt, sin_val));
174
175 data[2 * l] = out_re;
176 data[2 * l + 1] = out_im;
177 }
178
179 radix4_butterfly_inverse_i16(&mut data[..n_fft], n2, twiddle, modifier);
180 radix4_butterfly_inverse_i16(&mut data[n_fft..], n2, twiddle, modifier);
181
182 upshift_quads_i16(data);
183 }
184
185 pub fn run(&self, data: &mut [i16]) {
187 assert_eq!(
188 data.len(),
189 self.n_fft * 2,
190 "Q15 buffer length must be 2 * n_fft"
191 );
192
193 let use_radix4by2 = matches!(self.n_fft, 32 | 128 | 512 | 2048);
194
195 if self.ifft_flag {
196 if use_radix4by2 {
197 Self::radix4by2_butterfly_inverse_i16(
198 data,
199 self.n_fft,
200 self.twiddle,
201 RADIX4BY2_MODIFIER,
202 );
203 } else {
204 radix4_butterfly_inverse_i16(data, self.n_fft, self.twiddle, RADIX4_MODIFIER);
205 }
206 } else {
207 if use_radix4by2 {
208 Self::radix4by2_butterfly_i16(data, self.n_fft, self.twiddle, RADIX4BY2_MODIFIER);
209 } else {
210 radix4_butterfly_i16(data, self.n_fft, self.twiddle, RADIX4_MODIFIER);
211 }
212 }
213
214 if self.bit_reverse_flag {
215 bitreversal_i16(data, self.bit_rev_table);
216 }
217 }
218}
219
220pub struct CfftI32 {
236 pub n_fft: usize,
237 pub ifft_flag: bool,
238 pub bit_reverse_flag: bool,
239 pub twiddle: &'static [u32],
240 pub bit_rev_table: &'static [u16],
241}
242
243impl CfftI32 {
244 pub const fn new(n_fft: usize, ifft_flag: bool, bit_reverse_flag: bool) -> Self {
245 assert!(n_fft.is_power_of_two(), "n_fft must be a power of two");
246 assert!(16 <= n_fft && n_fft <= 4096, "n_fft must be in [16, 4096]",);
247
248 let twiddle: &'static [u32] = match n_fft {
249 16 => &TWIDDLE_TABLE_16_U32,
250 32 => &TWIDDLE_TABLE_32_U32,
251 64 => &TWIDDLE_TABLE_64_U32,
252 128 => &TWIDDLE_TABLE_128_U32,
253 256 => &TWIDDLE_TABLE_256_U32,
254 512 => &TWIDDLE_TABLE_512_U32,
255 1024 => &TWIDDLE_TABLE_1024_U32,
256 2048 => &TWIDDLE_TABLE_2048_U32,
257 4096 => &TWIDDLE_TABLE_4096_U32,
258 _ => unreachable!(),
259 };
260
261 let bit_rev_table: &'static [u16] = match n_fft {
262 16 => &BIT_REV_TABLE_16,
263 32 => &BIT_REV_TABLE_32,
264 64 => &BIT_REV_TABLE_64,
265 128 => &BIT_REV_TABLE_128,
266 256 => &BIT_REV_TABLE_256,
267 512 => &BIT_REV_TABLE_512,
268 1024 => &BIT_REV_TABLE_1024,
269 2048 => &BIT_REV_TABLE_2048,
270 4096 => &BIT_REV_TABLE_4096,
271 _ => unreachable!(),
272 };
273
274 Self {
275 n_fft,
276 ifft_flag,
277 bit_reverse_flag,
278 twiddle,
279 bit_rev_table,
280 }
281 }
282
283 fn radix4by2_butterfly_i32(
284 data: &mut [i32],
285 n_fft: usize,
286 twiddle: &[u32],
287 radix4_modifier: u16,
288 ) {
289 let n2 = n_fft >> 1;
290
291 for i in 0..n2 {
292 let cos_val = twiddle[2 * i] as i32;
293 let sin_val = twiddle[2 * i + 1] as i32;
294
295 let l = i + n2;
296
297 let xt = (data[2 * i] >> 2) - (data[2 * l] >> 2);
298 data[2 * i] = (data[2 * i] >> 2) + (data[2 * l] >> 2);
299
300 let yt = (data[2 * i + 1] >> 2) - (data[2 * l + 1] >> 2);
301 data[2 * i + 1] = (data[2 * l + 1] >> 2) + (data[2 * i + 1] >> 2);
302
303 let mut out_re = i32_mul(xt, cos_val);
304 let mut out_im = i32_mul(yt, cos_val);
305 out_re = out_re.wrapping_add(i32_mul(yt, sin_val));
306 out_im = out_im.wrapping_sub(i32_mul(xt, sin_val));
307
308 data[2 * l] = out_re.wrapping_shl(1);
309 data[2 * l + 1] = out_im.wrapping_shl(1);
310 }
311
312 radix4_butterfly_i32(&mut data[..n_fft], n2, twiddle, radix4_modifier);
313 radix4_butterfly_i32(&mut data[n_fft..], n2, twiddle, radix4_modifier);
314
315 upshift_quads_i32(data);
316 }
317
318 fn radix4by2_butterfly_inverse_i32(
319 data: &mut [i32],
320 n_fft: usize,
321 twiddle: &[u32],
322 radix4_modifier: u16,
323 ) {
324 let n2 = n_fft >> 1;
325
326 for i in 0..n2 {
327 let cos_val = twiddle[2 * i] as i32;
328 let sin_val = twiddle[2 * i + 1] as i32;
329
330 let l = i + n2;
331
332 let xt = (data[2 * i] >> 2) - (data[2 * l] >> 2);
333 data[2 * i] = (data[2 * i] >> 2) + (data[2 * l] >> 2);
334
335 let yt = (data[2 * i + 1] >> 2) - (data[2 * l + 1] >> 2);
336 data[2 * i + 1] = (data[2 * l + 1] >> 2) + (data[2 * i + 1] >> 2);
337
338 let mut out_re = i32_mul(xt, cos_val);
339 let mut out_im = i32_mul(yt, cos_val);
340 out_re = out_re.wrapping_sub(i32_mul(yt, sin_val));
341 out_im = out_im.wrapping_add(i32_mul(xt, sin_val));
342
343 data[2 * l] = out_re.wrapping_shl(1);
344 data[2 * l + 1] = out_im.wrapping_shl(1);
345 }
346
347 radix4_butterfly_inverse_i32(&mut data[..n_fft], n2, twiddle, radix4_modifier);
348 radix4_butterfly_inverse_i32(&mut data[n_fft..], n2, twiddle, radix4_modifier);
349
350 upshift_quads_i32(data);
351 }
352
353 pub fn run(&self, data: &mut [i32]) {
355 assert_eq!(
356 data.len(),
357 self.n_fft * 2,
358 "Q31 buffer length must be 2 * n_fft"
359 );
360
361 let use_radix4by2 = matches!(self.n_fft, 32 | 128 | 512 | 2048);
362
363 if self.ifft_flag {
364 if use_radix4by2 {
365 Self::radix4by2_butterfly_inverse_i32(
366 data,
367 self.n_fft,
368 self.twiddle,
369 RADIX4BY2_MODIFIER,
370 );
371 } else {
372 radix4_butterfly_inverse_i32(data, self.n_fft, self.twiddle, RADIX4_MODIFIER);
373 }
374 } else {
375 if use_radix4by2 {
376 Self::radix4by2_butterfly_i32(data, self.n_fft, self.twiddle, RADIX4BY2_MODIFIER);
377 } else {
378 radix4_butterfly_i32(data, self.n_fft, self.twiddle, RADIX4_MODIFIER);
379 }
380 }
381
382 if self.bit_reverse_flag {
383 bitreversal_i32(data, self.bit_rev_table);
384 }
385 }
386}