git.sesse.net Git - ffmpeg/blob - 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);
 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--;
 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 };