git.sesse.net Git - ffmpeg/blob - libavfilter/af_asupercut.c

   1 /*
   2  * Copyright (c) 2005 Boðaç Topaktaþ
   3  * Copyright (c) 2020 Paul B Mahol
   4  *
   5  * This file is part of FFmpeg.
   6  *
   7  * FFmpeg is free software; you can redistribute it and/or
   8  * modify it under the terms of the GNU Lesser General Public
   9  * License as published by the Free Software Foundation; either
  10  * version 2.1 of the License, or (at your option) any later version.
  11  *
  12  * FFmpeg is distributed in the hope that it will be useful,
  13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15  * Lesser General Public License for more details.
  16  *
  17  * You should have received a copy of the GNU Lesser General Public
  18  * License along with FFmpeg; if not, write to the Free Software
  19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  20  */
  21
  22 #include "libavutil/channel_layout.h"
  23 #include "libavutil/ffmath.h"
  24 #include "libavutil/opt.h"
  25 #include "avfilter.h"
  26 #include "audio.h"
  27 #include "formats.h"
  28
  29 typedef struct BiquadCoeffs {
  30     double a1, a2;
  31     double b0, b1, b2;
  32 } BiquadCoeffs;
  33
  34 typedef struct ASuperCutContext {
  35     const AVClass *class;
  36
  37     double cutoff;
  38     double level;
  39     double qfactor;
  40     int order;
  41
  42     int filter_count;
  43     int bypass;
  44
  45     BiquadCoeffs coeffs[10];
  46
  47     AVFrame *w;
  48
  49     int (*filter_channels)(AVFilterContext *ctx, void *arg, int jobnr, int nb_jobs);
  50 } ASuperCutContext;
  51
  52 static int query_formats(AVFilterContext *ctx)
  53 {
  54     AVFilterFormats *formats = NULL;
  55     AVFilterChannelLayouts *layouts = NULL;
  56     static const enum AVSampleFormat sample_fmts[] = {
  57         AV_SAMPLE_FMT_FLTP,
  58         AV_SAMPLE_FMT_DBLP,
  59         AV_SAMPLE_FMT_NONE
  60     };
  61     int ret;
  62
  63     formats = ff_make_format_list(sample_fmts);
  64     if (!formats)
  65         return AVERROR(ENOMEM);
  66     ret = ff_set_common_formats(ctx, formats);
  67     if (ret < 0)
  68         return ret;
  69
  70     layouts = ff_all_channel_counts();
  71     if (!layouts)
  72         return AVERROR(ENOMEM);
  73
  74     ret = ff_set_common_channel_layouts(ctx, layouts);
  75     if (ret < 0)
  76         return ret;
  77
  78     formats = ff_all_samplerates();
  79     return ff_set_common_samplerates(ctx, formats);
  80 }
  81
  82 static void calc_q_factors(int n, double *q)
  83 {
  84     for (int i = 0; i < n / 2; i++)
  85         q[i] = 1. / (-2. * cos(M_PI * (2. * (i + 1) + n - 1.) / (2. * n)));
  86 }
  87
  88 static int get_coeffs(AVFilterContext *ctx)
  89 {
  90     ASuperCutContext *s = ctx->priv;
  91     AVFilterLink *inlink = ctx->inputs[0];
  92     double w0 = s->cutoff / inlink->sample_rate;
  93     double K = tan(M_PI * w0);
  94     double q[10];
  95
  96     s->bypass = w0 >= 0.5;
  97     if (s->bypass)
  98         return 0;
  99
 100     if (!strcmp(ctx->filter->name, "asubcut")) {
 101         s->filter_count = s->order / 2 + (s->order & 1);
 102
 103         calc_q_factors(s->order, q);
 104
 105         if (s->order & 1) {
 106             BiquadCoeffs *coeffs = &s->coeffs[0];
 107             double omega = 2. * tan(M_PI * w0);
 108
 109             coeffs->b0 = 2. / (2. + omega);
 110             coeffs->b1 = -coeffs->b0;
 111             coeffs->b2 = 0.;
 112             coeffs->a1 = -(omega - 2.) / (2. + omega);
 113             coeffs->a2 = 0.;
 114         }
 115
 116         for (int b = (s->order & 1); b < s->filter_count; b++) {
 117             BiquadCoeffs *coeffs = &s->coeffs[b];
 118             const int idx = b - (s->order & 1);
 119             double norm = 1.0 / (1.0 + K / q[idx] + K * K);
 120
 121             coeffs->b0 = norm;
 122             coeffs->b1 = -2.0 * coeffs->b0;
 123             coeffs->b2 = coeffs->b0;
 124             coeffs->a1 = -2.0 * (K * K - 1.0) * norm;
 125             coeffs->a2 = -(1.0 - K / q[idx] + K * K) * norm;
 126         }
 127     } else if (!strcmp(ctx->filter->name, "asupercut")) {
 128         s->filter_count = s->order / 2 + (s->order & 1);
 129
 130         calc_q_factors(s->order, q);
 131
 132         if (s->order & 1) {
 133             BiquadCoeffs *coeffs = &s->coeffs[0];
 134             double omega = 2. * tan(M_PI * w0);
 135
 136             coeffs->b0 = omega / (2. + omega);
 137             coeffs->b1 = coeffs->b0;
 138             coeffs->b2 = 0.;
 139             coeffs->a1 = -(omega - 2.) / (2. + omega);
 140             coeffs->a2 = 0.;
 141         }
 142
 143         for (int b = (s->order & 1); b < s->filter_count; b++) {
 144             BiquadCoeffs *coeffs = &s->coeffs[b];
 145             const int idx = b - (s->order & 1);
 146             double norm = 1.0 / (1.0 + K / q[idx] + K * K);
 147
 148             coeffs->b0 = K * K * norm;
 149             coeffs->b1 = 2.0 * coeffs->b0;
 150             coeffs->b2 = coeffs->b0;
 151             coeffs->a1 = -2.0 * (K * K - 1.0) * norm;
 152             coeffs->a2 = -(1.0 - K / q[idx] + K * K) * norm;
 153         }
 154     } else if (!strcmp(ctx->filter->name, "asuperpass")) {
 155         double alpha, beta, gamma, theta;
 156         double theta_0 = 2. * M_PI * (s->cutoff / inlink->sample_rate);
 157         double d_E;
 158
 159         s->filter_count = s->order / 2;
 160         d_E = (2. * tan(theta_0 / (2. * s->qfactor))) / sin(theta_0);
 161
 162         for (int b = 0; b < s->filter_count; b += 2) {
 163             double D = 2. * sin(((b + 1) * M_PI) / (2. * s->filter_count));
 164             double A = (1. + pow((d_E / 2.), 2)) / (D * d_E / 2.);
 165             double d = sqrt((d_E * D) / (A + sqrt(A * A - 1.)));
 166             double B = D * (d_E / 2.) / d;
 167             double W = B + sqrt(B * B - 1.);
 168
 169             for (int j = 0; j < 2; j++) {
 170                 BiquadCoeffs *coeffs = &s->coeffs[b + j];
 171
 172                 if (j == 1)
 173                     theta = 2. * atan(tan(theta_0 / 2.) / W);
 174                 else
 175                     theta = 2. * atan(W * tan(theta_0 / 2.));
 176
 177                 beta = 0.5 * ((1. - (d / 2.) * sin(theta)) / (1. + (d / 2.) * sin(theta)));
 178                 gamma = (0.5 + beta) * cos(theta);
 179                 alpha = 0.5 * (0.5 - beta) * sqrt(1. + pow((W - (1. / W)) / d, 2.));
 180
 181                 coeffs->a1 =  2. * gamma;
 182                 coeffs->a2 = -2. * beta;
 183                 coeffs->b0 =  2. * alpha;
 184                 coeffs->b1 =  0.;
 185                 coeffs->b2 = -2. * alpha;
 186             }
 187         }
 188     } else if (!strcmp(ctx->filter->name, "asuperstop")) {
 189         double alpha, beta, gamma, theta;
 190         double theta_0 = 2. * M_PI * (s->cutoff / inlink->sample_rate);
 191         double d_E;
 192
 193         s->filter_count = s->order / 2;
 194         d_E = (2. * tan(theta_0 / (2. * s->qfactor))) / sin(theta_0);
 195
 196         for (int b = 0; b < s->filter_count; b += 2) {
 197             double D = 2. * sin(((b + 1) * M_PI) / (2. * s->filter_count));
 198             double A = (1. + pow((d_E / 2.), 2)) / (D * d_E / 2.);
 199             double d = sqrt((d_E * D) / (A + sqrt(A * A - 1.)));
 200             double B = D * (d_E / 2.) / d;
 201             double W = B + sqrt(B * B - 1.);
 202
 203             for (int j = 0; j < 2; j++) {
 204                 BiquadCoeffs *coeffs = &s->coeffs[b + j];
 205
 206                 if (j == 1)
 207                     theta = 2. * atan(tan(theta_0 / 2.) / W);
 208                 else
 209                     theta = 2. * atan(W * tan(theta_0 / 2.));
 210
 211                 beta = 0.5 * ((1. - (d / 2.) * sin(theta)) / (1. + (d / 2.) * sin(theta)));
 212                 gamma = (0.5 + beta) * cos(theta);
 213                 alpha = 0.5 * (0.5 + beta) * ((1. - cos(theta)) / (1. - cos(theta_0)));
 214
 215                 coeffs->a1 =  2. * gamma;
 216                 coeffs->a2 = -2. * beta;
 217                 coeffs->b0 =  2. * alpha;
 218                 coeffs->b1 = -4. * alpha * cos(theta_0);
 219                 coeffs->b2 =  2. * alpha;
 220             }
 221         }
 222     }
 223
 224     return 0;
 225 }
 226
 227 typedef struct ThreadData {
 228     AVFrame *in, *out;
 229 } ThreadData;
 230
 231 #define FILTER(name, type)                                          \
 232 static int filter_channels_## name(AVFilterContext *ctx, void *arg, \
 233                                    int jobnr, int nb_jobs)          \
 234 {                                                                   \
 235     ASuperCutContext *s = ctx->priv;                                \
 236     ThreadData *td = arg;                                           \
 237     AVFrame *out = td->out;                                         \
 238     AVFrame *in = td->in;                                           \
 239     const int start = (in->channels * jobnr) / nb_jobs;             \
 240     const int end = (in->channels * (jobnr+1)) / nb_jobs;           \
 241     const double level = s->level;                                  \
 242                                                                     \
 243     for (int ch = start; ch < end; ch++) {                          \
 244         const type *src = (const type *)in->extended_data[ch];      \
 245         type *dst = (type *)out->extended_data[ch];                 \
 246                                                                     \
 247         for (int b = 0; b < s->filter_count; b++) {                 \
 248             BiquadCoeffs *coeffs = &s->coeffs[b];                   \
 249             const type a1 = coeffs->a1;                             \
 250             const type a2 = coeffs->a2;                             \
 251             const type b0 = coeffs->b0;                             \
 252             const type b1 = coeffs->b1;                             \
 253             const type b2 = coeffs->b2;                             \
 254             type *w = ((type *)s->w->extended_data[ch]) + b * 2;    \
 255                                                                     \
 256             for (int n = 0; n < in->nb_samples; n++) {              \
 257                 type sin = b ? dst[n] : src[n] * level;             \
 258                 type sout = sin * b0 + w[0];                        \
 259                                                                     \
 260                 w[0] = b1 * sin + w[1] + a1 * sout;                 \
 261                 w[1] = b2 * sin + a2 * sout;                        \
 262                                                                     \
 263                 dst[n] = sout;                                      \
 264             }                                                       \
 265         }                                                           \
 266     }                                                               \
 267                                                                     \
 268     return 0;                                                       \
 269 }
 270
 271 FILTER(fltp, float)
 272 FILTER(dblp, double)
 273
 274 static int config_input(AVFilterLink *inlink)
 275 {
 276     AVFilterContext *ctx = inlink->dst;
 277     ASuperCutContext *s = ctx->priv;
 278
 279     switch (inlink->format) {
 280     case AV_SAMPLE_FMT_FLTP: s->filter_channels = filter_channels_fltp; break;
 281     case AV_SAMPLE_FMT_DBLP: s->filter_channels = filter_channels_dblp; break;
 282     }
 283
 284     s->w = ff_get_audio_buffer(inlink, 2 * 10);
 285     if (!s->w)
 286         return AVERROR(ENOMEM);
 287
 288     return get_coeffs(ctx);
 289 }
 290
 291 static int filter_frame(AVFilterLink *inlink, AVFrame *in)
 292 {
 293     AVFilterContext *ctx = inlink->dst;
 294     ASuperCutContext *s = ctx->priv;
 295     AVFilterLink *outlink = ctx->outputs[0];
 296     ThreadData td;
 297     AVFrame *out;
 298
 299     if (s->bypass)
 300         return ff_filter_frame(outlink, in);
 301
 302     if (av_frame_is_writable(in)) {
 303         out = in;
 304     } else {
 305         out = ff_get_audio_buffer(outlink, in->nb_samples);
 306         if (!out) {
 307             av_frame_free(&in);
 308             return AVERROR(ENOMEM);
 309         }
 310         av_frame_copy_props(out, in);
 311     }
 312
 313     td.in = in; td.out = out;
 314     ctx->internal->execute(ctx, s->filter_channels, &td, NULL, FFMIN(inlink->channels,
 315                                                                ff_filter_get_nb_threads(ctx)));
 316
 317     if (out != in)
 318         av_frame_free(&in);
 319     return ff_filter_frame(outlink, out);
 320 }
 321
 322 static int process_command(AVFilterContext *ctx, const char *cmd, const char *args,
 323                            char *res, int res_len, int flags)
 324 {
 325     int ret;
 326
 327     ret = ff_filter_process_command(ctx, cmd, args, res, res_len, flags);
 328     if (ret < 0)
 329         return ret;
 330
 331     return get_coeffs(ctx);
 332 }
 333
 334 static av_cold void uninit(AVFilterContext *ctx)
 335 {
 336     ASuperCutContext *s = ctx->priv;
 337
 338     av_frame_free(&s->w);
 339 }
 340
 341 #define OFFSET(x) offsetof(ASuperCutContext, x)
 342 #define FLAGS AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM|AV_OPT_FLAG_RUNTIME_PARAM
 343
 344 static const AVOption asupercut_options[] = {
 345     { "cutoff", "set cutoff frequency", OFFSET(cutoff), AV_OPT_TYPE_DOUBLE, {.dbl=20000}, 20000, 192000, FLAGS },
 346     { "order",  "set filter order",     OFFSET(order),  AV_OPT_TYPE_INT,    {.i64=10},        3,     20, FLAGS },
 347     { "level",  "set input level",      OFFSET(level),  AV_OPT_TYPE_DOUBLE, {.dbl=1.},        0.,    1., FLAGS },
 348     { NULL }
 349 };
 350
 351 AVFILTER_DEFINE_CLASS(asupercut);
 352
 353 static const AVFilterPad inputs[] = {
 354     {
 355         .name         = "default",
 356         .type         = AVMEDIA_TYPE_AUDIO,
 357         .filter_frame = filter_frame,
 358         .config_props = config_input,
 359     },
 360     { NULL }
 361 };
 362
 363 static const AVFilterPad outputs[] = {
 364     {
 365         .name = "default",
 366         .type = AVMEDIA_TYPE_AUDIO,
 367     },
 368     { NULL }
 369 };
 370
 371 AVFilter ff_af_asupercut = {
 372     .name            = "asupercut",
 373     .description     = NULL_IF_CONFIG_SMALL("Cut super frequencies."),
 374     .query_formats   = query_formats,
 375     .priv_size       = sizeof(ASuperCutContext),
 376     .priv_class      = &asupercut_class,
 377     .uninit          = uninit,
 378     .inputs          = inputs,
 379     .outputs         = outputs,
 380     .process_command = process_command,
 381     .flags           = AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC |
 382                        AVFILTER_FLAG_SLICE_THREADS,
 383 };
 384
 385 static const AVOption asubcut_options[] = {
 386     { "cutoff", "set cutoff frequency", OFFSET(cutoff), AV_OPT_TYPE_DOUBLE, {.dbl=20},  2, 200, FLAGS },
 387     { "order",  "set filter order",     OFFSET(order),  AV_OPT_TYPE_INT,    {.i64=10},  3,  20, FLAGS },
 388     { "level",  "set input level",      OFFSET(level),  AV_OPT_TYPE_DOUBLE, {.dbl=1.}, 0.,  1., FLAGS },
 389     { NULL }
 390 };
 391
 392 AVFILTER_DEFINE_CLASS(asubcut);
 393
 394 AVFilter ff_af_asubcut = {
 395     .name            = "asubcut",
 396     .description     = NULL_IF_CONFIG_SMALL("Cut subwoofer frequencies."),
 397     .query_formats   = query_formats,
 398     .priv_size       = sizeof(ASuperCutContext),
 399     .priv_class      = &asubcut_class,
 400     .uninit          = uninit,
 401     .inputs          = inputs,
 402     .outputs         = outputs,
 403     .process_command = process_command,
 404     .flags           = AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC |
 405                        AVFILTER_FLAG_SLICE_THREADS,
 406 };
 407
 408 static const AVOption asuperpass_asuperstop_options[] = {
 409     { "centerf","set center frequency", OFFSET(cutoff), AV_OPT_TYPE_DOUBLE, {.dbl=1000}, 2, 999999, FLAGS },
 410     { "order",  "set filter order",     OFFSET(order),  AV_OPT_TYPE_INT,    {.i64=4},    4,     20, FLAGS },
 411     { "qfactor","set Q-factor",         OFFSET(qfactor),AV_OPT_TYPE_DOUBLE, {.dbl=1.},0.01,   100., FLAGS },
 412     { "level",  "set input level",      OFFSET(level),  AV_OPT_TYPE_DOUBLE, {.dbl=1.},   0.,    2., FLAGS },
 413     { NULL }
 414 };
 415
 416 #define asuperpass_options asuperpass_asuperstop_options
 417 AVFILTER_DEFINE_CLASS(asuperpass);
 418
 419 AVFilter ff_af_asuperpass = {
 420     .name            = "asuperpass",
 421     .description     = NULL_IF_CONFIG_SMALL("Apply high order Butterworth band-pass filter."),
 422     .query_formats   = query_formats,
 423     .priv_size       = sizeof(ASuperCutContext),
 424     .priv_class      = &asuperpass_class,
 425     .uninit          = uninit,
 426     .inputs          = inputs,
 427     .outputs         = outputs,
 428     .process_command = process_command,
 429     .flags           = AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC |
 430                        AVFILTER_FLAG_SLICE_THREADS,
 431 };
 432
 433 #define asuperstop_options asuperpass_asuperstop_options
 434 AVFILTER_DEFINE_CLASS(asuperstop);
 435
 436 AVFilter ff_af_asuperstop = {
 437     .name            = "asuperstop",
 438     .description     = NULL_IF_CONFIG_SMALL("Apply high order Butterworth band-stop filter."),
 439     .query_formats   = query_formats,
 440     .priv_size       = sizeof(ASuperCutContext),
 441     .priv_class      = &asuperstop_class,
 442     .uninit          = uninit,
 443     .inputs          = inputs,
 444     .outputs         = outputs,
 445     .process_command = process_command,
 446     .flags           = AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC |
 447                        AVFILTER_FLAG_SLICE_THREADS,
 448 };