resample.c 14 KB
Newer Older
Michael Niedermayer's avatar
Michael Niedermayer committed
1 2
/*
 * audio resampling
3
 * Copyright (c) 2004-2012 Michael Niedermayer <michaelni@gmx.at>
Michael Niedermayer's avatar
Michael Niedermayer committed
4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28
 *
 * This file is part of FFmpeg.
 *
 * FFmpeg is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * FFmpeg is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with FFmpeg; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

/**
 * @file
 * audio resampling
 * @author Michael Niedermayer <michaelni@gmx.at>
 */

#include "libavutil/log.h"
29
#include "libavutil/avassert.h"
Michael Niedermayer's avatar
Michael Niedermayer committed
30 31 32
#include "swresample_internal.h"


33
typedef struct ResampleContext {
Michael Niedermayer's avatar
Michael Niedermayer committed
34
    const AVClass *av_class;
35
    uint8_t *filter_bank;
Michael Niedermayer's avatar
Michael Niedermayer committed
36
    int filter_length;
37
    int filter_alloc;
Michael Niedermayer's avatar
Michael Niedermayer committed
38 39 40 41 42 43 44 45 46
    int ideal_dst_incr;
    int dst_incr;
    int index;
    int frac;
    int src_incr;
    int compensation_distance;
    int phase_shift;
    int phase_mask;
    int linear;
47 48
    enum SwrFilterType filter_type;
    int kaiser_beta;
Michael Niedermayer's avatar
Michael Niedermayer committed
49
    double factor;
50 51 52
    enum AVSampleFormat format;
    int felem_size;
    int filter_shift;
53
} ResampleContext;
Michael Niedermayer's avatar
Michael Niedermayer committed
54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80

/**
 * 0th order modified bessel function of the first kind.
 */
static double bessel(double x){
    double v=1;
    double lastv=0;
    double t=1;
    int i;
    static const double inv[100]={
 1.0/( 1* 1), 1.0/( 2* 2), 1.0/( 3* 3), 1.0/( 4* 4), 1.0/( 5* 5), 1.0/( 6* 6), 1.0/( 7* 7), 1.0/( 8* 8), 1.0/( 9* 9), 1.0/(10*10),
 1.0/(11*11), 1.0/(12*12), 1.0/(13*13), 1.0/(14*14), 1.0/(15*15), 1.0/(16*16), 1.0/(17*17), 1.0/(18*18), 1.0/(19*19), 1.0/(20*20),
 1.0/(21*21), 1.0/(22*22), 1.0/(23*23), 1.0/(24*24), 1.0/(25*25), 1.0/(26*26), 1.0/(27*27), 1.0/(28*28), 1.0/(29*29), 1.0/(30*30),
 1.0/(31*31), 1.0/(32*32), 1.0/(33*33), 1.0/(34*34), 1.0/(35*35), 1.0/(36*36), 1.0/(37*37), 1.0/(38*38), 1.0/(39*39), 1.0/(40*40),
 1.0/(41*41), 1.0/(42*42), 1.0/(43*43), 1.0/(44*44), 1.0/(45*45), 1.0/(46*46), 1.0/(47*47), 1.0/(48*48), 1.0/(49*49), 1.0/(50*50),
 1.0/(51*51), 1.0/(52*52), 1.0/(53*53), 1.0/(54*54), 1.0/(55*55), 1.0/(56*56), 1.0/(57*57), 1.0/(58*58), 1.0/(59*59), 1.0/(60*60),
 1.0/(61*61), 1.0/(62*62), 1.0/(63*63), 1.0/(64*64), 1.0/(65*65), 1.0/(66*66), 1.0/(67*67), 1.0/(68*68), 1.0/(69*69), 1.0/(70*70),
 1.0/(71*71), 1.0/(72*72), 1.0/(73*73), 1.0/(74*74), 1.0/(75*75), 1.0/(76*76), 1.0/(77*77), 1.0/(78*78), 1.0/(79*79), 1.0/(80*80),
 1.0/(81*81), 1.0/(82*82), 1.0/(83*83), 1.0/(84*84), 1.0/(85*85), 1.0/(86*86), 1.0/(87*87), 1.0/(88*88), 1.0/(89*89), 1.0/(90*90),
 1.0/(91*91), 1.0/(92*92), 1.0/(93*93), 1.0/(94*94), 1.0/(95*95), 1.0/(96*96), 1.0/(97*97), 1.0/(98*98), 1.0/(99*99), 1.0/(10000)
    };

    x= x*x/4;
    for(i=0; v != lastv; i++){
        lastv=v;
        t *= x*inv[i];
        v += t;
81
        av_assert2(i<99);
Michael Niedermayer's avatar
Michael Niedermayer committed
82 83 84 85 86 87 88 89
    }
    return v;
}

