Initial revision
[ffmpeg.git] / libavcodec / resample.c
1 /*
2  * Sample rate convertion for both audio and video
3  * Copyright (c) 2000 Gerard Lantau.
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
18  */
19 #include <stdlib.h>
20 #include <stdio.h>
21 #include <string.h>
22 #include <math.h>
23 #include "avcodec.h"
24
25 #define NDEBUG
26 #include <assert.h>
27
28 typedef struct {
29     /* fractional resampling */
30     UINT32 incr; /* fractional increment */
31     UINT32 frac;
32     int last_sample;
33     /* integer down sample */
34     int iratio;  /* integer divison ratio */
35     int icount, isum;
36     int inv;
37 } ReSampleChannelContext;
38
39 struct ReSampleContext {
40     ReSampleChannelContext channel_ctx[2];
41     float ratio;
42     /* channel convert */
43     int input_channels, output_channels, filter_channels;
44 };
45
46
47 #define FRAC_BITS 16
48 #define FRAC (1 << FRAC_BITS)
49
50 static void init_mono_resample(ReSampleChannelContext *s, float ratio)
51 {
52     ratio = 1.0 / ratio;
53     s->iratio = (int)floor(ratio);
54     if (s->iratio == 0)
55         s->iratio = 1;
56     s->incr = (int)((ratio / s->iratio) * FRAC);
57     s->frac = 0;
58     s->last_sample = 0;
59     s->icount = s->iratio;
60     s->isum = 0;
61     s->inv = (FRAC / s->iratio);
62 }
63
64 /* fractional audio resampling */
65 static int fractional_resample(ReSampleChannelContext *s, short *output, short *input, int nb_samples)
66 {
67     unsigned int frac, incr;
68     int l0, l1;
69     short *q, *p, *pend;
70
71     l0 = s->last_sample;
72     incr = s->incr;
73     frac = s->frac;
74
75     p = input;
76     pend = input + nb_samples;
77     q = output;
78
79     l1 = *p++;
80     for(;;) {
81         /* interpolate */
82         *q++ = (l0 * (FRAC - frac) + l1 * frac) >> FRAC_BITS;
83         frac = frac + s->incr;
84         while (frac >= FRAC) {
85             if (p >= pend)
86                 goto the_end;
87             frac -= FRAC;
88             l0 = l1;
89             l1 = *p++;
90         }
91     }
92  the_end:
93     s->last_sample = l1;
94     s->frac = frac;
95     return q - output;
96 }
97
98 static int integer_downsample(ReSampleChannelContext *s, short *output, short *input, int nb_samples)
99 {
100     short *q, *p, *pend;
101     int c, sum;
102
103     p = input;
104     pend = input + nb_samples;
105     q = output;
106
107     c = s->icount;
108     sum = s->isum;
109
110     for(;;) {
111         sum += *p++;
112         if (--c == 0) {
113             *q++ = (sum * s->inv) >> FRAC_BITS;
114             c = s->iratio;
115             sum = 0;
116         }
117         if (p >= pend)
118             break;
119     }
120     s->isum = sum;
121     s->icount = c;
122     return q - output;
123 }
124
125 /* n1: number of samples */
126 static void stereo_to_mono(short *output, short *input, int n1)
127 {
128     short *p, *q;
129     int n = n1;
130
131     p = input;
132     q = output;
133     while (n >= 4) {
134         q[0] = (p[0] + p[1]) >> 1;
135         q[1] = (p[2] + p[3]) >> 1;
136         q[2] = (p[4] + p[5]) >> 1;
137         q[3] = (p[6] + p[7]) >> 1;
138         q += 4;
139         p += 8;
140         n -= 4;
141     }
142     while (n > 0) {
143         q[0] = (p[0] + p[1]) >> 1;
144         q++;
145         p += 2;
146         n--;
147     }
148 }
149
150 /* n1: number of samples */
151 static void mono_to_stereo(short *output, short *input, int n1)
152 {
153     short *p, *q;
154     int n = n1;
155     int v;
156
157     p = input;
158     q = output;
159     while (n >= 4) {
160         v = p[0]; q[0] = v; q[1] = v;
161         v = p[1]; q[2] = v; q[3] = v;
162         v = p[2]; q[4] = v; q[5] = v;
163         v = p[3]; q[6] = v; q[7] = v;
164         q += 8;
165         p += 4;
166         n -= 4;
167     }
168     while (n > 0) {
169         v = p[0]; q[0] = v; q[1] = v;
170         q += 2;
171         p += 1;
172         n--;
173     }
174 }
175
176 /* XXX: should use more abstract 'N' channels system */
177 static void stereo_split(short *output1, short *output2, short *input, int n)
178 {
179     int i;
180
181     for(i=0;i<n;i++) {
182         *output1++ = *input++;
183         *output2++ = *input++;
184     }
185 }
186
187 static void stereo_mux(short *output, short *input1, short *input2, int n)
188 {
189     int i;
190
191     for(i=0;i<n;i++) {
192         *output++ = *input1++;
193         *output++ = *input2++;
194     }
195 }
196
197 static int mono_resample(ReSampleChannelContext *s, short *output, short *input, int nb_samples)
198 {
199     short buf1[nb_samples];
200     short *buftmp;
201
202     /* first downsample by an integer factor with averaging filter */
203     if (s->iratio > 1) {
204         buftmp = buf1;
205         nb_samples = integer_downsample(s, buftmp, input, nb_samples);
206     } else {
207         buftmp = input;
208     }
209
210     /* then do a fractional resampling with linear interpolation */
211     if (s->incr != FRAC) {
212         nb_samples = fractional_resample(s, output, buftmp, nb_samples);
213     } else {
214         memcpy(output, buftmp, nb_samples * sizeof(short));
215     }
216     return nb_samples;
217 }
218
219 ReSampleContext *audio_resample_init(int output_channels, int input_channels, 
220                                       int output_rate, int input_rate)
221 {
222     ReSampleContext *s;
223     int i;
224     
225     if (output_channels > 2 || input_channels > 2)
226         return NULL;
227
228     s = av_mallocz(sizeof(ReSampleContext));
229     if (!s)
230         return NULL;
231
232     s->ratio = (float)output_rate / (float)input_rate;
233     
234     s->input_channels = input_channels;
235     s->output_channels = output_channels;
236     
237     s->filter_channels = s->input_channels;
238     if (s->output_channels < s->filter_channels)
239         s->filter_channels = s->output_channels;
240
241     for(i=0;i<s->filter_channels;i++) {
242         init_mono_resample(&s->channel_ctx[i], s->ratio);
243     }
244     return s;
245 }
246
247 /* resample audio. 'nb_samples' is the number of input samples */
248 /* XXX: optimize it ! */
249 /* XXX: do it with polyphase filters, since the quality here is
250    HORRIBLE. Return the number of samples available in output */
251 int audio_resample(ReSampleContext *s, short *output, short *input, int nb_samples)
252 {
253     int i, nb_samples1;
254     short bufin[2][nb_samples];
255     short bufout[2][(int)(nb_samples * s->ratio) + 16]; /* make some zoom to avoid round pb */
256     short *buftmp2[2], *buftmp3[2];
257
258     if (s->input_channels == s->output_channels && s->ratio == 1.0) {
259         /* nothing to do */
260         memcpy(output, input, nb_samples * s->input_channels * sizeof(short));
261         return nb_samples;
262     }
263
264     if (s->input_channels == 2 &&
265         s->output_channels == 1) {
266         buftmp2[0] = bufin[0];
267         buftmp3[0] = output;
268         stereo_to_mono(buftmp2[0], input, nb_samples);
269     } else if (s->output_channels == 2 && s->input_channels == 1) {
270         buftmp2[0] = input;
271         buftmp3[0] = bufout[0];
272     } else if (s->output_channels == 2) {
273         buftmp2[0] = bufin[0];
274         buftmp2[1] = bufin[1];
275         buftmp3[0] = bufout[0];
276         buftmp3[1] = bufout[1];
277         stereo_split(buftmp2[0], buftmp2[1], input, nb_samples);
278     } else {
279         buftmp2[0] = input;
280         buftmp3[0] = output;
281     }
282
283     /* resample each channel */
284     nb_samples1 = 0; /* avoid warning */
285     for(i=0;i<s->filter_channels;i++) {
286         nb_samples1 = mono_resample(&s->channel_ctx[i], buftmp3[i], buftmp2[i], nb_samples);
287     }
288
289     if (s->output_channels == 2 && s->input_channels == 1) {
290         mono_to_stereo(output, buftmp3[0], nb_samples1);
291     } else if (s->output_channels == 2) {
292         stereo_mux(output, buftmp3[0], buftmp3[1], nb_samples1);
293     }
294
295     return nb_samples1;
296 }
297
298 void audio_resample_close(ReSampleContext *s)
299 {
300     free(s);
301 }