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

   1 /*
   2  * Copyright (c) 2008-2009 Rob Sykes <robs@users.sourceforge.net>
   3  * Copyright (c) 2017 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/avassert.h"
  23 #include "libavutil/opt.h"
  24
  25 #include "libavcodec/avfft.h"
  26
  27 #include "audio.h"
  28 #include "avfilter.h"
  29 #include "internal.h"
  30
  31 typedef struct SincContext {
  32     const AVClass *class;
  33
  34     int sample_rate, nb_samples;
  35     float att, beta, phase, Fc0, Fc1, tbw0, tbw1;
  36     int num_taps[2];
  37     int round;
  38
  39     int n, rdft_len;
  40     float *coeffs;
  41     int64_t pts;
  42
  43     RDFTContext *rdft, *irdft;
  44 } SincContext;
  45
  46 static int request_frame(AVFilterLink *outlink)
  47 {
  48     AVFilterContext *ctx = outlink->src;
  49     SincContext *s = ctx->priv;
  50     const float *coeffs = s->coeffs;
  51     AVFrame *frame = NULL;
  52     int nb_samples;
  53
  54     nb_samples = FFMIN(s->nb_samples, s->n - s->pts);
  55     if (nb_samples <= 0)
  56         return AVERROR_EOF;
  57
  58     if (!(frame = ff_get_audio_buffer(outlink, nb_samples)))
  59         return AVERROR(ENOMEM);
  60
  61     memcpy(frame->data[0], coeffs + s->pts, nb_samples * sizeof(float));
  62
  63     frame->pts = s->pts;
  64     s->pts    += nb_samples;
  65
  66     return ff_filter_frame(outlink, frame);
  67 }
  68
  69 static int query_formats(AVFilterContext *ctx)
  70 {
  71     SincContext *s = ctx->priv;
  72     static const int64_t chlayouts[] = { AV_CH_LAYOUT_MONO, -1 };
  73     int sample_rates[] = { s->sample_rate, -1 };
  74     static const enum AVSampleFormat sample_fmts[] = { AV_SAMPLE_FMT_FLT,
  75                                                        AV_SAMPLE_FMT_NONE };
  76     AVFilterFormats *formats;
  77     AVFilterChannelLayouts *layouts;
  78     int ret;
  79
  80     formats = ff_make_format_list(sample_fmts);
  81     if (!formats)
  82         return AVERROR(ENOMEM);
  83     ret = ff_set_common_formats (ctx, formats);
  84     if (ret < 0)
  85         return ret;
  86
  87     layouts = ff_make_format64_list(chlayouts);
  88     if (!layouts)
  89         return AVERROR(ENOMEM);
  90     ret = ff_set_common_channel_layouts(ctx, layouts);
  91     if (ret < 0)
  92         return ret;
  93
  94     formats = ff_make_format_list(sample_rates);
  95     if (!formats)
  96         return AVERROR(ENOMEM);
  97     return ff_set_common_samplerates(ctx, formats);
  98 }
  99
 100 static float bessel_I_0(float x)
 101 {
 102     float term = 1, sum = 1, last_sum, x2 = x / 2;
 103     int i = 1;
 104
 105     do {
 106         float y = x2 / i++;
 107
 108         last_sum = sum;
 109         sum += term *= y * y;
 110     } while (sum != last_sum);
 111
 112     return sum;
 113 }
 114
 115 static float *make_lpf(int num_taps, float Fc, float beta, float rho,
 116                        float scale, int dc_norm)
 117 {
 118     int i, m = num_taps - 1;
 119     float *h = av_calloc(num_taps, sizeof(*h)), sum = 0;
 120     float mult = scale / bessel_I_0(beta), mult1 = 1.f / (.5f * m + rho);
 121
 122     av_assert0(Fc >= 0 && Fc <= 1);
 123
 124     for (i = 0; i <= m / 2; i++) {
 125         float z = i - .5f * m, x = z * M_PI, y = z * mult1;
 126         h[i] = x ? sinf(Fc * x) / x : Fc;
 127         sum += h[i] *= bessel_I_0(beta * sqrtf(1.f - y * y)) * mult;
 128         if (m - i != i) {
 129             h[m - i] = h[i];
 130             sum += h[i];
 131         }
 132     }
 133
 134     for (i = 0; dc_norm && i < num_taps; i++)
 135         h[i] *= scale / sum;
 136
 137     return h;
 138 }
 139
 140 static float kaiser_beta(float att, float tr_bw)
 141 {
 142     if (att >= 60.f) {
 143         static const float coefs[][4] = {
 144             {-6.784957e-10, 1.02856e-05, 0.1087556, -0.8988365 + .001},
 145             {-6.897885e-10, 1.027433e-05, 0.10876, -0.8994658 + .002},
 146             {-1.000683e-09, 1.030092e-05, 0.1087677, -0.9007898 + .003},
 147             {-3.654474e-10, 1.040631e-05, 0.1087085, -0.8977766 + .006},
 148             {8.106988e-09, 6.983091e-06, 0.1091387, -0.9172048 + .015},
 149             {9.519571e-09, 7.272678e-06, 0.1090068, -0.9140768 + .025},
 150             {-5.626821e-09, 1.342186e-05, 0.1083999, -0.9065452 + .05},
 151             {-9.965946e-08, 5.073548e-05, 0.1040967, -0.7672778 + .085},
 152             {1.604808e-07, -5.856462e-05, 0.1185998, -1.34824 + .1},
 153             {-1.511964e-07, 6.363034e-05, 0.1064627, -0.9876665 + .18},
 154         };
 155         float realm = logf(tr_bw / .0005f) / logf(2.f);
 156         float const *c0 = coefs[av_clip((int)realm, 0, FF_ARRAY_ELEMS(coefs) - 1)];
 157         float const *c1 = coefs[av_clip(1 + (int)realm, 0, FF_ARRAY_ELEMS(coefs) - 1)];
 158         float b0 = ((c0[0] * att + c0[1]) * att + c0[2]) * att + c0[3];
 159         float b1 = ((c1[0] * att + c1[1]) * att + c1[2]) * att + c1[3];
 160
 161         return b0 + (b1 - b0) * (realm - (int)realm);
 162     }
 163     if (att > 50.f)
 164         return .1102f * (att - 8.7f);
 165     if (att > 20.96f)
 166         return .58417f * powf(att - 20.96f, .4f) + .07886f * (att - 20.96f);
 167     return 0;
 168 }
 169
 170 static void kaiser_params(float att, float Fc, float tr_bw, float *beta, int *num_taps)
 171 {
 172     *beta = *beta < 0.f ? kaiser_beta(att, tr_bw * .5f / Fc): *beta;
 173     att = att < 60.f ? (att - 7.95f) / (2.285f * M_PI * 2.f) :
 174         ((.0007528358f-1.577737e-05 * *beta) * *beta + 0.6248022f) * *beta + .06186902f;
 175     *num_taps = !*num_taps ? ceilf(att/tr_bw + 1) : *num_taps;
 176 }
 177
 178 static float *lpf(float Fn, float Fc, float tbw, int *num_taps, float att, float *beta, int round)
 179 {
 180     int n = *num_taps;
 181
 182     if ((Fc /= Fn) <= 0.f || Fc >= 1.f) {
 183         *num_taps = 0;
 184         return NULL;
 185     }
 186
 187     att = att ? att : 120.f;
 188
 189     kaiser_params(att, Fc, (tbw ? tbw / Fn : .05f) * .5f, beta, num_taps);
 190
 191     if (!n) {
 192         n = *num_taps;
 193         *num_taps = av_clip(n, 11, 32767);
 194         if (round)
 195             *num_taps = 1 + 2 * (int)((int)((*num_taps / 2) * Fc + .5f) / Fc + .5f);
 196     }
 197
 198     return make_lpf(*num_taps |= 1, Fc, *beta, 0.f, 1.f, 0);
 199 }
 200
 201 static void invert(float *h, int n)
 202 {
 203     for (int i = 0; i < n; i++)
 204         h[i] = -h[i];
 205
 206     h[(n - 1) / 2] += 1;
 207 }
 208
 209 #define PACK(h, n)   h[1] = h[n]
 210 #define UNPACK(h, n) h[n] = h[1], h[n + 1] = h[1] = 0;
 211 #define SQR(a) ((a) * (a))
 212
 213 static float safe_log(float x)
 214 {
 215     av_assert0(x >= 0);
 216     if (x)
 217         return logf(x);
 218     return -26;
 219 }
 220
 221 static int fir_to_phase(SincContext *s, float **h, int *len, int *post_len, float phase)
 222 {
 223     float *pi_wraps, *work, phase1 = (phase > 50.f ? 100.f - phase : phase) / 50.f;
 224     int i, work_len, begin, end, imp_peak = 0, peak = 0;
 225     float imp_sum = 0, peak_imp_sum = 0;
 226     float prev_angle2 = 0, cum_2pi = 0, prev_angle1 = 0, cum_1pi = 0;
 227
 228     for (i = *len, work_len = 2 * 2 * 8; i > 1; work_len <<= 1, i >>= 1);
 229
 230     /* The first part is for work (+2 for (UN)PACK), the latter for pi_wraps. */
 231     work = av_calloc((work_len + 2) + (work_len / 2 + 1), sizeof(float));
 232     if (!work)
 233         return AVERROR(ENOMEM);
 234     pi_wraps = &work[work_len + 2];
 235
 236     memcpy(work, *h, *len * sizeof(*work));
 237
 238     av_rdft_end(s->rdft);
 239     av_rdft_end(s->irdft);
 240     s->rdft = s->irdft = NULL;
 241     s->rdft  = av_rdft_init(av_log2(work_len), DFT_R2C);
 242     s->irdft = av_rdft_init(av_log2(work_len), IDFT_C2R);
 243     if (!s->rdft || !s->irdft) {
 244         av_free(work);
 245         return AVERROR(ENOMEM);
 246     }
 247
 248     av_rdft_calc(s->rdft, work);   /* Cepstral: */
 249     UNPACK(work, work_len);
 250
 251     for (i = 0; i <= work_len; i += 2) {
 252         float angle = atan2f(work[i + 1], work[i]);
 253         float detect = 2 * M_PI;
 254         float delta = angle - prev_angle2;
 255         float adjust = detect * ((delta < -detect * .7f) - (delta > detect * .7f));
 256
 257         prev_angle2 = angle;
 258         cum_2pi += adjust;
 259         angle += cum_2pi;
 260         detect = M_PI;
 261         delta = angle - prev_angle1;
 262         adjust = detect * ((delta < -detect * .7f) - (delta > detect * .7f));
 263         prev_angle1 = angle;
 264         cum_1pi += fabsf(adjust);        /* fabs for when 2pi and 1pi have combined */
 265         pi_wraps[i >> 1] = cum_1pi;
 266
 267         work[i] = safe_log(sqrtf(SQR(work[i]) + SQR(work[i + 1])));
 268         work[i + 1] = 0;
 269     }
 270
 271     PACK(work, work_len);
 272     av_rdft_calc(s->irdft, work);
 273
 274     for (i = 0; i < work_len; i++)
 275         work[i] *= 2.f / work_len;
 276
 277     for (i = 1; i < work_len / 2; i++) {        /* Window to reject acausal components */
 278         work[i] *= 2;
 279         work[i + work_len / 2] = 0;
 280     }
 281     av_rdft_calc(s->rdft, work);
 282
 283     for (i = 2; i < work_len; i += 2)   /* Interpolate between linear & min phase */
 284         work[i + 1] = phase1 * i / work_len * pi_wraps[work_len >> 1] + (1 - phase1) * (work[i + 1] + pi_wraps[i >> 1]) - pi_wraps[i >> 1];
 285
 286     work[0] = exp(work[0]);
 287     work[1] = exp(work[1]);
 288     for (i = 2; i < work_len; i += 2) {
 289         float x = expf(work[i]);
 290
 291         work[i    ] = x * cosf(work[i + 1]);
 292         work[i + 1] = x * sinf(work[i + 1]);
 293     }
 294
 295     av_rdft_calc(s->irdft, work);
 296     for (i = 0; i < work_len; i++)
 297         work[i] *= 2.f / work_len;
 298
 299     /* Find peak pos. */
 300     for (i = 0; i <= (int) (pi_wraps[work_len >> 1] / M_PI + .5f); i++) {
 301         imp_sum += work[i];
 302         if (fabs(imp_sum) > fabs(peak_imp_sum)) {
 303             peak_imp_sum = imp_sum;
 304             peak = i;
 305         }
 306         if (work[i] > work[imp_peak])   /* For debug check only */
 307             imp_peak = i;
 308     }
 309
 310     while (peak && fabsf(work[peak - 1]) > fabsf(work[peak]) && (work[peak - 1] * work[peak] > 0)) {
 311         peak--;
 312     }
 313
 314     if (!phase1) {
 315         begin = 0;
 316     } else if (phase1 == 1) {
 317         begin = peak - *len / 2;
 318     } else {
 319         begin = (.997f - (2 - phase1) * .22f) * *len + .5f;
 320         end = (.997f + (0 - phase1) * .22f) * *len + .5f;
 321         begin = peak - (begin & ~3);
 322         end = peak + 1 + ((end + 3) & ~3);
 323         *len = end - begin;
 324         *h = av_realloc_f(*h, *len, sizeof(**h));
 325         if (!*h) {
 326             av_free(work);
 327             return AVERROR(ENOMEM);
 328         }
 329     }
 330
 331     for (i = 0; i < *len; i++) {
 332         (*h)[i] = work[(begin + (phase > 50.f ? *len - 1 - i : i) + work_len) & (work_len - 1)];
 333     }
 334     *post_len = phase > 50 ? peak - begin : begin + *len - (peak + 1);
 335
 336     av_log(s, AV_LOG_DEBUG, "%d nPI=%g peak-sum@%i=%g (val@%i=%g); len=%i post=%i (%g%%)\n",
 337            work_len, pi_wraps[work_len >> 1] / M_PI, peak, peak_imp_sum, imp_peak,
 338            work[imp_peak], *len, *post_len, 100.f - 100.f * *post_len / (*len - 1));
 339
 340     av_free(work);
 341
 342     return 0;
 343 }
 344
 345 static int config_output(AVFilterLink *outlink)
 346 {
 347     AVFilterContext *ctx = outlink->src;
 348     SincContext *s = ctx->priv;
 349     float Fn = s->sample_rate * .5f;
 350     float *h[2];
 351     int i, n, post_peak, longer;
 352
 353     outlink->sample_rate = s->sample_rate;
 354     s->pts = 0;
 355
 356     if (s->Fc0 >= Fn || s->Fc1 >= Fn) {
 357         av_log(ctx, AV_LOG_ERROR,
 358                "filter frequency must be less than %d/2.\n", s->sample_rate);
 359         return AVERROR(EINVAL);
 360     }
 361
 362     h[0] = lpf(Fn, s->Fc0, s->tbw0, &s->num_taps[0], s->att, &s->beta, s->round);
 363     h[1] = lpf(Fn, s->Fc1, s->tbw1, &s->num_taps[1], s->att, &s->beta, s->round);
 364
 365     if (h[0])
 366         invert(h[0], s->num_taps[0]);
 367
 368     longer = s->num_taps[1] > s->num_taps[0];
 369     n = s->num_taps[longer];
 370
 371     if (h[0] && h[1]) {
 372         for (i = 0; i < s->num_taps[!longer]; i++)
 373             h[longer][i + (n - s->num_taps[!longer]) / 2] += h[!longer][i];
 374
 375         if (s->Fc0 < s->Fc1)
 376             invert(h[longer], n);
 377
 378         av_free(h[!longer]);
 379     }
 380
 381     if (s->phase != 50.f) {
 382         int ret = fir_to_phase(s, &h[longer], &n, &post_peak, s->phase);
 383         if (ret < 0)
 384             return ret;
 385     } else {
 386         post_peak = n >> 1;
 387     }
 388
 389     s->n = 1 << (av_log2(n) + 1);
 390     s->rdft_len = 1 << av_log2(n);
 391     s->coeffs = av_calloc(s->n, sizeof(*s->coeffs));
 392     if (!s->coeffs)
 393         return AVERROR(ENOMEM);
 394
 395     for (i = 0; i < n; i++)
 396         s->coeffs[i] = h[longer][i];
 397     av_free(h[longer]);
 398
 399     av_rdft_end(s->rdft);
 400     av_rdft_end(s->irdft);
 401     s->rdft = s->irdft = NULL;
 402
 403     return 0;
 404 }
 405
 406 static av_cold void uninit(AVFilterContext *ctx)
 407 {
 408     SincContext *s = ctx->priv;
 409
 410     av_freep(&s->coeffs);
 411     av_rdft_end(s->rdft);
 412     av_rdft_end(s->irdft);
 413     s->rdft = s->irdft = NULL;
 414 }
 415
 416 static const AVFilterPad sinc_outputs[] = {
 417     {
 418         .name          = "default",
 419         .type          = AVMEDIA_TYPE_AUDIO,
 420         .config_props  = config_output,
 421         .request_frame = request_frame,
 422     },
 423     { NULL }
 424 };
 425
 426 #define AF AV_OPT_FLAG_AUDIO_PARAM|AV_OPT_FLAG_FILTERING_PARAM
 427 #define OFFSET(x) offsetof(SincContext, x)
 428
 429 static const AVOption sinc_options[] = {
 430     { "sample_rate", "set sample rate",                               OFFSET(sample_rate), AV_OPT_TYPE_INT,   {.i64=44100},  1, INT_MAX, AF },
 431     { "r",           "set sample rate",                               OFFSET(sample_rate), AV_OPT_TYPE_INT,   {.i64=44100},  1, INT_MAX, AF },
 432     { "nb_samples",  "set the number of samples per requested frame", OFFSET(nb_samples),  AV_OPT_TYPE_INT,   {.i64=1024},   1, INT_MAX, AF },
 433     { "n",           "set the number of samples per requested frame", OFFSET(nb_samples),  AV_OPT_TYPE_INT,   {.i64=1024},   1, INT_MAX, AF },
 434     { "hp",          "set high-pass filter frequency",                OFFSET(Fc0),         AV_OPT_TYPE_FLOAT, {.dbl=0},      0, INT_MAX, AF },
 435     { "lp",          "set low-pass filter frequency",                 OFFSET(Fc1),         AV_OPT_TYPE_FLOAT, {.dbl=0},      0, INT_MAX, AF },
 436     { "phase",       "set filter phase response",                     OFFSET(phase),       AV_OPT_TYPE_FLOAT, {.dbl=50},     0,     100, AF },
 437     { "beta",        "set kaiser window beta",                        OFFSET(beta),        AV_OPT_TYPE_FLOAT, {.dbl=-1},    -1,     256, AF },
 438     { "att",         "set stop-band attenuation",                     OFFSET(att),         AV_OPT_TYPE_FLOAT, {.dbl=120},   40,     180, AF },
 439     { "round",       "enable rounding",                               OFFSET(round),       AV_OPT_TYPE_BOOL,  {.i64=0},      0,       1, AF },
 440     { "hptaps",      "set number of taps for high-pass filter",       OFFSET(num_taps[0]), AV_OPT_TYPE_INT,   {.i64=0},      0,   32768, AF },
 441     { "lptaps",      "set number of taps for low-pass filter",        OFFSET(num_taps[1]), AV_OPT_TYPE_INT,   {.i64=0},      0,   32768, AF },
 442     { NULL }
 443 };
 444
 445 AVFILTER_DEFINE_CLASS(sinc);
 446
 447 const AVFilter ff_asrc_sinc = {
 448     .name          = "sinc",
 449     .description   = NULL_IF_CONFIG_SMALL("Generate a sinc kaiser-windowed low-pass, high-pass, band-pass, or band-reject FIR coefficients."),
 450     .priv_size     = sizeof(SincContext),
 451     .priv_class    = &sinc_class,
 452     .query_formats = query_formats,
 453     .uninit        = uninit,
 454     .inputs        = NULL,
 455     .outputs       = sinc_outputs,
 456 };