/**
 * builds a polyphase filterbank.
 * @param factor resampling factor
 * @param scale wanted sum of coefficients for each filter
90 91
 * @param filter_type  filter type
 * @param kaiser_beta  kaiser window beta
Michael Niedermayer's avatar
Michael Niedermayer committed
92 93
 * @return 0 on success, negative on error
 */
94 95
static int build_filter(ResampleContext *c, void *filter, double factor, int tap_count, int alloc, int phase_count, int scale,
                        int filter_type, int kaiser_beta){
Michael Niedermayer's avatar
Michael Niedermayer committed
96 97
    int ph, i;
    double x, y, w;
98
    double *tab = av_malloc_array(tap_count,  sizeof(*tab));
Michael Niedermayer's avatar
Michael Niedermayer committed
99 100 101 102 103 104 105 106 107 108 109 110 111 112 113
    const int center= (tap_count-1)/2;

    if (!tab)
        return AVERROR(ENOMEM);

    /* if upsampling, only need to interpolate, no filter */
    if (factor > 1.0)
        factor = 1.0;

    for(ph=0;ph<phase_count;ph++) {
        double norm = 0;
        for(i=0;i<tap_count;i++) {
            x = M_PI * ((double)(i - center) - (double)ph / phase_count) * factor;
            if (x == 0) y = 1.0;
            else        y = sin(x) / x;
114 115
            switch(filter_type){
            case SWR_FILTER_TYPE_CUBIC:{
Michael Niedermayer's avatar
Michael Niedermayer committed
116 117 118 119 120
                const float d= -0.5; //first order derivative = -0.5
                x = fabs(((double)(i - center) - (double)ph / phase_count) * factor);
                if(x<1.0) y= 1 - 3*x*x + 2*x*x*x + d*(            -x*x + x*x*x);
                else      y=                       d*(-4 + 8*x - 5*x*x + x*x*x);
                break;}
121
            case SWR_FILTER_TYPE_BLACKMAN_NUTTALL:
Michael Niedermayer's avatar
Michael Niedermayer committed
122 123 124
                w = 2.0*x / (factor*tap_count) + M_PI;
                y *= 0.3635819 - 0.4891775 * cos(w) + 0.1365995 * cos(2*w) - 0.0106411 * cos(3*w);
                break;
125
            case SWR_FILTER_TYPE_KAISER:
Michael Niedermayer's avatar
Michael Niedermayer committed
126
                w = 2.0*x / (factor*tap_count*M_PI);
127
                y *= bessel(kaiser_beta*sqrt(FFMAX(1-w*w, 0)));
Michael Niedermayer's avatar
Michael Niedermayer committed
128
                break;
129 130
            default:
                av_assert0(0);
Michael Niedermayer's avatar
Michael Niedermayer committed
131 132 133 134 135 136 137
            }

            tab[i] = y;
            norm += y;
        }

        /* normalize so that an uniform color remains the same */
138
        switch(c->format){
139
        case AV_SAMPLE_FMT_S16P:
140
            for(i=0;i<tap_count;i++)
141
                ((int16_t*)filter)[ph * alloc + i] = av_clip(lrintf(tab[i] * scale / norm), INT16_MIN, INT16_MAX);
142
            break;
143
        case AV_SAMPLE_FMT_S32P:
144
            for(i=0;i<tap_count;i++)
145
                ((int32_t*)filter)[ph * alloc + i] = av_clipl_int32(llrint(tab[i] * scale / norm));
146
            break;
147
        case AV_SAMPLE_FMT_FLTP:
148
            for(i=0;i<tap_count;i++)
149
                ((float*)filter)[ph * alloc + i] = tab[i] * scale / norm;
150
            break;
151
        case AV_SAMPLE_FMT_DBLP:
152
            for(i=0;i<tap_count;i++)
153
                ((double*)filter)[ph * alloc + i] = tab[i] * scale / norm;
154
            break;
Michael Niedermayer's avatar
Michael Niedermayer committed
155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197
        }
    }
#if 0
    {
#define LEN 1024
        int j,k;
        double sine[LEN + tap_count];
        double filtered[LEN];
        double maxff=-2, minff=2, maxsf=-2, minsf=2;
        for(i=0; i<LEN; i++){
            double ss=0, sf=0, ff=0;
            for(j=0; j<LEN+tap_count; j++)
                sine[j]= cos(i*j*M_PI/LEN);
            for(j=0; j<LEN; j++){
                double sum=0;
                ph=0;
                for(k=0; k<tap_count; k++)
                    sum += filter[ph * tap_count + k] * sine[k+j];
                filtered[j]= sum / (1<<FILTER_SHIFT);
                ss+= sine[j + center] * sine[j + center];
                ff+= filtered[j] * filtered[j];
                sf+= sine[j + center] * filtered[j];
            }
            ss= sqrt(2*ss/LEN);
            ff= sqrt(2*ff/LEN);
            sf= 2*sf/LEN;
            maxff= FFMAX(maxff, ff);
            minff= FFMIN(minff, ff);
            maxsf= FFMAX(maxsf, sf);
            minsf= FFMIN(minsf, sf);
            if(i%11==0){
                av_log(NULL, AV_LOG_ERROR, "i:%4d ss:%f ff:%13.6e-%13.6e sf:%13.6e-%13.6e\n", i, ss, maxff, minff, maxsf, minsf);
                minff=minsf= 2;
                maxff=maxsf= -2;
            }
        }
    }
#endif

    av_free(tab);
    return 0;
}

