aboutsummaryrefslogtreecommitdiffhomepage
path: root/libao2/pl_eq.c
blob: 8fdc2ae35c2f9f591cef03618dde6e2b6cb783bd (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
/*=============================================================================
//	
//  This software has been released under the terms of the GNU Public
//  license. See http://www.gnu.org/copyleft/gpl.html for details.
//
//  Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
//
//=============================================================================
*/

/* Equalizer plugin, implementation of a 10 band time domain graphic
   equalizer using IIR filters. The IIR filters are implemented using a
   Direct Form II approach. But has been modified (b1 == 0 always) to
   save computation.
*/
#define PLUGIN

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <inttypes.h>
#include <math.h>

#include "audio_out.h"
#include "audio_plugin.h"
#include "audio_plugin_internal.h"
#include "afmt.h"
#include "eq.h"

static ao_info_t info =
{
        "Equalizer audio plugin",
        "eq",
        "Anders",
        ""
};

LIBAO_PLUGIN_EXTERN(eq)


#define CH  6     // Max number of channels
#define L   2     // Storage for filter taps
#define KM  10    // Max number of octaves

#define Q   1.2247 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
		      gives 4dB suppression @ Fc*2 and Fc/2 */

// Center frequencies for band-pass filters
#define CF  {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}

// local data
typedef struct pl_eq_s
{
  int16_t   a[KM][L];        // A weights
  int16_t   b[KM][L];        // B weights
  int16_t   wq[CH][KM][L];   // Circular buffer for W data
  int16_t   g[CH][KM];       // Gain factor for each channel and band
  int16_t   K; 		     // Number of used eq bands
  int       channels;        // Number of channels
} pl_eq_t;

static pl_eq_t pl_eq;

// to set/get/query special features/parameters
static int control(int cmd,int arg){
  switch(cmd){
  case AOCONTROL_PLUGIN_SET_LEN:
    return CONTROL_OK;
  case AOCONTROL_PLUGIN_EQ_SET_GAIN:{
    float gain = ((equalizer_t*)arg)->gain;
    int ch     =((equalizer_t*)arg)->channel;
    int band   =((equalizer_t*)arg)->band;
    if(ch > CH || ch < 0 || band > KM || band < 0)
      return CONTROL_ERROR;
    
    pl_eq.g[ch][band]=(int16_t) 4096 * (pow(10.0,gain/20.0)-1.0);
    return CONTROL_OK;
  }
  case AOCONTROL_PLUGIN_EQ_GET_GAIN:{
    int ch     =((equalizer_t*)arg)->channel;
    int band   =((equalizer_t*)arg)->band;
    if(ch > CH || ch < 0 || band > KM || band < 0)
      return CONTROL_ERROR;
    
    ((equalizer_t*)arg)->gain = log10((float)pl_eq.g[ch][band]/4096.0+1) * 20.0;
    return CONTROL_OK;
  }
  }
  return CONTROL_UNKNOWN;
}

// return rounded 16bit int
static inline int16_t lround16(double n){
  return (int16_t)((n)>=0.0?(n)+0.5:(n)-0.5);
}

// 2nd order Band-pass Filter design
void bp2(int16_t* a, int16_t* b, float fc, float q){
  double th=2*3.141592654*fc;
  double C=(1 - tan(th*q/2))/(1 + tan(th*q/2));
  
  a[0] = lround16( 16383.0 * (1 + C) * cos(th));
  a[1] = lround16(-16383.0 * C);
  
  b[0] = lround16(-16383.0 * (C - 1)/2);
  b[1] = lround16(-16383.0 * 1.0050);
}

// empty buffers
static void reset(){
  int k,l,c;
  for(c=0;c<pl_eq.channels;c++)
    for(k=0;k<pl_eq.K;k++)
      for(l=0;l<L*2;l++)
	pl_eq.wq[c][k][l]=0;
}

// open & setup audio device
// return: 1=success 0=fail
static int init(){
  int   k   = 0;
  float F[KM] = CF;
  
  // Check input format
  if(ao_plugin_data.format != AFMT_S16_NE){
    fprintf(stderr,"[pl_eq] Input audio format not yet supported. \n");
    return 0;
  }

  // Check number of channels
  if(ao_plugin_data.channels>CH){
     fprintf(stderr,"[pl_eq] Too many channels, max is 6.\n");
    return 0;
  }
  pl_eq.channels=ao_plugin_data.channels;

  // Calculate number of active filters
  pl_eq.K=KM;
  while(F[pl_eq.K-1] > (float)ao_plugin_data.rate/2)
    pl_eq.K--;

  // Generate filter taps
  for(k=0;k<pl_eq.K;k++)
    bp2(pl_eq.a[k],pl_eq.b[k],F[k]/((float)ao_plugin_data.rate),Q);

  // Reset buffers
  reset();

  // Tell ao_plugin how much this plugin adds to the overall time delay
  ao_plugin_data.delay_fix-=2/((float)pl_eq.channels*(float)ao_plugin_data.rate);
  // Print some cool remark of what the plugin does
  printf("[pl_eq] Equalizer in use.\n");
  return 1;
}

// close plugin
static void uninit(){
}

// processes 'ao_plugin_data.len' bytes of 'data'
// called for every block of data
static int play(){
  uint16_t  	ci    	= pl_eq.channels; 	// Index for channels
  uint16_t	nch   	= pl_eq.channels;   	// Number of channels

  while(ci--){
    int16_t*	g	= pl_eq.g[ci]; 	// Gain factor 
    int16_t*	in    	= ((int16_t*)ao_plugin_data.data)+ci;
    int16_t*	out   	= ((int16_t*)ao_plugin_data.data)+ci;
    int16_t* 	end   	= in+ao_plugin_data.len/2; // Block loop end

    while(in < end){
      register int16_t k   = 0;	   	// Frequency band index
      register int32_t yt  = 0;    	// Total output from filters
      register int16_t x   = *in; 	// Current input sample
      in+=nch;

      // Run the filters 
      for(;k<pl_eq.K;k++){
	// Pointer to circular buffer wq
	register int16_t* wq = pl_eq.wq[ci][k];
#if 0
	// Calculate output from AR part of current filter
	register int32_t xt = (x*pl_eq.b[k][0]) >> 4;
	register int32_t w = xt + wq[0]*pl_eq.a[k][0] + wq[1]*pl_eq.a[k][1];
	// Calculate output form MA part of current filter
	yt+=(((w + wq[1]*pl_eq.b[k][1]) >> 10)*g[k]) >> 12;
	// Update circular buffer
	wq[1] = wq[0]; wq[0] = w >> 14;
      }	

      // Calculate output 
      *out=(int16_t)(yt+x);
#else
	// Calculate output from AR part of current filter
	register int32_t xt = (x*pl_eq.b[k][0]) / 48;
	register int32_t w = xt + wq[0]*pl_eq.a[k][0] + wq[1]*pl_eq.a[k][1];
	// Calculate output form MA part of current filter
	yt+=(((w + wq[1]*pl_eq.b[k][1]) >> 10)*g[k]) >> 12;
	// Update circular buffer
	wq[1] = wq[0]; wq[0] = w / 24576;
      }	

      // Calculate output 
      *out=(int16_t)(yt * 0.25 + x * 0.5);
#endif
      out+=nch;
    }
  }
  return 1;
}