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