Debian Codegraph @

plugins/LadspaEffect/swh/util/iir.h

raw
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