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     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].a1;                          \
 197         const double a2 = -iir->biquads[i].a2;                          \
 198         const double b0 = iir->biquads[i].b0;                           \
 199         const double b1 = iir->biquads[i].b1;                           \
 200         const double b2 = iir->biquads[i].b2;                           \
 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             int i;
 503
 504             for (i = 0; i < iir->nb_ab[0]; i++) {
 505                 double mag;
 506
 507                 if (isnan(iir->ab[0][2 * i]) || isnan(iir->ab[0][2 * i + 1]))
 508                     continue;
 509                 mag = hypot(iir->ab[0][2 * i], iir->ab[0][2 * i + 1]);
 510
 511                 if (mag > max_mag) {
 512                     max_mag = mag;
 513                     outmost_pole.a = i;
 514                 }
 515             }
 516
 517             for (i = 0; i < iir->nb_ab[0]; i++) {
 518                 if (isnan(iir->ab[0][2 * i]) || isnan(iir->ab[0][2 * i + 1]))
 519                     continue;
 520
 521                 if (iir->ab[0][2 * i    ] ==  iir->ab[0][2 * outmost_pole.a    ] &&
 522                     iir->ab[0][2 * i + 1] == -iir->ab[0][2 * outmost_pole.a + 1]) {
 523                     outmost_pole.b = i;
 524                     break;
 525                 }
 526             }
 527
 528             av_log(ctx, AV_LOG_VERBOSE, "outmost_pole is %d.%d\n", outmost_pole.a, outmost_pole.b);
 529
 530             if (outmost_pole.a < 0 || outmost_pole.b < 0)
 531                 return AVERROR(EINVAL);
 532
 533             for (i = 0; i < iir->nb_ab[1]; i++) {
 534                 double distance;
 535
 536                 if (isnan(iir->ab[1][2 * i]) || isnan(iir->ab[1][2 * i + 1]))
 537                     continue;
 538                 distance = hypot(iir->ab[0][2 * outmost_pole.a    ] - iir->ab[1][2 * i    ],
 539                                  iir->ab[0][2 * outmost_pole.a + 1] - iir->ab[1][2 * i + 1]);
 540
 541                 if (distance < min_distance) {
 542                     min_distance = distance;
 543                     nearest_zero.a = i;
 544                 }
 545             }
 546
 547             for (i = 0; i < iir->nb_ab[1]; i++) {
 548                 if (isnan(iir->ab[1][2 * i]) || isnan(iir->ab[1][2 * i + 1]))
 549                     continue;
 550
 551                 if (iir->ab[1][2 * i    ] ==  iir->ab[1][2 * nearest_zero.a    ] &&
 552                     iir->ab[1][2 * i + 1] == -iir->ab[1][2 * nearest_zero.a + 1]) {
 553                     nearest_zero.b = i;
 554                     break;
 555                 }
 556             }
 557
 558             av_log(ctx, AV_LOG_VERBOSE, "nearest_zero is %d.%d\n", nearest_zero.a, nearest_zero.b);
 559
 560             if (nearest_zero.a < 0 || nearest_zero.b < 0)
 561                 return AVERROR(EINVAL);
 562
 563             poles[0] = iir->ab[0][2 * outmost_pole.a    ];
 564             poles[1] = iir->ab[0][2 * outmost_pole.a + 1];
 565
 566             zeros[0] = iir->ab[1][2 * nearest_zero.a    ];
 567             zeros[1] = iir->ab[1][2 * nearest_zero.a + 1];
 568
 569             if (nearest_zero.a == nearest_zero.b && outmost_pole.a == outmost_pole.b) {
 570                 zeros[2] = 0;
 571                 zeros[3] = 0;
 572
 573                 poles[2] = 0;
 574                 poles[3] = 0;
 575             } else {
 576                 poles[2] = iir->ab[0][2 * outmost_pole.b    ];
 577                 poles[3] = iir->ab[0][2 * outmost_pole.b + 1];
 578
 579                 zeros[2] = iir->ab[1][2 * nearest_zero.b    ];
 580                 zeros[3] = iir->ab[1][2 * nearest_zero.b + 1];
 581             }
 582
 583             ret = expand(ctx, zeros, 2, b);
 584             if (ret < 0)
 585                 return ret;
 586
 587             ret = expand(ctx, poles, 2, a);
 588             if (ret < 0)
 589                 return ret;
 590
 591             iir->ab[0][2 * outmost_pole.a] = iir->ab[0][2 * outmost_pole.a + 1] = NAN;
 592             iir->ab[0][2 * outmost_pole.b] = iir->ab[0][2 * outmost_pole.b + 1] = NAN;
 593             iir->ab[1][2 * nearest_zero.a] = iir->ab[1][2 * nearest_zero.a + 1] = NAN;
 594             iir->ab[1][2 * nearest_zero.b] = iir->ab[1][2 * nearest_zero.b + 1] = NAN;
 595
 596             iir->biquads[current_biquad].a0 = 1.0;
 597             iir->biquads[current_biquad].a1 = a[2] / a[4];
 598             iir->biquads[current_biquad].a2 = a[0] / a[4];
 599             iir->biquads[current_biquad].b0 = b[4] / a[4] * (current_biquad ? 1.0 : iir->g);
 600             iir->biquads[current_biquad].b1 = b[2] / a[4] * (current_biquad ? 1.0 : iir->g);
 601             iir->biquads[current_biquad].b2 = b[0] / a[4] * (current_biquad ? 1.0 : iir->g);
 602
 603             av_log(ctx, AV_LOG_VERBOSE, "a=%f %f %f:b=%f %f %f\n",
 604                    iir->biquads[current_biquad].a0,
 605                    iir->biquads[current_biquad].a1,
 606                    iir->biquads[current_biquad].a2,
 607                    iir->biquads[current_biquad].b0,
 608                    iir->biquads[current_biquad].b1,
 609                    iir->biquads[current_biquad].b2);
 610
 611             current_biquad++;
 612         }
 613     }
 614
 615     return 0;
 616 }
 617
 618 static void convert_pr2zp(AVFilterContext *ctx, int channels)
 619 {
 620     AudioIIRContext *s = ctx->priv;
 621     int ch;
 622
 623     for (ch = 0; ch < channels; ch++) {
 624         IIRChannel *iir = &s->iir[ch];
 625         int n;
 626
 627         for (n = 0; n < iir->nb_ab[0]; n++) {
 628             double r = iir->ab[0][2*n];
 629             double angle = iir->ab[0][2*n+1];
 630
 631             iir->ab[0][2*n]   = r * cos(angle);
 632             iir->ab[0][2*n+1] = r * sin(angle);
 633         }
 634
 635         for (n = 0; n < iir->nb_ab[1]; n++) {
 636             double r = iir->ab[1][2*n];
 637             double angle = iir->ab[1][2*n+1];
 638
 639             iir->ab[1][2*n]   = r * cos(angle);
 640             iir->ab[1][2*n+1] = r * sin(angle);
 641         }
 642     }
 643 }
 644
 645 static void convert_pd2zp(AVFilterContext *ctx, int channels)
 646 {
 647     AudioIIRContext *s = ctx->priv;
 648     int ch;
 649
 650     for (ch = 0; ch < channels; ch++) {
 651         IIRChannel *iir = &s->iir[ch];
 652         int n;
 653
 654         for (n = 0; n < iir->nb_ab[0]; n++) {
 655             double r = iir->ab[0][2*n];
 656             double angle = M_PI*iir->ab[0][2*n+1]/180.;
 657
 658             iir->ab[0][2*n]   = r * cos(angle);
 659             iir->ab[0][2*n+1] = r * sin(angle);
 660         }
 661
 662         for (n = 0; n < iir->nb_ab[1]; n++) {
 663             double r = iir->ab[1][2*n];
 664             double angle = M_PI*iir->ab[1][2*n+1]/180.;
 665
 666             iir->ab[1][2*n]   = r * cos(angle);
 667             iir->ab[1][2*n+1] = r * sin(angle);
 668         }
 669     }
 670 }
 671
 672 static void drawtext(AVFrame *pic, int x, int y, const char *txt, uint32_t color)
 673 {
 674     const uint8_t *font;
 675     int font_height;
 676     int i;
 677
 678     font = avpriv_cga_font, font_height = 8;
 679
 680     for (i = 0; txt[i]; i++) {
 681         int char_y, mask;
 682
 683         uint8_t *p = pic->data[0] + y * pic->linesize[0] + (x + i * 8) * 4;
 684         for (char_y = 0; char_y < font_height; char_y++) {
 685             for (mask = 0x80; mask; mask >>= 1) {
 686                 if (font[txt[i] * font_height + char_y] & mask)
 687                     AV_WL32(p, color);
 688                 p += 4;
 689             }
 690             p += pic->linesize[0] - 8 * 4;
 691         }
 692     }
 693 }
 694
 695 static void draw_line(AVFrame *out, int x0, int y0, int x1, int y1, uint32_t color)
 696 {
 697     int dx = FFABS(x1-x0);
 698     int dy = FFABS(y1-y0), sy = y0 < y1 ? 1 : -1;
 699     int err = (dx>dy ? dx : -dy) / 2, e2;
 700
 701     for (;;) {
 702         AV_WL32(out->data[0] + y0 * out->linesize[0] + x0 * 4, color);
 703
 704         if (x0 == x1 && y0 == y1)
 705             break;
 706
 707         e2 = err;
 708
 709         if (e2 >-dx) {
 710             err -= dy;
 711             x0--;
 712         }
 713
 714         if (e2 < dy) {
 715             err += dx;
 716             y0 += sy;
 717         }
 718     }
 719 }
 720
 721 static void draw_response(AVFilterContext *ctx, AVFrame *out)
 722 {
 723     AudioIIRContext *s = ctx->priv;
 724     float *mag, *phase, *delay, min = FLT_MAX, max = FLT_MIN;
 725     float min_delay = FLT_MAX, max_delay = FLT_MIN;
 726     int prev_ymag = -1, prev_yphase = -1, prev_ydelay = -1;
 727     char text[32];
 728     int ch, i, x;
 729
 730     memset(out->data[0], 0, s->h * out->linesize[0]);
 731
 732     phase = av_malloc_array(s->w, sizeof(*phase));
 733     mag = av_malloc_array(s->w, sizeof(*mag));
 734     delay = av_malloc_array(s->w, sizeof(*delay));
 735     if (!mag || !phase || !delay)
 736         goto end;
 737
 738     ch = av_clip(s->ir_channel, 0, s->channels - 1);
 739     for (i = 0; i < s->w; i++) {
 740         const double *b = s->iir[ch].ab[0];
 741         const double *a = s->iir[ch].ab[1];
 742         double w = i * M_PI / (s->w - 1);
 743         double realz, realp;
 744         double imagz, imagp;
 745         double real, imag, div;
 746
 747         if (s->format == 0) {
 748             realz = 0., realp = 0.;
 749             imagz = 0., imagp = 0.;
 750             for (x = 0; x < s->iir[ch].nb_ab[1]; x++) {
 751                 realz += cos(-x * w) * a[x];
 752                 imagz += sin(-x * w) * a[x];
 753             }
 754
 755             for (x = 0; x < s->iir[ch].nb_ab[0]; x++) {
 756                 realp += cos(-x * w) * b[x];
 757                 imagp += sin(-x * w) * b[x];
 758             }
 759
 760             div = realp * realp + imagp * imagp;
 761             real = (realz * realp + imagz * imagp) / div;
 762             imag = (imagz * realp - imagp * realz) / div;
 763         } else {
 764             real = 1;
 765             imag = 0;
 766             for (x = 0; x < s->iir[ch].nb_ab[1]; x++) {
 767                 double ore, oim, re, im;
 768
 769                 re = cos(w) - a[2 * x];
 770                 im = sin(w) - a[2 * x + 1];
 771
 772                 ore = real;
 773                 oim = imag;
 774
 775                 real = ore * re - oim * im;
 776                 imag = ore * im + oim * re;
 777             }
 778
 779             for (x = 0; x < s->iir[ch].nb_ab[0]; x++) {
 780                 double ore, oim, re, im;
 781
 782                 re = cos(w) - b[2 * x];
 783                 im = sin(w) - b[2 * x + 1];
 784
 785                 ore = real;
 786                 oim = imag;
 787                 div = re * re + im * im;
 788
 789                 real = (ore * re + oim * im) / div;
 790                 imag = (oim * re - ore * im) / div;
 791             }
 792         }
 793
 794         mag[i] = s->iir[ch].g * hypot(real, imag);
 795         phase[i] = atan2(imag, real);
 796         min = fminf(min, mag[i]);
 797         max = fmaxf(max, mag[i]);
 798     }
 799
 800     for (i = 0; i < s->w - 1; i++) {
 801         float dw =  M_PI / (s->w - 1);
 802
 803         delay[i] = -(phase[i + 1] - phase[i]) / dw;
 804         min_delay = fminf(min_delay, delay[i]);
 805         max_delay = fmaxf(max_delay, delay[i]);
 806     }
 807
 808     delay[i] = delay[i - 1];
 809
 810     for (i = 0; i < s->w; i++) {
 811         int ymag = mag[i] / max * (s->h - 1);
 812         int ydelay = (delay[i] - min_delay) / (max_delay - min_delay) * (s->h - 1);
 813         int yphase = (0.5 * (1. + phase[i] / M_PI)) * (s->h - 1);
 814
 815         ymag = s->h - 1 - av_clip(ymag, 0, s->h - 1);
 816         yphase = s->h - 1 - av_clip(yphase, 0, s->h - 1);
 817         ydelay = s->h - 1 - av_clip(ydelay, 0, s->h - 1);
 818
 819         if (prev_ymag < 0)
 820             prev_ymag = ymag;
 821         if (prev_yphase < 0)
 822             prev_yphase = yphase;
 823         if (prev_ydelay < 0)
 824             prev_ydelay = ydelay;
 825
 826         draw_line(out, i,   ymag, FFMAX(i - 1, 0),   prev_ymag, 0xFFFF00FF);
 827         draw_line(out, i, yphase, FFMAX(i - 1, 0), prev_yphase, 0xFF00FF00);
 828         draw_line(out, i, ydelay, FFMAX(i - 1, 0), prev_ydelay, 0xFF00FFFF);
 829
 830         prev_ymag   = ymag;
 831         prev_yphase = yphase;
 832         prev_ydelay = ydelay;
 833     }
 834
 835     if (s->w > 400 && s->h > 100) {
 836         drawtext(out, 2, 2, "Max Magnitude:", 0xDDDDDDDD);
 837         snprintf(text, sizeof(text), "%.2f", max);
 838         drawtext(out, 15 * 8 + 2, 2, text, 0xDDDDDDDD);
 839
 840         drawtext(out, 2, 12, "Min Magnitude:", 0xDDDDDDDD);
 841         snprintf(text, sizeof(text), "%.2f", min);
 842         drawtext(out, 15 * 8 + 2, 12, text, 0xDDDDDDDD);
 843
 844         drawtext(out, 2, 22, "Max Delay:", 0xDDDDDDDD);
 845         snprintf(text, sizeof(text), "%.2f", max_delay);
 846         drawtext(out, 11 * 8 + 2, 22, text, 0xDDDDDDDD);
 847
 848         drawtext(out, 2, 32, "Min Delay:", 0xDDDDDDDD);
 849         snprintf(text, sizeof(text), "%.2f", min_delay);
 850         drawtext(out, 11 * 8 + 2, 32, text, 0xDDDDDDDD);
 851     }
 852
 853 end:
 854     av_free(delay);
 855     av_free(phase);
 856     av_free(mag);
 857 }
 858
 859 static int config_output(AVFilterLink *outlink)
 860 {
 861     AVFilterContext *ctx = outlink->src;
 862     AudioIIRContext *s = ctx->priv;
 863     AVFilterLink *inlink = ctx->inputs[0];
 864     int ch, ret, i;
 865
 866     s->channels = inlink->channels;
 867     s->iir = av_calloc(s->channels, sizeof(*s->iir));
 868     if (!s->iir)
 869         return AVERROR(ENOMEM);
 870
 871     ret = read_gains(ctx, s->g_str, inlink->channels);
 872     if (ret < 0)
 873         return ret;
 874
 875     ret = read_channels(ctx, inlink->channels, s->a_str, 0);
 876     if (ret < 0)
 877         return ret;
 878
 879     ret = read_channels(ctx, inlink->channels, s->b_str, 1);
 880     if (ret < 0)
 881         return ret;
 882
 883     if (s->format == 2) {
 884         convert_pr2zp(ctx, inlink->channels);
 885     } else if (s->format == 3) {
 886         convert_pd2zp(ctx, inlink->channels);
 887     }
 888
 889     av_frame_free(&s->video);
 890     if (s->response) {
 891         s->video = ff_get_video_buffer(ctx->outputs[1], s->w, s->h);
 892         if (!s->video)
 893             return AVERROR(ENOMEM);
 894
 895         draw_response(ctx, s->video);
 896     }
 897
 898     if (s->format == 0)
 899         av_log(ctx, AV_LOG_WARNING, "tf coefficients format is not recommended for too high number of zeros/poles.\n");
 900
 901     if (s->format > 0 && s->process == 0) {
 902         av_log(ctx, AV_LOG_WARNING, "Direct processsing is not recommended for zp coefficients format.\n");
 903
 904         ret = convert_zp2tf(ctx, inlink->channels);
 905         if (ret < 0)
 906             return ret;
 907     } else if (s->format == 0 && s->process == 1) {
 908         av_log(ctx, AV_LOG_ERROR, "Serial cascading is not implemented for transfer function.\n");
 909         return AVERROR_PATCHWELCOME;
 910     } else if (s->format > 0 && s->process == 1) {
 911         if (inlink->format == AV_SAMPLE_FMT_S16P)
 912             av_log(ctx, AV_LOG_WARNING, "Serial cascading is not recommended for i16 precision.\n");
 913
 914         ret = decompose_zp2biquads(ctx, inlink->channels);
 915         if (ret < 0)
 916             return ret;
 917     }
 918
 919     for (ch = 0; s->format == 0 && ch < inlink->channels; ch++) {
 920         IIRChannel *iir = &s->iir[ch];
 921
 922         for (i = 1; i < iir->nb_ab[0]; i++) {
 923             iir->ab[0][i] /= iir->ab[0][0];
 924         }
 925
 926         for (i = 0; i < iir->nb_ab[1]; i++) {
 927             iir->ab[1][i] *= iir->g / iir->ab[0][0];
 928         }
 929     }
 930
 931     switch (inlink->format) {
 932     case AV_SAMPLE_FMT_DBLP: s->iir_channel = s->process == 1 ? iir_ch_serial_dblp : iir_ch_dblp; break;
 933     case AV_SAMPLE_FMT_FLTP: s->iir_channel = s->process == 1 ? iir_ch_serial_fltp : iir_ch_fltp; break;
 934     case AV_SAMPLE_FMT_S32P: s->iir_channel = s->process == 1 ? iir_ch_serial_s32p : iir_ch_s32p; break;
 935     case AV_SAMPLE_FMT_S16P: s->iir_channel = s->process == 1 ? iir_ch_serial_s16p : iir_ch_s16p; break;
 936     }
 937
 938     return 0;
 939 }
 940
 941 static int filter_frame(AVFilterLink *inlink, AVFrame *in)
 942 {
 943     AVFilterContext *ctx = inlink->dst;
 944     AudioIIRContext *s = ctx->priv;
 945     AVFilterLink *outlink = ctx->outputs[0];
 946     ThreadData td;
 947     AVFrame *out;
 948     int ch, ret;
 949
 950     if (av_frame_is_writable(in)) {
 951         out = in;
 952     } else {
 953         out = ff_get_audio_buffer(outlink, in->nb_samples);
 954         if (!out) {
 955             av_frame_free(&in);
 956             return AVERROR(ENOMEM);
 957         }
 958         av_frame_copy_props(out, in);
 959     }
 960
 961     td.in  = in;
 962     td.out = out;
 963     ctx->internal->execute(ctx, s->iir_channel, &td, NULL, outlink->channels);
 964
 965     for (ch = 0; ch < outlink->channels; ch++) {
 966         if (s->iir[ch].clippings > 0)
 967             av_log(ctx, AV_LOG_WARNING, "Channel %d clipping %d times. Please reduce gain.\n",
 968                    ch, s->iir[ch].clippings);
 969         s->iir[ch].clippings = 0;
 970     }
 971
 972     if (in != out)
 973         av_frame_free(&in);
 974
 975     if (s->response) {
 976         AVFilterLink *outlink = ctx->outputs[1];
 977         int64_t old_pts = s->video->pts;
 978         int64_t new_pts = av_rescale_q(out->pts, ctx->inputs[0]->time_base, outlink->time_base);
 979
 980         if (new_pts > old_pts) {
 981             s->video->pts = new_pts;
 982             ret = ff_filter_frame(outlink, av_frame_clone(s->video));
 983             if (ret < 0)
 984                 return ret;
 985         }
 986     }
 987
 988     return ff_filter_frame(outlink, out);
 989 }
 990
 991 static int config_video(AVFilterLink *outlink)
 992 {
 993     AVFilterContext *ctx = outlink->src;
 994     AudioIIRContext *s = ctx->priv;
 995
 996     outlink->sample_aspect_ratio = (AVRational){1,1};
 997     outlink->w = s->w;
 998     outlink->h = s->h;
 999     outlink->frame_rate = s->rate;
1000     outlink->time_base = av_inv_q(outlink->frame_rate);
1001
1002     return 0;
1003 }
1004
1005 static av_cold int init(AVFilterContext *ctx)
1006 {
1007     AudioIIRContext *s = ctx->priv;
1008     AVFilterPad pad, vpad;
1009     int ret;
1010
1011     if (!s->a_str || !s->b_str || !s->g_str) {
1012         av_log(ctx, AV_LOG_ERROR, "Valid coefficients are mandatory.\n");
1013         return AVERROR(EINVAL);
1014     }
1015
1016     switch (s->precision) {
1017     case 0: s->sample_format = AV_SAMPLE_FMT_DBLP; break;
1018     case 1: s->sample_format = AV_SAMPLE_FMT_FLTP; break;
1019     case 2: s->sample_format = AV_SAMPLE_FMT_S32P; break;
1020     case 3: s->sample_format = AV_SAMPLE_FMT_S16P; break;
1021     default: return AVERROR_BUG;
1022     }
1023
1024     pad = (AVFilterPad){
1025         .name         = av_strdup("default"),
1026         .type         = AVMEDIA_TYPE_AUDIO,
1027         .config_props = config_output,
1028     };
1029
1030     if (!pad.name)
1031         return AVERROR(ENOMEM);
1032
1033     if (s->response) {
1034         vpad = (AVFilterPad){
1035             .name         = av_strdup("filter_response"),
1036             .type         = AVMEDIA_TYPE_VIDEO,
1037             .config_props = config_video,
1038         };
1039         if (!vpad.name)
1040             return AVERROR(ENOMEM);
1041     }
1042
1043     ret = ff_insert_outpad(ctx, 0, &pad);
1044     if (ret < 0)
1045         return ret;
1046
1047     if (s->response) {
1048         ret = ff_insert_outpad(ctx, 1, &vpad);
1049         if (ret < 0)
1050             return ret;
1051     }
1052
1053     return 0;
1054 }
1055
1056 static av_cold void uninit(AVFilterContext *ctx)
1057 {
1058     AudioIIRContext *s = ctx->priv;
1059     int ch;
1060
1061     if (s->iir) {
1062         for (ch = 0; ch < s->channels; ch++) {
1063             IIRChannel *iir = &s->iir[ch];
1064             av_freep(&iir->ab[0]);
1065             av_freep(&iir->ab[1]);
1066             av_freep(&iir->cache[0]);
1067             av_freep(&iir->cache[1]);
1068             av_freep(&iir->biquads);
1069         }
1070     }
1071     av_freep(&s->iir);
1072
1073     av_freep(&ctx->output_pads[0].name);
1074     if (s->response)
1075         av_freep(&ctx->output_pads[1].name);
1076     av_frame_free(&s->video);
1077 }
1078
1079 static const AVFilterPad inputs[] = {
1080     {
1081         .name         = "default",
1082         .type         = AVMEDIA_TYPE_AUDIO,
1083         .filter_frame = filter_frame,
1084     },
1085     { NULL }
1086 };
1087
1088 #define OFFSET(x) offsetof(AudioIIRContext, x)
1089 #define AF AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
1090 #define VF AV_OPT_FLAG_VIDEO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
1091
1092 static const AVOption aiir_options[] = {
1093     { "z", "set B/numerator/zeros coefficients",   OFFSET(b_str),    AV_OPT_TYPE_STRING, {.str="1+0i 1-0i"}, 0, 0, AF },
1094     { "p", "set A/denominator/poles coefficients", OFFSET(a_str),    AV_OPT_TYPE_STRING, {.str="1+0i 1-0i"}, 0, 0, AF },
1095     { "k", "set channels gains",                   OFFSET(g_str),    AV_OPT_TYPE_STRING, {.str="1|1"}, 0, 0, AF },
1096     { "dry", "set dry gain",                       OFFSET(dry_gain), AV_OPT_TYPE_DOUBLE, {.dbl=1},     0, 1, AF },
1097     { "wet", "set wet gain",                       OFFSET(wet_gain), AV_OPT_TYPE_DOUBLE, {.dbl=1},     0, 1, AF },
1098     { "f", "set coefficients format",              OFFSET(format),   AV_OPT_TYPE_INT,    {.i64=1},     0, 3, AF, "format" },
1099     { "tf", "transfer function",                   0,                AV_OPT_TYPE_CONST,  {.i64=0},     0, 0, AF, "format" },
1100     { "zp", "Z-plane zeros/poles",                 0,                AV_OPT_TYPE_CONST,  {.i64=1},     0, 0, AF, "format" },
1101     { "pr", "Z-plane zeros/poles (polar radians)", 0,                AV_OPT_TYPE_CONST,  {.i64=2},     0, 0, AF, "format" },
1102     { "pd", "Z-plane zeros/poles (polar degrees)", 0,                AV_OPT_TYPE_CONST,  {.i64=3},     0, 0, AF, "format" },
1103     { "r", "set kind of processing",               OFFSET(process),  AV_OPT_TYPE_INT,    {.i64=1},     0, 1, AF, "process" },
1104     { "d", "direct",                               0,                AV_OPT_TYPE_CONST,  {.i64=0},     0, 0, AF, "process" },
1105     { "s", "serial cascading",                     0,                AV_OPT_TYPE_CONST,  {.i64=1},     0, 0, AF, "process" },
1106     { "e", "set precision",                        OFFSET(precision),AV_OPT_TYPE_INT,    {.i64=0},     0, 3, AF, "precision" },
1107     { "dbl", "double-precision floating-point",    0,                AV_OPT_TYPE_CONST,  {.i64=0},     0, 0, AF, "precision" },
1108     { "flt", "single-precision floating-point",    0,                AV_OPT_TYPE_CONST,  {.i64=1},     0, 0, AF, "precision" },
1109     { "i32", "32-bit integers",                    0,                AV_OPT_TYPE_CONST,  {.i64=2},     0, 0, AF, "precision" },
1110     { "i16", "16-bit integers",                    0,                AV_OPT_TYPE_CONST,  {.i64=3},     0, 0, AF, "precision" },
1111     { "mix", "set mix",                            OFFSET(mix),      AV_OPT_TYPE_DOUBLE, {.dbl=1},     0, 1, AF },
1112     { "response", "show IR frequency response",    OFFSET(response), AV_OPT_TYPE_BOOL,   {.i64=0},     0, 1, VF },
1113     { "channel", "set IR channel to display frequency response", OFFSET(ir_channel), AV_OPT_TYPE_INT, {.i64=0}, 0, 1024, VF },
1114     { "size",   "set video size",                  OFFSET(w),        AV_OPT_TYPE_IMAGE_SIZE, {.str = "hd720"}, 0, 0, VF },
1115     { "rate",   "set video rate",                  OFFSET(rate),     AV_OPT_TYPE_VIDEO_RATE, {.str = "25"}, 0, INT32_MAX, VF },
1116     { NULL },
1117 };
1118
1119 AVFILTER_DEFINE_CLASS(aiir);
1120
1121 AVFilter ff_af_aiir = {
1122     .name          = "aiir",
1123     .description   = NULL_IF_CONFIG_SMALL("Apply Infinite Impulse Response filter with supplied coefficients."),
1124     .priv_size     = sizeof(AudioIIRContext),
1125     .priv_class    = &aiir_class,
1126     .init          = init,
1127     .uninit        = uninit,
1128     .query_formats = query_formats,
1129     .inputs        = inputs,
1130     .flags         = AVFILTER_FLAG_DYNAMIC_OUTPUTS |
1131                      AVFILTER_FLAG_SLICE_THREADS,
1132 };