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