349 lines · c
1/*2 * Rate conversion Plug-In3 * Copyright (c) 1999 by Jaroslav Kysela <perex@perex.cz>4 *5 *6 * This library is free software; you can redistribute it and/or modify7 * it under the terms of the GNU Library General Public License as8 * published by the Free Software Foundation; either version 2 of9 * 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 of13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the14 * GNU Library General Public License for more details.15 *16 * You should have received a copy of the GNU Library General Public17 * License along with this library; if not, write to the Free Software18 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA19 *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 1128#define BITS (1<<SHIFT)29#define R_MASK (BITS-1)30 31/*32 * Basic rate conversion plugin33 */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_DEBUG262 {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#endif274 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; /* silently 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