1 /* -*- linux-c -*-
2 Copyright (C) 2004 Tom Szilagyi
3
4 This program is free software; you can redistribute it and/or modify
5 it under the terms of the GNU General Public License as published by
6 the Free Software Foundation; either version 2 of the License, or
7 (at your option) any later version.
8
9 This program is distributed in the hope that it will be useful,
10 but WITHOUT ANY WARRANTY; without even the implied warranty of
11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 GNU General Public License for more details.
13
14 You should have received a copy of the GNU General Public License
15 along with this program; if not, write to the Free Software
16 Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
17
18 $Id: tap_utils.h,v 1.6 2009/08/17 11:16:19 tszilagyi Exp $
19 */
20 #ifndef _ISOC99_SOURCE
21 #define _ISOC99_SOURCE
22 #endif
23
24 #include <inttypes.h>
25
26 #ifndef M_PI
27 #define M_PI 3.14159265358979323846264338327
28 #endif
29
30
31
32 /* push a sample into a ringbuffer and return the sample falling out */
33 static inline
34 LADSPA_Data
push_buffer(LADSPA_Data insample,LADSPA_Data * buffer,unsigned long buflen,unsigned long * pos)35 push_buffer(LADSPA_Data insample, LADSPA_Data * buffer,
36 unsigned long buflen, unsigned long * pos) {
37
38 LADSPA_Data outsample;
39
40 outsample = buffer[*pos];
41 buffer[(*pos)++] = insample;
42
43 if (*pos >= buflen)
44 *pos = 0;
45
46 return outsample;
47 }
48
49
50 /* read a value from a ringbuffer.
51 * n == 0 returns the oldest sample from the buffer.
52 * n == buflen-1 returns the sample written to the buffer
53 * at the last push_buffer call.
54 * n must not exceed buflen-1, or your computer will explode.
55 */
56 static inline
57 LADSPA_Data
read_buffer(LADSPA_Data * buffer,unsigned long buflen,unsigned long pos,unsigned long n)58 read_buffer(LADSPA_Data * buffer, unsigned long buflen,
59 unsigned long pos, unsigned long n) {
60
61 while (n + pos >= buflen)
62 n -= buflen;
63 return buffer[n + pos];
64 }
65
66
67 /* overwrites a value in a ringbuffer, but pos stays the same.
68 * n == 0 overwrites the oldest sample pushed in the buffer.
69 * n == buflen-1 overwrites the sample written to the buffer
70 * at the last push_buffer call.
71 * n must not exceed buflen-1, or your computer... you know.
72 */
73 static inline
74 void
write_buffer(LADSPA_Data insample,LADSPA_Data * buffer,unsigned long buflen,unsigned long pos,unsigned long n)75 write_buffer(LADSPA_Data insample, LADSPA_Data * buffer, unsigned long buflen,
76 unsigned long pos, unsigned long n) {
77
78 while (n + pos >= buflen)
79 n -= buflen;
80 buffer[n + pos] = insample;
81 }
82
83
84
85
86 /* Please note that the majority of the definitions and helper
87 functions below have been derived from the source code of Steve
88 Harris's SWH plugins (particularly from the "biquad.h" file). While I
89 give him credit for his excellent work, I reserve myself to be blamed
90 for any bugs or malfunction. */
91
92
93 #define db2lin(x) ((x) > -90.0f ? powf(10.0f, (x) * 0.05f) : 0.0f)
94
95 #define ABS(x) (x)>0.0f?(x):-1.0f*(x)
96
97
98 #define LN_2_2 0.34657359f
99 #define LIMIT(v,l,u) ((v)<(l)?(l):((v)>(u)?(u):(v)))
100
101 #define BIQUAD_TYPE float
102 typedef BIQUAD_TYPE bq_t;
103
104
105 /* Biquad filter (adapted from lisp code by Eli Brandt,
106 http://www.cs.cmu.edu/~eli/) */
107
108 /* The prev. comment has been preserved from Steve Harris's biquad.h */
109
110 typedef struct {
111 bq_t a1;
112 bq_t a2;
113 bq_t b0;
114 bq_t b1;
115 bq_t b2;
116 bq_t x1;
117 bq_t x2;
118 bq_t y1;
119 bq_t y2;
120 } biquad;
121
122
biquad_init(biquad * f)123 static inline void biquad_init(biquad *f) {
124
125 f->x1 = 0.0f;
126 f->x2 = 0.0f;
127 f->y1 = 0.0f;
128 f->y2 = 0.0f;
129 }
130
131
132 static inline
133 void
eq_set_params(biquad * f,bq_t fc,bq_t gain,bq_t bw,bq_t fs)134 eq_set_params(biquad *f, bq_t fc, bq_t gain, bq_t bw, bq_t fs) {
135
136 bq_t w = 2.0f * M_PI * LIMIT(fc, 1.0f, fs/2.0f) / fs;
137 bq_t cw = cosf(w);
138 bq_t sw = sinf(w);
139 bq_t J = pow(10.0f, gain * 0.025f);
140 bq_t g = sw * sinhf(LN_2_2 * LIMIT(bw, 0.0001f, 4.0f) * w / sw);
141 bq_t a0r = 1.0f / (1.0f + (g / J));
142
143 f->b0 = (1.0f + (g * J)) * a0r;
144 f->b1 = (-2.0f * cw) * a0r;
145 f->b2 = (1.0f - (g * J)) * a0r;
146 f->a1 = -(f->b1);
147 f->a2 = ((g / J) - 1.0f) * a0r;
148 }
149
150
lp_set_params(biquad * f,bq_t fc,bq_t bw,bq_t fs)151 static inline void lp_set_params(biquad *f, bq_t fc, bq_t bw, bq_t fs) {
152 bq_t omega = 2.0 * M_PI * fc/fs;
153 bq_t sn = sin(omega);
154 bq_t cs = cos(omega);
155 bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
156
157 const float a0r = 1.0 / (1.0 + alpha);
158 #if 0
159 b0 = (1 - cs) /2;
160 b1 = 1 - cs;
161 b2 = (1 - cs) /2;
162 a0 = 1 + alpha;
163 a1 = -2 * cs;
164 a2 = 1 - alpha;
165 #endif
166 f->b0 = a0r * (1.0 - cs) * 0.5;
167 f->b1 = a0r * (1.0 - cs);
168 f->b2 = a0r * (1.0 - cs) * 0.5;
169 f->a1 = a0r * (2.0 * cs);
170 f->a2 = a0r * (alpha - 1.0);
171 }
172
173
174 static inline
175 void
hp_set_params(biquad * f,bq_t fc,bq_t bw,bq_t fs)176 hp_set_params(biquad *f, bq_t fc, bq_t bw, bq_t fs)
177 {
178 bq_t omega = 2.0 * M_PI * fc/fs;
179 bq_t sn = sin(omega);
180 bq_t cs = cos(omega);
181 bq_t alpha = sn * sinh(M_LN2 / 2.0 * bw * omega / sn);
182
183 const float a0r = 1.0 / (1.0 + alpha);
184
185 #if 0
186 b0 = (1 + cs) /2;
187 b1 = -(1 + cs);
188 b2 = (1 + cs) /2;
189 a0 = 1 + alpha;
190 a1 = -2 * cs;
191 a2 = 1 - alpha;
192 #endif
193 f->b0 = a0r * (1.0 + cs) * 0.5;
194 f->b1 = a0r * -(1.0 + cs);
195 f->b2 = a0r * (1.0 + cs) * 0.5;
196 f->a1 = a0r * (2.0 * cs);
197 f->a2 = a0r * (alpha - 1.0);
198 }
199
200
201 static inline
202 void
ls_set_params(biquad * f,bq_t fc,bq_t gain,bq_t slope,bq_t fs)203 ls_set_params(biquad *f, bq_t fc, bq_t gain, bq_t slope, bq_t fs)
204 {
205
206 bq_t w = 2.0f * M_PI * LIMIT(fc, 1.0, fs/2.0) / fs;
207 bq_t cw = cosf(w);
208 bq_t sw = sinf(w);
209 bq_t A = powf(10.0f, gain * 0.025f);
210 bq_t b = sqrt(((1.0f + A * A) / LIMIT(slope, 0.0001f, 1.0f)) - ((A -
211 1.0f) * (A - 1.0)));
212 bq_t apc = cw * (A + 1.0f);
213 bq_t amc = cw * (A - 1.0f);
214 bq_t bs = b * sw;
215 bq_t a0r = 1.0f / (A + 1.0f + amc + bs);
216
217 f->b0 = a0r * A * (A + 1.0f - amc + bs);
218 f->b1 = a0r * 2.0f * A * (A - 1.0f - apc);
219 f->b2 = a0r * A * (A + 1.0f - amc - bs);
220 f->a1 = a0r * 2.0f * (A - 1.0f + apc);
221 f->a2 = a0r * (-A - 1.0f - amc + bs);
222 }
223
224
225 static inline
226 void
hs_set_params(biquad * f,bq_t fc,bq_t gain,bq_t slope,bq_t fs)227 hs_set_params(biquad *f, bq_t fc, bq_t gain, bq_t slope, bq_t fs) {
228
229 bq_t w = 2.0f * M_PI * LIMIT(fc, 1.0, fs/2.0) / fs;
230 bq_t cw = cosf(w);
231 bq_t sw = sinf(w);
232 bq_t A = powf(10.0f, gain * 0.025f);
233 bq_t b = sqrt(((1.0f + A * A) / LIMIT(slope, 0.0001f, 1.0f)) - ((A -
234 1.0f) * (A - 1.0f)));
235 bq_t apc = cw * (A + 1.0f);
236 bq_t amc = cw * (A - 1.0f);
237 bq_t bs = b * sw;
238 bq_t a0r = 1.0f / (A + 1.0f - amc + bs);
239
240 f->b0 = a0r * A * (A + 1.0f + amc + bs);
241 f->b1 = a0r * -2.0f * A * (A - 1.0f + apc);
242 f->b2 = a0r * A * (A + 1.0f + amc - bs);
243 f->a1 = a0r * -2.0f * (A - 1.0f - apc);
244 f->a2 = a0r * (-A - 1.0f + amc + bs);
245 }
246
247
248 static inline
249 bq_t
biquad_run(biquad * f,bq_t x)250 biquad_run(biquad *f, bq_t x) {
251
252 union {
253 bq_t y;
254 uint32_t y_int;
255 } u;
256
257 u.y = f->b0 * x + f->b1 * f->x1 + f->b2 * f->x2
258 + f->a1 * f->y1 + f->a2 * f->y2;
259 if ((u.y_int & 0x7f800000) == 0)
260 u.y = 0.0f;
261 f->x2 = f->x1;
262 f->x1 = x;
263 f->y2 = f->y1;
264 f->y1 = u.y;
265
266 return u.y;
267 }
268