libao2/pl_resample.c

   1 /*=============================================================================
   2 //
   3 //  This software has been released under the terms of the GNU Public
   4 //  license. See http://www.gnu.org/copyleft/gpl.html for details.
   5 //
   6 //  Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
   7 //
   8 //=============================================================================
   9 */
  10
  11 /* This audio output plugin changes the sample rate. The output
  12    samplerate from this plugin is specified by using the switch
  13    `fout=F' where F is the desired output sample frequency
  14 */
  15
  16 #define PLUGIN
  17
  18 #include <stdio.h>
  19 #include <stdlib.h>
  20 #include <unistd.h>
  21 #include <inttypes.h>
  22
  23 #include "audio_out.h"
  24 #include "audio_plugin.h"
  25 #include "audio_plugin_internal.h"
  26 #include "afmt.h"
  27
  28 static ao_info_t info =
  29 {
  30         "Sample frequency conversion audio plugin",
  31         "resample",
  32         "Anders",
  33         ""
  34 };
  35
  36 LIBAO_PLUGIN_EXTERN(resample)
  37
  38 #define min(a,b)   (((a) < (b)) ? (a) : (b))
  39 #define max(a,b)   (((a) > (b)) ? (a) : (b))
  40
  41 /* Below definition selects the length of each poly phase component.
  42    Valid definitions are L8 and L16, where the number denotes the
  43    length of the filter. This definition affects the computational
  44    complexity (see play()), the performance (see filter.h) and the
  45    memory usage. The filterlenght is choosen to 8 if the machine is
  46    slow and to 16 if the machine is fast and has MMX.
  47 */
  48
  49 #if !defined(HAVE_SSE) && !defined(HAVE_3DNOW) //This machine is slow
  50
  51 #define W       W8      // Filter bank parameters
  52 #define L       8       // Filter length
  53 #ifdef HAVE_MMX
  54 #define FIR(x,w,y) *y=(int16_t)firn(x,w,8);
  55 #else /* HAVE_MMX */
  56 // Unrolled loop to speed up execution
  57 #define FIR(x,w,y){ \
  58   int16_t a = (w[0]*x[0]+w[1]*x[1]+w[2]*x[2]+w[3]*x[3]) >> 16; \
  59   int16_t b = (w[4]*x[4]+w[5]*x[5]+w[6]*x[6]+w[7]*x[7]) >> 16; \
  60   y[0]      = a+b; \
  61 }
  62 #endif /* HAVE_MMX */
  63
  64 #else  /* Fast machine */
  65
  66 #define W       W16
  67 #define L       16
  68 #define FIR(x,w,y) *y=(int16_t)firn(x,w,16);
  69
  70 #endif
  71
  72 #define CH  6   // Max number of channels
  73 #define UP  128  /* Up sampling factor. Increasing this value will
  74                     improve frequency accuracy. Think about the L1
  75                     cashing of filter parameters - how big can it be? */
  76
  77 #include "fir.h"
  78 #include "filter.h"
  79
  80 // local data
  81 typedef struct pl_resample_s
  82 {
  83   int16_t*      data;           // Data buffer
  84   int16_t*      w;              // Current filter weights
  85   uint16_t      dn;             // Down sampling factor
  86   uint16_t      up;             // Up sampling factor
  87   int           channels;       // Number of channels
  88   int           len;            // Lenght of buffer
  89   int16_t       ws[UP*L];       // List of all available filters
  90   int16_t       xs[CH][L*2];    // Circular buffers
  91 } pl_resample_t;
  92
  93 static pl_resample_t    pl_resample     = {NULL,NULL,1,1,1,0,W};
  94
  95 // to set/get/query special features/parameters
  96 static int control(int cmd,void *arg){
  97   switch(cmd){
  98   case AOCONTROL_PLUGIN_SET_LEN:
  99     if(pl_resample.data)
 100       free(pl_resample.data);
 101     pl_resample.len = ao_plugin_data.len;
 102     pl_resample.data=(int16_t*)malloc(pl_resample.len);
 103     if(!pl_resample.data)
 104       return CONTROL_ERROR;
 105     ao_plugin_data.len = (int)((double)ao_plugin_data.len *
 106                              ((double)pl_resample.dn)/
 107                              ((double)pl_resample.up));
 108     return CONTROL_OK;
 109   }
 110   return -1;
 111 }
 112
 113 // open & setup audio device
 114 // return: 1=success 0=fail
 115 static int init(){
 116   int fin=ao_plugin_data.rate;
 117   int fout=ao_plugin_cfg.pl_resample_fout;
 118   pl_resample.w=pl_resample.ws;
 119   pl_resample.up=UP;
 120
 121   // Sheck input format
 122   if(ao_plugin_data.format != AFMT_S16_NE){
 123     fprintf(stderr,"[pl_resample] Input audio format not yet suported. \n");
 124     return 0;
 125   }
 126   // Sanity check and calculate down sampling factor
 127   if((float)max(fin,fout)/(float)min(fin,fout) > 10){
 128     fprintf(stderr,"[pl_resample] The difference between fin and fout is too large.\n");
 129     return 0;
 130   }
 131   pl_resample.dn=(int)(0.5+((float)(fin*pl_resample.up))/((float)fout));
 132
 133   pl_resample.channels=ao_plugin_data.channels;
 134   if(ao_plugin_data.channels>CH){
 135      fprintf(stderr,"[pl_resample] Too many channels, max is 6.\n");
 136     return 0;
 137   }
 138
 139   // Tell the world what we are up to
 140   printf("[pl_resample] Up=%i, Down=%i, True fout=%f\n",
 141          pl_resample.up,pl_resample.dn,
 142          ((float)fin*pl_resample.up)/((float)pl_resample.dn));
 143
 144   // This plugin changes buffersize and adds some delay
 145   ao_plugin_data.sz_mult/=((float)pl_resample.up)/((float)pl_resample.dn);
 146   ao_plugin_data.delay_fix-= ((float)L/2) * (1/fout);
 147   ao_plugin_data.rate=fout;
 148   return 1;
 149 }
 150
 151 // close plugin
 152 static void uninit(){
 153   if(pl_resample.data)
 154     free(pl_resample.data);
 155   pl_resample.data=NULL;
 156 }
 157
 158 // empty buffers
 159 static void reset(){
 160 }
 161
 162 /* forward declarations */
 163 int upsample();
 164 int downsample();
 165
 166 // processes 'ao_plugin_data.len' bytes of 'data'
 167 // called for every block of data
 168 // FIXME: this routine needs to be optimized (it is probably possible to do a lot here)
 169 static int play(){
 170   if(pl_resample.up==pl_resample.dn){
 171     register int16_t*   in    = ((int16_t*)ao_plugin_data.data);
 172     register int16_t*   end   = in+ao_plugin_data.len/2;
 173     while(in < end) *(in++)>>=1;
 174     return 1;
 175   }
 176   if(pl_resample.up>pl_resample.dn)
 177     return upsample();
 178 //  if(pl_resample.up<pl_resample.dn)
 179     return downsample();
 180 }
 181
 182 int upsample(){
 183   static uint16_t       pwi = 0; // Index for w
 184   static uint16_t       pxi = 0; // Index for circular queue
 185
 186   uint16_t              ci    = pl_resample.channels;   // Index for channels
 187   uint16_t              nch   = pl_resample.channels;   // Number of channels
 188   uint16_t              len   = 0;                      // Number of input samples
 189   uint16_t              inc   = pl_resample.up/pl_resample.dn;
 190   uint16_t              level = pl_resample.up%pl_resample.dn;
 191   uint16_t              up    = pl_resample.up;
 192   uint16_t              dn    = pl_resample.dn;
 193
 194   register int16_t*     w     = pl_resample.w;
 195   register uint16_t     wi,xi; // Temporary indexes
 196
 197   // Index current channel
 198   while(ci--){
 199     // Temporary pointers
 200     register int16_t*   x     = pl_resample.xs[ci];
 201     register int16_t*   in    = ((int16_t*)ao_plugin_data.data)+ci;
 202     register int16_t*   out   = pl_resample.data+ci;
 203     int16_t*            end   = in+ao_plugin_data.len/2; // Block loop end
 204
 205     wi = pwi; xi = pxi;
 206
 207     while(in < end){
 208       register uint16_t i = inc;
 209       if(wi<level) i++;
 210
 211       xi=updateq(x,in,xi,L);
 212       in+=nch;
 213       while(i--){
 214         // Run the FIR filter
 215         FIR((&x[xi]),(&w[wi*L]),out);
 216         len++; out+=nch;
 217         // Update wi to point at the correct polyphase component
 218         wi=(wi+dn)%up;
 219       }
 220     }
 221   }
 222
 223   // Save values that needs to be kept for next time
 224   pwi = wi;
 225   pxi = xi;
 226
 227   // Set new data
 228   ao_plugin_data.len=len*2;
 229   ao_plugin_data.data=pl_resample.data;
 230   return 1;
 231 }
 232
 233 int downsample(){
 234   static uint16_t       pwi = 0; // Index for w
 235   static uint16_t       pxi = 0; // Index for circular queue
 236   static uint16_t       pi =  1; // Number of new samples to put in x queue
 237
 238   uint16_t              ci    = pl_resample.channels;   // Index for channels
 239   uint16_t              len   = 0;                      // Number of input samples
 240   uint16_t              nch   = pl_resample.channels;   // Number of channels
 241   uint16_t              inc   = pl_resample.dn/pl_resample.up;
 242   uint16_t              level = pl_resample.dn%pl_resample.up;
 243   uint16_t              up    = pl_resample.up;
 244   uint16_t              dn    = pl_resample.dn;
 245
 246   register uint16_t     i,wi,xi; // Temporary indexes
 247
 248
 249   // Index current channel
 250   while(ci--){
 251     // Temporary pointers
 252     register int16_t*   x     = pl_resample.xs[ci];
 253     register int16_t*   in    = ((int16_t*)ao_plugin_data.data)+ci;
 254     register int16_t*   out   = pl_resample.data+ci;
 255     // Block loop end
 256     register int16_t*   end   = in+ao_plugin_data.len/2;
 257     i = pi; wi = pwi; xi = pxi;
 258
 259     while(in < end){
 260
 261       xi=updateq(x,in,xi,L);
 262       in+=nch;
 263       if(!--i){
 264         // Run the FIR filter
 265         FIR((&x[xi]),(&pl_resample.w[wi*L]),out);
 266         len++;  out+=nch;
 267
 268         // Update wi to point at the correct polyphase component
 269         wi=(wi+dn)%up;
 270
 271         // Insert i number of new samples in queue
 272         i = inc;
 273         if(wi<level) i++;
 274       }
 275     }
 276   }
 277   // Save values that needs to be kept for next time
 278   pwi = wi;
 279   pxi = xi;
 280   pi = i;
 281   // Set new data
 282   ao_plugin_data.len=len*2;
 283   ao_plugin_data.data=pl_resample.data;
 284   return 1;
 285 }