Pileus Git - ~andy/linux/blob - 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         int get, put;
  54         snd_pcm_sframes_t old_src_frames, old_dst_frames;
  55         struct rate_channel channels[0];
  56 };
  57
  58 static void rate_init(struct snd_pcm_plugin *plugin)
  59 {
  60         unsigned int channel;
  61         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
  62         data->pos = 0;
  63         for (channel = 0; channel < plugin->src_format.channels; channel++) {
  64                 data->channels[channel].last_S1 = 0;
  65                 data->channels[channel].last_S2 = 0;
  66         }
  67 }
  68
  69 static void resample_expand(struct snd_pcm_plugin *plugin,
  70                             const struct snd_pcm_plugin_channel *src_channels,
  71                             struct snd_pcm_plugin_channel *dst_channels,
  72                             int src_frames, int dst_frames)
  73 {
  74         unsigned int pos = 0;
  75         signed int val;
  76         signed short S1, S2;
  77         char *src, *dst;
  78         unsigned int channel;
  79         int src_step, dst_step;
  80         int src_frames1, dst_frames1;
  81         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
  82         struct rate_channel *rchannels = data->channels;
  83
  84 #define GET_S16_LABELS
  85 #define PUT_S16_LABELS
  86 #include "plugin_ops.h"
  87 #undef GET_S16_LABELS
  88 #undef PUT_S16_LABELS
  89         void *get = get_s16_labels[data->get];
  90         void *put = put_s16_labels[data->put];
  91         signed short sample = 0;
  92
  93         for (channel = 0; channel < plugin->src_format.channels; channel++) {
  94                 pos = data->pos;
  95                 S1 = rchannels->last_S1;
  96                 S2 = rchannels->last_S2;
  97                 if (!src_channels[channel].enabled) {
  98                         if (dst_channels[channel].wanted)
  99                                 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
 100                         dst_channels[channel].enabled = 0;
 101                         continue;
 102                 }
 103                 dst_channels[channel].enabled = 1;
 104                 src = (char *)src_channels[channel].area.addr + src_channels[channel].area.first / 8;
 105                 dst = (char *)dst_channels[channel].area.addr + dst_channels[channel].area.first / 8;
 106                 src_step = src_channels[channel].area.step / 8;
 107                 dst_step = dst_channels[channel].area.step / 8;
 108                 src_frames1 = src_frames;
 109                 dst_frames1 = dst_frames;
 110                 while (dst_frames1-- > 0) {
 111                         if (pos & ~R_MASK) {
 112                                 pos &= R_MASK;
 113                                 S1 = S2;
 114                                 if (src_frames1-- > 0) {
 115                                         goto *get;
 116 #define GET_S16_END after_get
 117 #include "plugin_ops.h"
 118 #undef GET_S16_END
 119                                 after_get:
 120                                         S2 = sample;
 121                                         src += src_step;
 122                                 }
 123                         }
 124                         val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
 125                         if (val < -32768)
 126                                 val = -32768;
 127                         else if (val > 32767)
 128                                 val = 32767;
 129                         sample = val;
 130                         goto *put;
 131 #define PUT_S16_END after_put
 132 #include "plugin_ops.h"
 133 #undef PUT_S16_END
 134                 after_put:
 135                         dst += dst_step;
 136                         pos += data->pitch;
 137                 }
 138                 rchannels->last_S1 = S1;
 139                 rchannels->last_S2 = S2;
 140                 rchannels++;
 141         }
 142         data->pos = pos;
 143 }
 144
 145 static void resample_shrink(struct snd_pcm_plugin *plugin,
 146                             const struct snd_pcm_plugin_channel *src_channels,
 147                             struct snd_pcm_plugin_channel *dst_channels,
 148                             int src_frames, int dst_frames)
 149 {
 150         unsigned int pos = 0;
 151         signed int val;
 152         signed short S1, S2;
 153         char *src, *dst;
 154         unsigned int channel;
 155         int src_step, dst_step;
 156         int src_frames1, dst_frames1;
 157         struct rate_priv *data = (struct rate_priv *)plugin->extra_data;
 158         struct rate_channel *rchannels = data->channels;
 159
 160 #define GET_S16_LABELS
 161 #define PUT_S16_LABELS
 162 #include "plugin_ops.h"
 163 #undef GET_S16_LABELS
 164 #undef PUT_S16_LABELS
 165         void *get = get_s16_labels[data->get];
 166         void *put = put_s16_labels[data->put];
 167         signed short sample = 0;
 168
 169         for (channel = 0; channel < plugin->src_format.channels; ++channel) {
 170                 pos = data->pos;
 171                 S1 = rchannels->last_S1;
 172                 S2 = rchannels->last_S2;
 173                 if (!src_channels[channel].enabled) {
 174                         if (dst_channels[channel].wanted)
 175                                 snd_pcm_area_silence(&dst_channels[channel].area, 0, dst_frames, plugin->dst_format.format);
 176                         dst_channels[channel].enabled = 0;
 177                         continue;
 178                 }
 179                 dst_channels[channel].enabled = 1;
 180                 src = (char *)src_channels[channel].area.addr + src_channels[channel].area.first / 8;
 181                 dst = (char *)dst_channels[channel].area.addr + dst_channels[channel].area.first / 8;
 182                 src_step = src_channels[channel].area.step / 8;
 183                 dst_step = dst_channels[channel].area.step / 8;
 184                 src_frames1 = src_frames;
 185                 dst_frames1 = dst_frames;
 186                 while (dst_frames1 > 0) {
 187                         S1 = S2;
 188                         if (src_frames1-- > 0) {
 189                                 goto *get;
 190 #define GET_S16_END after_get
 191 #include "plugin_ops.h"
 192 #undef GET_S16_END
 193                         after_get:
 194                                 S2 = sample;
 195                                 src += src_step;
 196                         }
 197                         if (pos & ~R_MASK) {
 198                                 pos &= R_MASK;
 199                                 val = S1 + ((S2 - S1) * (signed int)pos) / BITS;
 200                                 if (val < -32768)
 201                                         val = -32768;
 202                                 else if (val > 32767)
 203                                         val = 32767;
 204                                 sample = val;
 205                                 goto *put;
 206 #define PUT_S16_END after_put
 207 #include "plugin_ops.h"
 208 #undef PUT_S16_END
 209                         after_put:
 210                                 dst += dst_step;
 211                                 dst_frames1--;
 212                         }
 213                         pos += data->pitch;
 214                 }
 215                 rchannels->last_S1 = S1;
 216                 rchannels->last_S2 = S2;
 217                 rchannels++;
 218         }
 219         data->pos = pos;
 220 }
 221
 222 static snd_pcm_sframes_t rate_src_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
 223 {
 224         struct rate_priv *data;
 225         snd_pcm_sframes_t res;
 226
 227         snd_assert(plugin != NULL, return -ENXIO);
 228         if (frames == 0)
 229                 return 0;
 230         data = (struct rate_priv *)plugin->extra_data;
 231         if (plugin->src_format.rate < plugin->dst_format.rate) {
 232                 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
 233         } else {
 234                 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);
 235         }
 236         if (data->old_src_frames > 0) {
 237                 snd_pcm_sframes_t frames1 = frames, res1 = data->old_dst_frames;
 238                 while (data->old_src_frames < frames1) {
 239                         frames1 >>= 1;
 240                         res1 <<= 1;
 241                 }
 242                 while (data->old_src_frames > frames1) {
 243                         frames1 <<= 1;
 244                         res1 >>= 1;
 245                 }
 246                 if (data->old_src_frames == frames1)
 247                         return res1;
 248         }
 249         data->old_src_frames = frames;
 250         data->old_dst_frames = res;
 251         return res;
 252 }
 253
 254 static snd_pcm_sframes_t rate_dst_frames(struct snd_pcm_plugin *plugin, snd_pcm_uframes_t frames)
 255 {
 256         struct rate_priv *data;
 257         snd_pcm_sframes_t res;
 258
 259         snd_assert(plugin != NULL, return -ENXIO);
 260         if (frames == 0)
 261                 return 0;
 262         data = (struct rate_priv *)plugin->extra_data;
 263         if (plugin->src_format.rate < plugin->dst_format.rate) {
 264                 res = (((frames << SHIFT) + (data->pitch / 2)) / data->pitch);
 265         } else {
 266                 res = (((frames * data->pitch) + (BITS/2)) >> SHIFT);
 267         }
 268         if (data->old_dst_frames > 0) {
 269                 snd_pcm_sframes_t frames1 = frames, res1 = data->old_src_frames;
 270                 while (data->old_dst_frames < frames1) {
 271                         frames1 >>= 1;
 272                         res1 <<= 1;
 273                 }
 274                 while (data->old_dst_frames > frames1) {
 275                         frames1 <<= 1;
 276                         res1 >>= 1;
 277                 }
 278                 if (data->old_dst_frames == frames1)
 279                         return res1;
 280         }
 281         data->old_dst_frames = frames;
 282         data->old_src_frames = res;
 283         return res;
 284 }
 285
 286 static snd_pcm_sframes_t rate_transfer(struct snd_pcm_plugin *plugin,
 287                              const struct snd_pcm_plugin_channel *src_channels,
 288                              struct snd_pcm_plugin_channel *dst_channels,
 289                              snd_pcm_uframes_t frames)
 290 {
 291         snd_pcm_uframes_t dst_frames;
 292         struct rate_priv *data;
 293
 294         snd_assert(plugin != NULL && src_channels != NULL && dst_channels != NULL, return -ENXIO);
 295         if (frames == 0)
 296                 return 0;
 297 #ifdef CONFIG_SND_DEBUG
 298         {
 299                 unsigned int channel;
 300                 for (channel = 0; channel < plugin->src_format.channels; channel++) {
 301                         snd_assert(src_channels[channel].area.first % 8 == 0 &&
 302                                    src_channels[channel].area.step % 8 == 0,
 303                                    return -ENXIO);
 304                         snd_assert(dst_channels[channel].area.first % 8 == 0 &&
 305                                    dst_channels[channel].area.step % 8 == 0,
 306                                    return -ENXIO);
 307                 }
 308         }
 309 #endif
 310
 311         dst_frames = rate_dst_frames(plugin, frames);
 312         if (dst_frames > dst_channels[0].frames)
 313                 dst_frames = dst_channels[0].frames;
 314         data = (struct rate_priv *)plugin->extra_data;
 315         data->func(plugin, src_channels, dst_channels, frames, dst_frames);
 316         return dst_frames;
 317 }
 318
 319 static int rate_action(struct snd_pcm_plugin *plugin,
 320                        enum snd_pcm_plugin_action action,
 321                        unsigned long udata)
 322 {
 323         snd_assert(plugin != NULL, return -ENXIO);
 324         switch (action) {
 325         case INIT:
 326         case PREPARE:
 327                 rate_init(plugin);
 328                 break;
 329         default:
 330                 break;
 331         }
 332         return 0;       /* silenty ignore other actions */
 333 }
 334
 335 int snd_pcm_plugin_build_rate(struct snd_pcm_substream *plug,
 336                               struct snd_pcm_plugin_format *src_format,
 337                               struct snd_pcm_plugin_format *dst_format,
 338                               struct snd_pcm_plugin **r_plugin)
 339 {
 340         int err;
 341         struct rate_priv *data;
 342         struct snd_pcm_plugin *plugin;
 343
 344         snd_assert(r_plugin != NULL, return -ENXIO);
 345         *r_plugin = NULL;
 346
 347         snd_assert(src_format->channels == dst_format->channels, return -ENXIO);
 348         snd_assert(src_format->channels > 0, return -ENXIO);
 349         snd_assert(snd_pcm_format_linear(src_format->format) != 0, return -ENXIO);
 350         snd_assert(snd_pcm_format_linear(dst_format->format) != 0, return -ENXIO);
 351         snd_assert(src_format->rate != dst_format->rate, return -ENXIO);
 352
 353         err = snd_pcm_plugin_build(plug, "rate conversion",
 354                                    src_format, dst_format,
 355                                    sizeof(struct rate_priv) +
 356                                    src_format->channels * sizeof(struct rate_channel),
 357                                    &plugin);
 358         if (err < 0)
 359                 return err;
 360         data = (struct rate_priv *)plugin->extra_data;
 361         data->get = getput_index(src_format->format);
 362         snd_assert(data->get >= 0 && data->get < 4*2*2, return -EINVAL);
 363         data->put = getput_index(dst_format->format);
 364         snd_assert(data->put >= 0 && data->put < 4*2*2, return -EINVAL);
 365
 366         if (src_format->rate < dst_format->rate) {
 367                 data->pitch = ((src_format->rate << SHIFT) + (dst_format->rate >> 1)) / dst_format->rate;
 368                 data->func = resample_expand;
 369         } else {
 370                 data->pitch = ((dst_format->rate << SHIFT) + (src_format->rate >> 1)) / src_format->rate;
 371                 data->func = resample_shrink;
 372         }
 373         data->pos = 0;
 374         rate_init(plugin);
 375         data->old_src_frames = data->old_dst_frames = 0;
 376         plugin->transfer = rate_transfer;
 377         plugin->src_frames = rate_src_frames;
 378         plugin->dst_frames = rate_dst_frames;
 379         plugin->action = rate_action;
 380         *r_plugin = plugin;
 381         return 0;
 382 }
 383
 384 #endif