#![no_std]
use core::slice;
use num_complex::Complex;
type Q15 = i16;
pub enum Direction {
Forward,
ForwardScaled,
Inverse,
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub enum FFTError {
InvalidFFTSize,
InvalidDataSize,
}
const SINETAB_LD_N: usize = 10;
const SINETAB_N: usize = 1 << SINETAB_LD_N;
static SINETAB: [i16; SINETAB_N - SINETAB_N / 4] = [
0, 201, 402, 603, 804, 1005, 1206, 1407, 1608, 1809, 2009, 2210, 2410, 2611, 2811, 3012, 3212,
3412, 3612, 3811, 4011, 4210, 4410, 4609, 4808, 5007, 5205, 5404, 5602, 5800, 5998, 6195, 6393,
6590, 6786, 6983, 7179, 7375, 7571, 7767, 7962, 8157, 8351, 8545, 8739, 8933, 9126, 9319, 9512,
9704, 9896, 10087, 10278, 10469, 10659, 10849, 11039, 11228, 11417, 11605, 11793, 11980, 12167,
12353, 12539, 12725, 12910, 13094, 13279, 13462, 13645, 13828, 14010, 14191, 14372, 14553,
14732, 14912, 15090, 15269, 15446, 15623, 15800, 15976, 16151, 16325, 16499, 16673, 16846,
17018, 17189, 17360, 17530, 17700, 17869, 18037, 18204, 18371, 18537, 18703, 18868, 19032,
19195, 19357, 19519, 19680, 19841, 20000, 20159, 20317, 20475, 20631, 20787, 20942, 21096,
21250, 21403, 21554, 21705, 21856, 22005, 22154, 22301, 22448, 22594, 22739, 22884, 23027,
23170, 23311, 23452, 23592, 23731, 23870, 24007, 24143, 24279, 24413, 24547, 24680, 24811,
24942, 25072, 25201, 25329, 25456, 25582, 25708, 25832, 25955, 26077, 26198, 26319, 26438,
26556, 26674, 26790, 26905, 27019, 27133, 27245, 27356, 27466, 27575, 27683, 27790, 27896,
28001, 28105, 28208, 28310, 28411, 28510, 28609, 28706, 28803, 28898, 28992, 29085, 29177,
29268, 29358, 29447, 29534, 29621, 29706, 29791, 29874, 29956, 30037, 30117, 30195, 30273,
30349, 30424, 30498, 30571, 30643, 30714, 30783, 30852, 30919, 30985, 31050, 31113, 31176,
31237, 31297, 31356, 31414, 31470, 31526, 31580, 31633, 31685, 31736, 31785, 31833, 31880,
31926, 31971, 32014, 32057, 32098, 32137, 32176, 32213, 32250, 32285, 32318, 32351, 32382,
32412, 32441, 32469, 32495, 32521, 32545, 32567, 32589, 32609, 32628, 32646, 32663, 32678,
32692, 32705, 32717, 32728, 32737, 32745, 32752, 32757, 32761, 32765, 32766, 32767, 32766,
32765, 32761, 32757, 32752, 32745, 32737, 32728, 32717, 32705, 32692, 32678, 32663, 32646,
32628, 32609, 32589, 32567, 32545, 32521, 32495, 32469, 32441, 32412, 32382, 32351, 32318,
32285, 32250, 32213, 32176, 32137, 32098, 32057, 32014, 31971, 31926, 31880, 31833, 31785,
31736, 31685, 31633, 31580, 31526, 31470, 31414, 31356, 31297, 31237, 31176, 31113, 31050,
30985, 30919, 30852, 30783, 30714, 30643, 30571, 30498, 30424, 30349, 30273, 30195, 30117,
30037, 29956, 29874, 29791, 29706, 29621, 29534, 29447, 29358, 29268, 29177, 29085, 28992,
28898, 28803, 28706, 28609, 28510, 28411, 28310, 28208, 28105, 28001, 27896, 27790, 27683,
27575, 27466, 27356, 27245, 27133, 27019, 26905, 26790, 26674, 26556, 26438, 26319, 26198,
26077, 25955, 25832, 25708, 25582, 25456, 25329, 25201, 25072, 24942, 24811, 24680, 24547,
24413, 24279, 24143, 24007, 23870, 23731, 23592, 23452, 23311, 23170, 23027, 22884, 22739,
22594, 22448, 22301, 22154, 22005, 21856, 21705, 21554, 21403, 21250, 21096, 20942, 20787,
20631, 20475, 20317, 20159, 20000, 19841, 19680, 19519, 19357, 19195, 19032, 18868, 18703,
18537, 18371, 18204, 18037, 17869, 17700, 17530, 17360, 17189, 17018, 16846, 16673, 16499,
16325, 16151, 15976, 15800, 15623, 15446, 15269, 15090, 14912, 14732, 14553, 14372, 14191,
14010, 13828, 13645, 13462, 13279, 13094, 12910, 12725, 12539, 12353, 12167, 11980, 11793,
11605, 11417, 11228, 11039, 10849, 10659, 10469, 10278, 10087, 9896, 9704, 9512, 9319, 9126,
8933, 8739, 8545, 8351, 8157, 7962, 7767, 7571, 7375, 7179, 6983, 6786, 6590, 6393, 6195, 5998,
5800, 5602, 5404, 5205, 5007, 4808, 4609, 4410, 4210, 4011, 3811, 3612, 3412, 3212, 3012, 2811,
2611, 2410, 2210, 2009, 1809, 1608, 1407, 1206, 1005, 804, 603, 402, 201, 0, -201, -402, -603,
-804, -1005, -1206, -1407, -1608, -1809, -2009, -2210, -2410, -2611, -2811, -3012, -3212,
-3412, -3612, -3811, -4011, -4210, -4410, -4609, -4808, -5007, -5205, -5404, -5602, -5800,
-5998, -6195, -6393, -6590, -6786, -6983, -7179, -7375, -7571, -7767, -7962, -8157, -8351,
-8545, -8739, -8933, -9126, -9319, -9512, -9704, -9896, -10087, -10278, -10469, -10659, -10849,
-11039, -11228, -11417, -11605, -11793, -11980, -12167, -12353, -12539, -12725, -12910, -13094,
-13279, -13462, -13645, -13828, -14010, -14191, -14372, -14553, -14732, -14912, -15090, -15269,
-15446, -15623, -15800, -15976, -16151, -16325, -16499, -16673, -16846, -17018, -17189, -17360,
-17530, -17700, -17869, -18037, -18204, -18371, -18537, -18703, -18868, -19032, -19195, -19357,
-19519, -19680, -19841, -20000, -20159, -20317, -20475, -20631, -20787, -20942, -21096, -21250,
-21403, -21554, -21705, -21856, -22005, -22154, -22301, -22448, -22594, -22739, -22884, -23027,
-23170, -23311, -23452, -23592, -23731, -23870, -24007, -24143, -24279, -24413, -24547, -24680,
-24811, -24942, -25072, -25201, -25329, -25456, -25582, -25708, -25832, -25955, -26077, -26198,
-26319, -26438, -26556, -26674, -26790, -26905, -27019, -27133, -27245, -27356, -27466, -27575,
-27683, -27790, -27896, -28001, -28105, -28208, -28310, -28411, -28510, -28609, -28706, -28803,
-28898, -28992, -29085, -29177, -29268, -29358, -29447, -29534, -29621, -29706, -29791, -29874,
-29956, -30037, -30117, -30195, -30273, -30349, -30424, -30498, -30571, -30643, -30714, -30783,
-30852, -30919, -30985, -31050, -31113, -31176, -31237, -31297, -31356, -31414, -31470, -31526,
-31580, -31633, -31685, -31736, -31785, -31833, -31880, -31926, -31971, -32014, -32057, -32098,
-32137, -32176, -32213, -32250, -32285, -32318, -32351, -32382, -32412, -32441, -32469, -32495,
-32521, -32545, -32567, -32589, -32609, -32628, -32646, -32663, -32678, -32692, -32705, -32717,
-32728, -32737, -32745, -32752, -32757, -32761, -32765, -32766,
];
fn q15_cmul(a: Complex<Q15>, b: Complex<Q15>) -> Complex<Q15> {
let rnd: i32 = 1 << 14;
Complex::new(
((a.re as i32 * b.re as i32 - a.im as i32 * b.im as i32 + rnd) >> 15) as i16,
((a.re as i32 * b.im as i32 + a.im as i32 * b.re as i32 + rnd) >> 15) as i16,
)
}
fn check_size(size: usize) -> Result<(), FFTError> {
if !(2..=SINETAB_N).contains(&size) {
return Err(FFTError::InvalidFFTSize);
}
const MSB: usize = (usize::MAX >> 1) + 1;
let allowed_size = MSB >> (size.leading_zeros());
if allowed_size != size {
return Err(FFTError::InvalidFFTSize);
}
Ok(())
}
pub fn fft_radix2_q15(data: &mut [Complex<Q15>], dir: Direction) -> Result<(), FFTError> {
let fft_size = data.len();
check_size(fft_size)?;
let (inverse, shift) = match dir {
Direction::Forward => (false, 0),
Direction::ForwardScaled => (false, 1),
Direction::Inverse => (true, 1),
};
let mut mr = 0usize;
for m in 1..fft_size {
const MSB: usize = (usize::MAX >> 1) + 1;
let l = MSB >> (fft_size - 1 - mr).leading_zeros();
mr = (mr & (l - 1)) + l;
if mr > m {
data.swap(m, mr);
}
}
let mut stage = 1;
let mut sine_step = (SINETAB_LD_N - 1) as isize;
while stage < fft_size {
let twiddle_step = stage << 1;
for group in 0..stage {
let sin_ndx = group << sine_step;
let wr = SINETAB[sin_ndx + SINETAB_N / 4] >> shift;
let wi = match inverse {
false => -SINETAB[sin_ndx],
true => SINETAB[sin_ndx],
} >> shift;
let w = Complex::new(wr, wi);
let mut i = group;
while i < fft_size {
let j = i + stage;
let temp = q15_cmul(w, data[j]);
let q = Complex::new(data[i].re >> shift, data[i].im >> shift);
data[i] = q + temp;
data[j] = q - temp;
i += twiddle_step;
}
}
sine_step -= 1;
stage = twiddle_step;
}
Ok(())
}
pub fn fft_radix2_real_q15(
input: &mut [i16],
output: &mut [Complex<Q15>],
scale: bool,
) -> Result<(), FFTError> {
let fft_len = input.len();
check_size(fft_len)?;
let half_fft_len = fft_len / 2;
if output.len() < half_fft_len + 1 {
return Err(FFTError::InvalidDataSize);
}
let ptr = input.as_mut_ptr() as *mut Complex<Q15>;
let fft_out = unsafe { slice::from_raw_parts_mut(ptr, half_fft_len) };
fft_radix2_q15(
&mut fft_out[0..half_fft_len],
if scale {
Direction::ForwardScaled
} else {
Direction::Forward
},
)?;
let shift = scale as usize;
output[0] = Complex::new((fft_out[0].im + fft_out[0].re) >> shift, 0);
for k in 1..half_fft_len {
let sin_ndx = k * SINETAB_N / fft_len;
let w = Complex::new(SINETAB[sin_ndx + SINETAB_N / 4], -SINETAB[sin_ndx]);
let zo = Complex::new(
(fft_out[half_fft_len - k].im + fft_out[k].im) >> (shift + 1),
(fft_out[half_fft_len - k].re - fft_out[k].re) >> (shift + 1),
);
let ze = Complex::new(
(fft_out[k].re + fft_out[half_fft_len - k].re) >> (shift + 1),
(fft_out[k].im - fft_out[half_fft_len - k].im) >> (shift + 1),
);
let r = ze + q15_cmul(zo, w);
output[k] = r;
}
output[half_fft_len] = Complex::new((fft_out[0].re - fft_out[0].im) >> shift, 0);
Ok(())
}
#[cfg(test)]
mod tests {
extern crate alloc;
use super::{check_size, FFTError, Q15, SINETAB, SINETAB_LD_N, SINETAB_N};
use alloc::vec::Vec;
use core::f64;
use core::mem::size_of;
use num_complex::Complex;
#[test]
fn verify_sinewave() {
for (i, v) in SINETAB.iter().enumerate() {
let fval = 32767.0 * f64::sin(2.0 * f64::consts::PI * (i as f64) / (SINETAB_N as f64));
let rval = fval.round() as i16;
assert_eq!(*v, rval);
}
}
#[test]
fn check_complex_lenth() {
assert_eq!(size_of::<Complex<Q15>>(), 2 * size_of::<Q15>());
}
#[test]
fn check_size_test() {
let valid = (1..=SINETAB_LD_N).map(|k| 1 << k).collect::<Vec<usize>>();
for s in 0..=SINETAB_N * 2 {
let result = check_size(s);
if valid.contains(&s) {
assert_eq!(result, Ok(()));
} else {
assert_eq!(result, Err(FFTError::InvalidFFTSize))
}
}
}
}