198
static ResampleContext *resample_init(ResampleContext *c, int out_rate, int in_rate, int filter_size, int phase_shift, int linear,
199 200
                                    double cutoff0, enum AVSampleFormat format, enum SwrFilterType filter_type, int kaiser_beta,
                                    double precision, int cheby){
201
    double cutoff = cutoff0? cutoff0 : 0.97;
Michael Niedermayer's avatar
Michael Niedermayer committed
202 203 204
    double factor= FFMIN(out_rate * cutoff / in_rate, 1.0);
    int phase_count= 1<<phase_shift;

205
    if (!c || c->phase_shift != phase_shift || c->linear!=linear || c->factor != factor
206 207
           || c->filter_length != FFMAX((int)ceil(filter_size/factor), 1) || c->format != format
           || c->filter_type != filter_type || c->kaiser_beta != kaiser_beta) {
208
        c = av_mallocz(sizeof(*c));
209 210 211
        if (!c)
            return NULL;

212 213
        c->format= format;

214 215
        c->felem_size= av_get_bytes_per_sample(c->format);

216
        switch(c->format){
217
        case AV_SAMPLE_FMT_S16P:
218 219
            c->filter_shift = 15;
            break;
220
        case AV_SAMPLE_FMT_S32P:
221 222
            c->filter_shift = 30;
            break;
223 224
        case AV_SAMPLE_FMT_FLTP:
        case AV_SAMPLE_FMT_DBLP:
225 226
            c->filter_shift = 0;
            break;
227 228
        default:
            av_log(NULL, AV_LOG_ERROR, "Unsupported sample format\n");
229
            av_assert0(0);
230 231
        }

232 233 234 235 236
        if (filter_size/factor > INT32_MAX/256) {
            av_log(NULL, AV_LOG_ERROR, "Filter length too large\n");
            goto error;
        }

237 238 239 240 241
        c->phase_shift   = phase_shift;
        c->phase_mask    = phase_count - 1;
        c->linear        = linear;
        c->factor        = factor;
        c->filter_length = FFMAX((int)ceil(filter_size/factor), 1);
242
        c->filter_alloc  = FFALIGN(c->filter_length, 8);
243
        c->filter_bank   = av_calloc(c->filter_alloc, (phase_count+1)*c->felem_size);
244 245
        c->filter_type   = filter_type;
        c->kaiser_beta   = kaiser_beta;
246 247
        if (!c->filter_bank)
            goto error;
248
        if (build_filter(c, (void*)c->filter_bank, factor, c->filter_length, c->filter_alloc, phase_count, 1<<c->filter_shift, filter_type, kaiser_beta))
249
            goto error;
250 251
        memcpy(c->filter_bank + (c->filter_alloc*phase_count+1)*c->felem_size, c->filter_bank, (c->filter_alloc-1)*c->felem_size);
        memcpy(c->filter_bank + (c->filter_alloc*phase_count  )*c->felem_size, c->filter_bank + (c->filter_alloc - 1)*c->felem_size, c->felem_size);
Michael Niedermayer's avatar
Michael Niedermayer committed
252 253 254
    }

    c->compensation_distance= 0;
255 256 257 258
    if(!av_reduce(&c->src_incr, &c->dst_incr, out_rate, in_rate * (int64_t)phase_count, INT32_MAX/2))
        goto error;
    c->ideal_dst_incr= c->dst_incr;

Michael Niedermayer's avatar
Michael Niedermayer committed
259 260 261 262 263
    c->index= -phase_count*((c->filter_length-1)/2);
    c->frac= 0;

    return c;
error:
264
    av_freep(&c->filter_bank);
Michael Niedermayer's avatar
Michael Niedermayer committed
265 266 267 268
    av_free(c);
    return NULL;
}

