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