1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22#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
33
34
35struct rate_channel {
36 signed short last_S1;
37 signed short last_S2;
38};
39
40typedef 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,
43 int src_frames, int dst_frames);
44
45struct rate_priv {
46 unsigned int pitch;
47 unsigned int pos;
48 rate_f func;
49 snd_pcm_sframes_t old_src_frames, old_dst_frames;
50 struct rate_channel channels[];
51};
52
53static void rate_init(struct snd_pcm_plugin *plugin)
54{
55 unsigned int channel;
56 struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
57 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
64static 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,
67 int src_frames, int dst_frames)
68{
69 unsigned int pos = 0;
70 signed int val;
71 signed short S1, S2;
72 signed short *src, *dst;
73 unsigned int channel;
74 int src_step, dst_step;
75 int src_frames1, dst_frames1;
76 struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
77 struct rate_channel *rchannels = data->channels;
78
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;
90 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;
96 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) {
103 S2 = *src;
104 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;
112 *dst = val;
113 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
123static 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,
126 int src_frames, int dst_frames)
127{
128 unsigned int pos = 0;
129 signed int val;
130 signed short S1, S2;
131 signed short *src, *dst;
132 unsigned int channel;
133 int src_step, dst_step;
134 int src_frames1, dst_frames1;
135 struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
136 struct rate_channel *rchannels = data->channels;
137
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;
149 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;
155 src_frames1 = src_frames;
156 dst_frames1 = dst_frames;
157 while (dst_frames1 > 0) {
158 S1 = S2;
159 if (src_frames1-- > 0) {
160 S2 = *src;
161 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;
170 *dst = val;
171 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
183static snd_pcm_sframes_t rate_src_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
184{
185 struct rate_priv *data;
186 snd_pcm_sframes_t res;
187
188 if (snd_BUG_ON(!plugin))
189 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 = DIV_ROUND_CLOSEST(frames << SHIFT, 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
216static 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 if (snd_BUG_ON(!plugin))
222 return -ENXIO;
223 if (frames == 0)
224 return 0;
225 data = (struct rate_priv *)plugin->extra_data;
226 if (plugin->src_format.rate < plugin->dst_format.rate) {
227 res = DIV_ROUND_CLOSEST(frames << SHIFT, 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
249static 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,
252 snd_pcm_uframes_t frames)
253{
254 snd_pcm_uframes_t dst_frames;
255 struct rate_priv *data;
256
257 if (snd_BUG_ON(!plugin || !src_channels || !dst_channels))
258 return -ENXIO;
259 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++) {
265 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;
271 }
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;
278 data = (struct rate_priv *)plugin->extra_data;
279 data->func(plugin, src_channels, dst_channels, frames, dst_frames);
280 return dst_frames;
281}
282
283static int rate_action(struct snd_pcm_plugin *plugin,
284 enum snd_pcm_plugin_action action,
285 unsigned long udata)
286{
287 if (snd_BUG_ON(!plugin))
288 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;
298}
299
300int 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 if (snd_BUG_ON(!r_plugin))
310 return -ENXIO;
311 *r_plugin = NULL;
312
313 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;
323
324 err = snd_pcm_plugin_build(plug, "rate conversion",
325 src_format, dst_format,
326 struct_size(data, channels,
327 src_format->channels),
328 &plugin);
329 if (err < 0)
330 return err;
331 data = (struct rate_priv *)plugin->extra_data;
332 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}
349