269
static void resample_free(ResampleContext **c){
Michael Niedermayer's avatar
Michael Niedermayer committed
270 271 272 273 274 275
    if(!*c)
        return;
    av_freep(&(*c)->filter_bank);
    av_freep(c);
}

276
static int set_compensation(ResampleContext *c, int sample_delta, int compensation_distance){
Michael Niedermayer's avatar
Michael Niedermayer committed
277
    c->compensation_distance= compensation_distance;
278 279 280 281 282
    if (compensation_distance)
        c->dst_incr = c->ideal_dst_incr - c->ideal_dst_incr * (int64_t)sample_delta / compensation_distance;
    else
        c->dst_incr = c->ideal_dst_incr;
    return 0;
Michael Niedermayer's avatar
Michael Niedermayer committed
283 284
}

285
#define TEMPLATE_RESAMPLE_S16
286
#include "resample_template.c"
287
#undef TEMPLATE_RESAMPLE_S16
288

289
#define TEMPLATE_RESAMPLE_S32
290
#include "resample_template.c"
291
#undef TEMPLATE_RESAMPLE_S32
292

293
#define TEMPLATE_RESAMPLE_FLT
294
#include "resample_template.c"
295
#undef TEMPLATE_RESAMPLE_FLT
Michael Niedermayer's avatar
Michael Niedermayer committed
296

297
#define TEMPLATE_RESAMPLE_DBL
298
#include "resample_template.c"
299
#undef TEMPLATE_RESAMPLE_DBL
300 301

// XXX FIXME the whole C loop should be written in asm so this x86 specific code here isnt needed
302
#if HAVE_MMXEXT_INLINE
303

304
#include "x86/resample_mmx.h"
305 306

#define TEMPLATE_RESAMPLE_S16_MMX2
307
#include "resample_template.c"
308
#undef TEMPLATE_RESAMPLE_S16_MMX2
309

310 311 312 313 314 315
#if HAVE_SSE_INLINE
#define TEMPLATE_RESAMPLE_FLT_SSE
#include "resample_template.c"
#undef TEMPLATE_RESAMPLE_FLT_SSE
#endif

