src/pcm/pcm_rate_linear.c

   1 /*
   2  *  Linear rate converter plugin
   3  *
   4  *  Copyright (c) 2000 by Abramo Bagnara <abramo@alsa-project.org>
   5  *                2004 by Jaroslav Kysela <perex@perex.cz>
   6  *                2006 by Takashi Iwai <tiwai@suse.de>
   7  *
   8  *   This library is free software; you can redistribute it and/or modify
   9  *   it under the terms of the GNU Lesser General Public License as
  10  *   published by the Free Software Foundation; either version 2.1 of
  11  *   the License, or (at your option) any later version.
  12  *
  13  *   This program is distributed in the hope that it will be useful,
  14  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  *   GNU Lesser General Public License for more details.
  17  *
  18  *   You should have received a copy of the GNU Lesser General Public
  19  *   License along with this library; if not, write to the Free Software
  20  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
  21  */
  22
  23 #include <inttypes.h>
  24 #include <byteswap.h>
  25 #include "pcm_local.h"
  26 #include "pcm_plugin.h"
  27 #include "pcm_rate.h"
  28
  29 #include "plugin_ops.h"
  30
  31
  32 /* LINEAR_DIV needs to be large enough to handle resampling from 192000 -> 8000 */
  33 #define LINEAR_DIV_SHIFT 19
  34 #define LINEAR_DIV (1<<LINEAR_DIV_SHIFT)
  35
  36 struct rate_linear {
  37         unsigned int get_idx;
  38         unsigned int put_idx;
  39         unsigned int pitch;
  40         unsigned int pitch_shift;       /* for expand interpolation */
  41         unsigned int channels;
  42         int16_t *old_sample;
  43         void (*func)(struct rate_linear *rate,
  44                      const snd_pcm_channel_area_t *dst_areas,
  45                      snd_pcm_uframes_t dst_offset, unsigned int dst_frames,
  46                      const snd_pcm_channel_area_t *src_areas,
  47                      snd_pcm_uframes_t src_offset, unsigned int src_frames);
  48 };
  49
  50 static snd_pcm_uframes_t input_frames(void *obj, snd_pcm_uframes_t frames)
  51 {
  52         struct rate_linear *rate = obj;
  53         if (frames == 0)
  54                 return 0;
  55         /* Round toward zero */
  56         return muldiv_near(frames, LINEAR_DIV, rate->pitch);
  57 }
  58
  59 static snd_pcm_uframes_t output_frames(void *obj, snd_pcm_uframes_t frames)
  60 {
  61         struct rate_linear *rate = obj;
  62         if (frames == 0)
  63                 return 0;
  64         /* Round toward zero */
  65         return muldiv_near(frames, rate->pitch, LINEAR_DIV);
  66 }
  67
  68 static void linear_expand(struct rate_linear *rate,
  69                           const snd_pcm_channel_area_t *dst_areas,
  70                           snd_pcm_uframes_t dst_offset, unsigned int dst_frames,
  71                           const snd_pcm_channel_area_t *src_areas,
  72                           snd_pcm_uframes_t src_offset, unsigned int src_frames)
  73 {
  74 #define GET16_LABELS
  75 #define PUT16_LABELS
  76 #include "plugin_ops.h"
  77 #undef GET16_LABELS
  78 #undef PUT16_LABELS
  79         void *get = get16_labels[rate->get_idx];
  80         void *put = put16_labels[rate->put_idx];
  81         unsigned int get_threshold = rate->pitch;
  82         unsigned int channel;
  83         unsigned int src_frames1;
  84         unsigned int dst_frames1;
  85         int16_t sample = 0;
  86         unsigned int pos;
  87
  88         for (channel = 0; channel < rate->channels; ++channel) {
  89                 const snd_pcm_channel_area_t *src_area = &src_areas[channel];
  90                 const snd_pcm_channel_area_t *dst_area = &dst_areas[channel];
  91                 const char *src;
  92                 char *dst;
  93                 int src_step, dst_step;
  94                 int16_t old_sample = 0;
  95                 int16_t new_sample;
  96                 int old_weight, new_weight;
  97                 src = snd_pcm_channel_area_addr(src_area, src_offset);
  98                 dst = snd_pcm_channel_area_addr(dst_area, dst_offset);
  99                 src_step = snd_pcm_channel_area_step(src_area);
 100                 dst_step = snd_pcm_channel_area_step(dst_area);
 101                 src_frames1 = 0;
 102                 dst_frames1 = 0;
 103                 new_sample = rate->old_sample[channel];
 104                 pos = get_threshold;
 105                 while (dst_frames1 < dst_frames) {
 106                         if (pos >= get_threshold) {
 107                                 pos -= get_threshold;
 108                                 old_sample = new_sample;
 109                                 if (src_frames1 < src_frames) {
 110                                         goto *get;
 111 #define GET16_END after_get
 112 #include "plugin_ops.h"
 113 #undef GET16_END
 114                                 after_get:
 115                                         new_sample = sample;
 116                                 }
 117                         }
 118                         new_weight = (pos << (16 - rate->pitch_shift)) / (get_threshold >> rate->pitch_shift);
 119                         old_weight = 0x10000 - new_weight;
 120                         sample = (old_sample * old_weight + new_sample * new_weight) >> 16;
 121                         goto *put;
 122 #define PUT16_END after_put
 123 #include "plugin_ops.h"
 124 #undef PUT16_END
 125                 after_put:
 126                         dst += dst_step;
 127                         dst_frames1++;
 128                         pos += LINEAR_DIV;
 129                         if (pos >= get_threshold) {
 130                                 src += src_step;
 131                                 src_frames1++;
 132                         }
 133                 }
 134                 rate->old_sample[channel] = new_sample;
 135         }
 136 }
 137
 138 /* optimized version for S16 format */
 139 static void linear_expand_s16(struct rate_linear *rate,
 140                               const snd_pcm_channel_area_t *dst_areas,
 141                               snd_pcm_uframes_t dst_offset, unsigned int dst_frames,
 142                               const snd_pcm_channel_area_t *src_areas,
 143                               snd_pcm_uframes_t src_offset, unsigned int src_frames)
 144 {
 145         unsigned int channel;
 146         unsigned int src_frames1;
 147         unsigned int dst_frames1;
 148         unsigned int get_threshold = rate->pitch;
 149         unsigned int pos;
 150
 151         for (channel = 0; channel < rate->channels; ++channel) {
 152                 const snd_pcm_channel_area_t *src_area = &src_areas[channel];
 153                 const snd_pcm_channel_area_t *dst_area = &dst_areas[channel];
 154                 const int16_t *src;
 155                 int16_t *dst;
 156                 int src_step, dst_step;
 157                 int16_t old_sample = 0;
 158                 int16_t new_sample;
 159                 int old_weight, new_weight;
 160                 src = snd_pcm_channel_area_addr(src_area, src_offset);
 161                 dst = snd_pcm_channel_area_addr(dst_area, dst_offset);
 162                 src_step = snd_pcm_channel_area_step(src_area) >> 1;
 163                 dst_step = snd_pcm_channel_area_step(dst_area) >> 1;
 164                 src_frames1 = 0;
 165                 dst_frames1 = 0;
 166                 new_sample = rate->old_sample[channel];
 167                 pos = get_threshold;
 168                 while (dst_frames1 < dst_frames) {
 169                         if (pos >= get_threshold) {
 170                                 pos -= get_threshold;
 171                                 old_sample = new_sample;
 172                                 if (src_frames1 < src_frames)
 173                                         new_sample = *src;
 174                         }
 175                         new_weight = (pos << (16 - rate->pitch_shift)) / (get_threshold >> rate->pitch_shift);
 176                         old_weight = 0x10000 - new_weight;
 177                         *dst = (old_sample * old_weight + new_sample * new_weight) >> 16;
 178                         dst += dst_step;
 179                         dst_frames1++;
 180                         pos += LINEAR_DIV;
 181                         if (pos >= get_threshold) {
 182                                 src += src_step;
 183                                 src_frames1++;
 184                         }
 185                 }
 186                 rate->old_sample[channel] = new_sample;
 187         }
 188 }
 189
 190 static void linear_shrink(struct rate_linear *rate,
 191                           const snd_pcm_channel_area_t *dst_areas,
 192                           snd_pcm_uframes_t dst_offset, unsigned int dst_frames,
 193                           const snd_pcm_channel_area_t *src_areas,
 194                           snd_pcm_uframes_t src_offset, unsigned int src_frames)
 195 {
 196 #define GET16_LABELS
 197 #define PUT16_LABELS
 198 #include "plugin_ops.h"
 199 #undef GET16_LABELS
 200 #undef PUT16_LABELS
 201         void *get = get16_labels[rate->get_idx];
 202         void *put = put16_labels[rate->put_idx];
 203         unsigned int get_increment = rate->pitch;
 204         unsigned int channel;
 205         unsigned int src_frames1;
 206         unsigned int dst_frames1;
 207         int16_t sample = 0;
 208         unsigned int pos;
 209
 210         for (channel = 0; channel < rate->channels; ++channel) {
 211                 const snd_pcm_channel_area_t *src_area = &src_areas[channel];
 212                 const snd_pcm_channel_area_t *dst_area = &dst_areas[channel];
 213                 const char *src;
 214                 char *dst;
 215                 int src_step, dst_step;
 216                 int16_t old_sample = 0;
 217                 int16_t new_sample = 0;
 218                 int old_weight, new_weight;
 219                 pos = LINEAR_DIV - get_increment; /* Force first sample to be copied */
 220                 src = snd_pcm_channel_area_addr(src_area, src_offset);
 221                 dst = snd_pcm_channel_area_addr(dst_area, dst_offset);
 222                 src_step = snd_pcm_channel_area_step(src_area);
 223                 dst_step = snd_pcm_channel_area_step(dst_area);
 224                 src_frames1 = 0;
 225                 dst_frames1 = 0;
 226                 while (src_frames1 < src_frames) {
 227
 228                         goto *get;
 229 #define GET16_END after_get
 230 #include "plugin_ops.h"
 231 #undef GET16_END
 232                 after_get:
 233                         new_sample = sample;
 234                         src += src_step;
 235                         src_frames1++;
 236                         pos += get_increment;
 237                         if (pos >= LINEAR_DIV) {
 238                                 pos -= LINEAR_DIV;
 239                                 old_weight = (pos << (32 - LINEAR_DIV_SHIFT)) / (get_increment >> (LINEAR_DIV_SHIFT - 16));
 240                                 new_weight = 0x10000 - old_weight;
 241                                 sample = (old_sample * old_weight + new_sample * new_weight) >> 16;
 242                                 goto *put;
 243 #define PUT16_END after_put
 244 #include "plugin_ops.h"
 245 #undef PUT16_END
 246                         after_put:
 247                                 dst += dst_step;
 248                                 dst_frames1++;
 249                                 if (CHECK_SANITY(dst_frames1 > dst_frames)) {
 250                                         SNDERR("dst_frames overflow");
 251                                         break;
 252                                 }
 253                         }
 254                         old_sample = new_sample;
 255                 }
 256         }
 257 }
 258
 259 /* optimized version for S16 format */
 260 static void linear_shrink_s16(struct rate_linear *rate,
 261                               const snd_pcm_channel_area_t *dst_areas,
 262                               snd_pcm_uframes_t dst_offset, unsigned int dst_frames,
 263                               const snd_pcm_channel_area_t *src_areas,
 264                               snd_pcm_uframes_t src_offset, unsigned int src_frames)
 265 {
 266         unsigned int get_increment = rate->pitch;
 267         unsigned int channel;
 268         unsigned int src_frames1;
 269         unsigned int dst_frames1;
 270         unsigned int pos = 0;
 271
 272         for (channel = 0; channel < rate->channels; ++channel) {
 273                 const snd_pcm_channel_area_t *src_area = &src_areas[channel];
 274                 const snd_pcm_channel_area_t *dst_area = &dst_areas[channel];
 275                 const int16_t *src;
 276                 int16_t *dst;
 277                 int src_step, dst_step;
 278                 int16_t old_sample = 0;
 279                 int16_t new_sample = 0;
 280                 int old_weight, new_weight;
 281                 pos = LINEAR_DIV - get_increment; /* Force first sample to be copied */
 282                 src = snd_pcm_channel_area_addr(src_area, src_offset);
 283                 dst = snd_pcm_channel_area_addr(dst_area, dst_offset);
 284                 src_step = snd_pcm_channel_area_step(src_area) >> 1;
 285                 dst_step = snd_pcm_channel_area_step(dst_area) >> 1 ;
 286                 src_frames1 = 0;
 287                 dst_frames1 = 0;
 288                 while (src_frames1 < src_frames) {
 289
 290                         new_sample = *src;
 291                         src += src_step;
 292                         src_frames1++;
 293                         pos += get_increment;
 294                         if (pos >= LINEAR_DIV) {
 295                                 pos -= LINEAR_DIV;
 296                                 old_weight = (pos << (32 - LINEAR_DIV_SHIFT)) / (get_increment >> (LINEAR_DIV_SHIFT - 16));
 297                                 new_weight = 0x10000 - old_weight;
 298                                 *dst = (old_sample * old_weight + new_sample * new_weight) >> 16;
 299                                 dst += dst_step;
 300                                 dst_frames1++;
 301                                 if (CHECK_SANITY(dst_frames1 > dst_frames)) {
 302                                         SNDERR("dst_frames overflow");
 303                                         break;
 304                                 }
 305                         }
 306                         old_sample = new_sample;
 307                 }
 308         }
 309 }
 310
 311 static void linear_convert(void *obj,
 312                            const snd_pcm_channel_area_t *dst_areas,
 313                            snd_pcm_uframes_t dst_offset, unsigned int dst_frames,
 314                            const snd_pcm_channel_area_t *src_areas,
 315                            snd_pcm_uframes_t src_offset, unsigned int src_frames)
 316 {
 317         struct rate_linear *rate = obj;
 318         rate->func(rate, dst_areas, dst_offset, dst_frames,
 319                    src_areas, src_offset, src_frames);
 320 }
 321
 322 static void linear_free(void *obj)
 323 {
 324         struct rate_linear *rate = obj;
 325
 326         free(rate->old_sample);
 327         rate->old_sample = NULL;
 328 }
 329
 330 static int linear_init(void *obj, snd_pcm_rate_info_t *info)
 331 {
 332         struct rate_linear *rate = obj;
 333
 334         rate->get_idx = snd_pcm_linear_get_index(info->in.format, SND_PCM_FORMAT_S16);
 335         rate->put_idx = snd_pcm_linear_put_index(SND_PCM_FORMAT_S16, info->out.format);
 336         if (info->in.rate < info->out.rate) {
 337                 if (info->in.format == info->out.format && info->in.format == SND_PCM_FORMAT_S16)
 338                         rate->func = linear_expand_s16;
 339                 else
 340                         rate->func = linear_expand;
 341                 /* pitch is get_threshold */
 342         } else {
 343                 if (info->in.format == info->out.format && info->in.format == SND_PCM_FORMAT_S16)
 344                         rate->func = linear_shrink_s16;
 345                 else
 346                         rate->func = linear_shrink;
 347                 /* pitch is get_increment */
 348         }
 349         rate->pitch = (((u_int64_t)info->out.rate * LINEAR_DIV) +
 350                        (info->in.rate / 2)) / info->in.rate;
 351         rate->channels = info->channels;
 352
 353         free(rate->old_sample);
 354         rate->old_sample = malloc(sizeof(*rate->old_sample) * rate->channels);
 355         if (! rate->old_sample)
 356                 return -ENOMEM;
 357
 358         return 0;
 359 }
 360
 361 static int linear_adjust_pitch(void *obj, snd_pcm_rate_info_t *info)
 362 {
 363         struct rate_linear *rate = obj;
 364         snd_pcm_uframes_t cframes;
 365
 366         rate->pitch = (((u_int64_t)info->out.period_size * LINEAR_DIV) +
 367                        (info->in.period_size/2) ) / info->in.period_size;
 368
 369         cframes = input_frames(rate, info->out.period_size);
 370         while (cframes != info->in.period_size) {
 371                 snd_pcm_uframes_t cframes_new;
 372                 if (cframes > info->in.period_size)
 373                         rate->pitch++;
 374                 else
 375                         rate->pitch--;
 376                 cframes_new = input_frames(rate, info->out.period_size);
 377                 if ((cframes > info->in.period_size && cframes_new < info->in.period_size) ||
 378                     (cframes < info->in.period_size && cframes_new > info->in.period_size)) {
 379                         SNDERR("invalid pcm period_size %ld -> %ld",
 380                                info->in.period_size, info->out.period_size);
 381                         return -EIO;
 382                 }
 383                 cframes = cframes_new;
 384         }
 385         if (rate->pitch >= LINEAR_DIV) {
 386                 /* shift for expand linear interpolation */
 387                 rate->pitch_shift = 0;
 388                 while ((rate->pitch >> rate->pitch_shift) >= (1 << 16))
 389                         rate->pitch_shift++;
 390         }
 391         return 0;
 392 }
 393
 394 static void linear_reset(void *obj)
 395 {
 396         struct rate_linear *rate = obj;
 397
 398         /* for expand */
 399         if (rate->old_sample)
 400                 memset(rate->old_sample, 0, sizeof(*rate->old_sample) * rate->channels);
 401 }
 402
 403 static void linear_close(void *obj)
 404 {
 405         free(obj);
 406 }
 407
 408 static int get_supported_rates(ATTRIBUTE_UNUSED void *rate,
 409                                unsigned int *rate_min, unsigned int *rate_max)
 410 {
 411         *rate_min = SND_PCM_PLUGIN_RATE_MIN;
 412         *rate_max = SND_PCM_PLUGIN_RATE_MAX;
 413         return 0;
 414 }
 415
 416 static void linear_dump(ATTRIBUTE_UNUSED void *rate, snd_output_t *out)
 417 {
 418         snd_output_printf(out, "Converter: linear-interpolation\n");
 419 }
 420
 421 static const snd_pcm_rate_ops_t linear_ops = {
 422         .close = linear_close,
 423         .init = linear_init,
 424         .free = linear_free,
 425         .reset = linear_reset,
 426         .adjust_pitch = linear_adjust_pitch,
 427         .convert = linear_convert,
 428         .input_frames = input_frames,
 429         .output_frames = output_frames,
 430         .version = SND_PCM_RATE_PLUGIN_VERSION,
 431         .get_supported_rates = get_supported_rates,
 432         .dump = linear_dump,
 433 };
 434
 435 int SND_PCM_RATE_PLUGIN_ENTRY(linear) (ATTRIBUTE_UNUSED unsigned int version,
 436                                        void **objp, snd_pcm_rate_ops_t *ops)
 437 {
 438         struct rate_linear *rate;
 439
 440         rate = calloc(1, sizeof(*rate));
 441         if (! rate)
 442                 return -ENOMEM;
 443
 444         *objp = rate;
 445         *ops = linear_ops;
 446         return 0;
 447 }