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[0];
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 snd_assert(plugin != NULL, return -ENXIO);
189 if (frames == 0)
190 return 0;
191 data = (struct rate_priv *)plugin->extra_data;
192 if (plugin->src_format.rate < plugin->dst_format.rate) {
193 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
194 } else {
195 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);
196 }
197 if (data->old_src_frames > 0) {
198 snd_pcm_sframes_t frames1 = frames, res1 = data->old_dst_frames;
199 while (data->old_src_frames < frames1) {
200 frames1 >>= 1;
201 res1 <<= 1;
202 }
203 while (data->old_src_frames > frames1) {
204 frames1 <<= 1;
205 res1 >>= 1;
206 }
207 if (data->old_src_frames == frames1)
208 return res1;
209 }
210 data->old_src_frames = frames;
211 data->old_dst_frames = res;
212 return res;
213}
214
215static snd_pcm_sframes_t rate_dst_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
216{
217 struct rate_priv *data;
218 snd_pcm_sframes_t res;
219
220 snd_assert(plugin != NULL, return -ENXIO);
221 if (frames == 0)
222 return 0;
223 data = (struct rate_priv *)plugin->extra_data;
224 if (plugin->src_format.rate < plugin->dst_format.rate) {
225 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);
226 } else {
227 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
228 }
229 if (data->old_dst_frames > 0) {
230 snd_pcm_sframes_t frames1 = frames, res1 = data->old_src_frames;
231 while (data->old_dst_frames < frames1) {
232 frames1 >>= 1;
233 res1 <<= 1;
234 }
235 while (data->old_dst_frames > frames1) {
236 frames1 <<= 1;
237 res1 >>= 1;
238 }
239 if (data->old_dst_frames == frames1)
240 return res1;
241 }
242 data->old_dst_frames = frames;
243 data->old_src_frames = res;
244 return res;
245}
246
247static snd_pcm_sframes_t rate_transfer(struct snd_pcm_plugin *plugin,
248 const struct snd_pcm_plugin_channel *src_channels,
249 struct snd_pcm_plugin_channel *dst_channels,
250 snd_pcm_uframes_t frames)
251{
252 snd_pcm_uframes_t dst_frames;
253 struct rate_priv *data;
254
255 snd_assert(plugin != NULL && src_channels != NULL && dst_channels != NULL, return -ENXIO);
256 if (frames == 0)
257 return 0;
258#ifdef CONFIG_SND_DEBUG
259 {
260 unsigned int channel;
261 for (channel = 0; channel < plugin->src_format.channels; channel++) {
262 snd_assert(src_channels[channel].area.first % 8 == 0 &&
263 src_channels[channel].area.step % 8 == 0,
264 return -ENXIO);
265 snd_assert(dst_channels[channel].area.first % 8 == 0 &&
266 dst_channels[channel].area.step % 8 == 0,
267 return -ENXIO);
268 }
269 }
270#endif
271
272 dst_frames = rate_dst_frames(plugin, frames);
273 if (dst_frames > dst_channels[0].frames)
274 dst_frames = dst_channels[0].frames;
275 data = (struct rate_priv *)plugin->extra_data;
276 data->func(plugin, src_channels, dst_channels, frames, dst_frames);
277 return dst_frames;
278}
279
280static int rate_action(struct snd_pcm_plugin *plugin,
281 enum snd_pcm_plugin_action action,
282 unsigned long udata)
283{
284 snd_assert(plugin != NULL, return -ENXIO);
285 switch (action) {
286 case INIT:
287 case PREPARE:
288 rate_init(plugin);
289 break;
290 default:
291 break;
292 }
293 return 0;
294}
295
296int snd_pcm_plugin_build_rate(struct snd_pcm_substream *plug,
297 struct snd_pcm_plugin_format *src_format,
298 struct snd_pcm_plugin_format *dst_format,
299 struct snd_pcm_plugin **r_plugin)
300{
301 int err;
302 struct rate_priv *data;
303 struct snd_pcm_plugin *plugin;
304
305 snd_assert(r_plugin != NULL, return -ENXIO);
306 *r_plugin = NULL;
307
308 snd_assert(src_format->channels == dst_format->channels, return -ENXIO);
309 snd_assert(src_format->channels > 0, return -ENXIO);
310 snd_assert(src_format->format == SNDRV_PCM_FORMAT_S16, return -ENXIO);
311 snd_assert(dst_format->format == SNDRV_PCM_FORMAT_S16, return -ENXIO);
312 snd_assert(src_format->rate != dst_format->rate, return -ENXIO);
313
314 err = snd_pcm_plugin_build(plug, "rate conversion",
315 src_format, dst_format,
316 sizeof(struct rate_priv) +
317 src_format->channels * sizeof(struct rate_channel),
318 &plugin);
319 if (err < 0)
320 return err;
321 data = (struct rate_priv *)plugin->extra_data;
322 if (src_format->rate < dst_format->rate) {
323 data->pitch = ((src_format->rate << SHIFT) + (dst_format->rate >> 1)) / dst_format->rate;
324 data->func = resample_expand;
325 } else {
326 data->pitch = ((dst_format->rate << SHIFT) + (src_format->rate >> 1)) / src_format->rate;
327 data->func = resample_shrink;
328 }
329 data->pos = 0;
330 rate_init(plugin);
331 data->old_src_frames = data->old_dst_frames = 0;
332 plugin->transfer = rate_transfer;
333 plugin->src_frames = rate_src_frames;
334 plugin->dst_frames = rate_dst_frames;
335 plugin->action = rate_action;
336 *r_plugin = plugin;
337 return 0;
338}
339
