resample: fix avresample_get_delay() return value
[ffmpeg.git] / libavresample / resample.c
1 /*
2  * Copyright (c) 2004 Michael Niedermayer <michaelni@gmx.at>
3  * Copyright (c) 2012 Justin Ruggles <justin.ruggles@gmail.com>
4  *
5  * This file is part of Libav.
6  *
7  * Libav 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  * Libav 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 Libav; 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/common.h"
23 #include "libavutil/libm.h"
24 #include "libavutil/log.h"
25 #include "internal.h"
26 #include "resample.h"
27 #include "audio_data.h"
28
29 struct ResampleContext {
30     AVAudioResampleContext *avr;
31     AudioData *buffer;
32     uint8_t *filter_bank;
33     int filter_length;
34     int ideal_dst_incr;
35     int dst_incr;
36     int index;
37     int frac;
38     int src_incr;
39     int compensation_distance;
40     int phase_shift;
41     int phase_mask;
42     int linear;
43     enum AVResampleFilterType filter_type;
44     int kaiser_beta;
45     double factor;
46     void (*set_filter)(void *filter, double *tab, int phase, int tap_count);
47     void (*resample_one)(struct ResampleContext *c, int no_filter, void *dst0,
48                          int dst_index, const void *src0, int src_size,
49                          int index, int frac);
50     int padding_size;
51 };
52
53
54 /* double template */
55 #define CONFIG_RESAMPLE_DBL
56 #include "resample_template.c"
57 #undef CONFIG_RESAMPLE_DBL
58
59 /* float template */
60 #define CONFIG_RESAMPLE_FLT
61 #include "resample_template.c"
62 #undef CONFIG_RESAMPLE_FLT
63
64 /* s32 template */
65 #define CONFIG_RESAMPLE_S32
66 #include "resample_template.c"
67 #undef CONFIG_RESAMPLE_S32
68
69 /* s16 template */
70 #include "resample_template.c"
71
72
73 /* 0th order modified bessel function of the first kind. */
74 static double bessel(double x)
75 {
76     double v     = 1;
77     double lastv = 0;
78     double t     = 1;
79     int i;
80
81     x = x * x / 4;
82     for (i = 1; v != lastv; i++) {
83         lastv = v;
84         t    *= x / (i * i);
85         v    += t;
86     }
87     return v;
88 }
89
90 /* Build a polyphase filterbank. */
91 static int build_filter(ResampleContext *c)
92 {
93     int ph, i;
94     double x, y, w, factor;
95     double *tab;
96     int tap_count    = c->filter_length;
97     int phase_count  = 1 << c->phase_shift;
98     const int center = (tap_count - 1) / 2;
99
100     tab = av_malloc(tap_count * sizeof(*tab));
101     if (!tab)
102         return AVERROR(ENOMEM);
103
104     /* if upsampling, only need to interpolate, no filter */
105     factor = FFMIN(c->factor, 1.0);
106
107     for (ph = 0; ph < phase_count; ph++) {
108         double norm = 0;
109         for (i = 0; i < tap_count; i++) {
110             x = M_PI * ((double)(i - center) - (double)ph / phase_count) * factor;
111             if (x == 0) y = 1.0;
112             else        y = sin(x) / x;
113             switch (c->filter_type) {
114             case AV_RESAMPLE_FILTER_TYPE_CUBIC: {
115                 const float d = -0.5; //first order derivative = -0.5
116                 x = fabs(((double)(i - center) - (double)ph / phase_count) * factor);
117                 if (x < 1.0) y = 1 - 3 * x*x + 2 * x*x*x + d * (                -x*x + x*x*x);
118                 else         y =                           d * (-4 + 8 * x - 5 * x*x + x*x*x);
119                 break;
120             }
121             case AV_RESAMPLE_FILTER_TYPE_BLACKMAN_NUTTALL:
122                 w  = 2.0 * x / (factor * tap_count) + M_PI;
123                 y *= 0.3635819 - 0.4891775 * cos(    w) +
124                                  0.1365995 * cos(2 * w) -
125                                  0.0106411 * cos(3 * w);
126                 break;
127             case AV_RESAMPLE_FILTER_TYPE_KAISER:
128                 w  = 2.0 * x / (factor * tap_count * M_PI);
129                 y *= bessel(c->kaiser_beta * sqrt(FFMAX(1 - w * w, 0)));
130                 break;
131             }
132
133             tab[i] = y;
134             norm  += y;
135         }
136         /* normalize so that an uniform color remains the same */
137         for (i = 0; i < tap_count; i++)
138             tab[i] = tab[i] / norm;
139
140         c->set_filter(c->filter_bank, tab, ph, tap_count);
141     }
142
143     av_free(tab);
144     return 0;
145 }
146
147 ResampleContext *ff_audio_resample_init(AVAudioResampleContext *avr)
148 {
149     ResampleContext *c;
150     int out_rate    = avr->out_sample_rate;
151     int in_rate     = avr->in_sample_rate;
152     double factor   = FFMIN(out_rate * avr->cutoff / in_rate, 1.0);
153     int phase_count = 1 << avr->phase_shift;
154     int felem_size;
155
156     if (avr->internal_sample_fmt != AV_SAMPLE_FMT_S16P &&
157         avr->internal_sample_fmt != AV_SAMPLE_FMT_S32P &&
158         avr->internal_sample_fmt != AV_SAMPLE_FMT_FLTP &&
159         avr->internal_sample_fmt != AV_SAMPLE_FMT_DBLP) {
160         av_log(avr, AV_LOG_ERROR, "Unsupported internal format for "
161                "resampling: %s\n",
162                av_get_sample_fmt_name(avr->internal_sample_fmt));
163         return NULL;
164     }
165     c = av_mallocz(sizeof(*c));
166     if (!c)
167         return NULL;
168
169     c->avr           = avr;
170     c->phase_shift   = avr->phase_shift;
171     c->phase_mask    = phase_count - 1;
172     c->linear        = avr->linear_interp;
173     c->factor        = factor;
174     c->filter_length = FFMAX((int)ceil(avr->filter_size / factor), 1);
175     c->filter_type   = avr->filter_type;
176     c->kaiser_beta   = avr->kaiser_beta;
177
178     switch (avr->internal_sample_fmt) {
179     case AV_SAMPLE_FMT_DBLP:
180         c->resample_one  = resample_one_dbl;
181         c->set_filter    = set_filter_dbl;
182         break;
183     case AV_SAMPLE_FMT_FLTP:
184         c->resample_one  = resample_one_flt;
185         c->set_filter    = set_filter_flt;
186         break;
187     case AV_SAMPLE_FMT_S32P:
188         c->resample_one  = resample_one_s32;
189         c->set_filter    = set_filter_s32;
190         break;
191     case AV_SAMPLE_FMT_S16P:
192         c->resample_one  = resample_one_s16;
193         c->set_filter    = set_filter_s16;
194         break;
195     }
196
197     felem_size = av_get_bytes_per_sample(avr->internal_sample_fmt);
198     c->filter_bank = av_mallocz(c->filter_length * (phase_count + 1) * felem_size);
199     if (!c->filter_bank)
200         goto error;
201
202     if (build_filter(c) < 0)
203         goto error;
204
205     memcpy(&c->filter_bank[(c->filter_length * phase_count + 1) * felem_size],
206            c->filter_bank, (c->filter_length - 1) * felem_size);
207     memcpy(&c->filter_bank[c->filter_length * phase_count * felem_size],
208            &c->filter_bank[(c->filter_length - 1) * felem_size], felem_size);
209
210     c->compensation_distance = 0;
211     if (!av_reduce(&c->src_incr, &c->dst_incr, out_rate,
212                    in_rate * (int64_t)phase_count, INT32_MAX / 2))
213         goto error;
214     c->ideal_dst_incr = c->dst_incr;
215
216     c->padding_size   = (c->filter_length - 1) / 2;
217     c->index = -phase_count * ((c->filter_length - 1) / 2);
218     c->frac  = 0;
219
220     /* allocate internal buffer */
221     c->buffer = ff_audio_data_alloc(avr->resample_channels, 0,
222                                     avr->internal_sample_fmt,
223                                     "resample buffer");
224     if (!c->buffer)
225         goto error;
226
227     av_log(avr, AV_LOG_DEBUG, "resample: %s from %d Hz to %d Hz\n",
228            av_get_sample_fmt_name(avr->internal_sample_fmt),
229            avr->in_sample_rate, avr->out_sample_rate);
230
231     return c;
232
233 error:
234     ff_audio_data_free(&c->buffer);
235     av_free(c->filter_bank);
236     av_free(c);
237     return NULL;
238 }
239
240 void ff_audio_resample_free(ResampleContext **c)
241 {
242     if (!*c)
243         return;
244     ff_audio_data_free(&(*c)->buffer);
245     av_free((*c)->filter_bank);
246     av_freep(c);
247 }
248
249 int avresample_set_compensation(AVAudioResampleContext *avr, int sample_delta,
250                                 int compensation_distance)
251 {
252     ResampleContext *c;
253     AudioData *fifo_buf = NULL;
254     int ret = 0;
255
256     if (compensation_distance < 0)
257         return AVERROR(EINVAL);
258     if (!compensation_distance && sample_delta)
259         return AVERROR(EINVAL);
260
261     if (!avr->resample_needed) {
262 #if FF_API_RESAMPLE_CLOSE_OPEN
263         /* if resampling was not enabled previously, re-initialize the
264            AVAudioResampleContext and force resampling */
265         int fifo_samples;
266         int restore_matrix = 0;
267         double matrix[AVRESAMPLE_MAX_CHANNELS * AVRESAMPLE_MAX_CHANNELS] = { 0 };
268
269         /* buffer any remaining samples in the output FIFO before closing */
270         fifo_samples = av_audio_fifo_size(avr->out_fifo);
271         if (fifo_samples > 0) {
272             fifo_buf = ff_audio_data_alloc(avr->out_channels, fifo_samples,
273                                            avr->out_sample_fmt, NULL);
274             if (!fifo_buf)
275                 return AVERROR(EINVAL);
276             ret = ff_audio_data_read_from_fifo(avr->out_fifo, fifo_buf,
277                                                fifo_samples);
278             if (ret < 0)
279                 goto reinit_fail;
280         }
281         /* save the channel mixing matrix */
282         if (avr->am) {
283             ret = avresample_get_matrix(avr, matrix, AVRESAMPLE_MAX_CHANNELS);
284             if (ret < 0)
285                 goto reinit_fail;
286             restore_matrix = 1;
287         }
288
289         /* close the AVAudioResampleContext */
290         avresample_close(avr);
291
292         avr->force_resampling = 1;
293
294         /* restore the channel mixing matrix */
295         if (restore_matrix) {
296             ret = avresample_set_matrix(avr, matrix, AVRESAMPLE_MAX_CHANNELS);
297             if (ret < 0)
298                 goto reinit_fail;
299         }
300
301         /* re-open the AVAudioResampleContext */
302         ret = avresample_open(avr);
303         if (ret < 0)
304             goto reinit_fail;
305
306         /* restore buffered samples to the output FIFO */
307         if (fifo_samples > 0) {
308             ret = ff_audio_data_add_to_fifo(avr->out_fifo, fifo_buf, 0,
309                                             fifo_samples);
310             if (ret < 0)
311                 goto reinit_fail;
312             ff_audio_data_free(&fifo_buf);
313         }
314 #else
315         av_log(avr, AV_LOG_ERROR, "Unable to set resampling compensation\n");
316         return AVERROR(EINVAL);
317 #endif
318     }
319     c = avr->resample;
320     c->compensation_distance = compensation_distance;
321     if (compensation_distance) {
322         c->dst_incr = c->ideal_dst_incr - c->ideal_dst_incr *
323                       (int64_t)sample_delta / compensation_distance;
324     } else {
325         c->dst_incr = c->ideal_dst_incr;
326     }
327     return 0;
328
329 reinit_fail:
330     ff_audio_data_free(&fifo_buf);
331     return ret;
332 }
333
334 static int resample(ResampleContext *c, void *dst, const void *src,
335                     int *consumed, int src_size, int dst_size, int update_ctx)
336 {
337     int dst_index;
338     int index         = c->index;
339     int frac          = c->frac;
340     int dst_incr_frac = c->dst_incr % c->src_incr;
341     int dst_incr      = c->dst_incr / c->src_incr;
342     int compensation_distance = c->compensation_distance;
343
344     if (!dst != !src)
345         return AVERROR(EINVAL);
346
347     if (compensation_distance == 0 && c->filter_length == 1 &&
348         c->phase_shift == 0) {
349         int64_t index2 = ((int64_t)index) << 32;
350         int64_t incr   = (1LL << 32) * c->dst_incr / c->src_incr;
351         dst_size       = FFMIN(dst_size,
352                                (src_size-1-index) * (int64_t)c->src_incr /
353                                c->dst_incr);
354
355         if (dst) {
356             for(dst_index = 0; dst_index < dst_size; dst_index++) {
357                 c->resample_one(c, 1, dst, dst_index, src, 0, index2 >> 32, 0);
358                 index2 += incr;
359             }
360         } else {
361             dst_index = dst_size;
362         }
363         index += dst_index * dst_incr;
364         index += (frac + dst_index * (int64_t)dst_incr_frac) / c->src_incr;
365         frac   = (frac + dst_index * (int64_t)dst_incr_frac) % c->src_incr;
366     } else {
367         for (dst_index = 0; dst_index < dst_size; dst_index++) {
368             int sample_index = index >> c->phase_shift;
369
370             if (sample_index + c->filter_length > src_size ||
371                 -sample_index >= src_size)
372                 break;
373
374             if (dst)
375                 c->resample_one(c, 0, dst, dst_index, src, src_size, index, frac);
376
377             frac  += dst_incr_frac;
378             index += dst_incr;
379             if (frac >= c->src_incr) {
380                 frac -= c->src_incr;
381                 index++;
382             }
383             if (dst_index + 1 == compensation_distance) {
384                 compensation_distance = 0;
385                 dst_incr_frac = c->ideal_dst_incr % c->src_incr;
386                 dst_incr      = c->ideal_dst_incr / c->src_incr;
387             }
388         }
389     }
390     if (consumed)
391         *consumed = FFMAX(index, 0) >> c->phase_shift;
392
393     if (update_ctx) {
394         if (index >= 0)
395             index &= c->phase_mask;
396
397         if (compensation_distance) {
398             compensation_distance -= dst_index;
399             if (compensation_distance <= 0)
400                 return AVERROR_BUG;
401         }
402         c->frac     = frac;
403         c->index    = index;
404         c->dst_incr = dst_incr_frac + c->src_incr*dst_incr;
405         c->compensation_distance = compensation_distance;
406     }
407
408     return dst_index;
409 }
410
411 int ff_audio_resample(ResampleContext *c, AudioData *dst, AudioData *src)
412 {
413     int ch, in_samples, in_leftover, consumed = 0, out_samples = 0;
414     int ret = AVERROR(EINVAL);
415
416     in_samples  = src ? src->nb_samples : 0;
417     in_leftover = c->buffer->nb_samples;
418
419     /* add input samples to the internal buffer */
420     if (src) {
421         ret = ff_audio_data_combine(c->buffer, in_leftover, src, 0, in_samples);
422         if (ret < 0)
423             return ret;
424     } else if (!in_leftover) {
425         /* no remaining samples to flush */
426         return 0;
427     } else {
428         /* TODO: pad buffer to flush completely */
429     }
430
431     /* calculate output size and reallocate output buffer if needed */
432     /* TODO: try to calculate this without the dummy resample() run */
433     if (!dst->read_only && dst->allow_realloc) {
434         out_samples = resample(c, NULL, NULL, NULL, c->buffer->nb_samples,
435                                INT_MAX, 0);
436         ret = ff_audio_data_realloc(dst, out_samples);
437         if (ret < 0) {
438             av_log(c->avr, AV_LOG_ERROR, "error reallocating output\n");
439             return ret;
440         }
441     }
442
443     /* resample each channel plane */
444     for (ch = 0; ch < c->buffer->channels; ch++) {
445         out_samples = resample(c, (void *)dst->data[ch],
446                                (const void *)c->buffer->data[ch], &consumed,
447                                c->buffer->nb_samples, dst->allocated_samples,
448                                ch + 1 == c->buffer->channels);
449     }
450     if (out_samples < 0) {
451         av_log(c->avr, AV_LOG_ERROR, "error during resampling\n");
452         return out_samples;
453     }
454
455     /* drain consumed samples from the internal buffer */
456     ff_audio_data_drain(c->buffer, consumed);
457
458     av_dlog(c->avr, "resampled %d in + %d leftover to %d out + %d leftover\n",
459             in_samples, in_leftover, out_samples, c->buffer->nb_samples);
460
461     dst->nb_samples = out_samples;
462     return 0;
463 }
464
465 int avresample_get_delay(AVAudioResampleContext *avr)
466 {
467     ResampleContext *c = avr->resample;
468
469     if (!avr->resample_needed || !avr->resample)
470         return 0;
471
472     return FFMAX(c->buffer->nb_samples - c->padding_size, 0);
473 }