diff options
Diffstat (limited to 'fft/fft.c')
| -rw-r--r-- | fft/fft.c | 154 |
1 files changed, 154 insertions, 0 deletions
diff --git a/fft/fft.c b/fft/fft.c new file mode 100644 index 0000000..69c0971 --- /dev/null +++ b/fft/fft.c @@ -0,0 +1,154 @@ +#include "fft.h" +#include <math.h> +#include <string.h> + +void fft_write(struct fmplayer_fft_data *data, const int16_t *buf, unsigned len) { + if (len > FFTLEN) { + unsigned discard = FFTLEN - len; + buf += discard*2; + len = FFTLEN; + } + unsigned towrite = FFTLEN - data->ind; + if (towrite > len) towrite = len; + for (unsigned i = 0; i < towrite; i++) { + data->buf[data->ind+i] = ((uint32_t)buf[2*i+0] + buf[2*i+1]) / 2; + } + data->ind = (data->ind + towrite) % FFTLEN; + buf += towrite*2; + len -= towrite; + + for (unsigned i = 0; i < len; i++) { + data->buf[i] = ((uint32_t)buf[2*i+0] + buf[2*i+1]) / 2; + } + data->ind = (data->ind + len) % FFTLEN; +} + +static const uint16_t fftfreqtab[FFTDISPLEN+1] = { + 0, + 18, 19, 21, 22, 23, 25, 26, 28, 29, 31, + 33, 35, 37, 39, 42, 44, 47, 50, 53, 56, + 59, 63, 66, 70, 75, 79, 84, 89, 94, 100, + 106, 112, 119, 126, 133, 141, 150, 158, 168, 178, + 189, 200, 212, 224, 238, 252, 267, 283, 300, 317, + 336, 356, 378, 400, 424, 449, 476, 504, 534, 566, + 600, 635, 673, 713, 756, 801, 848, 899, 952, 1009, +}; + +enum { + HFFTLENBIT = 12, + HFFTLEN = 1<<HFFTLENBIT, +}; + +static uint16_t window[FFTLEN]; +static float tritab[FFTLEN + FFTLEN/4]; + +void fft_init_table(void) { + const double pi = acos(0.0) * 2.0; + double alpha = 0.54; + double beta = 1.0 - alpha; + for (unsigned i = 0; i < FFTLEN; i++) { + double v = alpha - beta * cos(2.0*pi*i/(FFTLEN-1)); + window[i] = v * (1<<16); + } + for (unsigned i = 0; i < (FFTLEN + FFTLEN/4); i++) { + tritab[i] = sin(2.0*pi*i/FFTLEN); + } +} + +static float coscalc(unsigned i) { + return tritab[(i & (FFTLEN-1)) + FFTLEN/4]; +} + +static float sincalc(unsigned i) { + return tritab[i & (FFTLEN-1)]; +} + +static float ar(unsigned i) { + return 0.5f*(1.0f-sincalc(i)); +} + +static float ai(unsigned i) { + return -0.5f*coscalc(i); +} + +static float br(unsigned i) { + return 0.5f*(1.0f+sincalc(i)); +} + +static float bi(unsigned i) { + return 0.5f*coscalc(i); +} + +static void fft_real(float *fftbuf) { + unsigned b = 0; + for (unsigned i = 0; i < HFFTLEN; i++) { + unsigned ii = 0; + for (unsigned bit = 0; bit < HFFTLENBIT; bit++) { + ii |= ((i >> bit) & 1u) << (HFFTLENBIT-bit-1); + } + fftbuf[(!b)*FFTLEN+i*2+0] = fftbuf[b*FFTLEN+ii*2+0]; + fftbuf[(!b)*FFTLEN+i*2+1] = fftbuf[b*FFTLEN+ii*2+1]; + } + b = !b; + for (unsigned bit = 0; bit < HFFTLENBIT; bit++) { + for (unsigned i = 0; i < HFFTLEN; i++) { + unsigned ei = i & ~(1u<<bit); + unsigned oi = i | (1u<<bit); + float are = fftbuf[b*FFTLEN+oi*2+0]; + float aim = fftbuf[b*FFTLEN+oi*2+1]; + float bre = coscalc(i<<(HFFTLENBIT-bit)); + float bim = sincalc(i<<(HFFTLENBIT-bit)); + float cre = are*bre - aim*bim; + float cim = are*bim + aim*bre; + fftbuf[(!b)*FFTLEN+i*2+0] = fftbuf[b*FFTLEN+ei*2+0] + cre; + fftbuf[(!b)*FFTLEN+i*2+1] = fftbuf[b*FFTLEN+ei*2+1] + cim; + } + b = !b; + } + for (unsigned i = 0; i < HFFTLEN; i++) { + float xr = fftbuf[b*FFTLEN+i*2+0]; + float rxr = fftbuf[b*FFTLEN+0]; + if (i) rxr = fftbuf[b*FFTLEN+(HFFTLEN-i)*2+0]; + float xi = fftbuf[b*FFTLEN+i*2+1]; + float rxi = fftbuf[b*FFTLEN+1]; + if (i) rxi = fftbuf[b*FFTLEN+(HFFTLEN-i)*2+1]; + fftbuf[(!b)*FFTLEN+i*2+0] = xr * ar(i) - xi * ai(i) + rxr * br(i) + rxi * bi(i); + fftbuf[(!b)*FFTLEN+i*2+1] = xi * ar(i) + xr * ai(i) + rxr * bi(i) - rxi * br(i); + } + if (!b) { + memcpy(fftbuf, fftbuf+FFTLEN, FFTLEN*sizeof(fftbuf[0])); + } +} + +void fft_calc(struct fmplayer_fft_disp_data *ddata, struct fmplayer_fft_input_data *idata) { + for (int i = 0; i < FFTLEN; i++) { + int fi = (i + idata->fdata.ind) % FFTLEN; + idata->work[i] = (((int32_t)idata->fdata.buf[fi]) * window[i]) >> 16; + } + for (int i = 0; i < FFTLEN; i++) { + idata->fwork[i] = ((float)idata->work[i])/32768; + } + fft_real(idata->fwork); + for (int i = 1; i < FFTLEN/2; i++) { + float re = idata->fwork[i*2]; + float im = idata->fwork[i*2+1]; + idata->fwork[i] = sqrtf((re*re) + (im*im)); + } + for (int i = 0; i < FFTLEN/2; i++) { + idata->fwork[i] = idata->fwork[i] / sqrtf(FFTLEN); + } + float dbuf[FFTDISPLEN]; + for (int i = 0; i < FFTDISPLEN; i++) { + dbuf[i] = 0.0f; + for (int j = fftfreqtab[i]; j < fftfreqtab[i+1]; j++) { + dbuf[i] += idata->fwork[j]; + } + dbuf[i] /= fftfreqtab[i+1] - fftfreqtab[i]; + } + for (int i = 0; i < FFTDISPLEN; i++) { + float res = (dbuf[i] > (1.0f / 256)) ? (4.0f*log2f(dbuf[i]) + 32.0f) : 0.0f; + if (res > 31.0f) res = 31.0f; + if (res < 0.0f) res = 0.0f; + ddata->buf[i] = res; + } +} |