main/libresample/src/resample.c

   1 /**********************************************************************
   2
   3   resample.c
   4
   5   Real-time library interface by Dominic Mazzoni
   6
   7   Based on resample-1.7:
   8     http://www-ccrma.stanford.edu/~jos/resample/
   9
  10   License: LGPL - see the file LICENSE.txt for more information
  11
  12   This is the main source file, implementing all of the API
  13   functions and handling all of the buffering logic.
  14
  15 **********************************************************************/
  16
  17 /* External interface */
  18 #include "../include/libresample.h"
  19
  20 /* Definitions */
  21 #include "resample_defs.h"
  22
  23 #include "filterkit.h"
  24
  25 #include <stdlib.h>
  26 #include <stdio.h>
  27 #include <math.h>
  28 #include <string.h>
  29
  30 typedef struct {
  31    float  *Imp;
  32    float  *ImpD;
  33    float   LpScl;
  34    UWORD   Nmult;
  35    UWORD   Nwing;
  36    double  minFactor;
  37    double  maxFactor;
  38    UWORD   XSize;
  39    float  *X;
  40    UWORD   Xp; /* Current "now"-sample pointer for input */
  41    UWORD   Xread; /* Position to put new samples */
  42    UWORD   Xoff;
  43    UWORD   YSize;
  44    float  *Y;
  45    UWORD   Yp;
  46    double  Time;
  47 } rsdata;
  48
  49 void *resample_dup(const void * handle)
  50 {
  51    const rsdata *cpy = (const rsdata *)handle;
  52    rsdata *hp = (rsdata *)malloc(sizeof(rsdata));
  53
  54    hp->minFactor = cpy->minFactor;
  55    hp->maxFactor = cpy->maxFactor;
  56    hp->Nmult = cpy->Nmult;
  57    hp->LpScl = cpy->LpScl;
  58    hp->Nwing = cpy->Nwing;
  59
  60    hp->Imp = (float *)malloc(hp->Nwing * sizeof(float));
  61    memcpy(hp->Imp, cpy->Imp, hp->Nwing * sizeof(float));
  62    hp->ImpD = (float *)malloc(hp->Nwing * sizeof(float));
  63    memcpy(hp->ImpD, cpy->ImpD, hp->Nwing * sizeof(float));
  64
  65    hp->Xoff = cpy->Xoff;
  66    hp->XSize = cpy->XSize;
  67    hp->X = (float *)malloc((hp->XSize + hp->Xoff) * sizeof(float));
  68    memcpy(hp->X, cpy->X, (hp->XSize + hp->Xoff) * sizeof(float));
  69    hp->Xp = cpy->Xp;
  70    hp->Xread = cpy->Xread;
  71    hp->YSize = cpy->YSize;
  72    hp->Y = (float *)malloc(hp->YSize * sizeof(float));
  73    memcpy(hp->Y, cpy->Y, hp->YSize * sizeof(float));
  74    hp->Yp = cpy->Yp;
  75    hp->Time = cpy->Time;
  76
  77    return (void *)hp;
  78 }
  79
  80 void *resample_open(int highQuality, double minFactor, double maxFactor)
  81 {
  82    double *Imp64;
  83    double Rolloff, Beta;
  84    rsdata *hp;
  85    UWORD   Xoff_min, Xoff_max;
  86    int i;
  87
  88    /* Just exit if we get invalid factors */
  89    if (minFactor <= 0.0 || maxFactor <= 0.0 || maxFactor < minFactor) {
  90       #if defined(DEBUG)
  91       fprintf(stderr,
  92               "libresample: "
  93               "minFactor and maxFactor must be positive real numbers,\n"
  94               "and maxFactor should be larger than minFactor.\n");
  95       #endif
  96       return 0;
  97    }
  98
  99    hp = (rsdata *)malloc(sizeof(rsdata));
 100
 101    hp->minFactor = minFactor;
 102    hp->maxFactor = maxFactor;
 103
 104    if (highQuality)
 105       hp->Nmult = 35;
 106    else
 107       hp->Nmult = 11;
 108
 109    hp->LpScl = 1.0;
 110    hp->Nwing = Npc*(hp->Nmult-1)/2; /* # of filter coeffs in right wing */
 111
 112    Rolloff = 0.90;
 113    Beta = 6;
 114
 115    Imp64 = (double *)malloc(hp->Nwing * sizeof(double));
 116
 117    lrsLpFilter(Imp64, hp->Nwing, 0.5*Rolloff, Beta, Npc);
 118
 119    hp->Imp = (float *)malloc(hp->Nwing * sizeof(float));
 120    hp->ImpD = (float *)malloc(hp->Nwing * sizeof(float));
 121    for(i=0; i<hp->Nwing; i++)
 122       hp->Imp[i] = Imp64[i];
 123
 124    /* Storing deltas in ImpD makes linear interpolation
 125       of the filter coefficients faster */
 126    for (i=0; i<hp->Nwing-1; i++)
 127       hp->ImpD[i] = hp->Imp[i+1] - hp->Imp[i];
 128
 129    /* Last coeff. not interpolated */
 130    hp->ImpD[hp->Nwing-1] = - hp->Imp[hp->Nwing-1];
 131
 132    free(Imp64);
 133
 134    /* Calc reach of LP filter wing (plus some creeping room) */
 135    Xoff_min = ((hp->Nmult+1)/2.0) * MAX(1.0, 1.0/minFactor) + 10;
 136    Xoff_max = ((hp->Nmult+1)/2.0) * MAX(1.0, 1.0/maxFactor) + 10;
 137    hp->Xoff = MAX(Xoff_min, Xoff_max);
 138
 139    /* Make the inBuffer size at least 4096, but larger if necessary
 140       in order to store the minimum reach of the LP filter and then some.
 141       Then allocate the buffer an extra Xoff larger so that
 142       we can zero-pad up to Xoff zeros at the end when we reach the
 143       end of the input samples. */
 144    hp->XSize = MAX(2*hp->Xoff+10, 4096);
 145    hp->X = (float *)malloc((hp->XSize + hp->Xoff) * sizeof(float));
 146    hp->Xp = hp->Xoff;
 147    hp->Xread = hp->Xoff;
 148
 149    /* Need Xoff zeros at begining of X buffer */
 150    for(i=0; i<hp->Xoff; i++)
 151       hp->X[i]=0;
 152
 153    /* Make the outBuffer long enough to hold the entire processed
 154       output of one inBuffer */
 155    hp->YSize = (int)(((double)hp->XSize)*maxFactor+2.0);
 156    hp->Y = (float *)malloc(hp->YSize * sizeof(float));
 157    hp->Yp = 0;
 158
 159    hp->Time = (double)hp->Xoff; /* Current-time pointer for converter */
 160
 161    return (void *)hp;
 162 }
 163
 164 int resample_get_filter_width(const void   *handle)
 165 {
 166    const rsdata *hp = (const rsdata *)handle;
 167    return hp->Xoff;
 168 }
 169
 170 int resample_process(void   *handle,
 171                      double  factor,
 172                      float  *inBuffer,
 173                      int     inBufferLen,
 174                      int     lastFlag,
 175                      int    *inBufferUsed, /* output param */
 176                      float  *outBuffer,
 177                      int     outBufferLen)
 178 {
 179    rsdata *hp = (rsdata *)handle;
 180    float  *Imp = hp->Imp;
 181    float  *ImpD = hp->ImpD;
 182    float  LpScl = hp->LpScl;
 183    UWORD  Nwing = hp->Nwing;
 184    BOOL interpFilt = FALSE; /* TRUE means interpolate filter coeffs */
 185    int outSampleCount;
 186    UWORD Nout, Ncreep, Nreuse;
 187    int Nx;
 188    int i, len;
 189
 190    #if defined(DEBUG)
 191    fprintf(stderr, "resample_process: in=%d, out=%d lastFlag=%d\n",
 192            inBufferLen, outBufferLen, lastFlag);
 193    #endif
 194
 195    /* Initialize inBufferUsed and outSampleCount to 0 */
 196    *inBufferUsed = 0;
 197    outSampleCount = 0;
 198
 199    if (factor < hp->minFactor || factor > hp->maxFactor) {
 200       #if defined(DEBUG)
 201       fprintf(stderr,
 202               "libresample: factor %f is not between "
 203               "minFactor=%f and maxFactor=%f",
 204               factor, hp->minFactor, hp->maxFactor);
 205       #endif
 206       return -1;
 207    }
 208
 209    /* Start by copying any samples still in the Y buffer to the output
 210       buffer */
 211    if (hp->Yp && (outBufferLen-outSampleCount)>0) {
 212       len = MIN(outBufferLen-outSampleCount, hp->Yp);
 213       for(i=0; i<len; i++)
 214          outBuffer[outSampleCount+i] = hp->Y[i];
 215       outSampleCount += len;
 216       for(i=0; i<hp->Yp-len; i++)
 217          hp->Y[i] = hp->Y[i+len];
 218       hp->Yp -= len;
 219    }
 220
 221    /* If there are still output samples left, return now - we need
 222       the full output buffer available to us... */
 223    if (hp->Yp)
 224       return outSampleCount;
 225
 226    /* Account for increased filter gain when using factors less than 1 */
 227    if (factor < 1)
 228       LpScl = LpScl*factor;
 229
 230    for(;;) {
 231
 232       /* This is the maximum number of samples we can process
 233          per loop iteration */
 234
 235       #if defined(DEBUG)
 236       printf("XSize: %d Xoff: %d Xread: %d Xp: %d lastFlag: %d\n",
 237              hp->XSize, hp->Xoff, hp->Xread, hp->Xp, lastFlag);
 238       #endif
 239
 240       /* Copy as many samples as we can from the input buffer into X */
 241       len = hp->XSize - hp->Xread;
 242
 243       if (len >= (inBufferLen - (*inBufferUsed)))
 244          len = (inBufferLen - (*inBufferUsed));
 245
 246       for(i=0; i<len; i++)
 247          hp->X[hp->Xread + i] = inBuffer[(*inBufferUsed) + i];
 248
 249       *inBufferUsed += len;
 250       hp->Xread += len;
 251
 252       if (lastFlag && (*inBufferUsed == inBufferLen)) {
 253          /* If these are the last samples, zero-pad the
 254             end of the input buffer and make sure we process
 255             all the way to the end */
 256          Nx = hp->Xread - hp->Xoff;
 257          for(i=0; i<hp->Xoff; i++)
 258             hp->X[hp->Xread + i] = 0;
 259       }
 260       else
 261          Nx = hp->Xread - 2 * hp->Xoff;
 262
 263       #if defined(DEBUG)
 264       fprintf(stderr, "new len=%d Nx=%d\n", len, Nx);
 265       #endif
 266
 267       if (Nx <= 0)
 268          break;
 269
 270       /* Resample stuff in input buffer */
 271       if (factor >= 1) {      /* SrcUp() is faster if we can use it */
 272          Nout = lrsSrcUp(hp->X, hp->Y, factor, &hp->Time, Nx,
 273                          Nwing, LpScl, Imp, ImpD, interpFilt);
 274       }
 275       else {
 276          Nout = lrsSrcUD(hp->X, hp->Y, factor, &hp->Time, Nx,
 277                          Nwing, LpScl, Imp, ImpD, interpFilt);
 278       }
 279
 280       #if defined(DEBUG)
 281       printf("Nout: %d\n", Nout);
 282       #endif
 283
 284       hp->Time -= Nx;         /* Move converter Nx samples back in time */
 285       hp->Xp += Nx;           /* Advance by number of samples processed */
 286
 287       /* Calc time accumulation in Time */
 288       Ncreep = (int)(hp->Time) - hp->Xoff;
 289       if (Ncreep) {
 290          hp->Time -= Ncreep;  /* Remove time accumulation */
 291          hp->Xp += Ncreep;    /* and add it to read pointer */
 292       }
 293
 294       /* Copy part of input signal that must be re-used */
 295       Nreuse = hp->Xread - (hp->Xp - hp->Xoff);
 296
 297       for (i=0; i<Nreuse; i++)
 298          hp->X[i] = hp->X[i + (hp->Xp - hp->Xoff)];
 299
 300       #if defined(DEBUG)
 301       printf("New Xread=%d\n", Nreuse);
 302       #endif
 303
 304       hp->Xread = Nreuse;  /* Pos in input buff to read new data into */
 305       hp->Xp = hp->Xoff;
 306
 307       /* Check to see if output buff overflowed (shouldn't happen!) */
 308       if (Nout > hp->YSize) {
 309          #if defined(DEBUG)
 310          printf("Nout: %d YSize: %d\n", Nout, hp->YSize);
 311          #endif
 312          fprintf(stderr, "libresample: Output array overflow!\n");
 313          return -1;
 314       }
 315
 316       hp->Yp = Nout;
 317
 318       /* Copy as many samples as possible to the output buffer */
 319       if (hp->Yp && (outBufferLen-outSampleCount)>0) {
 320          len = MIN(outBufferLen-outSampleCount, hp->Yp);
 321          for(i=0; i<len; i++)
 322             outBuffer[outSampleCount+i] = hp->Y[i];
 323          outSampleCount += len;
 324          for(i=0; i<hp->Yp-len; i++)
 325             hp->Y[i] = hp->Y[i+len];
 326          hp->Yp -= len;
 327       }
 328
 329       /* If there are still output samples left, return now,
 330          since we need the full output buffer available */
 331       if (hp->Yp)
 332          break;
 333    }
 334
 335    return outSampleCount;
 336 }
 337
 338 void resample_close(void *handle)
 339 {
 340    rsdata *hp = (rsdata *)handle;
 341    free(hp->X);
 342    free(hp->Y);
 343    free(hp->Imp);
 344    free(hp->ImpD);
 345    free(hp);
 346 }
 347