Merge master.kernel.org:/home/rmk/linux-2.6-serial
[linux-2.6] / sound / core / oss / rate.c
1 /*
2  *  Rate conversion Plug-In
3  *  Copyright (c) 1999 by Jaroslav Kysela <perex@suse.cz>
4  *
5  *
6  *   This library is free software; you can redistribute it and/or modify
7  *   it under the terms of the GNU Library General Public License as
8  *   published by the Free Software Foundation; either version 2 of
9  *   the License, or (at your option) any later version.
10  *
11  *   This program is distributed in the hope that it will be useful,
12  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
13  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  *   GNU Library General Public License for more details.
15  *
16  *   You should have received a copy of the GNU Library General Public
17  *   License along with this library; if not, write to the Free Software
18  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
19  *
20  */
21   
22 #include <sound/driver.h>
23 #include <linux/time.h>
24 #include <sound/core.h>
25 #include <sound/pcm.h>
26 #include "pcm_plugin.h"
27
28 #define SHIFT   11
29 #define BITS    (1<<SHIFT)
30 #define R_MASK  (BITS-1)
31
32 /*
33  *  Basic rate conversion plugin
34  */
35
36 struct rate_channel {
37         signed short last_S1;
38         signed short last_S2;
39 };
40  
41 typedef void (*rate_f)(struct snd_pcm_plugin *plugin,
42                        const struct snd_pcm_plugin_channel *src_channels,
43                        struct snd_pcm_plugin_channel *dst_channels,
44                        int src_frames, int dst_frames);
45
46 struct rate_priv {
47         unsigned int pitch;
48         unsigned int pos;
49         rate_f func;
50         int get, put;
51         snd_pcm_sframes_t old_src_frames, old_dst_frames;
52         struct rate_channel channels[0];
53 };
54
55 static void rate_init(struct snd_pcm_plugin *plugin)
56 {
57         unsigned int channel;
58         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
59         data->pos = 0;
60         for (channel = 0; channel < plugin->src_format.channels; channel++) {
61                 data->channels[channel].last_S1 = 0;
62                 data->channels[channel].last_S2 = 0;
63         }
64 }
65
66 static void resample_expand(struct snd_pcm_plugin *plugin,
67                             const struct snd_pcm_plugin_channel *src_channels,
68                             struct snd_pcm_plugin_channel *dst_channels,
69                             int src_frames, int dst_frames)
70 {
71         unsigned int pos = 0;
72         signed int val;
73         signed short S1, S2;
74         char *src, *dst;
75         unsigned int channel;
76         int src_step, dst_step;
77         int src_frames1, dst_frames1;
78         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
79         struct rate_channel *rchannels = data->channels;
80
81 #define GET_S16_LABELS
82 #define PUT_S16_LABELS
83 #include "plugin_ops.h"
84 #undef GET_S16_LABELS
85 #undef PUT_S16_LABELS
86         void *get = get_s16_labels[data->get];
87         void *put = put_s16_labels[data->put];
88         signed short sample = 0;
89         
90         for (channel = 0; channel < plugin->src_format.channels; channel++) {
91                 pos = data->pos;
92                 S1 = rchannels->last_S1;
93                 S2 = rchannels->last_S2;
94                 if (!src_channels[channel].enabled) {
95                         if (dst_channels[channel].wanted)
96                                 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
97                         dst_channels[channel].enabled = 0;
98                         continue;
99                 }
100                 dst_channels[channel].enabled = 1;
101                 src = (char *)src_channels[channel].area.addr + src_channels[channel].area.first / 8;
102                 dst = (char *)dst_channels[channel].area.addr + dst_channels[channel].area.first / 8;
103                 src_step = src_channels[channel].area.step / 8;
104                 dst_step = dst_channels[channel].area.step / 8;
105                 src_frames1 = src_frames;
106                 dst_frames1 = dst_frames;
107                 while (dst_frames1-- > 0) {
108                         if (pos & ~R_MASK) {
109                                 pos &= R_MASK;
110                                 S1 = S2;
111                                 if (src_frames1-- > 0) {
112                                         goto *get;
113 #define GET_S16_END after_get
114 #include "plugin_ops.h"
115 #undef GET_S16_END
116                                 after_get:
117                                         S2 = sample;
118                                         src += src_step;
119                                 }
120                         }
121                         val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
122                         if (val < -32768)
123                                 val = -32768;
124                         else if (val > 32767)
125                                 val = 32767;
126                         sample = val;
127                         goto *put;
128 #define PUT_S16_END after_put
129 #include "plugin_ops.h"
130 #undef PUT_S16_END
131                 after_put:
132                         dst += dst_step;
133                         pos += data->pitch;
134                 }
135                 rchannels->last_S1 = S1;
136                 rchannels->last_S2 = S2;
137                 rchannels++;
138         }
139         data->pos = pos;
140 }
141
142 static void resample_shrink(struct snd_pcm_plugin *plugin,
143                             const struct snd_pcm_plugin_channel *src_channels,
144                             struct snd_pcm_plugin_channel *dst_channels,
145                             int src_frames, int dst_frames)
146 {
147         unsigned int pos = 0;
148         signed int val;
149         signed short S1, S2;
150         char *src, *dst;
151         unsigned int channel;
152         int src_step, dst_step;
153         int src_frames1, dst_frames1;
154         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
155         struct rate_channel *rchannels = data->channels;
156         
157 #define GET_S16_LABELS
158 #define PUT_S16_LABELS
159 #include "plugin_ops.h"
160 #undef GET_S16_LABELS
161 #undef PUT_S16_LABELS
162         void *get = get_s16_labels[data->get];
163         void *put = put_s16_labels[data->put];
164         signed short sample = 0;
165
166         for (channel = 0; channel < plugin->src_format.channels; ++channel) {
167                 pos = data->pos;
168                 S1 = rchannels->last_S1;
169                 S2 = rchannels->last_S2;
170                 if (!src_channels[channel].enabled) {
171                         if (dst_channels[channel].wanted)
172                                 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
173                         dst_channels[channel].enabled = 0;
174                         continue;
175                 }
176                 dst_channels[channel].enabled = 1;
177                 src = (char *)src_channels[channel].area.addr + src_channels[channel].area.first / 8;
178                 dst = (char *)dst_channels[channel].area.addr + dst_channels[channel].area.first / 8;
179                 src_step = src_channels[channel].area.step / 8;
180                 dst_step = dst_channels[channel].area.step / 8;
181                 src_frames1 = src_frames;
182                 dst_frames1 = dst_frames;
183                 while (dst_frames1 > 0) {
184                         S1 = S2;
185                         if (src_frames1-- > 0) {
186                                 goto *get;
187 #define GET_S16_END after_get
188 #include "plugin_ops.h"
189 #undef GET_S16_END
190                         after_get:
191                                 S2 = sample;
192                                 src += src_step;
193                         }
194                         if (pos & ~R_MASK) {
195                                 pos &= R_MASK;
196                                 val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
197                                 if (val < -32768)
198                                         val = -32768;
199                                 else if (val > 32767)
200                                         val = 32767;
201                                 sample = val;
202                                 goto *put;
203 #define PUT_S16_END after_put
204 #include "plugin_ops.h"
205 #undef PUT_S16_END
206                         after_put:
207                                 dst += dst_step;
208                                 dst_frames1--;
209                         }
210                         pos += data->pitch;
211                 }
212                 rchannels->last_S1 = S1;
213                 rchannels->last_S2 = S2;
214                 rchannels++;
215         }
216         data->pos = pos;
217 }
218
219 static snd_pcm_sframes_t rate_src_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
220 {
221         struct rate_priv *data;
222         snd_pcm_sframes_t res;
223
224         snd_assert(plugin != NULL, return -ENXIO);
225         if (frames == 0)
226                 return 0;
227         data = (struct rate_priv *)plugin->extra_data;
228         if (plugin->src_format.rate < plugin->dst_format.rate) {
229                 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
230         } else {
231                 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);          
232         }
233         if (data->old_src_frames > 0) {
234                 snd_pcm_sframes_t frames1 = frames, res1 = data->old_dst_frames;
235                 while (data->old_src_frames < frames1) {
236                         frames1 >>= 1;
237                         res1 <<= 1;
238                 }
239                 while (data->old_src_frames > frames1) {
240                         frames1 <<= 1;
241                         res1 >>= 1;
242                 }
243                 if (data->old_src_frames == frames1)
244                         return res1;
245         }
246         data->old_src_frames = frames;
247         data->old_dst_frames = res;
248         return res;
249 }
250
251 static snd_pcm_sframes_t rate_dst_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
252 {
253         struct rate_priv *data;
254         snd_pcm_sframes_t res;
255
256         snd_assert(plugin != NULL, return -ENXIO);
257         if (frames == 0)
258                 return 0;
259         data = (struct rate_priv *)plugin->extra_data;
260         if (plugin->src_format.rate < plugin->dst_format.rate) {
261                 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);
262         } else {
263                 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
264         }
265         if (data->old_dst_frames > 0) {
266                 snd_pcm_sframes_t frames1 = frames, res1 = data->old_src_frames;
267                 while (data->old_dst_frames < frames1) {
268                         frames1 >>= 1;
269                         res1 <<= 1;
270                 }
271                 while (data->old_dst_frames > frames1) {
272                         frames1 <<= 1;
273                         res1 >>= 1;
274                 }
275                 if (data->old_dst_frames == frames1)
276                         return res1;
277         }
278         data->old_dst_frames = frames;
279         data->old_src_frames = res;
280         return res;
281 }
282
283 static snd_pcm_sframes_t rate_transfer(struct snd_pcm_plugin *plugin,
284                              const struct snd_pcm_plugin_channel *src_channels,
285                              struct snd_pcm_plugin_channel *dst_channels,
286                              snd_pcm_uframes_t frames)
287 {
288         snd_pcm_uframes_t dst_frames;
289         struct rate_priv *data;
290
291         snd_assert(plugin != NULL && src_channels != NULL && dst_channels != NULL, return -ENXIO);
292         if (frames == 0)
293                 return 0;
294 #ifdef CONFIG_SND_DEBUG
295         {
296                 unsigned int channel;
297                 for (channel = 0; channel < plugin->src_format.channels; channel++) {
298                         snd_assert(src_channels[channel].area.first % 8 == 0 &&
299                                    src_channels[channel].area.step % 8 == 0,
300                                    return -ENXIO);
301                         snd_assert(dst_channels[channel].area.first % 8 == 0 &&
302                                    dst_channels[channel].area.step % 8 == 0,
303                                    return -ENXIO);
304                 }
305         }
306 #endif
307
308         dst_frames = rate_dst_frames(plugin, frames);
309         if (dst_frames > dst_channels[0].frames)
310                 dst_frames = dst_channels[0].frames;
311         data = (struct rate_priv *)plugin->extra_data;
312         data->func(plugin, src_channels, dst_channels, frames, dst_frames);
313         return dst_frames;
314 }
315
316 static int rate_action(struct snd_pcm_plugin *plugin,
317                        enum snd_pcm_plugin_action action,
318                        unsigned long udata)
319 {
320         snd_assert(plugin != NULL, return -ENXIO);
321         switch (action) {
322         case INIT:
323         case PREPARE:
324                 rate_init(plugin);
325                 break;
326         default:
327                 break;
328         }
329         return 0;       /* silenty ignore other actions */
330 }
331
332 int snd_pcm_plugin_build_rate(struct snd_pcm_substream *plug,
333                               struct snd_pcm_plugin_format *src_format,
334                               struct snd_pcm_plugin_format *dst_format,
335                               struct snd_pcm_plugin **r_plugin)
336 {
337         int err;
338         struct rate_priv *data;
339         struct snd_pcm_plugin *plugin;
340
341         snd_assert(r_plugin != NULL, return -ENXIO);
342         *r_plugin = NULL;
343
344         snd_assert(src_format->channels == dst_format->channels, return -ENXIO);
345         snd_assert(src_format->channels > 0, return -ENXIO);
346         snd_assert(snd_pcm_format_linear(src_format->format) != 0, return -ENXIO);
347         snd_assert(snd_pcm_format_linear(dst_format->format) != 0, return -ENXIO);
348         snd_assert(src_format->rate != dst_format->rate, return -ENXIO);
349
350         err = snd_pcm_plugin_build(plug, "rate conversion",
351                                    src_format, dst_format,
352                                    sizeof(struct rate_priv) +
353                                    src_format->channels * sizeof(struct rate_channel),
354                                    &plugin);
355         if (err < 0)
356                 return err;
357         data = (struct rate_priv *)plugin->extra_data;
358         data->get = getput_index(src_format->format);
359         snd_assert(data->get >= 0 && data->get < 4*2*2, return -EINVAL);
360         data->put = getput_index(dst_format->format);
361         snd_assert(data->put >= 0 && data->put < 4*2*2, return -EINVAL);
362
363         if (src_format->rate < dst_format->rate) {
364                 data->pitch = ((src_format->rate << SHIFT) + (dst_format->rate >> 1)) / dst_format->rate;
365                 data->func = resample_expand;
366         } else {
367                 data->pitch = ((dst_format->rate << SHIFT) + (src_format->rate >> 1)) / src_format->rate;
368                 data->func = resample_shrink;
369         }
370         data->pos = 0;
371         rate_init(plugin);
372         data->old_src_frames = data->old_dst_frames = 0;
373         plugin->transfer = rate_transfer;
374         plugin->src_frames = rate_src_frames;
375         plugin->dst_frames = rate_dst_frames;
376         plugin->action = rate_action;
377         *r_plugin = plugin;
378         return 0;
379 }