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