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