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 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; /* silenty ignore other actions */ 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