1 /* 2 * Rate conversion Plug-In 3 * Copyright (c) 1999 by Jaroslav Kysela <perex@perex.cz> 4 * 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 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 * Basic rate conversion plugin 33 */ 34 35 struct rate_channel { 36 signed short last_S1; 37 signed short last_S2; 38 }; 39 40 typedef 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 45 struct 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 53 static 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 64 static 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 123 static 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 S1 = *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 183 static 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 215 static 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 247 static 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 280 static 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; /* silenty ignore other actions */ 294 } 295 296 int 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