Pull bugzilla-5737 into release branch
[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
24 #ifdef CONFIG_SND_PCM_OSS_PLUGINS
25
26 #include <linux/time.h>
27 #include <sound/core.h>
28 #include <sound/pcm.h>
29 #include "pcm_plugin.h"
30
31 #define SHIFT   11
32 #define BITS    (1<<SHIFT)
33 #define R_MASK  (BITS-1)
34
35 /*
36  *  Basic rate conversion plugin
37  */
38
39 struct rate_channel {
40         signed short last_S1;
41         signed short last_S2;
42 };
43  
44 typedef void (*rate_f)(struct snd_pcm_plugin *plugin,
45                        const struct snd_pcm_plugin_channel *src_channels,
46                        struct snd_pcm_plugin_channel *dst_channels,
47                        int src_frames, int dst_frames);
48
49 struct rate_priv {
50         unsigned int pitch;
51         unsigned int pos;
52         rate_f func;
53         snd_pcm_sframes_t old_src_frames, old_dst_frames;
54         struct rate_channel channels[0];
55 };
56
57 static void rate_init(struct snd_pcm_plugin *plugin)
58 {
59         unsigned int channel;
60         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
61         data->pos = 0;
62         for (channel = 0; channel < plugin->src_format.channels; channel++) {
63                 data->channels[channel].last_S1 = 0;
64                 data->channels[channel].last_S2 = 0;
65         }
66 }
67
68 static void resample_expand(struct snd_pcm_plugin *plugin,
69                             const struct snd_pcm_plugin_channel *src_channels,
70                             struct snd_pcm_plugin_channel *dst_channels,
71                             int src_frames, int dst_frames)
72 {
73         unsigned int pos = 0;
74         signed int val;
75         signed short S1, S2;
76         signed short *src, *dst;
77         unsigned int channel;
78         int src_step, dst_step;
79         int src_frames1, dst_frames1;
80         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
81         struct rate_channel *rchannels = data->channels;
82         
83         for (channel = 0; channel < plugin->src_format.channels; channel++) {
84                 pos = data->pos;
85                 S1 = rchannels->last_S1;
86                 S2 = rchannels->last_S2;
87                 if (!src_channels[channel].enabled) {
88                         if (dst_channels[channel].wanted)
89                                 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
90                         dst_channels[channel].enabled = 0;
91                         continue;
92                 }
93                 dst_channels[channel].enabled = 1;
94                 src = (signed short *)src_channels[channel].area.addr +
95                         src_channels[channel].area.first / 8 / 2;
96                 dst = (signed short *)dst_channels[channel].area.addr +
97                         dst_channels[channel].area.first / 8 / 2;
98                 src_step = src_channels[channel].area.step / 8 / 2;
99                 dst_step = dst_channels[channel].area.step / 8 / 2;
100                 src_frames1 = src_frames;
101                 dst_frames1 = dst_frames;
102                 while (dst_frames1-- > 0) {
103                         if (pos & ~R_MASK) {
104                                 pos &= R_MASK;
105                                 S1 = S2;
106                                 if (src_frames1-- > 0) {
107                                         S2 = *src;
108                                         src += src_step;
109                                 }
110                         }
111                         val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
112                         if (val < -32768)
113                                 val = -32768;
114                         else if (val > 32767)
115                                 val = 32767;
116                         *dst = val;
117                         dst += dst_step;
118                         pos += data->pitch;
119                 }
120                 rchannels->last_S1 = S1;
121                 rchannels->last_S2 = S2;
122                 rchannels++;
123         }
124         data->pos = pos;
125 }
126
127 static void resample_shrink(struct snd_pcm_plugin *plugin,
128                             const struct snd_pcm_plugin_channel *src_channels,
129                             struct snd_pcm_plugin_channel *dst_channels,
130                             int src_frames, int dst_frames)
131 {
132         unsigned int pos = 0;
133         signed int val;
134         signed short S1, S2;
135         signed short *src, *dst;
136         unsigned int channel;
137         int src_step, dst_step;
138         int src_frames1, dst_frames1;
139         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
140         struct rate_channel *rchannels = data->channels;
141
142         for (channel = 0; channel < plugin->src_format.channels; ++channel) {
143                 pos = data->pos;
144                 S1 = rchannels->last_S1;
145                 S2 = rchannels->last_S2;
146                 if (!src_channels[channel].enabled) {
147                         if (dst_channels[channel].wanted)
148                                 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
149                         dst_channels[channel].enabled = 0;
150                         continue;
151                 }
152                 dst_channels[channel].enabled = 1;
153                 src = (signed short *)src_channels[channel].area.addr +
154                         src_channels[channel].area.first / 8 / 2;
155                 dst = (signed short *)dst_channels[channel].area.addr +
156                         dst_channels[channel].area.first / 8 / 2;
157                 src_step = src_channels[channel].area.step / 8 / 2;
158                 dst_step = dst_channels[channel].area.step / 8 / 2;
159                 src_frames1 = src_frames;
160                 dst_frames1 = dst_frames;
161                 while (dst_frames1 > 0) {
162                         S1 = S2;
163                         if (src_frames1-- > 0) {
164                                 S1 = *src;
165                                 src += src_step;
166                         }
167                         if (pos & ~R_MASK) {
168                                 pos &= R_MASK;
169                                 val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
170                                 if (val < -32768)
171                                         val = -32768;
172                                 else if (val > 32767)
173                                         val = 32767;
174                                 *dst = val;
175                                 dst += dst_step;
176                                 dst_frames1--;
177                         }
178                         pos += data->pitch;
179                 }
180                 rchannels->last_S1 = S1;
181                 rchannels->last_S2 = S2;
182                 rchannels++;
183         }
184         data->pos = pos;
185 }
186
187 static snd_pcm_sframes_t rate_src_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
188 {
189         struct rate_priv *data;
190         snd_pcm_sframes_t res;
191
192         snd_assert(plugin != NULL, return -ENXIO);
193         if (frames == 0)
194                 return 0;
195         data = (struct rate_priv *)plugin->extra_data;
196         if (plugin->src_format.rate < plugin->dst_format.rate) {
197                 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
198         } else {
199                 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);          
200         }
201         if (data->old_src_frames > 0) {
202                 snd_pcm_sframes_t frames1 = frames, res1 = data->old_dst_frames;
203                 while (data->old_src_frames < frames1) {
204                         frames1 >>= 1;
205                         res1 <<= 1;
206                 }
207                 while (data->old_src_frames > frames1) {
208                         frames1 <<= 1;
209                         res1 >>= 1;
210                 }
211                 if (data->old_src_frames == frames1)
212                         return res1;
213         }
214         data->old_src_frames = frames;
215         data->old_dst_frames = res;
216         return res;
217 }
218
219 static snd_pcm_sframes_t rate_dst_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 << SHIFT) + (data->pitch / 2)) / data->pitch);
230         } else {
231                 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
232         }
233         if (data->old_dst_frames > 0) {
234                 snd_pcm_sframes_t frames1 = frames, res1 = data->old_src_frames;
235                 while (data->old_dst_frames < frames1) {
236                         frames1 >>= 1;
237                         res1 <<= 1;
238                 }
239                 while (data->old_dst_frames > frames1) {
240                         frames1 <<= 1;
241                         res1 >>= 1;
242                 }
243                 if (data->old_dst_frames == frames1)
244                         return res1;
245         }
246         data->old_dst_frames = frames;
247         data->old_src_frames = res;
248         return res;
249 }
250
251 static snd_pcm_sframes_t rate_transfer(struct snd_pcm_plugin *plugin,
252                              const struct snd_pcm_plugin_channel *src_channels,
253                              struct snd_pcm_plugin_channel *dst_channels,
254                              snd_pcm_uframes_t frames)
255 {
256         snd_pcm_uframes_t dst_frames;
257         struct rate_priv *data;
258
259         snd_assert(plugin != NULL && src_channels != NULL && dst_channels != NULL, return -ENXIO);
260         if (frames == 0)
261                 return 0;
262 #ifdef CONFIG_SND_DEBUG
263         {
264                 unsigned int channel;
265                 for (channel = 0; channel < plugin->src_format.channels; channel++) {
266                         snd_assert(src_channels[channel].area.first % 8 == 0 &&
267                                    src_channels[channel].area.step % 8 == 0,
268                                    return -ENXIO);
269                         snd_assert(dst_channels[channel].area.first % 8 == 0 &&
270                                    dst_channels[channel].area.step % 8 == 0,
271                                    return -ENXIO);
272                 }
273         }
274 #endif
275
276         dst_frames = rate_dst_frames(plugin, frames);
277         if (dst_frames > dst_channels[0].frames)
278                 dst_frames = dst_channels[0].frames;
279         data = (struct rate_priv *)plugin->extra_data;
280         data->func(plugin, src_channels, dst_channels, frames, dst_frames);
281         return dst_frames;
282 }
283
284 static int rate_action(struct snd_pcm_plugin *plugin,
285                        enum snd_pcm_plugin_action action,
286                        unsigned long udata)
287 {
288         snd_assert(plugin != NULL, return -ENXIO);
289         switch (action) {
290         case INIT:
291         case PREPARE:
292                 rate_init(plugin);
293                 break;
294         default:
295                 break;
296         }
297         return 0;       /* silenty ignore other actions */
298 }
299
300 int snd_pcm_plugin_build_rate(struct snd_pcm_substream *plug,
301                               struct snd_pcm_plugin_format *src_format,
302                               struct snd_pcm_plugin_format *dst_format,
303                               struct snd_pcm_plugin **r_plugin)
304 {
305         int err;
306         struct rate_priv *data;
307         struct snd_pcm_plugin *plugin;
308
309         snd_assert(r_plugin != NULL, return -ENXIO);
310         *r_plugin = NULL;
311
312         snd_assert(src_format->channels == dst_format->channels, return -ENXIO);
313         snd_assert(src_format->channels > 0, return -ENXIO);
314         snd_assert(src_format->format == SNDRV_PCM_FORMAT_S16, return -ENXIO);
315         snd_assert(dst_format->format == SNDRV_PCM_FORMAT_S16, return -ENXIO);
316         snd_assert(src_format->rate != dst_format->rate, return -ENXIO);
317
318         err = snd_pcm_plugin_build(plug, "rate conversion",
319                                    src_format, dst_format,
320                                    sizeof(struct rate_priv) +
321                                    src_format->channels * sizeof(struct rate_channel),
322                                    &plugin);
323         if (err < 0)
324                 return err;
325         data = (struct rate_priv *)plugin->extra_data;
326         if (src_format->rate < dst_format->rate) {
327                 data->pitch = ((src_format->rate << SHIFT) + (dst_format->rate >> 1)) / dst_format->rate;
328                 data->func = resample_expand;
329         } else {
330                 data->pitch = ((dst_format->rate << SHIFT) + (src_format->rate >> 1)) / src_format->rate;
331                 data->func = resample_shrink;
332         }
333         data->pos = 0;
334         rate_init(plugin);
335         data->old_src_frames = data->old_dst_frames = 0;
336         plugin->transfer = rate_transfer;
337         plugin->src_frames = rate_src_frames;
338         plugin->dst_frames = rate_dst_frames;
339         plugin->action = rate_action;
340         *r_plugin = plugin;
341         return 0;
342 }
343
344 #endif