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

   1 /*
   2  * Copyright (c) Paul B Mahol
   3  * Copyright (c) Laurent de Soras, 2005
   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 #define NB_COEFS 16
  30
  31 typedef struct AFreqShift {
  32     const AVClass *class;
  33
  34     double shift;
  35     double level;
  36
  37     double c[NB_COEFS];
  38
  39     int64_t in_samples;
  40
  41     AVFrame *i1, *o1;
  42     AVFrame *i2, *o2;
  43
  44     void (*filter_channel)(AVFilterContext *ctx,
  45                            int nb_samples,
  46                            int sample_rate,
  47                            const double *src, double *dst,
  48                            double *i1, double *o1,
  49                            double *i2, double *o2);
  50 } AFreqShift;
  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_DBLP,
  58         AV_SAMPLE_FMT_NONE
  59     };
  60     int ret;
  61
  62     formats = ff_make_format_list(sample_fmts);
  63     if (!formats)
  64         return AVERROR(ENOMEM);
  65     ret = ff_set_common_formats(ctx, formats);
  66     if (ret < 0)
  67         return ret;
  68
  69     layouts = ff_all_channel_counts();
  70     if (!layouts)
  71         return AVERROR(ENOMEM);
  72
  73     ret = ff_set_common_channel_layouts(ctx, layouts);
  74     if (ret < 0)
  75         return ret;
  76
  77     formats = ff_all_samplerates();
  78     return ff_set_common_samplerates(ctx, formats);
  79 }
  80
  81 static void pfilter_channel(AVFilterContext *ctx,
  82                             int nb_samples,
  83                             int sample_rate,
  84                             const double *src, double *dst,
  85                             double *i1, double *o1,
  86                             double *i2, double *o2)
  87 {
  88     AFreqShift *s = ctx->priv;
  89     const double *c = s->c;
  90     const double level = s->level;
  91     double shift = s->shift * M_PI;
  92     double cos_theta = cos(shift);
  93     double sin_theta = sin(shift);
  94
  95     for (int n = 0; n < nb_samples; n++) {
  96         double xn1 = src[n], xn2 = src[n];
  97         double I, Q;
  98
  99         for (int j = 0; j < NB_COEFS / 2; j++) {
 100             I = c[j] * (xn1 + o2[j]) - i2[j];
 101             i2[j] = i1[j];
 102             i1[j] = xn1;
 103             o2[j] = o1[j];
 104             o1[j] = I;
 105             xn1 = I;
 106         }
 107
 108         for (int j = NB_COEFS / 2; j < NB_COEFS; j++) {
 109             Q = c[j] * (xn2 + o2[j]) - i2[j];
 110             i2[j] = i1[j];
 111             i1[j] = xn2;
 112             o2[j] = o1[j];
 113             o1[j] = Q;
 114             xn2 = Q;
 115         }
 116         Q = o2[NB_COEFS - 1];
 117
 118         dst[n] = (I * cos_theta - Q * sin_theta) * level;
 119     }
 120 }
 121
 122 static void ffilter_channel(AVFilterContext *ctx,
 123                             int nb_samples,
 124                             int sample_rate,
 125                             const double *src, double *dst,
 126                             double *i1, double *o1,
 127                             double *i2, double *o2)
 128 {
 129     AFreqShift *s = ctx->priv;
 130     const double *c = s->c;
 131     const double level = s->level;
 132     double ts = 1. / sample_rate;
 133     double shift = s->shift;
 134     int64_t N = s->in_samples;
 135
 136     for (int n = 0; n < nb_samples; n++) {
 137         double xn1 = src[n], xn2 = src[n];
 138         double I, Q, theta;
 139
 140         for (int j = 0; j < NB_COEFS / 2; j++) {
 141             I = c[j] * (xn1 + o2[j]) - i2[j];
 142             i2[j] = i1[j];
 143             i1[j] = xn1;
 144             o2[j] = o1[j];
 145             o1[j] = I;
 146             xn1 = I;
 147         }
 148
 149         for (int j = NB_COEFS / 2; j < NB_COEFS; j++) {
 150             Q = c[j] * (xn2 + o2[j]) - i2[j];
 151             i2[j] = i1[j];
 152             i1[j] = xn2;
 153             o2[j] = o1[j];
 154             o1[j] = Q;
 155             xn2 = Q;
 156         }
 157         Q = o2[NB_COEFS - 1];
 158
 159         theta = 2. * M_PI * fmod(shift * (N + n) * ts, 1.);
 160         dst[n] = (I * cos(theta) - Q * sin(theta)) * level;
 161     }
 162 }
 163
 164 static void compute_transition_param(double *K, double *Q, double transition)
 165 {
 166     double kksqrt, e, e2, e4, k, q;
 167
 168     k  = tan((1. - transition * 2.) * M_PI / 4.);
 169     k *= k;
 170     kksqrt = pow(1 - k * k, 0.25);
 171     e = 0.5 * (1. - kksqrt) / (1. + kksqrt);
 172     e2 = e * e;
 173     e4 = e2 * e2;
 174     q = e * (1. + e4 * (2. + e4 * (15. + 150. * e4)));
 175
 176     *Q = q;
 177     *K = k;
 178 }
 179
 180 static double ipowp(double x, int64_t n)
 181 {
 182     double z = 1.;
 183
 184     while (n != 0) {
 185         if (n & 1)
 186             z *= x;
 187         n >>= 1;
 188         x *= x;
 189     }
 190
 191     return z;
 192 }
 193
 194 static double compute_acc_num(double q, int order, int c)
 195 {
 196     int64_t i = 0;
 197     int j = 1;
 198     double acc = 0.;
 199     double q_ii1;
 200
 201     do {
 202         q_ii1  = ipowp(q, i * (i + 1));
 203         q_ii1 *= sin((i * 2 + 1) * c * M_PI / order) * j;
 204         acc   += q_ii1;
 205
 206         j = -j;
 207         i++;
 208     } while (fabs(q_ii1) > 1e-100);
 209
 210     return acc;
 211 }
 212
 213 static double compute_acc_den(double q, int order, int c)
 214 {
 215     int64_t i = 1;
 216     int j = -1;
 217     double acc = 0.;
 218     double q_i2;
 219
 220     do {
 221         q_i2  = ipowp(q, i * i);
 222         q_i2 *= cos(i * 2 * c * M_PI / order) * j;
 223         acc  += q_i2;
 224
 225         j = -j;
 226         i++;
 227     } while (fabs(q_i2) > 1e-100);
 228
 229     return acc;
 230 }
 231
 232 static double compute_coef(int index, double k, double q, int order)
 233 {
 234     const int    c    = index + 1;
 235     const double num  = compute_acc_num(q, order, c) * pow(q, 0.25);
 236     const double den  = compute_acc_den(q, order, c) + 0.5;
 237     const double ww   = num / den;
 238     const double wwsq = ww * ww;
 239
 240     const double x    = sqrt((1 - wwsq * k) * (1 - wwsq / k)) / (1 + wwsq);
 241     const double coef = (1 - x) / (1 + x);
 242
 243     return coef;
 244 }
 245
 246 static void compute_coefs(double *coef_arr, int nbr_coefs, double transition)
 247 {
 248     const int order = nbr_coefs * 2 + 1;
 249     double k, q;
 250
 251     compute_transition_param(&k, &q, transition);
 252
 253     for (int n = 0; n < nbr_coefs; n++)
 254         coef_arr[(n / 2) + (n & 1) * nbr_coefs / 2] = compute_coef(n, k, q, order);
 255 }
 256
 257 static int config_input(AVFilterLink *inlink)
 258 {
 259     AVFilterContext *ctx = inlink->dst;
 260     AFreqShift *s = ctx->priv;
 261
 262     compute_coefs(s->c, NB_COEFS, 2. * 20. / inlink->sample_rate);
 263
 264     s->i1 = ff_get_audio_buffer(inlink, NB_COEFS);
 265     s->o1 = ff_get_audio_buffer(inlink, NB_COEFS);
 266     s->i2 = ff_get_audio_buffer(inlink, NB_COEFS);
 267     s->o2 = ff_get_audio_buffer(inlink, NB_COEFS);
 268     if (!s->i1 || !s->o1 || !s->i2 || !s->o2)
 269         return AVERROR(ENOMEM);
 270
 271     if (!strcmp(ctx->filter->name, "afreqshift"))
 272         s->filter_channel = ffilter_channel;
 273     else
 274         s->filter_channel = pfilter_channel;
 275
 276     return 0;
 277 }
 278
 279 typedef struct ThreadData {
 280     AVFrame *in, *out;
 281 } ThreadData;
 282
 283 static int filter_channels(AVFilterContext *ctx, void *arg, int jobnr, int nb_jobs)
 284 {
 285     AFreqShift *s = ctx->priv;
 286     ThreadData *td = arg;
 287     AVFrame *out = td->out;
 288     AVFrame *in = td->in;
 289     const int start = (in->channels * jobnr) / nb_jobs;
 290     const int end = (in->channels * (jobnr+1)) / nb_jobs;
 291
 292     for (int ch = start; ch < end; ch++) {
 293         s->filter_channel(ctx, in->nb_samples,
 294                           in->sample_rate,
 295                           (const double *)in->extended_data[ch],
 296                           (double *)out->extended_data[ch],
 297                           (double *)s->i1->extended_data[ch],
 298                           (double *)s->o1->extended_data[ch],
 299                           (double *)s->i2->extended_data[ch],
 300                           (double *)s->o2->extended_data[ch]);
 301     }
 302
 303     return 0;
 304 }
 305
 306 static int filter_frame(AVFilterLink *inlink, AVFrame *in)
 307 {
 308     AVFilterContext *ctx = inlink->dst;
 309     AVFilterLink *outlink = ctx->outputs[0];
 310     AFreqShift *s = ctx->priv;
 311     AVFrame *out;
 312     ThreadData td;
 313
 314     if (av_frame_is_writable(in)) {
 315         out = in;
 316     } else {
 317         out = ff_get_audio_buffer(outlink, in->nb_samples);
 318         if (!out) {
 319             av_frame_free(&in);
 320             return AVERROR(ENOMEM);
 321         }
 322         av_frame_copy_props(out, in);
 323     }
 324
 325     td.in = in; td.out = out;
 326     ctx->internal->execute(ctx, filter_channels, &td, NULL, FFMIN(inlink->channels,
 327                                                             ff_filter_get_nb_threads(ctx)));
 328
 329     s->in_samples += in->nb_samples;
 330
 331     if (out != in)
 332         av_frame_free(&in);
 333     return ff_filter_frame(outlink, out);
 334 }
 335
 336 static av_cold void uninit(AVFilterContext *ctx)
 337 {
 338     AFreqShift *s = ctx->priv;
 339
 340     av_frame_free(&s->i1);
 341     av_frame_free(&s->o1);
 342     av_frame_free(&s->i2);
 343     av_frame_free(&s->o2);
 344 }
 345
 346 #define OFFSET(x) offsetof(AFreqShift, x)
 347 #define FLAGS AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM|AV_OPT_FLAG_RUNTIME_PARAM
 348
 349 static const AVOption afreqshift_options[] = {
 350     { "shift", "set frequency shift", OFFSET(shift), AV_OPT_TYPE_DOUBLE, {.dbl=0}, -INT_MAX, INT_MAX, FLAGS },
 351     { "level", "set output level",    OFFSET(level), AV_OPT_TYPE_DOUBLE, {.dbl=1},      0.0,     1.0, FLAGS },
 352     { NULL }
 353 };
 354
 355 AVFILTER_DEFINE_CLASS(afreqshift);
 356
 357 static const AVFilterPad inputs[] = {
 358     {
 359         .name         = "default",
 360         .type         = AVMEDIA_TYPE_AUDIO,
 361         .filter_frame = filter_frame,
 362         .config_props = config_input,
 363     },
 364     { NULL }
 365 };
 366
 367 static const AVFilterPad outputs[] = {
 368     {
 369         .name = "default",
 370         .type = AVMEDIA_TYPE_AUDIO,
 371     },
 372     { NULL }
 373 };
 374
 375 AVFilter ff_af_afreqshift = {
 376     .name            = "afreqshift",
 377     .description     = NULL_IF_CONFIG_SMALL("Apply frequency shifting to input audio."),
 378     .query_formats   = query_formats,
 379     .priv_size       = sizeof(AFreqShift),
 380     .priv_class      = &afreqshift_class,
 381     .uninit          = uninit,
 382     .inputs          = inputs,
 383     .outputs         = outputs,
 384     .process_command = ff_filter_process_command,
 385     .flags           = AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC |
 386                        AVFILTER_FLAG_SLICE_THREADS,
 387 };
 388
 389 static const AVOption aphaseshift_options[] = {
 390     { "shift", "set phase shift", OFFSET(shift), AV_OPT_TYPE_DOUBLE, {.dbl=0}, -1.0, 1.0, FLAGS },
 391     { "level", "set output level",OFFSET(level), AV_OPT_TYPE_DOUBLE, {.dbl=1},  0.0, 1.0, FLAGS },
 392     { NULL }
 393 };
 394
 395 AVFILTER_DEFINE_CLASS(aphaseshift);
 396
 397 AVFilter ff_af_aphaseshift = {
 398     .name            = "aphaseshift",
 399     .description     = NULL_IF_CONFIG_SMALL("Apply phase shifting to input audio."),
 400     .query_formats   = query_formats,
 401     .priv_size       = sizeof(AFreqShift),
 402     .priv_class      = &aphaseshift_class,
 403     .uninit          = uninit,
 404     .inputs          = inputs,
 405     .outputs         = outputs,
 406     .process_command = ff_filter_process_command,
 407     .flags           = AVFILTER_FLAG_SUPPORT_TIMELINE_GENERIC |
 408                        AVFILTER_FLAG_SLICE_THREADS,
 409 };