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