blob: 2fa9299a440dbb99e767c079e664d0a1133e8762 [file] [log] [blame]
Linus Torvalds1da177e2005-04-16 15:20:36 -07001/*
2 * Rate conversion Plug-In
Jaroslav Kyselac1017a42007-10-15 09:50:19 +02003 * Copyright (c) 1999 by Jaroslav Kysela <perex@perex.cz>
Linus Torvalds1da177e2005-04-16 15:20:36 -07004 *
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
Linus Torvalds1da177e2005-04-16 15:20:36 -070022#include <linux/time.h>
23#include <sound/core.h>
24#include <sound/pcm.h>
25#include "pcm_plugin.h"
26
27#define SHIFT 11
28#define BITS (1<<SHIFT)
29#define R_MASK (BITS-1)
30
31/*
32 * Basic rate conversion plugin
33 */
34
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010035struct rate_channel {
Linus Torvalds1da177e2005-04-16 15:20:36 -070036 signed short last_S1;
37 signed short last_S2;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010038};
Linus Torvalds1da177e2005-04-16 15:20:36 -070039
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010040typedef void (*rate_f)(struct snd_pcm_plugin *plugin,
41 const struct snd_pcm_plugin_channel *src_channels,
42 struct snd_pcm_plugin_channel *dst_channels,
Linus Torvalds1da177e2005-04-16 15:20:36 -070043 int src_frames, int dst_frames);
44
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010045struct rate_priv {
Linus Torvalds1da177e2005-04-16 15:20:36 -070046 unsigned int pitch;
47 unsigned int pos;
48 rate_f func;
Linus Torvalds1da177e2005-04-16 15:20:36 -070049 snd_pcm_sframes_t old_src_frames, old_dst_frames;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010050 struct rate_channel channels[0];
51};
Linus Torvalds1da177e2005-04-16 15:20:36 -070052
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010053static void rate_init(struct snd_pcm_plugin *plugin)
Linus Torvalds1da177e2005-04-16 15:20:36 -070054{
55 unsigned int channel;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010056 struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
Linus Torvalds1da177e2005-04-16 15:20:36 -070057 data->pos = 0;
58 for (channel = 0; channel < plugin->src_format.channels; channel++) {
59 data->channels[channel].last_S1 = 0;
60 data->channels[channel].last_S2 = 0;
61 }
62}
63
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010064static void resample_expand(struct snd_pcm_plugin *plugin,
65 const struct snd_pcm_plugin_channel *src_channels,
66 struct snd_pcm_plugin_channel *dst_channels,
Linus Torvalds1da177e2005-04-16 15:20:36 -070067 int src_frames, int dst_frames)
68{
69 unsigned int pos = 0;
70 signed int val;
71 signed short S1, S2;
Takashi Iwai0534ab42006-01-13 12:09:12 +010072 signed short *src, *dst;
Linus Torvalds1da177e2005-04-16 15:20:36 -070073 unsigned int channel;
74 int src_step, dst_step;
75 int src_frames1, dst_frames1;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +010076 struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
77 struct rate_channel *rchannels = data->channels;
Linus Torvalds1da177e2005-04-16 15:20:36 -070078
79 for (channel = 0; channel < plugin->src_format.channels; channel++) {
80 pos = data->pos;
81 S1 = rchannels->last_S1;
82 S2 = rchannels->last_S2;
83 if (!src_channels[channel].enabled) {
84 if (dst_channels[channel].wanted)
85 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
86 dst_channels[channel].enabled = 0;
87 continue;
88 }
89 dst_channels[channel].enabled = 1;
Takashi Iwai0534ab42006-01-13 12:09:12 +010090 src = (signed short *)src_channels[channel].area.addr +
91 src_channels[channel].area.first / 8 / 2;
92 dst = (signed short *)dst_channels[channel].area.addr +
93 dst_channels[channel].area.first / 8 / 2;
94 src_step = src_channels[channel].area.step / 8 / 2;
95 dst_step = dst_channels[channel].area.step / 8 / 2;
Linus Torvalds1da177e2005-04-16 15:20:36 -070096 src_frames1 = src_frames;
97 dst_frames1 = dst_frames;
98 while (dst_frames1-- > 0) {
99 if (pos & ~R_MASK) {
100 pos &= R_MASK;
101 S1 = S2;
102 if (src_frames1-- > 0) {
Takashi Iwai0534ab42006-01-13 12:09:12 +0100103 S2 = *src;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700104 src += src_step;
105 }
106 }
107 val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
108 if (val < -32768)
109 val = -32768;
110 else if (val > 32767)
111 val = 32767;
Takashi Iwai0534ab42006-01-13 12:09:12 +0100112 *dst = val;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700113 dst += dst_step;
114 pos += data->pitch;
115 }
116 rchannels->last_S1 = S1;
117 rchannels->last_S2 = S2;
118 rchannels++;
119 }
120 data->pos = pos;
121}
122
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100123static void resample_shrink(struct snd_pcm_plugin *plugin,
124 const struct snd_pcm_plugin_channel *src_channels,
125 struct snd_pcm_plugin_channel *dst_channels,
Linus Torvalds1da177e2005-04-16 15:20:36 -0700126 int src_frames, int dst_frames)
127{
128 unsigned int pos = 0;
129 signed int val;
130 signed short S1, S2;
Takashi Iwai0534ab42006-01-13 12:09:12 +0100131 signed short *src, *dst;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700132 unsigned int channel;
133 int src_step, dst_step;
134 int src_frames1, dst_frames1;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100135 struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
136 struct rate_channel *rchannels = data->channels;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700137
138 for (channel = 0; channel < plugin->src_format.channels; ++channel) {
139 pos = data->pos;
140 S1 = rchannels->last_S1;
141 S2 = rchannels->last_S2;
142 if (!src_channels[channel].enabled) {
143 if (dst_channels[channel].wanted)
144 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
145 dst_channels[channel].enabled = 0;
146 continue;
147 }
148 dst_channels[channel].enabled = 1;
Takashi Iwai0534ab42006-01-13 12:09:12 +0100149 src = (signed short *)src_channels[channel].area.addr +
150 src_channels[channel].area.first / 8 / 2;
151 dst = (signed short *)dst_channels[channel].area.addr +
152 dst_channels[channel].area.first / 8 / 2;
153 src_step = src_channels[channel].area.step / 8 / 2;
154 dst_step = dst_channels[channel].area.step / 8 / 2;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700155 src_frames1 = src_frames;
156 dst_frames1 = dst_frames;
157 while (dst_frames1 > 0) {
158 S1 = S2;
159 if (src_frames1-- > 0) {
Steve Chen5370d962009-02-21 08:05:04 -0600160 S2 = *src;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700161 src += src_step;
162 }
163 if (pos & ~R_MASK) {
164 pos &= R_MASK;
165 val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
166 if (val < -32768)
167 val = -32768;
168 else if (val > 32767)
169 val = 32767;
Takashi Iwai0534ab42006-01-13 12:09:12 +0100170 *dst = val;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700171 dst += dst_step;
172 dst_frames1--;
173 }
174 pos += data->pitch;
175 }
176 rchannels->last_S1 = S1;
177 rchannels->last_S2 = S2;
178 rchannels++;
179 }
180 data->pos = pos;
181}
182
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100183static snd_pcm_sframes_t rate_src_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700184{
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100185 struct rate_priv *data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700186 snd_pcm_sframes_t res;
187
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200188 if (snd_BUG_ON(!plugin))
189 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700190 if (frames == 0)
191 return 0;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100192 data = (struct rate_priv *)plugin->extra_data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700193 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
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100216static snd_pcm_sframes_t rate_dst_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700217{
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100218 struct rate_priv *data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700219 snd_pcm_sframes_t res;
220
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200221 if (snd_BUG_ON(!plugin))
222 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700223 if (frames == 0)
224 return 0;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100225 data = (struct rate_priv *)plugin->extra_data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700226 if (plugin->src_format.rate < plugin->dst_format.rate) {
227 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);
228 } else {
229 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
230 }
231 if (data->old_dst_frames > 0) {
232 snd_pcm_sframes_t frames1 = frames, res1 = data->old_src_frames;
233 while (data->old_dst_frames < frames1) {
234 frames1 >>= 1;
235 res1 <<= 1;
236 }
237 while (data->old_dst_frames > frames1) {
238 frames1 <<= 1;
239 res1 >>= 1;
240 }
241 if (data->old_dst_frames == frames1)
242 return res1;
243 }
244 data->old_dst_frames = frames;
245 data->old_src_frames = res;
246 return res;
247}
248
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100249static snd_pcm_sframes_t rate_transfer(struct snd_pcm_plugin *plugin,
250 const struct snd_pcm_plugin_channel *src_channels,
251 struct snd_pcm_plugin_channel *dst_channels,
Linus Torvalds1da177e2005-04-16 15:20:36 -0700252 snd_pcm_uframes_t frames)
253{
254 snd_pcm_uframes_t dst_frames;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100255 struct rate_priv *data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700256
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200257 if (snd_BUG_ON(!plugin || !src_channels || !dst_channels))
258 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700259 if (frames == 0)
260 return 0;
261#ifdef CONFIG_SND_DEBUG
262 {
263 unsigned int channel;
264 for (channel = 0; channel < plugin->src_format.channels; channel++) {
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200265 if (snd_BUG_ON(src_channels[channel].area.first % 8 ||
266 src_channels[channel].area.step % 8))
267 return -ENXIO;
268 if (snd_BUG_ON(dst_channels[channel].area.first % 8 ||
269 dst_channels[channel].area.step % 8))
270 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700271 }
272 }
273#endif
274
275 dst_frames = rate_dst_frames(plugin, frames);
276 if (dst_frames > dst_channels[0].frames)
277 dst_frames = dst_channels[0].frames;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100278 data = (struct rate_priv *)plugin->extra_data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700279 data->func(plugin, src_channels, dst_channels, frames, dst_frames);
280 return dst_frames;
281}
282
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100283static int rate_action(struct snd_pcm_plugin *plugin,
284 enum snd_pcm_plugin_action action,
Takashi Iwai47eaebf2005-11-17 10:18:00 +0100285 unsigned long udata)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700286{
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200287 if (snd_BUG_ON(!plugin))
288 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700289 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
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100300int 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)
Linus Torvalds1da177e2005-04-16 15:20:36 -0700304{
305 int err;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100306 struct rate_priv *data;
307 struct snd_pcm_plugin *plugin;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700308
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200309 if (snd_BUG_ON(!r_plugin))
310 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700311 *r_plugin = NULL;
312
Takashi Iwai7eaa9432008-08-08 17:09:09 +0200313 if (snd_BUG_ON(src_format->channels != dst_format->channels))
314 return -ENXIO;
315 if (snd_BUG_ON(src_format->channels <= 0))
316 return -ENXIO;
317 if (snd_BUG_ON(src_format->format != SNDRV_PCM_FORMAT_S16))
318 return -ENXIO;
319 if (snd_BUG_ON(dst_format->format != SNDRV_PCM_FORMAT_S16))
320 return -ENXIO;
321 if (snd_BUG_ON(src_format->rate == dst_format->rate))
322 return -ENXIO;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700323
324 err = snd_pcm_plugin_build(plug, "rate conversion",
325 src_format, dst_format,
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100326 sizeof(struct rate_priv) +
327 src_format->channels * sizeof(struct rate_channel),
Linus Torvalds1da177e2005-04-16 15:20:36 -0700328 &plugin);
329 if (err < 0)
330 return err;
Takashi Iwai6ac77bc2005-11-17 14:01:49 +0100331 data = (struct rate_priv *)plugin->extra_data;
Linus Torvalds1da177e2005-04-16 15:20:36 -0700332 if (src_format->rate < dst_format->rate) {
333 data->pitch = ((src_format->rate << SHIFT) + (dst_format->rate >> 1)) / dst_format->rate;
334 data->func = resample_expand;
335 } else {
336 data->pitch = ((dst_format->rate << SHIFT) + (src_format->rate >> 1)) / src_format->rate;
337 data->func = resample_shrink;
338 }
339 data->pos = 0;
340 rate_init(plugin);
341 data->old_src_frames = data->old_dst_frames = 0;
342 plugin->transfer = rate_transfer;
343 plugin->src_frames = rate_src_frames;
344 plugin->dst_frames = rate_dst_frames;
345 plugin->action = rate_action;
346 *r_plugin = plugin;
347 return 0;
348}