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