src/lib/doslib/ext/vorbtool/resample.c

   1 /* resample.c: see resample.h for interesting stuff */
   2
   3 #ifdef HAVE_CONFIG_H
   4 #include "config.h"
   5 #endif
   6
   7 #include <math.h>
   8 #include <stdlib.h>
   9 #include <string.h>
  10 #include <stdarg.h>
  11 #include <assert.h>
  12
  13 #include "resample.h"
  14
  15 /* Some systems don't define this */
  16 #ifndef M_PI
  17 #define M_PI       3.14159265358979323846
  18 #endif
  19
  20 static int hcf(int arg1, int arg2)
  21 {
  22     int mult = 1;
  23
  24     while (~(arg1 | arg2) & 1)
  25         arg1 >>= 1, arg2 >>= 1, mult <<= 1;
  26
  27     while (arg1 > 0)
  28     {
  29         if (~(arg1 & arg2) & 1)
  30         {
  31             arg1 >>= (~arg1 & 1);
  32             arg2 >>= (~arg2 & 1);
  33         }
  34         else if (arg1 < arg2)
  35             arg2 = (arg2 - arg1) >> 1;
  36         else
  37             arg1 = (arg1 - arg2) >> 1;
  38     }
  39
  40     return arg2 * mult;
  41 }
  42
  43
  44 static void filt_sinc(float *dest, int N, int step, double fc, double gain, int width)
  45 {
  46     double s = fc / step;
  47     int mid, x;
  48     float *endpoint = dest + N,
  49         *base = dest,
  50         *origdest = dest;
  51
  52     assert(width <= N);
  53
  54     if ((N & 1) == 0)
  55     {
  56         *dest = 0.0;
  57         dest += width;
  58         if (dest >= endpoint)
  59             dest = ++base;
  60         N--;
  61     }
  62
  63     mid = N / 2;
  64     x = -mid;
  65
  66     while (N--)
  67     {
  68         *dest = (x ? sin(x * M_PI * s) / (x * M_PI) * step : fc) * gain;
  69         x++;
  70         dest += width;
  71         if (dest >= endpoint)
  72             dest = ++base;
  73     }
  74     assert(dest == origdest + width);
  75 }
  76
  77
  78 static double I_zero(double x)
  79 {
  80     int n = 0;
  81     double u = 1.0,
  82         s = 1.0,
  83         t;
  84
  85     do
  86     {
  87         n += 2;
  88         t = x / n;
  89         u *= t * t;
  90         s += u;
  91     } while (u > 1e-21 * s);
  92
  93     return s;
  94 }
  95
  96
  97 static void win_kaiser(float *dest, int N, double alpha, int width)
  98 {
  99     double I_alpha, midsq;
 100     int x;
 101     float *endpoint = dest + N,
 102         *base = dest,
 103         *origdest = dest;
 104
 105     assert(width <= N);
 106
 107     if ((N & 1) == 0)
 108     {
 109         *dest = 0.0;
 110         dest += width;
 111         if (dest >= endpoint)
 112             dest = ++base;
 113         N--;
 114     }
 115
 116     x = -(N / 2);
 117     midsq = (double)(x - 1) * (double)(x - 1);
 118     I_alpha = I_zero(alpha);
 119
 120     while (N--)
 121     {
 122         *dest *= I_zero(alpha * sqrt(1.0 - ((double)x * (double)x) / midsq)) / I_alpha;
 123         x++;
 124         dest += width;
 125         if (dest >= endpoint)
 126             dest = ++base;
 127     }
 128     assert(dest == origdest + width);
 129 }
 130
 131
 132 int res_init(res_state *state, int channels, int outfreq, int infreq, res_parameter op1, ...)
 133 {
 134     double beta = 16.0,
 135         cutoff = 0.80,
 136         gain = 1.0;
 137     int taps = 45;
 138
 139     int factor;
 140
 141     assert(state);
 142     assert(channels > 0);
 143     assert(outfreq > 0);
 144     assert(infreq > 0);
 145     assert(taps > 0);
 146
 147     if (state == NULL || channels <= 0 || outfreq <= 0 || infreq <= 0 || taps <= 0)
 148         return -1;
 149
 150     if (op1 != RES_END)
 151     {
 152         va_list argp;
 153         va_start(argp, op1);
 154         do
 155         {
 156             switch (op1)
 157             {
 158             case RES_GAIN:
 159                 gain = va_arg(argp, double);
 160                 break;
 161
 162             case RES_CUTOFF:
 163                 cutoff = va_arg(argp, double);
 164                 assert(cutoff > 0.01 && cutoff <= 1.0);
 165                 break;
 166
 167             case RES_TAPS:
 168                 taps = va_arg(argp, int);
 169                 assert(taps > 2 && taps < 1000);
 170                 break;
 171
 172             case RES_BETA:
 173                 beta = va_arg(argp, double);
 174                 assert(beta > 2.0);
 175                 break;
 176             default:
 177                 assert("arglist" == "valid");
 178                 return -1;
 179             }
 180             op1 = va_arg(argp, res_parameter);
 181         } while (op1 != RES_END);
 182         va_end(argp);
 183     }
 184
 185     factor = hcf(infreq, outfreq);
 186     outfreq /= factor;
 187     infreq /= factor;
 188
 189     /* adjust to rational values for downsampling */
 190     if (outfreq < infreq)
 191     {
 192         /* push the cutoff frequency down to the output frequency */
 193         cutoff = cutoff * outfreq / infreq;
 194
 195         /* compensate for the sharper roll-off requirement
 196          * by using a bigger hammer */
 197         taps = taps * infreq/outfreq;
 198     }
 199
 200     assert(taps >= (infreq + outfreq - 1) / outfreq);
 201
 202     if ((state->table = calloc(outfreq * taps, sizeof(float))) == NULL)
 203         return -1;
 204     if ((state->pool = calloc(channels * taps, sizeof(SAMPLE))) == NULL)
 205     {
 206         free(state->table);
 207         state->table = NULL;
 208         return -1;
 209     }
 210
 211     state->poolfill = taps / 2 + 1;
 212     state->channels = channels;
 213     state->outfreq = outfreq;
 214     state->infreq = infreq;
 215     state->taps = taps;
 216     state->offset = 0;
 217
 218     filt_sinc(state->table, outfreq * taps, outfreq, cutoff, gain, taps);
 219     win_kaiser(state->table, outfreq * taps, beta, taps);
 220
 221     return 0;
 222 }
 223
 224
 225 static SAMPLE sum(float const *scale, int count, SAMPLE const *source, SAMPLE const *trigger, SAMPLE const *reset, int srcstep)
 226 {
 227     float total = 0.0;
 228
 229     while (count--)
 230     {
 231         total += *source * *scale;
 232
 233         if (source == trigger)
 234             source = reset, srcstep = 1;
 235         source -= srcstep;
 236         scale++;
 237     }
 238
 239     return total;
 240 }
 241
 242
 243 static int push(res_state const * const state, SAMPLE *pool, int * const poolfill, int * const offset, SAMPLE *dest, int dststep, SAMPLE const *source, int srcstep, size_t srclen)
 244 {
 245     SAMPLE    * const destbase = dest,
 246         *poolhead = pool + *poolfill,
 247         *poolend = pool + state->taps,
 248         *newpool = pool;
 249     SAMPLE const *refill, *base, *endpoint;
 250     int    lencheck;
 251
 252
 253     assert(state);
 254     assert(pool);
 255     assert(poolfill);
 256     assert(dest);
 257     assert(source);
 258
 259     assert(state->poolfill != -1);
 260
 261     lencheck = res_push_check(state, srclen);
 262
 263     /* fill the pool before diving in */
 264     while (poolhead < poolend && srclen > 0)
 265     {
 266         *poolhead++ = *source;
 267         source += srcstep;
 268         srclen--;
 269     }
 270
 271     if ((long)srclen <= 0)
 272         return 0;
 273
 274     base = source;
 275     endpoint = source + srclen * srcstep;
 276
 277     while (source < endpoint)
 278     {
 279         *dest = sum(state->table + *offset * state->taps, state->taps, source, base, poolend, srcstep);
 280         dest += dststep;
 281         *offset += state->infreq;
 282         while (*offset >= state->outfreq)
 283         {
 284             *offset -= state->outfreq;
 285             source += srcstep;
 286         }
 287     }
 288
 289     assert(dest == destbase + lencheck * dststep);
 290
 291     /* pretend that source has that underrun data we're not going to get */
 292     srclen += (source - endpoint) / srcstep;
 293
 294     /* if we didn't get enough to completely replace the pool, then shift things about a bit */
 295     if (srclen < state->taps)
 296     {
 297         refill = pool + srclen;
 298         while (refill < poolend)
 299             *newpool++ = *refill++;
 300
 301         refill = source - srclen * srcstep;
 302     }
 303     else
 304         refill = source - state->taps * srcstep;
 305
 306     /* pull in fresh pool data */
 307     while (refill < endpoint)
 308     {
 309         *newpool++ = *refill;
 310         refill += srcstep;
 311     }
 312
 313     assert(newpool > pool);
 314     assert(newpool <= poolend);
 315
 316     *poolfill = newpool - pool;
 317
 318     return (dest - destbase) / dststep;
 319 }
 320
 321
 322 int res_push_max_input(res_state const * const state, size_t maxoutput)
 323 {
 324     return maxoutput * state->infreq / state->outfreq;
 325 }
 326
 327
 328 int res_push_check(res_state const * const state, size_t srclen)
 329 {
 330     if (state->poolfill < state->taps)
 331         srclen -= state->taps - state->poolfill;
 332
 333     return (srclen * state->outfreq - state->offset + state->infreq - 1) / state->infreq;
 334 }
 335
 336
 337 int res_push(res_state *state, SAMPLE **dstlist, SAMPLE const **srclist, size_t srclen)
 338 {
 339     int result = -1, poolfill = -1, offset = -1, i;
 340
 341     assert(state);
 342     assert(dstlist);
 343     assert(srclist);
 344     assert(state->poolfill >= 0);
 345
 346     for (i = 0; i < state->channels; i++)
 347     {
 348         poolfill = state->poolfill;
 349         offset = state->offset;
 350         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dstlist[i], 1, srclist[i], 1, srclen);
 351     }
 352     state->poolfill = poolfill;
 353     state->offset = offset;
 354
 355     return result;
 356 }
 357
 358
 359 int res_push_interleaved(res_state *state, SAMPLE *dest, SAMPLE const *source, size_t srclen)
 360 {
 361     int result = -1, poolfill = -1, offset = -1, i;
 362
 363     assert(state);
 364     assert(dest);
 365     assert(source);
 366     assert(state->poolfill >= 0);
 367
 368     for (i = 0; i < state->channels; i++)
 369     {
 370         poolfill = state->poolfill;
 371         offset = state->offset;
 372         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dest + i, state->channels, source + i, state->channels, srclen);
 373     }
 374     state->poolfill = poolfill;
 375     state->offset = offset;
 376
 377     return result;
 378 }
 379
 380
 381 int res_drain(res_state *state, SAMPLE **dstlist)
 382 {
 383     SAMPLE *tail;
 384     int result = -1, poolfill = -1, offset = -1, i;
 385
 386     assert(state);
 387     assert(dstlist);
 388     assert(state->poolfill >= 0);
 389
 390     if ((tail = calloc(state->taps, sizeof(SAMPLE))) == NULL)
 391         return -1;
 392
 393     for (i = 0; i < state->channels; i++)
 394     {
 395         poolfill = state->poolfill;
 396         offset = state->offset;
 397         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dstlist[i], 1, tail, 1, state->taps / 2 - 1);
 398     }
 399
 400     free(tail);
 401
 402     state->poolfill = -1;
 403
 404     return result;
 405 }
 406
 407
 408 int res_drain_interleaved(res_state *state, SAMPLE *dest)
 409 {
 410     SAMPLE *tail;
 411     int result = -1, poolfill = -1, offset = -1, i;
 412
 413     assert(state);
 414     assert(dest);
 415     assert(state->poolfill >= 0);
 416
 417     if ((tail = calloc(state->taps, sizeof(SAMPLE))) == NULL)
 418         return -1;
 419
 420     for (i = 0; i < state->channels; i++)
 421     {
 422         poolfill = state->poolfill;
 423         offset = state->offset;
 424         result = push(state, state->pool + i * state->taps, &poolfill, &offset, dest + i, state->channels, tail, 1, state->taps / 2 - 1);
 425     }
 426
 427     free(tail);
 428
 429     state->poolfill = -1;
 430
 431     return result;
 432 }
 433
 434
 435 void res_clear(res_state *state)
 436 {
 437     assert(state);
 438     assert(state->table);
 439     assert(state->pool);
 440
 441     free(state->table);
 442     free(state->pool);
 443     memset(state, 0, sizeof(*state));
 444 }