| 1 |
#ifndef IIR_H |
| 2 |
#define IIR_H |
| 3 |
|
| 4 |
#include <ladspa-util.h> |
| 5 |
|
| 6 |
/* header file for IIR framework */ |
| 7 |
|
| 8 |
typedef struct iir_stage iir_stage_t; |
| 9 |
typedef struct iirf iirf_t; |
| 10 |
// defines this to float if your brave and you'll see what happens.. |
| 11 |
#define gliirt float |
| 12 |
#define CLAMP(x,mi,ma) ( (x < mi) ? mi : ( (x>ma) ? ma : x)) |
| 13 |
#ifndef MIN |
| 14 |
#define MIN(a, b) ((a)<(b)?(a):(b)) |
| 15 |
#endif |
| 16 |
#ifndef MAX |
| 17 |
#define MAX(a, b) ((a)>(b)?(a):(b)) |
| 18 |
#endif |
| 19 |
/* alloc zeroed mem, malloc/calloc syntax. */ |
| 20 |
#define ALLOC(type) (type *)calloc(1, sizeof(type)) |
| 21 |
#define ALLOCN(n, type) (n == 0 ? NULL : (type *)calloc((n), sizeof(type))) |
| 22 |
|
| 23 |
/* supported filter modes by lib */ |
| 24 |
#define IIR_STAGE_LOWPASS 0 |
| 25 |
#define IIR_STAGE_HIGHPASS 1 |
| 26 |
#define IIR_STAGE_BANDPASS 2 |
| 27 |
#define IIR_STAGE_BANDPASS_A 3 |
| 28 |
|
| 29 |
struct iir_stage { |
| 30 |
int np; /* Number of poles */ |
| 31 |
int mode; /* Filter mode low/high/bandpass... */ |
| 32 |
int availst; /* Number of allocated stages */ |
| 33 |
int nstages; /* Number of active filterstages */ |
| 34 |
int na; /* number of a coefficients per stage */ |
| 35 |
int nb; /* number of b coefficients per stage */ |
| 36 |
gliirt fc; /* cutoff/center frequency */ |
| 37 |
gliirt bw; /* bandwidth for bandpass */ |
| 38 |
gliirt ppr; /* percent of ripple in passband */ |
| 39 |
gliirt spr; /* percent of ripple in stopband */ |
| 40 |
gliirt **coeff; /* Actual filter coefficients */ |
| 41 |
}; |
| 42 |
|
| 43 |
struct iirf { |
| 44 |
gliirt *iring; |
| 45 |
gliirt *oring; |
| 46 |
int ipos; |
| 47 |
int opos; |
| 48 |
}; |
| 49 |
// allocate ringbuffers for iir calculation |
| 50 |
static inline iirf_t* init_iirf_t(iir_stage_t* gt) { |
| 51 |
int i; |
| 52 |
iirf_t* iirf=ALLOCN(gt->availst,iirf_t); |
| 53 |
|
| 54 |
for(i=0;i<gt->availst;i++){ |
| 55 |
iirf[i].iring=ALLOCN(gt->na,gliirt); |
| 56 |
iirf[i].oring=ALLOCN(gt->nb+1,gliirt); |
| 57 |
iirf[i].ipos=0; |
| 58 |
iirf[i].opos=0; |
| 59 |
} |
| 60 |
|
| 61 |
return iirf; |
| 62 |
}; |
| 63 |
|
| 64 |
static inline void free_iirf_t(iirf_t* iirf, iir_stage_t* gt) { |
| 65 |
int i; |
| 66 |
for(i=0;i<gt->availst;i++){ |
| 67 |
if (iirf[i].iring) free(iirf[i].iring); |
| 68 |
if (iirf[i].oring) free(iirf[i].oring); |
| 69 |
} |
| 70 |
if (iirf) free(iirf); |
| 71 |
}; |
| 72 |
|
| 73 |
static inline void reset_iirf_t(iirf_t* iirf, iir_stage_t* gt, int n) { |
| 74 |
int i; |
| 75 |
for(i=0;i<n; ++i) { |
| 76 |
memset(iirf[i].iring, 0, sizeof(gliirt)*gt->na); |
| 77 |
memset(iirf[i].oring, 0, sizeof(gliirt)*(gt->nb+1)); |
| 78 |
} |
| 79 |
}; |
| 80 |
|
| 81 |
iir_stage_t *init_iir_stage(int mode, int nstages, int na, int nb); |
| 82 |
void combine_iir_stages(int mode, iir_stage_t* gt, iir_stage_t *first, iir_stage_t *second, int upf, int ups); |
| 83 |
void free_iir_stage(iir_stage_t *gt); |
| 84 |
void calc_2polebandpass(iirf_t* iirf, iir_stage_t* gt, float fc, float bw, long sample_rate); |
| 85 |
// for chebyshev we need iir stages with na=3, nb=2 |
| 86 |
// na are the forward coefficients |
| 87 |
// nb are the recursive coefficients |
| 88 |
int chebyshev(iirf_t* iirf, iir_stage_t* gt, int n, int mode, float fc, float pr); |
| 89 |
|
| 90 |
/* calculate butterworth coefficients |
| 91 |
* coefficient calculation taken from http://musicdsp.org/showArchiveComment.php?ArchiveID=38 |
| 92 |
* mode = 0 -> lowpass |
| 93 |
* mode !=0 -> highpass |
| 94 |
* |
| 95 |
* f -> cutoff frequency |
| 96 |
* r -> resonance |
| 97 |
*/ |
| 98 |
|
| 99 |
static inline void butterworth_stage(iir_stage_t *gt, int mode, float f, float r, long sample_rate) |
| 100 |
{ |
| 101 |
float c, a1, a2, a3, b1, b2; |
| 102 |
|
| 103 |
/* lowpass coefficients */ |
| 104 |
|
| 105 |
if (mode==0) { |
| 106 |
c = 1.0f / tan(M_PI * f / sample_rate ) ; |
| 107 |
|
| 108 |
a1 = 1.0f / ( 1.0f + r * c + c * c); |
| 109 |
a2 = 2.0f * a1; |
| 110 |
a3 = a1; |
| 111 |
b1 = -2.0f * ( 1.0f - c*c) * a1; |
| 112 |
b2 = -( 1.0f - r * c + c * c) * a1; |
| 113 |
} else { |
| 114 |
/* highpass coefficients */ |
| 115 |
c = tan(M_PI * f / sample_rate ); |
| 116 |
|
| 117 |
a1 = 1.0f / ( 1.0f + r * c + c * c); |
| 118 |
a2 = -2.0f*a1; |
| 119 |
a3 = a1; |
| 120 |
b1 = -2.0f * ( c*c - 1.0f) * a1; |
| 121 |
b2 = -( 1.0f - r * c + c * c) * a1; |
| 122 |
} |
| 123 |
|
| 124 |
gt->fc = f; |
| 125 |
gt->nstages = 1; |
| 126 |
|
| 127 |
gt->coeff[0][0] = a1; |
| 128 |
gt->coeff[0][1] = a2; |
| 129 |
gt->coeff[0][2] = a3; |
| 130 |
gt->coeff[0][3] = b1; |
| 131 |
gt->coeff[0][4] = b2; |
| 132 |
}; |
| 133 |
|
| 134 |
/* process function */ |
| 135 |
static inline void iir_process_buffer(iirf_t* iirf, iir_stage_t* gt, const float *indata, float *outdata, const long numSampsToProcess, int add) { |
| 136 |
long pos; |
| 137 |
int i,nb,nt,j,z,ipos,opos; |
| 138 |
|
| 139 |
if(gt->nstages==0) { |
| 140 |
if (indata==outdata) |
| 141 |
return; |
| 142 |
memcpy(outdata, indata, numSampsToProcess*sizeof(float)); |
| 143 |
return; |
| 144 |
} |
| 145 |
|
| 146 |
nb=gt->nb+1; |
| 147 |
nt=gt->na+gt->nb; |
| 148 |
|
| 149 |
ipos = iirf[0].ipos; |
| 150 |
opos = iirf[0].opos; |
| 151 |
|
| 152 |
if (add==0) |
| 153 |
for(pos=0; pos<numSampsToProcess; pos++) { |
| 154 |
iirf[0].iring[ipos]=indata[pos]; |
| 155 |
for(i=0;i<gt->nstages;i++){ |
| 156 |
if (i>0) |
| 157 |
iirf[i].iring[ipos]=iirf[i-1].oring[opos]; |
| 158 |
iirf[i].oring[opos]=0.0; |
| 159 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 160 |
z=ipos; |
| 161 |
for(j=0;j<gt->na;j++){ |
| 162 |
if(z==-1) |
| 163 |
z=gt->na-1; |
| 164 |
iirf[i].oring[opos]+=gt->coeff[i][j]*iirf[i].iring[z--]; |
| 165 |
} |
| 166 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 167 |
z=opos-1; |
| 168 |
for(j=gt->na;j<nt;j++){ |
| 169 |
if (z==-1) |
| 170 |
z=gt->nb; |
| 171 |
iirf[i].oring[opos]+=gt->coeff[i][j]*iirf[i].oring[z--]; |
| 172 |
} |
| 173 |
} |
| 174 |
/* No matter if we process it in place */ |
| 175 |
outdata[pos]=(float)iirf[gt->nstages-1].oring[opos]; |
| 176 |
|
| 177 |
/* Adjust ringbuffers */ |
| 178 |
ipos++; |
| 179 |
if (ipos==gt->na) |
| 180 |
ipos=0; |
| 181 |
opos++; |
| 182 |
if (opos==nb) |
| 183 |
opos=0; |
| 184 |
} |
| 185 |
else |
| 186 |
for(pos=0; pos<numSampsToProcess; pos++) { |
| 187 |
iirf[0].iring[ipos]=indata[pos]; |
| 188 |
for(i=0;i<gt->nstages;i++){ |
| 189 |
if (i>0) |
| 190 |
iirf[i].iring[ipos]=iirf[i-1].oring[opos]; |
| 191 |
iirf[i].oring[opos]=0.0; |
| 192 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 193 |
z=ipos; |
| 194 |
for(j=0;j<gt->na;j++){ |
| 195 |
if(z==-1) |
| 196 |
z=gt->na-1; |
| 197 |
iirf[i].oring[opos]+=gt->coeff[i][j]*iirf[i].iring[z--]; |
| 198 |
} |
| 199 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 200 |
z=opos-1; |
| 201 |
for(j=gt->na;j<nt;j++){ |
| 202 |
if (z==-1) |
| 203 |
z=gt->nb; |
| 204 |
iirf[i].oring[opos]+=gt->coeff[i][j]*iirf[i].oring[z--]; |
| 205 |
} |
| 206 |
} |
| 207 |
/* Now it matters if we process it in place */ |
| 208 |
outdata[pos]+=(float)iirf[gt->nstages-1].oring[opos]; |
| 209 |
|
| 210 |
/* Adjust ringbuffers */ |
| 211 |
ipos++; |
| 212 |
if (ipos==gt->na) |
| 213 |
ipos=0; |
| 214 |
opos++; |
| 215 |
if (opos==nb) |
| 216 |
opos=0; |
| 217 |
} |
| 218 |
|
| 219 |
iirf[0].ipos = ipos; |
| 220 |
iirf[0].opos = opos; |
| 221 |
}; |
| 222 |
|
| 223 |
/* process function for 3a and 2b coeffs */ |
| 224 |
static inline void iir_process_buffer_1s_5(iirf_t* iirf, iir_stage_t* gt, const float *indata, |
| 225 |
float *outdata, const long numSampsToProcess, int add) { |
| 226 |
long pos; |
| 227 |
|
| 228 |
if (add==0) |
| 229 |
for(pos=0; pos<numSampsToProcess; pos++) { |
| 230 |
iirf[0].iring[0]=iirf[0].iring[1]; |
| 231 |
iirf[0].iring[1]=iirf[0].iring[2]; |
| 232 |
iirf[0].iring[2]=indata[pos]; |
| 233 |
iirf[0].oring[0]=iirf[0].oring[1]; |
| 234 |
iirf[0].oring[1]=iirf[0].oring[2]; |
| 235 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 236 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 237 |
iirf[0].oring[2] = flush_to_zero(gt->coeff[0][0]*iirf[0].iring[2] + |
| 238 |
gt->coeff[0][1]*iirf[0].iring[1] + |
| 239 |
gt->coeff[0][2]*iirf[0].iring[0] + |
| 240 |
gt->coeff[0][3]*iirf[0].oring[1] + |
| 241 |
gt->coeff[0][4]*iirf[0].oring[0]); |
| 242 |
outdata[pos]=(float)iirf[0].oring[2]; |
| 243 |
} |
| 244 |
else |
| 245 |
for(pos=0; pos<numSampsToProcess; pos++) { |
| 246 |
iirf[0].iring[0]=iirf[0].iring[1]; |
| 247 |
iirf[0].iring[1]=iirf[0].iring[2]; |
| 248 |
iirf[0].iring[2]=indata[pos]; |
| 249 |
iirf[0].oring[0]=iirf[0].oring[1]; |
| 250 |
iirf[0].oring[1]=iirf[0].oring[2]; |
| 251 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 252 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 253 |
outdata[pos] += iirf[0].oring[2] = flush_to_zero(gt->coeff[0][0]*iirf[0].iring[2] + |
| 254 |
gt->coeff[0][1]*iirf[0].iring[1] + |
| 255 |
gt->coeff[0][2]*iirf[0].iring[0] + |
| 256 |
gt->coeff[0][3]*iirf[0].oring[1] + |
| 257 |
gt->coeff[0][4]*iirf[0].oring[0]); |
| 258 |
} |
| 259 |
}; |
| 260 |
|
| 261 |
/* process function */ |
| 262 |
static inline void iir_process_buffer_ns_5(iirf_t* iirf, iir_stage_t* gt, const float *indata, float *outdata, const long numSampsToProcess, int add) { |
| 263 |
long pos; |
| 264 |
int i; |
| 265 |
|
| 266 |
if (add==0) |
| 267 |
for(pos=0; pos<numSampsToProcess; pos++) { |
| 268 |
iirf[0].iring[0]=iirf[0].iring[1]; |
| 269 |
iirf[0].iring[1]=iirf[0].iring[2]; |
| 270 |
iirf[0].iring[2]=indata[pos]; |
| 271 |
|
| 272 |
iirf[0].oring[0]=iirf[0].oring[1]; |
| 273 |
iirf[0].oring[1]=iirf[0].oring[2]; |
| 274 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 275 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 276 |
iirf[0].oring[2] = flush_to_zero(gt->coeff[0][0]*iirf[0].iring[2] + |
| 277 |
gt->coeff[0][1]*iirf[0].iring[1] + |
| 278 |
gt->coeff[0][2]*iirf[0].iring[0] + |
| 279 |
gt->coeff[0][3]*iirf[0].oring[1] + |
| 280 |
gt->coeff[0][4]*iirf[0].oring[0]); |
| 281 |
|
| 282 |
for(i=1;i<gt->nstages;i++){ |
| 283 |
iirf[i].iring[0]=iirf[i].iring[1]; |
| 284 |
iirf[i].iring[1]=iirf[i].iring[2]; |
| 285 |
iirf[i].iring[2]=iirf[i-1].oring[2]; |
| 286 |
|
| 287 |
iirf[i].oring[0]=iirf[i].oring[1]; |
| 288 |
iirf[i].oring[1]=iirf[i].oring[2]; |
| 289 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 290 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 291 |
iirf[i].oring[2] = |
| 292 |
flush_to_zero(gt->coeff[i][0]*iirf[i].iring[2] + |
| 293 |
gt->coeff[i][1]*iirf[i].iring[1] + |
| 294 |
gt->coeff[i][2]*iirf[i].iring[0] + |
| 295 |
gt->coeff[i][3]*iirf[i].oring[1] + |
| 296 |
gt->coeff[i][4]*iirf[i].oring[0]); |
| 297 |
} |
| 298 |
/* No matter if we process it in place */ |
| 299 |
outdata[pos]=(float)iirf[gt->nstages-1].oring[2]; |
| 300 |
} |
| 301 |
else |
| 302 |
for(pos=0; pos<numSampsToProcess; pos++) { |
| 303 |
iirf[0].iring[0]=iirf[0].iring[1]; |
| 304 |
iirf[0].iring[1]=iirf[0].iring[2]; |
| 305 |
iirf[0].iring[2]=indata[pos]; |
| 306 |
|
| 307 |
iirf[0].oring[0]=iirf[0].oring[1]; |
| 308 |
iirf[0].oring[1]=iirf[0].oring[2]; |
| 309 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 310 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 311 |
iirf[0].oring[2] = flush_to_zero(gt->coeff[0][0]*iirf[0].iring[2] + |
| 312 |
gt->coeff[0][1]*iirf[0].iring[1] + |
| 313 |
gt->coeff[0][2]*iirf[0].iring[0] + |
| 314 |
gt->coeff[0][3]*iirf[0].oring[1] + |
| 315 |
gt->coeff[0][4]*iirf[0].oring[0]); |
| 316 |
|
| 317 |
for(i=1;i<gt->nstages;i++){ |
| 318 |
iirf[i].iring[0]=iirf[i].iring[1]; |
| 319 |
iirf[i].iring[1]=iirf[i].iring[2]; |
| 320 |
iirf[i].iring[2]=iirf[i-1].oring[2]; |
| 321 |
|
| 322 |
iirf[i].oring[0]=iirf[i].oring[1]; |
| 323 |
iirf[i].oring[1]=iirf[i].oring[2]; |
| 324 |
/* y[n]=a0*x[n]+a1*x[n-1]+... */ |
| 325 |
/* y[n]=y[n]+b1*y[n-1]+b2*y[n-2]+... */ |
| 326 |
iirf[i].oring[2] = flush_to_zero( |
| 327 |
gt->coeff[i][0]*iirf[i].iring[2] + |
| 328 |
gt->coeff[i][1]*iirf[i].iring[1] + |
| 329 |
gt->coeff[i][2]*iirf[i].iring[0] + |
| 330 |
gt->coeff[i][3]*iirf[i].oring[1] + |
| 331 |
gt->coeff[i][4]*iirf[i].oring[0]); |
| 332 |
} |
| 333 |
/* No matter if we process it in place */ |
| 334 |
outdata[pos]+=(float)iirf[gt->nstages-1].oring[2]; |
| 335 |
} |
| 336 |
}; |
| 337 |
|
| 338 |
#endif |