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