1 /* FFT analysis - spectrogram
2 * Copyright (C) 2013, 2019 Robin Gareus <robin@gareus.org>
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2, or (at your option)
7 * any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software Foundation,
16 * Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 */
18
19 #include <fftw3.h>
20 #include <pthread.h>
21 #include <stdio.h>
22 #include <sys/types.h>
23
24 #ifndef MIN
25 #define MIN(A, B) ((A) < (B) ? (A) : (B))
26 #endif
27
28 static pthread_mutex_t fftw_planner_lock = PTHREAD_MUTEX_INITIALIZER;
29 static unsigned int instance_count = 0;
30
31 typedef enum {
32 W_HANN = 0,
33 W_HAMMMIN,
34 W_NUTTALL,
35 W_BLACKMAN_NUTTALL,
36 W_BLACKMAN_HARRIS,
37 W_FLAT_TOP
38 } window_t;
39
40 /******************************************************************************
41 * internal FFT abstraction
42 */
43 struct FFTAnalysis {
44 uint32_t window_size;
45 window_t window_type;
46 uint32_t data_size;
47 double rate;
48 double freq_per_bin;
49 double phasediff_step;
50 float* window;
51 float* fft_in;
52 float* fft_out;
53 float* power;
54 float* phase;
55 float* phase_h;
56 fftwf_plan fftplan;
57
58 float* ringbuf;
59 uint32_t rboff;
60 uint32_t smps;
61 uint32_t sps;
62 uint32_t step;
63 double phasediff_bin;
64 };
65
66 /* ****************************************************************************
67 * windows
68 */
69 static double
ft_hannhamm(float * window,uint32_t n,double a,double b)70 ft_hannhamm (float* window, uint32_t n, double a, double b)
71 {
72 double sum = 0.0;
73 const double c = 2.0 * M_PI / (n - 1.0);
74 for (uint32_t i = 0; i < n; ++i) {
75 window[i] = a - b * cos (c * i);
76 sum += window[i];
77 }
78 return sum;
79 }
80
81 static double
ft_bnh(float * window,uint32_t n,double a0,double a1,double a2,double a3)82 ft_bnh (float* window, uint32_t n, double a0, double a1, double a2, double a3)
83 {
84 double sum = 0.0;
85 const double c = 2.0 * M_PI / (n - 1.0);
86 const double c2 = 2.0 * c;
87 const double c3 = 3.0 * c;
88
89 for (uint32_t i = 0; i < n; ++i) {
90 window[i] = a0 - a1 * cos(c * i) + a2 * cos(c2 * i) - a3 * cos(c3 * i);
91 sum += window[i];
92 }
93 return sum;
94 }
95
96 static double
ft_flattop(float * window,uint32_t n)97 ft_flattop (float* window, uint32_t n)
98 {
99 double sum = 0.0;
100 const double c = 2.0 * M_PI / (n - 1.0);
101 const double c2 = 2.0 * c;
102 const double c3 = 3.0 * c;
103 const double c4 = 4.0 * c;
104
105 const double a0 = 1.0;
106 const double a1 = 1.93;
107 const double a2 = 1.29;
108 const double a3 = 0.388;
109 const double a4 = 0.028;
110
111 for (uint32_t i = 0; i < n; ++i) {
112 window[i] = a0 - a1 * cos(c * i) + a2 * cos(c2 * i) - a3 * cos(c3 * i) + a4 * cos(c4 * i);
113 sum += window[i];
114 }
115 return sum;
116 }
117
118
119 /* ****************************************************************************
120 * internal private functions
121 */
122 static float*
ft_gen_window(struct FFTAnalysis * ft)123 ft_gen_window (struct FFTAnalysis* ft)
124 {
125 if (ft->window) {
126 return ft->window;
127 }
128
129 ft->window = (float*)malloc (sizeof (float) * ft->window_size);
130 double sum = .0;
131
132 /* https://en.wikipedia.org/wiki/Window_function */
133 switch (ft->window_type) {
134 default:
135 case W_HANN:
136 sum = ft_hannhamm (ft->window, ft->window_size, .5, .5);
137 break;
138 case W_HAMMMIN:
139 sum = ft_hannhamm (ft->window, ft->window_size, .54, .46);
140 break;
141 case W_NUTTALL:
142 sum = ft_bnh (ft->window, ft->window_size, .355768, .487396, .144232, .012604);
143 break;
144 case W_BLACKMAN_NUTTALL:
145 sum = ft_bnh (ft->window, ft->window_size, .3635819, .4891775, .1365995, .0106411);
146 break;
147 case W_BLACKMAN_HARRIS:
148 sum = ft_bnh (ft->window, ft->window_size, .35875, .48829, .14128, .01168);
149 break;
150 case W_FLAT_TOP:
151 sum = ft_flattop (ft->window, ft->window_size);
152 break;
153 }
154
155 const double isum = 2.0 / sum;
156 for (uint32_t i = 0; i < ft->window_size; i++) {
157 ft->window[i] *= isum;
158 }
159
160 return ft->window;
161 }
162
163 static void
ft_analyze(struct FFTAnalysis * ft)164 ft_analyze (struct FFTAnalysis* ft)
165 {
166 fftwf_execute (ft->fftplan);
167
168 memcpy (ft->phase_h, ft->phase, sizeof (float) * ft->data_size);
169 ft->power[0] = ft->fft_out[0] * ft->fft_out[0];
170 ft->phase[0] = 0;
171
172 #define FRe (ft->fft_out[i])
173 #define FIm (ft->fft_out[ft->window_size - i])
174 for (uint32_t i = 1; i < ft->data_size - 1; ++i) {
175 ft->power[i] = (FRe * FRe) + (FIm * FIm);
176 ft->phase[i] = atan2f (FIm, FRe);
177 }
178 #undef FRe
179 #undef FIm
180 }
181
182 /******************************************************************************
183 * public API (static for direct source inclusion)
184 */
185 #ifndef FFTX_FN_PREFIX
186 #define FFTX_FN_PREFIX static
187 #endif
188
189 FFTX_FN_PREFIX
190 void
fftx_reset(struct FFTAnalysis * ft)191 fftx_reset (struct FFTAnalysis* ft)
192 {
193 for (uint32_t i = 0; i < ft->data_size; ++i) {
194 ft->power[i] = 0;
195 ft->phase[i] = 0;
196 ft->phase_h[i] = 0;
197 }
198 for (uint32_t i = 0; i < ft->window_size; ++i) {
199 ft->ringbuf[i] = 0;
200 ft->fft_out[i] = 0;
201 }
202 ft->rboff = 0;
203 ft->smps = 0;
204 ft->step = 0;
205 }
206
207 FFTX_FN_PREFIX
208 void
fftx_init(struct FFTAnalysis * ft,uint32_t window_size,double rate,double fps)209 fftx_init (struct FFTAnalysis* ft, uint32_t window_size, double rate, double fps)
210 {
211 ft->rate = rate;
212 ft->window_size = window_size;
213 ft->window_type = W_HANN;
214 ft->data_size = window_size / 2;
215 ft->window = NULL;
216 ft->rboff = 0;
217 ft->smps = 0;
218 ft->step = 0;
219 ft->sps = (fps > 0) ? ceil (rate / fps) : 0;
220 ft->freq_per_bin = ft->rate / ft->data_size / 2.f;
221 ft->phasediff_step = M_PI / ft->data_size;
222 ft->phasediff_bin = 0;
223
224 ft->ringbuf = (float*)malloc (window_size * sizeof (float));
225 ft->fft_in = (float*)fftwf_malloc (sizeof (float) * window_size);
226 ft->fft_out = (float*)fftwf_malloc (sizeof (float) * window_size);
227 ft->power = (float*)malloc (ft->data_size * sizeof (float));
228 ft->phase = (float*)malloc (ft->data_size * sizeof (float));
229 ft->phase_h = (float*)malloc (ft->data_size * sizeof (float));
230
231 fftx_reset (ft);
232
233 pthread_mutex_lock (&fftw_planner_lock);
234 ft->fftplan = fftwf_plan_r2r_1d (window_size, ft->fft_in, ft->fft_out, FFTW_R2HC, FFTW_MEASURE);
235 ++instance_count;
236 pthread_mutex_unlock (&fftw_planner_lock);
237 }
238
239 FFTX_FN_PREFIX
240 void
fftx_set_window(struct FFTAnalysis * ft,window_t type)241 fftx_set_window (struct FFTAnalysis* ft, window_t type)
242 {
243 if (ft->window_type == type) {
244 return;
245 }
246 ft->window_type = type;
247 free (ft->window);
248 ft->window = NULL;
249 }
250
251 FFTX_FN_PREFIX
252 void
fftx_free(struct FFTAnalysis * ft)253 fftx_free (struct FFTAnalysis* ft)
254 {
255 if (!ft) {
256 return;
257 }
258 pthread_mutex_lock (&fftw_planner_lock);
259 fftwf_destroy_plan (ft->fftplan);
260 if (instance_count > 0) {
261 --instance_count;
262 }
263 #ifdef WITH_STATIC_FFTW_CLEANUP
264 /* use this only when statically linking to a local fftw!
265 *
266 * "After calling fftw_cleanup, all existing plans become undefined,
267 * and you should not attempt to execute them nor to destroy them."
268 * [http://www.fftw.org/fftw3_doc/Using-Plans.html]
269 *
270 * If libfftwf is shared with other plugins or the host this can
271 * cause undefined behavior.
272 */
273 if (instance_count == 0) {
274 fftwf_cleanup ();
275 }
276 #endif
277 pthread_mutex_unlock (&fftw_planner_lock);
278 free (ft->window);
279 free (ft->ringbuf);
280 fftwf_free (ft->fft_in);
281 fftwf_free (ft->fft_out);
282 free (ft->power);
283 free (ft->phase);
284 free (ft->phase_h);
285 free (ft);
286 }
287
288 static int
_fftx_run(struct FFTAnalysis * ft,const uint32_t n_samples,float const * const data)289 _fftx_run (struct FFTAnalysis* ft,
290 const uint32_t n_samples, float const* const data)
291 {
292 assert (n_samples <= ft->window_size);
293
294 float* const f_buf = ft->fft_in;
295 float* const r_buf = ft->ringbuf;
296
297 const uint32_t n_off = ft->rboff;
298 const uint32_t n_siz = ft->window_size;
299 const uint32_t n_old = n_siz - n_samples;
300
301 for (uint32_t i = 0; i < n_samples; ++i) {
302 r_buf[(i + n_off) % n_siz] = data[i];
303 f_buf[n_old + i] = data[i];
304 }
305
306 ft->rboff = (ft->rboff + n_samples) % n_siz;
307 #if 1
308 ft->smps += n_samples;
309 if (ft->smps < ft->sps) {
310 return -1;
311 }
312 ft->step = ft->smps;
313 ft->smps = 0;
314 #else
315 ft->step = n_samples;
316 #endif
317
318 /* copy samples from ringbuffer into fft-buffer */
319 const uint32_t p0s = (n_off + n_samples) % n_siz;
320 if (p0s + n_old >= n_siz) {
321 const uint32_t n_p1 = n_siz - p0s;
322 const uint32_t n_p2 = n_old - n_p1;
323 memcpy (f_buf, &r_buf[p0s], sizeof (float) * n_p1);
324 memcpy (&f_buf[n_p1], &r_buf[0], sizeof (float) * n_p2);
325 } else {
326 memcpy (&f_buf[0], &r_buf[p0s], sizeof (float) * n_old);
327 }
328
329 /* apply window function */
330 float const* const window = ft_gen_window (ft);
331 for (uint32_t i = 0; i < ft->window_size; i++) {
332 ft->fft_in[i] *= window[i];
333 }
334
335 /* ..and analyze */
336 ft_analyze (ft);
337
338 ft->phasediff_bin = ft->phasediff_step * (double)ft->step;
339 return 0;
340 }
341
342 FFTX_FN_PREFIX
343 int
fftx_run(struct FFTAnalysis * ft,const uint32_t n_samples,float const * const data)344 fftx_run (struct FFTAnalysis* ft,
345 const uint32_t n_samples, float const* const data)
346 {
347 if (n_samples <= ft->window_size) {
348 return _fftx_run (ft, n_samples, data);
349 }
350
351 int rv = -1;
352 uint32_t n = 0;
353 while (n < n_samples) {
354 uint32_t step = MIN (ft->window_size, n_samples - n);
355 if (!_fftx_run (ft, step, &data[n])) {
356 rv = 0;
357 }
358 n += step;
359 }
360 return rv;
361 }
362
363 FFTX_FN_PREFIX
364 void
fa_analyze_dsp(struct FFTAnalysis * ft,void (* run)(void *,uint32_t,float *),void * handle)365 fa_analyze_dsp (struct FFTAnalysis* ft,
366 void (*run) (void*, uint32_t, float*), void* handle)
367 {
368 float* buf = ft->fft_in;
369
370 /* pre-run 8K samples... (re-init/flush effect) */
371 uint32_t prerun_n_samples = 8192;
372 while (prerun_n_samples > 0) {
373 uint32_t n_samples = MIN (prerun_n_samples, ft->window_size);
374 memset (buf, 0, sizeof (float) * n_samples);
375 run (handle, n_samples, buf);
376 prerun_n_samples -= n_samples;
377 }
378
379 /* delta impulse */
380 memset (buf, 0, sizeof (float) * ft->window_size);
381 *buf = 1.0;
382 /* call plugin's run() function -- in-place processing */
383 run (handle, ft->window_size, buf);
384 ft->step = ft->window_size;
385 /* ..and analyze */
386 ft_analyze (ft);
387 }
388
389 /* ***************************************************************************
390 * convenient access functions
391 */
392
393 FFTX_FN_PREFIX
394 uint32_t
fftx_bins(struct FFTAnalysis * ft)395 fftx_bins (struct FFTAnalysis* ft)
396 {
397 return ft->data_size;
398 }
399
400 FFTX_FN_PREFIX
401 inline float
fast_log2(float val)402 fast_log2 (float val)
403 {
404 union {
405 float f;
406 int i;
407 } t;
408 t.f = val;
409 int* const exp_ptr = &t.i;
410 int x = *exp_ptr;
411 const int log_2 = ((x >> 23) & 255) - 128;
412 x &= ~(255 << 23);
413 x += 127 << 23;
414 *exp_ptr = x;
415 val = ((-1.0f / 3) * t.f + 2) * t.f - 2.0f / 3;
416 return (val + log_2);
417 }
418
419 FFTX_FN_PREFIX
420 inline float
fast_log(const float val)421 fast_log (const float val)
422 {
423 return (fast_log2 (val) * 0.69314718f);
424 }
425
426 FFTX_FN_PREFIX
427 inline float
fast_log10(const float val)428 fast_log10 (const float val)
429 {
430 return fast_log2 (val) / 3.312500f;
431 }
432
433 FFTX_FN_PREFIX
434 inline float
fftx_power_to_dB(float a)435 fftx_power_to_dB (float a)
436 {
437 /* 10 instead of 20 because of squared signal -- no sqrt(powerp[]) */
438 return a > 1e-12 ? 10.0 * fast_log10 (a) : -INFINITY;
439 }
440
441 FFTX_FN_PREFIX
442 float
fftx_power_at_bin(struct FFTAnalysis * ft,const int b)443 fftx_power_at_bin (struct FFTAnalysis* ft, const int b)
444 {
445 return (fftx_power_to_dB (ft->power[b]));
446 }
447
448 FFTX_FN_PREFIX
449 float
fftx_freq_at_bin(struct FFTAnalysis * ft,const int b)450 fftx_freq_at_bin (struct FFTAnalysis* ft, const int b)
451 {
452 /* calc phase: difference minus expected difference */
453 float phase = ft->phase[b] - ft->phase_h[b] - (float)b * ft->phasediff_bin;
454 /* clamp to -M_PI .. M_PI */
455 int over = phase / M_PI;
456 over += (over >= 0) ? (over & 1) : -(over & 1);
457 phase -= M_PI * (float)over;
458 /* scale according to overlap */
459 phase *= (ft->data_size / ft->step) / M_PI;
460 return ft->freq_per_bin * ((float)b + phase);
461 }
462