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 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 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 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 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 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 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 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 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