diff options
-rw-r--r-- | apps/eq.c | 129 | ||||
-rw-r--r-- | apps/eq.h | 2 |
2 files changed, 75 insertions, 56 deletions
@@ -34,7 +34,7 @@ | |||
34 | 34 | ||
35 | #define DIV64(x, y, z) (long)(((long long)(x) << (z))/(y)) | 35 | #define DIV64(x, y, z) (long)(((long long)(x) << (z))/(y)) |
36 | /* This macro requires the EMAC unit to be in fractional mode | 36 | /* This macro requires the EMAC unit to be in fractional mode |
37 | when the coef generator routines are called. If this can't be guaranteeed, | 37 | when the coef generator routines are called. If this can't be guaranteed, |
38 | then add "&& 0" below. This will use a slower coef calculation on Coldfire. | 38 | then add "&& 0" below. This will use a slower coef calculation on Coldfire. |
39 | */ | 39 | */ |
40 | #if defined(CPU_COLDFIRE) && !defined(SIMULATOR) | 40 | #if defined(CPU_COLDFIRE) && !defined(SIMULATOR) |
@@ -188,12 +188,12 @@ static long dbtoA(long db) | |||
188 | } | 188 | } |
189 | 189 | ||
190 | /* Calculate first order shelving filter coefficients. | 190 | /* Calculate first order shelving filter coefficients. |
191 | cutoff is a value from 0 to 0x80000000, where 0 represents 0 hz and | 191 | * Note that the filter is not compatible with the eq_filter routine. |
192 | 0x80000000 represents nyquist (samplerate/2). | 192 | * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and |
193 | ad is gain at 0 hz, and an is gain at Nyquist frequency. Both are s3.27 | 193 | * 0x80000000 represents the Nyquist frequency (samplerate/2). |
194 | format. | 194 | * @param ad gain at 0 Hz. s3.27 fixed point. |
195 | c is a pointer where the coefs will be stored. The coefs are s0.31 format. | 195 | * @param an gain at Nyquist frequency. s3.27 fixed point. |
196 | Note that the filter is not compatible with the eq_filter routine. | 196 | * @param c pointer to coefficient storage. The coefs are s0.31 format. |
197 | */ | 197 | */ |
198 | void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c) | 198 | void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c) |
199 | { | 199 | { |
@@ -215,13 +215,14 @@ void filter_bishelf_coefs(unsigned long cutoff, long ad, long an, int32_t *c) | |||
215 | c[2] = -DIV64(a1, a0, 31); | 215 | c[2] = -DIV64(a1, a0, 31); |
216 | } | 216 | } |
217 | 217 | ||
218 | /* Calculate second order section peaking filter coefficients. | 218 | /** |
219 | cutoff is a value from 0 to 0x80000000, where 0 represents 0 hz and | 219 | * Calculate second order section peaking filter coefficients. |
220 | 0x80000000 represents nyquist (samplerate/2). | 220 | * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and |
221 | Q is an unsigned 16.16 fixed point number, lower bound is artificially set | 221 | * 0x80000000 represents the Nyquist frequency (samplerate/2). |
222 | at 0.5. | 222 | * @param Q 16.16 fixed point value describing Q factor. Lower bound |
223 | db is s15.16 fixed point and describes gain/attenuation at peak freq. | 223 | * is artificially set at 0.5. |
224 | c is a pointer where the coefs will be stored. | 224 | * @param db s15.16 fixed point value describing gain/attenuation at peak freq. |
225 | * @param c pointer to coefficient storage. Coefficients are s3.28 format. | ||
225 | */ | 226 | */ |
226 | void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | 227 | void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) |
227 | { | 228 | { |
@@ -232,72 +233,90 @@ void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | |||
232 | int32_t a0, a1, a2; /* these are all s3.28 format */ | 233 | int32_t a0, a1, a2; /* these are all s3.28 format */ |
233 | int32_t b0, b1, b2; | 234 | int32_t b0, b1, b2; |
234 | 235 | ||
235 | /* possible numerical ranges listed after each coef */ | 236 | /* possible numerical ranges are in comments by each coef */ |
236 | b0 = one + FRACMUL(alpha, A); /* [1.25..5] */ | 237 | b0 = one + FRACMUL(alpha, A); /* [1 .. 5] */ |
237 | b1 = a1 = -2*(cc >> 3); /* [-2..2] */ | 238 | b1 = a1 = -2*(cc >> 3); /* [-2 .. 2] */ |
238 | b2 = one - FRACMUL(alpha, A); /* [-3..0.75] */ | 239 | b2 = one - FRACMUL(alpha, A); /* [-3 .. 1] */ |
239 | a0 = one + DIV64(alpha, A, 27); /* [1.25..5] */ | 240 | a0 = one + DIV64(alpha, A, 27); /* [1 .. 5] */ |
240 | a2 = one - DIV64(alpha, A, 27); /* [-3..0.75] */ | 241 | a2 = one - DIV64(alpha, A, 27); /* [-3 .. 1] */ |
241 | 242 | ||
242 | c[0] = DIV64(b0, a0, 28); | 243 | c[0] = DIV64(b0, a0, 28); /* [0.25 .. 4] */ |
243 | c[1] = DIV64(b1, a0, 28); | 244 | c[1] = DIV64(b1, a0, 28); /* [-2 .. 2] */ |
244 | c[2] = DIV64(b2, a0, 28); | 245 | c[2] = DIV64(b2, a0, 28); /* [-2.4 .. 1] */ |
245 | c[3] = DIV64(-a1, a0, 28); | 246 | c[3] = DIV64(-a1, a0, 28); /* [-2 .. 2] */ |
246 | c[4] = DIV64(-a2, a0, 28); | 247 | c[4] = DIV64(-a2, a0, 28); /* [-0.6 .. 1] */ |
247 | } | 248 | } |
248 | 249 | ||
249 | /* Calculate coefficients for lowshelf filter */ | 250 | /** |
251 | * Calculate coefficients for lowshelf filter. Parameters are as for | ||
252 | * eq_pk_coefs, but the coefficient format is s5.26 fixed point. | ||
253 | */ | ||
250 | void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | 254 | void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) |
251 | { | 255 | { |
252 | long cs; | 256 | long cs; |
253 | const long one = 1 << 24; /* s7.24 */ | 257 | const long one = 1 << 25; /* s6.25 */ |
254 | const long A = dbtoA(db); | 258 | const long A = dbtoA(db); |
255 | const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */ | 259 | const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */ |
256 | const long ap1 = (A >> 5) + one; | 260 | const long ap1 = (A >> 4) + one; |
257 | const long am1 = (A >> 5) - one; | 261 | const long am1 = (A >> 4) - one; |
258 | const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); | 262 | const long twosqrtalpha = 2*FRACMUL(fsqrt(A >> 3, 26), alpha); |
259 | int32_t a0, a1, a2; /* these are all s7.24 format */ | 263 | int32_t a0, a1, a2; /* these are all s6.25 format */ |
260 | int32_t b0, b1, b2; | 264 | int32_t b0, b1, b2; |
261 | 265 | ||
266 | /* [0.1 .. 40] */ | ||
262 | b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2; | 267 | b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2; |
263 | b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3; | 268 | /* [-16 .. 63.4] */ |
264 | b2 = FRACMUL(A, ap1 - FRACMUL(am1, cs) - twosqrtalpha) << 2; | 269 | b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3; |
265 | a0 = ap1 + FRACMUL(am1, cs) + twosqrtalpha; | 270 | /* [0 .. 31.7] */ |
266 | a1 = -2*((am1 + FRACMUL(ap1, cs))); | 271 | b2 = FRACMUL(A, ap1 - FRACMUL(am1, cs) - twosqrtalpha) << 2; |
272 | /* [0.5 .. 10] */ | ||
273 | a0 = ap1 + FRACMUL(am1, cs) + twosqrtalpha; | ||
274 | /* [-16 .. 4] */ | ||
275 | a1 = -2*((am1 + FRACMUL(ap1, cs))); | ||
276 | /* [0 .. 8] */ | ||
267 | a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha; | 277 | a2 = ap1 + FRACMUL(am1, cs) - twosqrtalpha; |
268 | 278 | ||
269 | c[0] = DIV64(b0, a0, 24); | 279 | c[0] = DIV64(b0, a0, 26); /* [0.06 .. 15.9] */ |
270 | c[1] = DIV64(b1, a0, 24); | 280 | c[1] = DIV64(b1, a0, 26); /* [-2 .. 31.7] */ |
271 | c[2] = DIV64(b2, a0, 24); | 281 | c[2] = DIV64(b2, a0, 26); /* [0 .. 15.9] */ |
272 | c[3] = DIV64(-a1, a0, 24); | 282 | c[3] = DIV64(-a1, a0, 26); /* [-2 .. 2] */ |
273 | c[4] = DIV64(-a2, a0, 24); | 283 | c[4] = DIV64(-a2, a0, 26); /* [0 .. 1] */ |
274 | } | 284 | } |
275 | 285 | ||
276 | /* Calculate coefficients for highshelf filter */ | 286 | /** |
287 | * Calculate coefficients for highshelf filter. Parameters are as for | ||
288 | * eq_pk_coefs, but the coefficient format is s5.26 fixed point. | ||
289 | */ | ||
277 | void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | 290 | void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) |
278 | { | 291 | { |
279 | long cs; | 292 | long cs; |
280 | const long one = 1 << 24; /* s7.24 */ | 293 | const int one = 1 << 25; /* s6.25 */ |
281 | const long A = dbtoA(db); | 294 | const int A = dbtoA(db); |
282 | const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */ | 295 | const int alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */ |
283 | const long ap1 = (A >> 5) + one; | 296 | const int ap1 = (A >> 4) + one; |
284 | const long am1 = (A >> 5) - one; | 297 | const int am1 = (A >> 4) - one; |
285 | const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); | 298 | const int twosqrtalpha = 2*FRACMUL(fsqrt(A >> 3, 26), alpha); |
286 | int32_t a0, a1, a2; /* these are all s7.24 format */ | 299 | int32_t a0, a1, a2; /* these are all s6.25 format */ |
287 | int32_t b0, b1, b2; | 300 | int32_t b0, b1, b2; |
288 | 301 | ||
302 | /* [0.1 .. 40] */ | ||
289 | b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2; | 303 | b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2; |
304 | /* [-63.5 .. 16] */ | ||
290 | b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3; | 305 | b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3; |
306 | /* [0 .. 32] */ | ||
291 | b2 = FRACMUL(A, ap1 + FRACMUL(am1, cs) - twosqrtalpha) << 2; | 307 | b2 = FRACMUL(A, ap1 + FRACMUL(am1, cs) - twosqrtalpha) << 2; |
308 | /* [0.5 .. 10] */ | ||
292 | a0 = ap1 - FRACMUL(am1, cs) + twosqrtalpha; | 309 | a0 = ap1 - FRACMUL(am1, cs) + twosqrtalpha; |
310 | /* [-4 .. 16] */ | ||
293 | a1 = 2*((am1 - FRACMUL(ap1, cs))); | 311 | a1 = 2*((am1 - FRACMUL(ap1, cs))); |
312 | /* [0 .. 8] */ | ||
294 | a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha; | 313 | a2 = ap1 - FRACMUL(am1, cs) - twosqrtalpha; |
295 | 314 | ||
296 | c[0] = DIV64(b0, a0, 24); | 315 | c[0] = DIV64(b0, a0, 26); /* [0 .. 16] */ |
297 | c[1] = DIV64(b1, a0, 24); | 316 | c[1] = DIV64(b1, a0, 26); /* [-31.7 .. 2] */ |
298 | c[2] = DIV64(b2, a0, 24); | 317 | c[2] = DIV64(b2, a0, 26); /* [0 .. 16] */ |
299 | c[3] = DIV64(-a1, a0, 24); | 318 | c[3] = DIV64(-a1, a0, 26); /* [-2 .. 2] */ |
300 | c[4] = DIV64(-a2, a0, 24); | 319 | c[4] = DIV64(-a2, a0, 26); /* [0 .. 1] */ |
301 | } | 320 | } |
302 | 321 | ||
303 | #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) || defined(SIMULATOR) | 322 | #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM)) || defined(SIMULATOR) |
@@ -26,7 +26,7 @@ | |||
26 | and need to be changed when they change. | 26 | and need to be changed when they change. |
27 | */ | 27 | */ |
28 | #define EQ_PEAK_SHIFT 4 | 28 | #define EQ_PEAK_SHIFT 4 |
29 | #define EQ_SHELF_SHIFT 8 | 29 | #define EQ_SHELF_SHIFT 6 |
30 | 30 | ||
31 | struct eqfilter { | 31 | struct eqfilter { |
32 | int32_t coefs[5]; /* Order is b0, b1, b2, a1, a2 */ | 32 | int32_t coefs[5]; /* Order is b0, b1, b2, a1, a2 */ |