release/src-rt-6.x/linux/linux-2.6/sound/core/oss/rate.c

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