1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
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 = len - FFTLEN;
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;
}
}
|
