libaf/af_resample.c

   1 /*=============================================================================
   2 //
   3 //  This software has been released under the terms of the GNU General Public
   4 //  license. See http://www.gnu.org/copyleft/gpl.html for details.
   5 //
   6 //  Copyright 2002 Anders Johansson ajh@atri.curtin.edu.au
   7 //
   8 //=============================================================================
   9 */
  10
  11 /* This audio filter changes the sample rate. */
  12 #include <stdio.h>
  13 #include <stdlib.h>
  14 #include <inttypes.h>
  15
  16 #include "af.h"
  17 #include "dsp.h"
  18
  19 /* Below definition selects the length of each poly phase component.
  20    Valid definitions are L8 and L16, where the number denotes the
  21    length of the filter. This definition affects the computational
  22    complexity (see play()), the performance (see filter.h) and the
  23    memory usage. The filterlength is choosen to 8 if the machine is
  24    slow and to 16 if the machine is fast and has MMX.
  25 */
  26
  27 #if !defined(HAVE_MMX) // This machine is slow
  28 #define L8
  29 #else
  30 #define L16
  31 #endif
  32
  33 #include "af_resample.h"
  34
  35 // Filtering types
  36 #define RSMP_LIN        (0<<0)  // Linear interpolation
  37 #define RSMP_INT        (1<<0)  // 16 bit integer
  38 #define RSMP_FLOAT      (2<<0)  // 32 bit floating point
  39 #define RSMP_MASK       (3<<0)
  40
  41 // Defines for sloppy or exact resampling
  42 #define FREQ_SLOPPY     (0<<2)
  43 #define FREQ_EXACT      (1<<2)
  44 #define FREQ_MASK       (1<<2)
  45
  46 // Accuracy for linear interpolation
  47 #define STEPACCURACY 32
  48
  49 // local data
  50 typedef struct af_resample_s
  51 {
  52   void*         w;      // Current filter weights
  53   void**        xq;     // Circular buffers
  54   uint32_t      xi;     // Index for circular buffers
  55   uint32_t      wi;     // Index for w
  56   uint32_t      i;      // Number of new samples to put in x queue
  57   uint32_t      dn;     // Down sampling factor
  58   uint32_t      up;     // Up sampling factor
  59   uint64_t      step;   // Step size for linear interpolation
  60   uint64_t      pt;     // Pointer remainder for linear interpolation
  61   int           setup;  // Setup parameters cmdline or through postcreate
  62 } af_resample_t;
  63
  64 // Fast linear interpolation resample with modest audio quality
  65 static int linint(af_data_t* c,af_data_t* l, af_resample_t* s)
  66 {
  67   uint32_t      len   = 0;              // Number of input samples
  68   uint32_t      nch   = l->nch;         // Words pre transfer
  69   uint64_t      step  = s->step;
  70   int16_t*      in16  = ((int16_t*)c->audio);
  71   int16_t*      out16 = ((int16_t*)l->audio);
  72   int32_t*      in32  = ((int32_t*)c->audio);
  73   int32_t*      out32 = ((int32_t*)l->audio);
  74   uint64_t      end   = ((((uint64_t)c->len)/2LL)<<STEPACCURACY);
  75   uint64_t      pt    = s->pt;
  76   uint16_t      tmp;
  77
  78   switch (nch){
  79   case 1:
  80     while(pt < end){
  81       out16[len++]=in16[pt>>STEPACCURACY];
  82       pt+=step;
  83     }
  84     s->pt=pt & ((1LL<<STEPACCURACY)-1);
  85     break;
  86   case 2:
  87     end/=2;
  88     while(pt < end){
  89       out32[len++]=in32[pt>>STEPACCURACY];
  90       pt+=step;
  91     }
  92     len=(len<<1);
  93     s->pt=pt & ((1LL<<STEPACCURACY)-1);
  94     break;
  95   default:
  96     end /=nch;
  97     while(pt < end){
  98       tmp=nch;
  99       do {
 100         tmp--;
 101         out16[len+tmp]=in16[tmp+(pt>>STEPACCURACY)*nch];
 102       } while (tmp);
 103       len+=nch;
 104       pt+=step;
 105     }
 106     s->pt=pt & ((1LL<<STEPACCURACY)-1);
 107   }
 108   return len;
 109 }
 110
 111 /* Determine resampling type and format */
 112 static int set_types(struct af_instance_s* af, af_data_t* data)
 113 {
 114   af_resample_t* s = af->setup;
 115   int rv = AF_OK;
 116   float rd = 0;
 117
 118   // Make sure this filter isn't redundant
 119   if((af->data->rate == data->rate) || (af->data->rate == 0))
 120     return AF_DETACH;
 121   /* If sloppy and small resampling difference (2%) */
 122   rd = abs((float)af->data->rate - (float)data->rate)/(float)data->rate;
 123   if((((s->setup & FREQ_MASK) == FREQ_SLOPPY) && (rd < 0.02) &&
 124       (data->format != (AF_FORMAT_FLOAT_NE))) ||
 125      ((s->setup & RSMP_MASK) == RSMP_LIN)){
 126     s->setup = (s->setup & ~RSMP_MASK) | RSMP_LIN;
 127     af->data->format = AF_FORMAT_S16_NE;
 128     af->data->bps    = 2;
 129     af_msg(AF_MSG_VERBOSE,"[resample] Using linear interpolation. \n");
 130   }
 131   else{
 132     /* If the input format is float or if float is explicitly selected
 133        use float, otherwise use int */
 134     if((data->format == (AF_FORMAT_FLOAT_NE)) ||
 135        ((s->setup & RSMP_MASK) == RSMP_FLOAT)){
 136       s->setup = (s->setup & ~RSMP_MASK) | RSMP_FLOAT;
 137       af->data->format = AF_FORMAT_FLOAT_NE;
 138       af->data->bps    = 4;
 139     }
 140     else{
 141       s->setup = (s->setup & ~RSMP_MASK) | RSMP_INT;
 142       af->data->format = AF_FORMAT_S16_NE;
 143       af->data->bps    = 2;
 144     }
 145     af_msg(AF_MSG_VERBOSE,"[resample] Using %s processing and %s frequecy"
 146            " conversion.\n",
 147            ((s->setup & RSMP_MASK) == RSMP_FLOAT)?"floating point":"integer",
 148            ((s->setup & FREQ_MASK) == FREQ_SLOPPY)?"inexact":"exact");
 149   }
 150
 151   if(af->data->format != data->format || af->data->bps != data->bps)
 152     rv = AF_FALSE;
 153   data->format = af->data->format;
 154   data->bps = af->data->bps;
 155   af->data->nch = data->nch;
 156   return rv;
 157 }
 158
 159 // Initialization and runtime control
 160 static int control(struct af_instance_s* af, int cmd, void* arg)
 161 {
 162   switch(cmd){
 163   case AF_CONTROL_REINIT:{
 164     af_resample_t* s   = (af_resample_t*)af->setup;
 165     af_data_t*     n   = (af_data_t*)arg; // New configureation
 166     int            i,d = 0;
 167     int            rv  = AF_OK;
 168
 169     // Free space for circular bufers
 170     if(s->xq){
 171       for(i=1;i<af->data->nch;i++)
 172         if(s->xq[i])
 173           free(s->xq[i]);
 174       free(s->xq);
 175       s->xq = NULL;
 176     }
 177
 178     if(AF_DETACH == (rv = set_types(af,n)))
 179       return AF_DETACH;
 180
 181     // If linear interpolation
 182     if((s->setup & RSMP_MASK) == RSMP_LIN){
 183       s->pt=0LL;
 184       s->step=((uint64_t)n->rate<<STEPACCURACY)/(uint64_t)af->data->rate+1LL;
 185       af_msg(AF_MSG_DEBUG0,"[resample] Linear interpolation step: 0x%016"PRIX64".\n",
 186              s->step);
 187       af->mul.n = af->data->rate;
 188       af->mul.d = n->rate;
 189       af_frac_cancel(&af->mul);
 190       return rv;
 191     }
 192
 193     // Calculate up and down sampling factors
 194     d=af_gcd(af->data->rate,n->rate);
 195
 196     // If sloppy resampling is enabled limit the upsampling factor
 197     if(((s->setup & FREQ_MASK) == FREQ_SLOPPY) && (af->data->rate/d > 5000)){
 198       int up=af->data->rate/2;
 199       int dn=n->rate/2;
 200       int m=2;
 201       while(af->data->rate/(d*m) > 5000){
 202         d=af_gcd(up,dn);
 203         up/=2; dn/=2; m*=2;
 204       }
 205       d*=m;
 206     }
 207
 208     // Create space for circular bufers
 209     s->xq = malloc(n->nch*sizeof(void*));
 210     for(i=0;i<n->nch;i++)
 211       s->xq[i] = malloc(2*L*af->data->bps);
 212     s->xi = 0;
 213
 214     // Check if the the design needs to be redone
 215     if(s->up != af->data->rate/d || s->dn != n->rate/d){
 216       float* w;
 217       float* wt;
 218       float fc;
 219       int j;
 220       s->up = af->data->rate/d;
 221       s->dn = n->rate/d;
 222       s->wi = 0;
 223       s->i = 0;
 224
 225       // Calculate cuttof frequency for filter
 226       fc = 1/(float)(max(s->up,s->dn));
 227       // Allocate space for polyphase filter bank and protptype filter
 228       w = malloc(sizeof(float) * s->up *L);
 229       if(NULL != s->w)
 230         free(s->w);
 231       s->w = malloc(L*s->up*af->data->bps);
 232
 233       // Design prototype filter type using Kaiser window with beta = 10
 234       if(NULL == w || NULL == s->w ||
 235          -1 == af_filter_design_fir(s->up*L, w, &fc, LP|KAISER , 10.0)){
 236         af_msg(AF_MSG_ERROR,"[resample] Unable to design prototype filter.\n");
 237         return AF_ERROR;
 238       }
 239       // Copy data from prototype to polyphase filter
 240       wt=w;
 241       for(j=0;j<L;j++){//Columns
 242         for(i=0;i<s->up;i++){//Rows
 243           if((s->setup & RSMP_MASK) == RSMP_INT){
 244             float t=(float)s->up*32767.0*(*wt);
 245             ((int16_t*)s->w)[i*L+j] = (int16_t)((t>=0.0)?(t+0.5):(t-0.5));
 246           }
 247           else
 248             ((float*)s->w)[i*L+j] = (float)s->up*(*wt);
 249           wt++;
 250         }
 251       }
 252       free(w);
 253       af_msg(AF_MSG_VERBOSE,"[resample] New filter designed up: %i "
 254              "down: %i\n", s->up, s->dn);
 255     }
 256
 257     // Set multiplier and delay
 258     af->delay = (double)(1000*L/2)/((double)n->rate);
 259     af->mul.n = s->up;
 260     af->mul.d = s->dn;
 261     return rv;
 262   }
 263   case AF_CONTROL_COMMAND_LINE:{
 264     af_resample_t* s   = (af_resample_t*)af->setup;
 265     int rate=0;
 266     int type=RSMP_INT;
 267     int sloppy=1;
 268     sscanf((char*)arg,"%i:%i:%i", &rate, &sloppy, &type);
 269     s->setup = (sloppy?FREQ_SLOPPY:FREQ_EXACT) |
 270       (clamp(type,RSMP_LIN,RSMP_FLOAT));
 271     return af->control(af,AF_CONTROL_RESAMPLE_RATE | AF_CONTROL_SET, &rate);
 272   }
 273   case AF_CONTROL_POST_CREATE:
 274     if((((af_cfg_t*)arg)->force & AF_INIT_FORMAT_MASK) == AF_INIT_FLOAT)
 275       ((af_resample_t*)af->setup)->setup = RSMP_FLOAT;
 276     return AF_OK;
 277   case AF_CONTROL_RESAMPLE_RATE | AF_CONTROL_SET:
 278     // Reinit must be called after this function has been called
 279
 280     // Sanity check
 281     if(((int*)arg)[0] < 8000 || ((int*)arg)[0] > 192000){
 282       af_msg(AF_MSG_ERROR,"[resample] The output sample frequency "
 283              "must be between 8kHz and 192kHz. Current value is %i \n",
 284              ((int*)arg)[0]);
 285       return AF_ERROR;
 286     }
 287
 288     af->data->rate=((int*)arg)[0];
 289     af_msg(AF_MSG_VERBOSE,"[resample] Changing sample rate "
 290            "to %iHz\n",af->data->rate);
 291     return AF_OK;
 292   }
 293   return AF_UNKNOWN;
 294 }
 295
 296 // Deallocate memory
 297 static void uninit(struct af_instance_s* af)
 298 {
 299   if(af->data)
 300     free(af->data->audio);
 301   free(af->data);
 302 }
 303
 304 // Filter data through filter
 305 static af_data_t* play(struct af_instance_s* af, af_data_t* data)
 306 {
 307   int            len = 0;        // Length of output data
 308   af_data_t*     c   = data;     // Current working data
 309   af_data_t*     l   = af->data; // Local data
 310   af_resample_t* s   = (af_resample_t*)af->setup;
 311
 312   if(AF_OK != RESIZE_LOCAL_BUFFER(af,data))
 313     return NULL;
 314
 315   // Run resampling
 316   switch(s->setup & RSMP_MASK){
 317   case(RSMP_INT):
 318 # define FORMAT_I 1
 319     if(s->up>s->dn){
 320 #     define UP
 321 #     include "af_resample.h"
 322 #     undef UP
 323     }
 324     else{
 325 #     define DN
 326 #     include "af_resample.h"
 327 #     undef DN
 328     }
 329     break;
 330   case(RSMP_FLOAT):
 331 # undef FORMAT_I
 332 # define FORMAT_F 1
 333     if(s->up>s->dn){
 334 #     define UP
 335 #     include "af_resample.h"
 336 #     undef UP
 337     }
 338     else{
 339 #     define DN
 340 #     include "af_resample.h"
 341 #     undef DN
 342     }
 343     break;
 344   case(RSMP_LIN):
 345     len = linint(c, l, s);
 346     break;
 347   }
 348
 349   // Set output data
 350   c->audio = l->audio;
 351   c->len   = len*l->bps;
 352   c->rate  = l->rate;
 353
 354   return c;
 355 }
 356
 357 // Allocate memory and set function pointers
 358 static int af_open(af_instance_t* af){
 359   af->control=control;
 360   af->uninit=uninit;
 361   af->play=play;
 362   af->mul.n=1;
 363   af->mul.d=1;
 364   af->data=calloc(1,sizeof(af_data_t));
 365   af->setup=calloc(1,sizeof(af_resample_t));
 366   if(af->data == NULL || af->setup == NULL)
 367     return AF_ERROR;
 368   ((af_resample_t*)af->setup)->setup = RSMP_INT | FREQ_SLOPPY;
 369   return AF_OK;
 370 }
 371
 372 // Description of this plugin
 373 af_info_t af_info_resample = {
 374   "Sample frequency conversion",
 375   "resample",
 376   "Anders",
 377   "",
 378   AF_FLAGS_REENTRANT,
 379   af_open
 380 };
 381