avfilter/af_sofalizer: speed up fast convolution
[ffmpeg.git] / libavfilter / af_aiir.c
1 /*
2  * Copyright (c) 2018 Paul B Mahol
3  *
4  * This file is part of FFmpeg.
5  *
6  * FFmpeg is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU Lesser General Public
8  * License as published by the Free Software Foundation; either
9  * version 2.1 of the License, or (at your option) any later version.
10  *
11  * FFmpeg is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
14  * Lesser General Public License for more details.
15  *
16  * You should have received a copy of the GNU Lesser General Public
17  * License along with FFmpeg; if not, write to the Free Software
18  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
19  */
20
21 #include <float.h>
22
23 #include "libavutil/avassert.h"
24 #include "libavutil/avstring.h"
25 #include "libavutil/intreadwrite.h"
26 #include "libavutil/opt.h"
27 #include "libavutil/xga_font_data.h"
28 #include "audio.h"
29 #include "avfilter.h"
30 #include "internal.h"
31
32 typedef struct ThreadData {
33     AVFrame *in, *out;
34 } ThreadData;
35
36 typedef struct Pair {
37     int a, b;
38 } Pair;
39
40 typedef struct BiquadContext {
41     double a0, a1, a2;
42     double b0, b1, b2;
43     double i1, i2;
44     double o1, o2;
45 } BiquadContext;
46
47 typedef struct IIRChannel {
48     int nb_ab[2];
49     double *ab[2];
50     double g;
51     double *cache[2];
52     BiquadContext *biquads;
53     int clippings;
54 } IIRChannel;
55
56 typedef struct AudioIIRContext {
57     const AVClass *class;
58     char *a_str, *b_str, *g_str;
59     double dry_gain, wet_gain;
60     int format;
61     int process;
62     int precision;
63     int response;
64     int w, h;
65     int ir_channel;
66     AVRational rate;
67
68     AVFrame *video;
69
70     IIRChannel *iir;
71     int channels;
72     enum AVSampleFormat sample_format;
73
74     int (*iir_channel)(AVFilterContext *ctx, void *arg, int ch, int nb_jobs);
75 } AudioIIRContext;
76
77 static int query_formats(AVFilterContext *ctx)
78 {
79     AudioIIRContext *s = ctx->priv;
80     AVFilterFormats *formats;
81     AVFilterChannelLayouts *layouts;
82     enum AVSampleFormat sample_fmts[] = {
83         AV_SAMPLE_FMT_DBLP,
84         AV_SAMPLE_FMT_NONE
85     };
86     static const enum AVPixelFormat pix_fmts[] = {
87         AV_PIX_FMT_RGB0,
88         AV_PIX_FMT_NONE
89     };
90     int ret;
91
92     if (s->response) {
93         AVFilterLink *videolink = ctx->outputs[1];
94
95         formats = ff_make_format_list(pix_fmts);
96         if ((ret = ff_formats_ref(formats, &videolink->in_formats)) < 0)
97             return ret;
98     }
99
100     layouts = ff_all_channel_counts();
101     if (!layouts)
102         return AVERROR(ENOMEM);
103     ret = ff_set_common_channel_layouts(ctx, layouts);
104     if (ret < 0)
105         return ret;
106
107     sample_fmts[0] = s->sample_format;
108     formats = ff_make_format_list(sample_fmts);
109     if (!formats)
110         return AVERROR(ENOMEM);
111     ret = ff_set_common_formats(ctx, formats);
112     if (ret < 0)
113         return ret;
114
115     formats = ff_all_samplerates();
116     if (!formats)
117         return AVERROR(ENOMEM);
118     return ff_set_common_samplerates(ctx, formats);
119 }
120
121 #define IIR_CH(name, type, min, max, need_clipping)                     \
122 static int iir_ch_## name(AVFilterContext *ctx, void *arg, int ch, int nb_jobs)  \
123 {                                                                       \
124     AudioIIRContext *s = ctx->priv;                                     \
125     const double ig = s->dry_gain;                                      \
126     const double og = s->wet_gain;                                      \
127     ThreadData *td = arg;                                               \
128     AVFrame *in = td->in, *out = td->out;                               \
129     const type *src = (const type *)in->extended_data[ch];              \
130     double *ic = (double *)s->iir[ch].cache[0];                         \
131     double *oc = (double *)s->iir[ch].cache[1];                         \
132     const int nb_a = s->iir[ch].nb_ab[0];                               \
133     const int nb_b = s->iir[ch].nb_ab[1];                               \
134     const double *a = s->iir[ch].ab[0];                                 \
135     const double *b = s->iir[ch].ab[1];                                 \
136     int *clippings = &s->iir[ch].clippings;                             \
137     type *dst = (type *)out->extended_data[ch];                         \
138     int n;                                                              \
139                                                                         \
140     for (n = 0; n < in->nb_samples; n++) {                              \
141         double sample = 0.;                                             \
142         int x;                                                          \
143                                                                         \
144         memmove(&ic[1], &ic[0], (nb_b - 1) * sizeof(*ic));              \
145         memmove(&oc[1], &oc[0], (nb_a - 1) * sizeof(*oc));              \
146         ic[0] = src[n] * ig;                                            \
147         for (x = 0; x < nb_b; x++)                                      \
148             sample += b[x] * ic[x];                                     \
149                                                                         \
150         for (x = 1; x < nb_a; x++)                                      \
151             sample -= a[x] * oc[x];                                     \
152                                                                         \
153         oc[0] = sample;                                                 \
154         sample *= og;                                                   \
155         if (need_clipping && sample < min) {                            \
156             (*clippings)++;                                             \
157             dst[n] = min;                                               \
158         } else if (need_clipping && sample > max) {                     \
159             (*clippings)++;                                             \
160             dst[n] = max;                                               \
161         } else {                                                        \
162             dst[n] = sample;                                            \
163         }                                                               \
164     }                                                                   \
165                                                                         \
166     return 0;                                                           \
167 }
168
169 IIR_CH(s16p, int16_t, INT16_MIN, INT16_MAX, 1)
170 IIR_CH(s32p, int32_t, INT32_MIN, INT32_MAX, 1)
171 IIR_CH(fltp, float,         -1.,        1., 0)
172 IIR_CH(dblp, double,        -1.,        1., 0)
173
174 #define SERIAL_IIR_CH(name, type, min, max, need_clipping)                  \
175 static int iir_ch_serial_## name(AVFilterContext *ctx, void *arg, int ch, int nb_jobs)  \
176 {                                                                       \
177     AudioIIRContext *s = ctx->priv;                                     \
178     const double ig = s->dry_gain;                                      \
179     const double og = s->wet_gain;                                      \
180     ThreadData *td = arg;                                               \
181     AVFrame *in = td->in, *out = td->out;                               \
182     const type *src = (const type *)in->extended_data[ch];              \
183     type *dst = (type *)out->extended_data[ch];                         \
184     IIRChannel *iir = &s->iir[ch];                                      \
185     int *clippings = &iir->clippings;                                   \
186     int nb_biquads = (FFMAX(iir->nb_ab[0], iir->nb_ab[1]) + 1) / 2;     \
187     int n, i;                                                           \
188                                                                         \
189     for (i = 0; i < nb_biquads; i++) {                                  \
190         const double a1 = -iir->biquads[i].a1;                          \
191         const double a2 = -iir->biquads[i].a2;                          \
192         const double b0 = iir->biquads[i].b0;                           \
193         const double b1 = iir->biquads[i].b1;                           \
194         const double b2 = iir->biquads[i].b2;                           \
195         double i1 = iir->biquads[i].i1;                                 \
196         double i2 = iir->biquads[i].i2;                                 \
197         double o1 = iir->biquads[i].o1;                                 \
198         double o2 = iir->biquads[i].o2;                                 \
199                                                                         \
200         for (n = 0; n < in->nb_samples; n++) {                          \
201             double sample = ig * (i ? dst[n] : src[n]);                 \
202             double o0 = sample * b0 + i1 * b1 + i2 * b2 + o1 * a1 + o2 * a2; \
203                                                                         \
204             i2 = i1;                                                    \
205             i1 = src[n];                                                \
206             o2 = o1;                                                    \
207             o1 = o0;                                                    \
208             o0 *= og;                                                   \
209                                                                         \
210             if (need_clipping && o0 < min) {                            \
211                 (*clippings)++;                                         \
212                 dst[n] = min;                                           \
213             } else if (need_clipping && o0 > max) {                     \
214                 (*clippings)++;                                         \
215                 dst[n] = max;                                           \
216             } else {                                                    \
217                 dst[n] = o0;                                            \
218             }                                                           \
219         }                                                               \
220         iir->biquads[i].i1 = i1;                                        \
221         iir->biquads[i].i2 = i2;                                        \
222         iir->biquads[i].o1 = o1;                                        \
223         iir->biquads[i].o2 = o2;                                        \
224     }                                                                   \
225                                                                         \
226     return 0;                                                           \
227 }
228
229 SERIAL_IIR_CH(s16p, int16_t, INT16_MIN, INT16_MAX, 1)
230 SERIAL_IIR_CH(s32p, int32_t, INT32_MIN, INT32_MAX, 1)
231 SERIAL_IIR_CH(fltp, float,         -1.,        1., 0)
232 SERIAL_IIR_CH(dblp, double,        -1.,        1., 0)
233
234 static void count_coefficients(char *item_str, int *nb_items)
235 {
236     char *p;
237
238     if (!item_str)
239         return;
240
241     *nb_items = 1;
242     for (p = item_str; *p && *p != '|'; p++) {
243         if (*p == ' ')
244             (*nb_items)++;
245     }
246 }
247
248 static int read_gains(AVFilterContext *ctx, char *item_str, int nb_items)
249 {
250     AudioIIRContext *s = ctx->priv;
251     char *p, *arg, *old_str, *prev_arg = NULL, *saveptr = NULL;
252     int i;
253
254     p = old_str = av_strdup(item_str);
255     if (!p)
256         return AVERROR(ENOMEM);
257     for (i = 0; i < nb_items; i++) {
258         if (!(arg = av_strtok(p, "|", &saveptr)))
259             arg = prev_arg;
260
261         if (!arg) {
262             av_freep(&old_str);
263             return AVERROR(EINVAL);
264         }
265
266         p = NULL;
267         if (sscanf(arg, "%lf", &s->iir[i].g) != 1) {
268             av_log(ctx, AV_LOG_ERROR, "Invalid gains supplied: %s\n", arg);
269             av_freep(&old_str);
270             return AVERROR(EINVAL);
271         }
272
273         prev_arg = arg;
274     }
275
276     av_freep(&old_str);
277
278     return 0;
279 }
280
281 static int read_tf_coefficients(AVFilterContext *ctx, char *item_str, int nb_items, double *dst)
282 {
283     char *p, *arg, *old_str, *saveptr = NULL;
284     int i;
285
286     p = old_str = av_strdup(item_str);
287     if (!p)
288         return AVERROR(ENOMEM);
289     for (i = 0; i < nb_items; i++) {
290         if (!(arg = av_strtok(p, " ", &saveptr)))
291             break;
292
293         p = NULL;
294         if (sscanf(arg, "%lf", &dst[i]) != 1) {
295             av_log(ctx, AV_LOG_ERROR, "Invalid coefficients supplied: %s\n", arg);
296             av_freep(&old_str);
297             return AVERROR(EINVAL);
298         }
299     }
300
301     av_freep(&old_str);
302
303     return 0;
304 }
305
306 static int read_zp_coefficients(AVFilterContext *ctx, char *item_str, int nb_items, double *dst, const char *format)
307 {
308     char *p, *arg, *old_str, *saveptr = NULL;
309     int i;
310
311     p = old_str = av_strdup(item_str);
312     if (!p)
313         return AVERROR(ENOMEM);
314     for (i = 0; i < nb_items; i++) {
315         if (!(arg = av_strtok(p, " ", &saveptr)))
316             break;
317
318         p = NULL;
319         if (sscanf(arg, format, &dst[i*2], &dst[i*2+1]) != 2) {
320             av_log(ctx, AV_LOG_ERROR, "Invalid coefficients supplied: %s\n", arg);
321             av_freep(&old_str);
322             return AVERROR(EINVAL);
323         }
324     }
325
326     av_freep(&old_str);
327
328     return 0;
329 }
330
331 static const char *format[] = { "%lf", "%lf %lfi", "%lf %lfr", "%lf %lfd" };
332
333 static int read_channels(AVFilterContext *ctx, int channels, uint8_t *item_str, int ab)
334 {
335     AudioIIRContext *s = ctx->priv;
336     char *p, *arg, *old_str, *prev_arg = NULL, *saveptr = NULL;
337     int i, ret;
338
339     p = old_str = av_strdup(item_str);
340     if (!p)
341         return AVERROR(ENOMEM);
342     for (i = 0; i < channels; i++) {
343         IIRChannel *iir = &s->iir[i];
344
345         if (!(arg = av_strtok(p, "|", &saveptr)))
346             arg = prev_arg;
347
348         if (!arg) {
349             av_freep(&old_str);
350             return AVERROR(EINVAL);
351         }
352
353         count_coefficients(arg, &iir->nb_ab[ab]);
354
355         p = NULL;
356         iir->cache[ab] = av_calloc(iir->nb_ab[ab] + 1, sizeof(double));
357         iir->ab[ab] = av_calloc(iir->nb_ab[ab] * (!!s->format + 1), sizeof(double));
358         if (!iir->ab[ab] || !iir->cache[ab]) {
359             av_freep(&old_str);
360             return AVERROR(ENOMEM);
361         }
362
363         if (s->format) {
364             ret = read_zp_coefficients(ctx, arg, iir->nb_ab[ab], iir->ab[ab], format[s->format]);
365         } else {
366             ret = read_tf_coefficients(ctx, arg, iir->nb_ab[ab], iir->ab[ab]);
367         }
368         if (ret < 0) {
369             av_freep(&old_str);
370             return ret;
371         }
372         prev_arg = arg;
373     }
374
375     av_freep(&old_str);
376
377     return 0;
378 }
379
380 static void multiply(double wre, double wim, int npz, double *coeffs)
381 {
382     double nwre = -wre, nwim = -wim;
383     double cre, cim;
384     int i;
385
386     for (i = npz; i >= 1; i--) {
387         cre = coeffs[2 * i + 0];
388         cim = coeffs[2 * i + 1];
389
390         coeffs[2 * i + 0] = (nwre * cre - nwim * cim) + coeffs[2 * (i - 1) + 0];
391         coeffs[2 * i + 1] = (nwre * cim + nwim * cre) + coeffs[2 * (i - 1) + 1];
392     }
393
394     cre = coeffs[0];
395     cim = coeffs[1];
396     coeffs[0] = nwre * cre - nwim * cim;
397     coeffs[1] = nwre * cim + nwim * cre;
398 }
399
400 static int expand(AVFilterContext *ctx, double *pz, int nb, double *coeffs)
401 {
402     int i;
403
404     coeffs[0] = 1.0;
405     coeffs[1] = 0.0;
406
407     for (i = 0; i < nb; i++) {
408         coeffs[2 * (i + 1)    ] = 0.0;
409         coeffs[2 * (i + 1) + 1] = 0.0;
410     }
411
412     for (i = 0; i < nb; i++)
413         multiply(pz[2 * i], pz[2 * i + 1], nb, coeffs);
414
415     for (i = 0; i < nb + 1; i++) {
416         if (fabs(coeffs[2 * i + 1]) > FLT_EPSILON) {
417             av_log(ctx, AV_LOG_ERROR, "coeff: %f of z^%d is not real; poles/zeros are not complex conjugates.\n",
418                    coeffs[2 * i + 1], i);
419             return AVERROR(EINVAL);
420         }
421     }
422
423     return 0;
424 }
425
426 static int convert_zp2tf(AVFilterContext *ctx, int channels)
427 {
428     AudioIIRContext *s = ctx->priv;
429     int ch, i, j, ret = 0;
430
431     for (ch = 0; ch < channels; ch++) {
432         IIRChannel *iir = &s->iir[ch];
433         double *topc, *botc;
434
435         topc = av_calloc((iir->nb_ab[0] + 1) * 2, sizeof(*topc));
436         botc = av_calloc((iir->nb_ab[1] + 1) * 2, sizeof(*botc));
437         if (!topc || !botc) {
438             ret = AVERROR(ENOMEM);
439             goto fail;
440         }
441
442         ret = expand(ctx, iir->ab[0], iir->nb_ab[0], botc);
443         if (ret < 0) {
444             goto fail;
445         }
446
447         ret = expand(ctx, iir->ab[1], iir->nb_ab[1], topc);
448         if (ret < 0) {
449             goto fail;
450         }
451
452         for (j = 0, i = iir->nb_ab[1]; i >= 0; j++, i--) {
453             iir->ab[1][j] = topc[2 * i];
454         }
455         iir->nb_ab[1]++;
456
457         for (j = 0, i = iir->nb_ab[0]; i >= 0; j++, i--) {
458             iir->ab[0][j] = botc[2 * i];
459         }
460         iir->nb_ab[0]++;
461
462 fail:
463         av_free(topc);
464         av_free(botc);
465         if (ret < 0)
466             break;
467     }
468
469     return ret;
470 }
471
472 static int decompose_zp2biquads(AVFilterContext *ctx, int channels)
473 {
474     AudioIIRContext *s = ctx->priv;
475     int ch, ret;
476
477     for (ch = 0; ch < channels; ch++) {
478         IIRChannel *iir = &s->iir[ch];
479         int nb_biquads = (FFMAX(iir->nb_ab[0], iir->nb_ab[1]) + 1) / 2;
480         int current_biquad = 0;
481
482         iir->biquads = av_calloc(nb_biquads, sizeof(BiquadContext));
483         if (!iir->biquads)
484             return AVERROR(ENOMEM);
485
486         while (nb_biquads--) {
487             Pair outmost_pole = { -1, -1 };
488             Pair nearest_zero = { -1, -1 };
489             double zeros[4] = { 0 };
490             double poles[4] = { 0 };
491             double b[6] = { 0 };
492             double a[6] = { 0 };
493             double min_distance = DBL_MAX;
494             double max_mag = 0;
495             int i;
496
497             for (i = 0; i < iir->nb_ab[0]; i++) {
498                 double mag;
499
500                 if (isnan(iir->ab[0][2 * i]) || isnan(iir->ab[0][2 * i + 1]))
501                     continue;
502                 mag = hypot(iir->ab[0][2 * i], iir->ab[0][2 * i + 1]);
503
504                 if (mag > max_mag) {
505                     max_mag = mag;
506                     outmost_pole.a = i;
507                 }
508             }
509
510             for (i = 0; i < iir->nb_ab[1]; i++) {
511                 if (isnan(iir->ab[0][2 * i]) || isnan(iir->ab[0][2 * i + 1]))
512                     continue;
513
514                 if (iir->ab[0][2 * i    ] ==  iir->ab[0][2 * outmost_pole.a    ] &&
515                     iir->ab[0][2 * i + 1] == -iir->ab[0][2 * outmost_pole.a + 1]) {
516                     outmost_pole.b = i;
517                     break;
518                 }
519             }
520
521             av_log(ctx, AV_LOG_VERBOSE, "outmost_pole is %d.%d\n", outmost_pole.a, outmost_pole.b);
522
523             if (outmost_pole.a < 0 || outmost_pole.b < 0)
524                 return AVERROR(EINVAL);
525
526             for (i = 0; i < iir->nb_ab[1]; i++) {
527                 double distance;
528
529                 if (isnan(iir->ab[1][2 * i]) || isnan(iir->ab[1][2 * i + 1]))
530                     continue;
531                 distance = hypot(iir->ab[0][2 * outmost_pole.a    ] - iir->ab[1][2 * i    ],
532                                  iir->ab[0][2 * outmost_pole.a + 1] - iir->ab[1][2 * i + 1]);
533
534                 if (distance < min_distance) {
535                     min_distance = distance;
536                     nearest_zero.a = i;
537                 }
538             }
539
540             for (i = 0; i < iir->nb_ab[1]; i++) {
541                 if (isnan(iir->ab[1][2 * i]) || isnan(iir->ab[1][2 * i + 1]))
542                     continue;
543
544                 if (iir->ab[1][2 * i    ] ==  iir->ab[1][2 * nearest_zero.a    ] &&
545                     iir->ab[1][2 * i + 1] == -iir->ab[1][2 * nearest_zero.a + 1]) {
546                     nearest_zero.b = i;
547                     break;
548                 }
549             }
550
551             av_log(ctx, AV_LOG_VERBOSE, "nearest_zero is %d.%d\n", nearest_zero.a, nearest_zero.b);
552
553             if (nearest_zero.a < 0 || nearest_zero.b < 0)
554                 return AVERROR(EINVAL);
555
556             poles[0] = iir->ab[0][2 * outmost_pole.a    ];
557             poles[1] = iir->ab[0][2 * outmost_pole.a + 1];
558
559             zeros[0] = iir->ab[1][2 * nearest_zero.a    ];
560             zeros[1] = iir->ab[1][2 * nearest_zero.a + 1];
561
562             if (nearest_zero.a == nearest_zero.b && outmost_pole.a == outmost_pole.b) {
563                 zeros[2] = 0;
564                 zeros[3] = 0;
565
566                 poles[2] = 0;
567                 poles[3] = 0;
568             } else {
569                 poles[2] = iir->ab[0][2 * outmost_pole.b    ];
570                 poles[3] = iir->ab[0][2 * outmost_pole.b + 1];
571
572                 zeros[2] = iir->ab[1][2 * nearest_zero.b    ];
573                 zeros[3] = iir->ab[1][2 * nearest_zero.b + 1];
574             }
575
576             ret = expand(ctx, zeros, 2, b);
577             if (ret < 0)
578                 return ret;
579
580             ret = expand(ctx, poles, 2, a);
581             if (ret < 0)
582                 return ret;
583
584             iir->ab[0][2 * outmost_pole.a] = iir->ab[0][2 * outmost_pole.a + 1] = NAN;
585             iir->ab[0][2 * outmost_pole.b] = iir->ab[0][2 * outmost_pole.b + 1] = NAN;
586             iir->ab[1][2 * nearest_zero.a] = iir->ab[1][2 * nearest_zero.a + 1] = NAN;
587             iir->ab[1][2 * nearest_zero.b] = iir->ab[1][2 * nearest_zero.b + 1] = NAN;
588
589             iir->biquads[current_biquad].a0 = 1.0;
590             iir->biquads[current_biquad].a1 = a[2] / a[4];
591             iir->biquads[current_biquad].a2 = a[0] / a[4];
592             iir->biquads[current_biquad].b0 = b[4] / a[4] * (current_biquad ? 1.0 : iir->g);
593             iir->biquads[current_biquad].b1 = b[2] / a[4] * (current_biquad ? 1.0 : iir->g);
594             iir->biquads[current_biquad].b2 = b[0] / a[4] * (current_biquad ? 1.0 : iir->g);
595
596             av_log(ctx, AV_LOG_VERBOSE, "a=%f %f %f:b=%f %f %f\n",
597                    iir->biquads[current_biquad].a0,
598                    iir->biquads[current_biquad].a1,
599                    iir->biquads[current_biquad].a2,
600                    iir->biquads[current_biquad].b0,
601                    iir->biquads[current_biquad].b1,
602                    iir->biquads[current_biquad].b2);
603
604             current_biquad++;
605         }
606     }
607
608     return 0;
609 }
610
611 static void convert_pr2zp(AVFilterContext *ctx, int channels)
612 {
613     AudioIIRContext *s = ctx->priv;
614     int ch;
615
616     for (ch = 0; ch < channels; ch++) {
617         IIRChannel *iir = &s->iir[ch];
618         int n;
619
620         for (n = 0; n < iir->nb_ab[0]; n++) {
621             double r = iir->ab[0][2*n];
622             double angle = iir->ab[0][2*n+1];
623
624             iir->ab[0][2*n]   = r * cos(angle);
625             iir->ab[0][2*n+1] = r * sin(angle);
626         }
627
628         for (n = 0; n < iir->nb_ab[1]; n++) {
629             double r = iir->ab[1][2*n];
630             double angle = iir->ab[1][2*n+1];
631
632             iir->ab[1][2*n]   = r * cos(angle);
633             iir->ab[1][2*n+1] = r * sin(angle);
634         }
635     }
636 }
637
638 static void convert_pd2zp(AVFilterContext *ctx, int channels)
639 {
640     AudioIIRContext *s = ctx->priv;
641     int ch;
642
643     for (ch = 0; ch < channels; ch++) {
644         IIRChannel *iir = &s->iir[ch];
645         int n;
646
647         for (n = 0; n < iir->nb_ab[0]; n++) {
648             double r = iir->ab[0][2*n];
649             double angle = M_PI*iir->ab[0][2*n+1]/180.;
650
651             iir->ab[0][2*n]   = r * cos(angle);
652             iir->ab[0][2*n+1] = r * sin(angle);
653         }
654
655         for (n = 0; n < iir->nb_ab[1]; n++) {
656             double r = iir->ab[1][2*n];
657             double angle = M_PI*iir->ab[1][2*n+1]/180.;
658
659             iir->ab[1][2*n]   = r * cos(angle);
660             iir->ab[1][2*n+1] = r * sin(angle);
661         }
662     }
663 }
664
665 static void drawtext(AVFrame *pic, int x, int y, const char *txt, uint32_t color)
666 {
667     const uint8_t *font;
668     int font_height;
669     int i;
670
671     font = avpriv_cga_font, font_height = 8;
672
673     for (i = 0; txt[i]; i++) {
674         int char_y, mask;
675
676         uint8_t *p = pic->data[0] + y * pic->linesize[0] + (x + i * 8) * 4;
677         for (char_y = 0; char_y < font_height; char_y++) {
678             for (mask = 0x80; mask; mask >>= 1) {
679                 if (font[txt[i] * font_height + char_y] & mask)
680                     AV_WL32(p, color);
681                 p += 4;
682             }
683             p += pic->linesize[0] - 8 * 4;
684         }
685     }
686 }
687
688 static void draw_line(AVFrame *out, int x0, int y0, int x1, int y1, uint32_t color)
689 {
690     int dx = FFABS(x1-x0);
691     int dy = FFABS(y1-y0), sy = y0 < y1 ? 1 : -1;
692     int err = (dx>dy ? dx : -dy) / 2, e2;
693
694     for (;;) {
695         AV_WL32(out->data[0] + y0 * out->linesize[0] + x0 * 4, color);
696
697         if (x0 == x1 && y0 == y1)
698             break;
699
700         e2 = err;
701
702         if (e2 >-dx) {
703             err -= dy;
704             x0--;
705         }
706
707         if (e2 < dy) {
708             err += dx;
709             y0 += sy;
710         }
711     }
712 }
713
714 static void draw_response(AVFilterContext *ctx, AVFrame *out)
715 {
716     AudioIIRContext *s = ctx->priv;
717     float *mag, *phase, min = FLT_MAX, max = FLT_MIN;
718     int prev_ymag = -1, prev_yphase = -1;
719     char text[32];
720     int ch, i, x;
721
722     memset(out->data[0], 0, s->h * out->linesize[0]);
723
724     phase = av_malloc_array(s->w, sizeof(*phase));
725     mag = av_malloc_array(s->w, sizeof(*mag));
726     if (!mag || !phase)
727         goto end;
728
729     ch = av_clip(s->ir_channel, 0, s->channels - 1);
730     for (i = 0; i < s->w; i++) {
731         const double *b = s->iir[ch].ab[0];
732         const double *a = s->iir[ch].ab[1];
733         double w = i * M_PI / (s->w - 1);
734         double realz, realp;
735         double imagz, imagp;
736         double real, imag, div;
737
738         if (s->format == 0) {
739             realz = 0., realp = 0.;
740             imagz = 0., imagp = 0.;
741             for (x = 0; x < s->iir[ch].nb_ab[1]; x++) {
742                 realz += cos(-x * w) * a[x];
743                 imagz += sin(-x * w) * a[x];
744             }
745
746             for (x = 0; x < s->iir[ch].nb_ab[0]; x++) {
747                 realp += cos(-x * w) * b[x];
748                 imagp += sin(-x * w) * b[x];
749             }
750
751             div = realp * realp + imagp * imagp;
752             real = (realz * realp + imagz * imagp) / div;
753             imag = (imagz * realp - imagp * realz) / div;
754         } else {
755             real = 1;
756             imag = 0;
757             for (x = 0; x < s->iir[ch].nb_ab[1]; x++) {
758                 double ore, oim, re, im;
759
760                 re = cos(w) - a[2 * x];
761                 im = sin(w) - a[2 * x + 1];
762
763                 ore = real;
764                 oim = imag;
765
766                 real = ore * re - oim * im;
767                 imag = ore * im + oim * re;
768             }
769
770             for (x = 0; x < s->iir[ch].nb_ab[0]; x++) {
771                 double ore, oim, re, im;
772
773                 re = cos(w) - b[2 * x];
774                 im = sin(w) - b[2 * x + 1];
775
776                 ore = real;
777                 oim = imag;
778                 div = re * re + im * im;
779
780                 real = (ore * re + oim * im) / div;
781                 imag = (oim * re - ore * im) / div;
782             }
783         }
784
785         mag[i] = s->iir[ch].g * hypot(real, imag);
786         phase[i] = atan2(imag, real);
787         min = fminf(min, mag[i]);
788         max = fmaxf(max, mag[i]);
789     }
790
791     for (i = 0; i < s->w; i++) {
792         int ymag = mag[i] / max * (s->h - 1);
793         int yphase = (0.5 * (1. + phase[i] / M_PI)) * (s->h - 1);
794
795         ymag = s->h - 1 - av_clip(ymag, 0, s->h - 1);
796         yphase = s->h - 1 - av_clip(yphase, 0, s->h - 1);
797
798         if (prev_ymag < 0)
799             prev_ymag = ymag;
800         if (prev_yphase < 0)
801             prev_yphase = yphase;
802
803         draw_line(out, i,   ymag, FFMAX(i - 1, 0),   prev_ymag, 0xFFFF00FF);
804         draw_line(out, i, yphase, FFMAX(i - 1, 0), prev_yphase, 0xFF00FF00);
805
806         prev_ymag   = ymag;
807         prev_yphase = yphase;
808     }
809
810     if (s->w > 400 && s->h > 100) {
811         drawtext(out, 2, 2, "Max Magnitude:", 0xDDDDDDDD);
812         snprintf(text, sizeof(text), "%.2f", max);
813         drawtext(out, 15 * 8 + 2, 2, text, 0xDDDDDDDD);
814
815         drawtext(out, 2, 12, "Min Magnitude:", 0xDDDDDDDD);
816         snprintf(text, sizeof(text), "%.2f", min);
817         drawtext(out, 15 * 8 + 2, 12, text, 0xDDDDDDDD);
818     }
819
820 end:
821     av_free(phase);
822     av_free(mag);
823 }
824
825 static int config_output(AVFilterLink *outlink)
826 {
827     AVFilterContext *ctx = outlink->src;
828     AudioIIRContext *s = ctx->priv;
829     AVFilterLink *inlink = ctx->inputs[0];
830     int ch, ret, i;
831
832     s->channels = inlink->channels;
833     s->iir = av_calloc(s->channels, sizeof(*s->iir));
834     if (!s->iir)
835         return AVERROR(ENOMEM);
836
837     ret = read_gains(ctx, s->g_str, inlink->channels);
838     if (ret < 0)
839         return ret;
840
841     ret = read_channels(ctx, inlink->channels, s->a_str, 0);
842     if (ret < 0)
843         return ret;
844
845     ret = read_channels(ctx, inlink->channels, s->b_str, 1);
846     if (ret < 0)
847         return ret;
848
849     if (s->format == 2) {
850         convert_pr2zp(ctx, inlink->channels);
851     } else if (s->format == 3) {
852         convert_pd2zp(ctx, inlink->channels);
853     }
854
855     av_frame_free(&s->video);
856     if (s->response) {
857         s->video = ff_get_video_buffer(ctx->outputs[1], s->w, s->h);
858         if (!s->video)
859             return AVERROR(ENOMEM);
860
861         draw_response(ctx, s->video);
862     }
863
864     if (s->format == 0)
865         av_log(ctx, AV_LOG_WARNING, "tf coefficients format is not recommended for too high number of zeros/poles.\n");
866
867     if (s->format > 0 && s->process == 0) {
868         av_log(ctx, AV_LOG_WARNING, "Direct processsing is not recommended for zp coefficients format.\n");
869
870         ret = convert_zp2tf(ctx, inlink->channels);
871         if (ret < 0)
872             return ret;
873     } else if (s->format == 0 && s->process == 1) {
874         av_log(ctx, AV_LOG_ERROR, "Serial cascading is not implemented for transfer function.\n");
875         return AVERROR_PATCHWELCOME;
876     } else if (s->format > 0 && s->process == 1) {
877         if (inlink->format == AV_SAMPLE_FMT_S16P)
878             av_log(ctx, AV_LOG_WARNING, "Serial cascading is not recommended for i16 precision.\n");
879
880         ret = decompose_zp2biquads(ctx, inlink->channels);
881         if (ret < 0)
882             return ret;
883     }
884
885     for (ch = 0; s->format == 0 && ch < inlink->channels; ch++) {
886         IIRChannel *iir = &s->iir[ch];
887
888         for (i = 1; i < iir->nb_ab[0]; i++) {
889             iir->ab[0][i] /= iir->ab[0][0];
890         }
891
892         for (i = 0; i < iir->nb_ab[1]; i++) {
893             iir->ab[1][i] *= iir->g / iir->ab[0][0];
894         }
895     }
896
897     switch (inlink->format) {
898     case AV_SAMPLE_FMT_DBLP: s->iir_channel = s->process == 1 ? iir_ch_serial_dblp : iir_ch_dblp; break;
899     case AV_SAMPLE_FMT_FLTP: s->iir_channel = s->process == 1 ? iir_ch_serial_fltp : iir_ch_fltp; break;
900     case AV_SAMPLE_FMT_S32P: s->iir_channel = s->process == 1 ? iir_ch_serial_s32p : iir_ch_s32p; break;
901     case AV_SAMPLE_FMT_S16P: s->iir_channel = s->process == 1 ? iir_ch_serial_s16p : iir_ch_s16p; break;
902     }
903
904     return 0;
905 }
906
907 static int filter_frame(AVFilterLink *inlink, AVFrame *in)
908 {
909     AVFilterContext *ctx = inlink->dst;
910     AudioIIRContext *s = ctx->priv;
911     AVFilterLink *outlink = ctx->outputs[0];
912     ThreadData td;
913     AVFrame *out;
914     int ch, ret;
915
916     if (av_frame_is_writable(in)) {
917         out = in;
918     } else {
919         out = ff_get_audio_buffer(outlink, in->nb_samples);
920         if (!out) {
921             av_frame_free(&in);
922             return AVERROR(ENOMEM);
923         }
924         av_frame_copy_props(out, in);
925     }
926
927     td.in  = in;
928     td.out = out;
929     ctx->internal->execute(ctx, s->iir_channel, &td, NULL, outlink->channels);
930
931     for (ch = 0; ch < outlink->channels; ch++) {
932         if (s->iir[ch].clippings > 0)
933             av_log(ctx, AV_LOG_WARNING, "Channel %d clipping %d times. Please reduce gain.\n",
934                    ch, s->iir[ch].clippings);
935         s->iir[ch].clippings = 0;
936     }
937
938     if (in != out)
939         av_frame_free(&in);
940
941     if (s->response) {
942         AVFilterLink *outlink = ctx->outputs[1];
943         int64_t old_pts = s->video->pts;
944         int64_t new_pts = av_rescale_q(out->pts, ctx->inputs[0]->time_base, outlink->time_base);
945
946         if (new_pts > old_pts) {
947             s->video->pts = new_pts;
948             ret = ff_filter_frame(outlink, av_frame_clone(s->video));
949             if (ret < 0)
950                 return ret;
951         }
952     }
953
954     return ff_filter_frame(outlink, out);
955 }
956
957 static int config_video(AVFilterLink *outlink)
958 {
959     AVFilterContext *ctx = outlink->src;
960     AudioIIRContext *s = ctx->priv;
961
962     outlink->sample_aspect_ratio = (AVRational){1,1};
963     outlink->w = s->w;
964     outlink->h = s->h;
965     outlink->frame_rate = s->rate;
966     outlink->time_base = av_inv_q(outlink->frame_rate);
967
968     return 0;
969 }
970
971 static av_cold int init(AVFilterContext *ctx)
972 {
973     AudioIIRContext *s = ctx->priv;
974     AVFilterPad pad, vpad;
975     int ret;
976
977     if (!s->a_str || !s->b_str || !s->g_str) {
978         av_log(ctx, AV_LOG_ERROR, "Valid coefficients are mandatory.\n");
979         return AVERROR(EINVAL);
980     }
981
982     switch (s->precision) {
983     case 0: s->sample_format = AV_SAMPLE_FMT_DBLP; break;
984     case 1: s->sample_format = AV_SAMPLE_FMT_FLTP; break;
985     case 2: s->sample_format = AV_SAMPLE_FMT_S32P; break;
986     case 3: s->sample_format = AV_SAMPLE_FMT_S16P; break;
987     default: return AVERROR_BUG;
988     }
989
990     pad = (AVFilterPad){
991         .name         = av_strdup("default"),
992         .type         = AVMEDIA_TYPE_AUDIO,
993         .config_props = config_output,
994     };
995
996     if (!pad.name)
997         return AVERROR(ENOMEM);
998
999     if (s->response) {
1000         vpad = (AVFilterPad){
1001             .name         = av_strdup("filter_response"),
1002             .type         = AVMEDIA_TYPE_VIDEO,
1003             .config_props = config_video,
1004         };
1005         if (!vpad.name)
1006             return AVERROR(ENOMEM);
1007     }
1008
1009     ret = ff_insert_outpad(ctx, 0, &pad);
1010     if (ret < 0)
1011         return ret;
1012
1013     if (s->response) {
1014         ret = ff_insert_outpad(ctx, 1, &vpad);
1015         if (ret < 0)
1016             return ret;
1017     }
1018
1019     return 0;
1020 }
1021
1022 static av_cold void uninit(AVFilterContext *ctx)
1023 {
1024     AudioIIRContext *s = ctx->priv;
1025     int ch;
1026
1027     if (s->iir) {
1028         for (ch = 0; ch < s->channels; ch++) {
1029             IIRChannel *iir = &s->iir[ch];
1030             av_freep(&iir->ab[0]);
1031             av_freep(&iir->ab[1]);
1032             av_freep(&iir->cache[0]);
1033             av_freep(&iir->cache[1]);
1034             av_freep(&iir->biquads);
1035         }
1036     }
1037     av_freep(&s->iir);
1038
1039     av_freep(&ctx->output_pads[0].name);
1040     if (s->response)
1041         av_freep(&ctx->output_pads[1].name);
1042     av_frame_free(&s->video);
1043 }
1044
1045 static const AVFilterPad inputs[] = {
1046     {
1047         .name         = "default",
1048         .type         = AVMEDIA_TYPE_AUDIO,
1049         .filter_frame = filter_frame,
1050     },
1051     { NULL }
1052 };
1053
1054 #define OFFSET(x) offsetof(AudioIIRContext, x)
1055 #define AF AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
1056 #define VF AV_OPT_FLAG_VIDEO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
1057
1058 static const AVOption aiir_options[] = {
1059     { "z", "set B/numerator/zeros coefficients",   OFFSET(b_str),    AV_OPT_TYPE_STRING, {.str="1+0i 1-0i"}, 0, 0, AF },
1060     { "p", "set A/denominator/poles coefficients", OFFSET(a_str),    AV_OPT_TYPE_STRING, {.str="1+0i 1-0i"}, 0, 0, AF },
1061     { "k", "set channels gains",                   OFFSET(g_str),    AV_OPT_TYPE_STRING, {.str="1|1"}, 0, 0, AF },
1062     { "dry", "set dry gain",                       OFFSET(dry_gain), AV_OPT_TYPE_DOUBLE, {.dbl=1},     0, 1, AF },
1063     { "wet", "set wet gain",                       OFFSET(wet_gain), AV_OPT_TYPE_DOUBLE, {.dbl=1},     0, 1, AF },
1064     { "f", "set coefficients format",              OFFSET(format),   AV_OPT_TYPE_INT,    {.i64=1},     0, 3, AF, "format" },
1065     { "tf", "transfer function",                   0,                AV_OPT_TYPE_CONST,  {.i64=0},     0, 0, AF, "format" },
1066     { "zp", "Z-plane zeros/poles",                 0,                AV_OPT_TYPE_CONST,  {.i64=1},     0, 0, AF, "format" },
1067     { "pr", "Z-plane zeros/poles (polar radians)", 0,                AV_OPT_TYPE_CONST,  {.i64=2},     0, 0, AF, "format" },
1068     { "pd", "Z-plane zeros/poles (polar degrees)", 0,                AV_OPT_TYPE_CONST,  {.i64=3},     0, 0, AF, "format" },
1069     { "r", "set kind of processing",               OFFSET(process),  AV_OPT_TYPE_INT,    {.i64=1},     0, 1, AF, "process" },
1070     { "d", "direct",                               0,                AV_OPT_TYPE_CONST,  {.i64=0},     0, 0, AF, "process" },
1071     { "s", "serial cascading",                     0,                AV_OPT_TYPE_CONST,  {.i64=1},     0, 0, AF, "process" },
1072     { "e", "set precision",                        OFFSET(precision),AV_OPT_TYPE_INT,    {.i64=0},     0, 3, AF, "precision" },
1073     { "dbl", "double-precision floating-point",    0,                AV_OPT_TYPE_CONST,  {.i64=0},     0, 0, AF, "precision" },
1074     { "flt", "single-precision floating-point",    0,                AV_OPT_TYPE_CONST,  {.i64=1},     0, 0, AF, "precision" },
1075     { "i32", "32-bit integers",                    0,                AV_OPT_TYPE_CONST,  {.i64=2},     0, 0, AF, "precision" },
1076     { "i16", "16-bit integers",                    0,                AV_OPT_TYPE_CONST,  {.i64=3},     0, 0, AF, "precision" },
1077     { "response", "show IR frequency response",    OFFSET(response), AV_OPT_TYPE_BOOL,   {.i64=0},     0, 1, VF },
1078     { "channel", "set IR channel to display frequency response", OFFSET(ir_channel), AV_OPT_TYPE_INT, {.i64=0}, 0, 1024, VF },
1079     { "size",   "set video size",                  OFFSET(w),        AV_OPT_TYPE_IMAGE_SIZE, {.str = "hd720"}, 0, 0, VF },
1080     { "rate",   "set video rate",                  OFFSET(rate),     AV_OPT_TYPE_VIDEO_RATE, {.str = "25"}, 0, INT32_MAX, VF },
1081     { NULL },
1082 };
1083
1084 AVFILTER_DEFINE_CLASS(aiir);
1085
1086 AVFilter ff_af_aiir = {
1087     .name          = "aiir",
1088     .description   = NULL_IF_CONFIG_SMALL("Apply Infinite Impulse Response filter with supplied coefficients."),
1089     .priv_size     = sizeof(AudioIIRContext),
1090     .priv_class    = &aiir_class,
1091     .init          = init,
1092     .uninit        = uninit,
1093     .query_formats = query_formats,
1094     .inputs        = inputs,
1095     .flags         = AVFILTER_FLAG_DYNAMIC_OUTPUTS |
1096                      AVFILTER_FLAG_SLICE_THREADS,
1097 };