sound/core/oss/rate.c

   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                                 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
 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         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 = (((frames << SHIFT) + (data->pitch / 2)) / 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
 216 static 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 = (((frames << SHIFT) + (data->pitch / 2)) / 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
 249 static 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
 283 static 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
 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         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                                    sizeof(struct rate_priv) +
 327                                    src_format->channels * sizeof(struct rate_channel),
 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 }