libaf/af_sub.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@watri.uwa.edu.au
   7 //
   8 //=============================================================================
   9 */
  10
  11 /* This filter adds a sub-woofer channels to the audio stream by
  12    averaging the left and right channel and low-pass filter them. The
  13    low-pass filter is implemented as a 4th order IIR Butterworth
  14    filter, with a variable cutoff frequency between 10 and 300 Hz. The
  15    filter gives 24dB/octave attenuation. There are two runtime
  16    controls one for setting which channel to insert the sub-audio into
  17    called AF_CONTROL_SUB_CH and one for setting the cutoff frequency
  18    called AF_CONTROL_SUB_FC.
  19 */
  20
  21 #include <stdio.h>
  22 #include <stdlib.h>
  23 #include <string.h>
  24
  25 #include "af.h"
  26 #include "dsp.h"
  27
  28 // Q value for low-pass filter
  29 #define Q 1.0
  30
  31 // Analog domain biquad section
  32 typedef struct{
  33   float a[3];           // Numerator coefficients
  34   float b[3];           // Denominator coefficients
  35 } biquad_t;
  36
  37 // S-parameters for designing 4th order Butterworth filter
  38 static biquad_t sp[2] = {{{1.0,0.0,0.0},{1.0,0.765367,1.0}},
  39                          {{1.0,0.0,0.0},{1.0,1.847759,1.0}}};
  40
  41 // Data for specific instances of this filter
  42 typedef struct af_sub_s
  43 {
  44   float w[2][4];        // Filter taps for low-pass filter
  45   float q[2][2];        // Circular queues
  46   float fc;             // Cutoff frequency [Hz] for low-pass filter
  47   float k;              // Filter gain;
  48   int ch;               // Channel number which to insert the filtered data
  49
  50 }af_sub_t;
  51
  52 // Initialization and runtime control
  53 static int control(struct af_instance_s* af, int cmd, void* arg)
  54 {
  55   af_sub_t* s   = af->setup;
  56
  57   switch(cmd){
  58   case AF_CONTROL_REINIT:{
  59     // Sanity check
  60     if(!arg) return AF_ERROR;
  61
  62     af->data->rate   = ((af_data_t*)arg)->rate;
  63     af->data->nch    = max(s->ch+1,((af_data_t*)arg)->nch);
  64     af->data->format = AF_FORMAT_FLOAT_NE;
  65     af->data->bps    = 4;
  66
  67     // Design low-pass filter
  68     s->k = 1.0;
  69     if((-1 == af_filter_szxform(sp[0].a, sp[0].b, Q, s->fc,
  70        (float)af->data->rate, &s->k, s->w[0])) ||
  71        (-1 == af_filter_szxform(sp[1].a, sp[1].b, Q, s->fc,
  72        (float)af->data->rate, &s->k, s->w[1])))
  73       return AF_ERROR;
  74     return af_test_output(af,(af_data_t*)arg);
  75   }
  76   case AF_CONTROL_COMMAND_LINE:{
  77     int   ch=5;
  78     float fc=60.0;
  79     sscanf(arg,"%f:%i", &fc , &ch);
  80     if(AF_OK != control(af,AF_CONTROL_SUB_CH | AF_CONTROL_SET, &ch))
  81       return AF_ERROR;
  82     return control(af,AF_CONTROL_SUB_FC | AF_CONTROL_SET, &fc);
  83   }
  84   case AF_CONTROL_SUB_CH | AF_CONTROL_SET: // Requires reinit
  85     // Sanity check
  86     if((*(int*)arg >= AF_NCH) || (*(int*)arg < 0)){
  87       af_msg(AF_MSG_ERROR,"[sub] Subwoofer channel number must be between "
  88              " 0 and %i current value is %i\n", AF_NCH-1, *(int*)arg);
  89       return AF_ERROR;
  90     }
  91     s->ch = *(int*)arg;
  92     return AF_OK;
  93   case AF_CONTROL_SUB_CH | AF_CONTROL_GET:
  94     *(int*)arg = s->ch;
  95     return AF_OK;
  96   case AF_CONTROL_SUB_FC | AF_CONTROL_SET: // Requires reinit
  97     // Sanity check
  98     if((*(float*)arg > 300) || (*(float*)arg < 20)){
  99       af_msg(AF_MSG_ERROR,"[sub] Cutoff frequency must be between 20Hz and"
 100              " 300Hz current value is %0.2f",*(float*)arg);
 101       return AF_ERROR;
 102     }
 103     // Set cutoff frequency
 104     s->fc = *(float*)arg;
 105     return AF_OK;
 106   case AF_CONTROL_SUB_FC | AF_CONTROL_GET:
 107     *(float*)arg = s->fc;
 108     return AF_OK;
 109   }
 110   return AF_UNKNOWN;
 111 }
 112
 113 // Deallocate memory
 114 static void uninit(struct af_instance_s* af)
 115 {
 116   if(af->data)
 117     free(af->data);
 118   if(af->setup)
 119     free(af->setup);
 120 }
 121
 122 #ifndef IIR
 123 #define IIR(in,w,q,out) { \
 124   float h0 = (q)[0]; \
 125   float h1 = (q)[1]; \
 126   float hn = (in) - h0 * (w)[0] - h1 * (w)[1];  \
 127   out = hn + h0 * (w)[2] + h1 * (w)[3];  \
 128   (q)[1] = h0; \
 129   (q)[0] = hn; \
 130 }
 131 #endif
 132
 133 // Filter data through filter
 134 static af_data_t* play(struct af_instance_s* af, af_data_t* data)
 135 {
 136   af_data_t*    c   = data;      // Current working data
 137   af_sub_t*     s   = af->setup; // Setup for this instance
 138   float*        a   = c->audio;  // Audio data
 139   int           len = c->len/4;  // Number of samples in current audio block
 140   int           nch = c->nch;    // Number of channels
 141   int           ch  = s->ch;     // Channel in which to insert the sub audio
 142   register int  i;
 143
 144   // Run filter
 145   for(i=0;i<len;i+=nch){
 146     // Average left and right
 147     register float x = 0.5 * (a[i] + a[i+1]);
 148     IIR(x * s->k, s->w[0], s->q[0], x);
 149     IIR(x , s->w[1], s->q[1], a[i+ch]);
 150   }
 151
 152   return c;
 153 }
 154
 155 // Allocate memory and set function pointers
 156 static int af_open(af_instance_t* af){
 157   af_sub_t* s;
 158   af->control=control;
 159   af->uninit=uninit;
 160   af->play=play;
 161   af->mul=1;
 162   af->data=calloc(1,sizeof(af_data_t));
 163   af->setup=s=calloc(1,sizeof(af_sub_t));
 164   if(af->data == NULL || af->setup == NULL)
 165     return AF_ERROR;
 166   // Set default values
 167   s->ch = 5;     // Channel nr 6
 168   s->fc = 60;    // Cutoff frequency 60Hz
 169   return AF_OK;
 170 }
 171
 172 // Description of this filter
 173 af_info_t af_info_sub = {
 174     "Audio filter for adding a sub-base channel",
 175     "sub",
 176     "Anders",
 177     "",
 178     AF_FLAGS_NOT_REENTRANT,
 179     af_open
 180 };