316 317
#if HAVE_SSE2_INLINE
#define TEMPLATE_RESAMPLE_S16_SSE2
318
#include "resample_template.c"
319
#undef TEMPLATE_RESAMPLE_S16_SSE2
320
#endif
321

322
#endif // HAVE_MMXEXT_INLINE
Michael Niedermayer's avatar
Michael Niedermayer committed
323

324
static int multiple_resample(ResampleContext *c, AudioData *dst, int dst_size, AudioData *src, int src_size, int *consumed){
Michael Niedermayer's avatar
Michael Niedermayer committed
325
    int i, ret= -1;
326
    int av_unused mm_flags = av_get_cpu_flags();
327
    int need_emms= 0;
Michael Niedermayer's avatar
Michael Niedermayer committed
328 329

    for(i=0; i<dst->ch_count; i++){
330
#if HAVE_MMXEXT_INLINE
331 332
#if HAVE_SSE2_INLINE
             if(c->format == AV_SAMPLE_FMT_S16P && (mm_flags&AV_CPU_FLAG_SSE2)) ret= swri_resample_int16_sse2 (c, (int16_t*)dst->ch[i], (const int16_t*)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
333 334
        else
#endif
335 336 337 338
             if(c->format == AV_SAMPLE_FMT_S16P && (mm_flags&AV_CPU_FLAG_MMX2 )){
                 ret= swri_resample_int16_mmx2 (c, (int16_t*)dst->ch[i], (const int16_t*)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
                 need_emms= 1;
             } else
339 340 341
#endif
             if(c->format == AV_SAMPLE_FMT_S16P) ret= swri_resample_int16(c, (int16_t*)dst->ch[i], (const int16_t*)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
        else if(c->format == AV_SAMPLE_FMT_S32P) ret= swri_resample_int32(c, (int32_t*)dst->ch[i], (const int32_t*)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
342 343 344 345
#if HAVE_SSE_INLINE
        else if(c->format == AV_SAMPLE_FMT_FLTP && (mm_flags&AV_CPU_FLAG_SSE))
                                                 ret= swri_resample_float_sse (c, (float*)dst->ch[i], (const float*)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
#endif
346 347
        else if(c->format == AV_SAMPLE_FMT_FLTP) ret= swri_resample_float(c, (float  *)dst->ch[i], (const float  *)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
        else if(c->format == AV_SAMPLE_FMT_DBLP) ret= swri_resample_double(c,(double *)dst->ch[i], (const double *)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
Michael Niedermayer's avatar
Michael Niedermayer committed
348
    }
349 350
    if(need_emms)
        emms_c();
Michael Niedermayer's avatar
Michael Niedermayer committed
351 352
    return ret;
}
353

354
static int64_t get_delay(struct SwrContext *s, int64_t base){
355
    ResampleContext *c = s->resample;
356 357 358 359 360 361
    int64_t num = s->in_buffer_count - (c->filter_length-1)/2;
    num <<= c->phase_shift;
    num -= c->index;
    num *= c->src_incr;
    num -= c->frac;
    return av_rescale(num, base, s->in_sample_rate*(int64_t)c->src_incr << c->phase_shift);
362
}
363

364 365 366 367 368 369 370 371 372 373 374 375 376 377 378 379
static int resample_flush(struct SwrContext *s) {
    AudioData *a= &s->in_buffer;
    int i, j, ret;
    if((ret = swri_realloc_audio(a, s->in_buffer_index + 2*s->in_buffer_count)) < 0)
        return ret;
    av_assert0(a->planar);
    for(i=0; i<a->ch_count; i++){
        for(j=0; j<s->in_buffer_count; j++){
            memcpy(a->ch[i] + (s->in_buffer_index+s->in_buffer_count+j  )*a->bps,
                a->ch[i] + (s->in_buffer_index+s->in_buffer_count-j-1)*a->bps, a->bps);
        }
    }
    s->in_buffer_count += (s->in_buffer_count+1)/2;
    return 0;
}

380 381 382 383
struct Resampler const swri_resampler={
  resample_init,
  resample_free,
  multiple_resample,
384
  resample_flush,
385 386 387
  set_compensation,
  get_delay,
};