resample.c 13.5 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 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113
    int ph, i;
    double x, y, w;
    double *tab = av_malloc(tap_count * sizeof(*tab));
    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
        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);
237
        c->filter_alloc  = FFALIGN(c->filter_length, 8);
238
        c->filter_bank   = av_calloc(c->filter_alloc, (phase_count+1)*c->felem_size);
239 240
        c->filter_type   = filter_type;
        c->kaiser_beta   = kaiser_beta;
241 242
        if (!c->filter_bank)
            goto error;
243
        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))
244
            goto error;
245 246
        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
247 248 249
    }

    c->compensation_distance= 0;
250 251 252 253
    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
254 255 256 257 258 259 260 261 262 263
    c->index= -phase_count*((c->filter_length-1)/2);
    c->frac= 0;

    return c;
error:
    av_free(c->filter_bank);
    av_free(c);
    return NULL;
}

264
static void resample_free(ResampleContext **c){
Michael Niedermayer's avatar
Michael Niedermayer committed
265 266 267 268 269 270
    if(!*c)
        return;
    av_freep(&(*c)->filter_bank);
    av_freep(c);
}

271
static int set_compensation(ResampleContext *c, int sample_delta, int compensation_distance){
Michael Niedermayer's avatar
Michael Niedermayer committed
272
    c->compensation_distance= compensation_distance;
273 274 275 276 277
    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
278 279
}

280
#define TEMPLATE_RESAMPLE_S16
281
#include "resample_template.c"
282
#undef TEMPLATE_RESAMPLE_S16
283

284
#define TEMPLATE_RESAMPLE_S32
285
#include "resample_template.c"
286
#undef TEMPLATE_RESAMPLE_S32
287

288
#define TEMPLATE_RESAMPLE_FLT
289
#include "resample_template.c"
290
#undef TEMPLATE_RESAMPLE_FLT
Michael Niedermayer's avatar
Michael Niedermayer committed
291

292
#define TEMPLATE_RESAMPLE_DBL
293
#include "resample_template.c"
294
#undef TEMPLATE_RESAMPLE_DBL
295 296

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

299
#include "x86/resample_mmx.h"
300 301

#define TEMPLATE_RESAMPLE_S16_MMX2
302
#include "resample_template.c"
303
#undef TEMPLATE_RESAMPLE_S16_MMX2
304

305
#if HAVE_SSSE3_INLINE
306
#define TEMPLATE_RESAMPLE_S16_SSSE3
307
#include "resample_template.c"
308
#undef TEMPLATE_RESAMPLE_S16_SSSE3
309
#endif
310

311
#endif // HAVE_MMXEXT_INLINE
Michael Niedermayer's avatar
Michael Niedermayer committed
312

313
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
314
    int i, ret= -1;
315
    int av_unused mm_flags = av_get_cpu_flags();
316
    int need_emms= 0;
Michael Niedermayer's avatar
Michael Niedermayer committed
317 318

    for(i=0; i<dst->ch_count; i++){
319
#if HAVE_MMXEXT_INLINE
320
#if HAVE_SSSE3_INLINE
321
             if(c->format == AV_SAMPLE_FMT_S16P && (mm_flags&AV_CPU_FLAG_SSSE3)) ret= swri_resample_int16_ssse3(c, (int16_t*)dst->ch[i], (const int16_t*)src->ch[i], consumed, src_size, dst_size, i+1==dst->ch_count);
322 323
        else
#endif
324 325 326 327
             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
328 329 330 331 332
#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);
        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
333
    }
334 335
    if(need_emms)
        emms_c();
Michael Niedermayer's avatar
Michael Niedermayer committed
336 337
    return ret;
}
338

339
static int64_t get_delay(struct SwrContext *s, int64_t base){
340
    ResampleContext *c = s->resample;
341 342 343 344 345 346
    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);
347
}
348

349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364
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;
}

365 366 367 368
struct Resampler const swri_resampler={
  resample_init,
  resample_free,
  multiple_resample,
369
  resample_flush,
370 371 372
  set_compensation,
  get_delay,
};