1 /***************************************************************************
3 * Open \______ \ ____ ____ | | _\_ |__ _______ ___
4 * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ /
5 * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < <
6 * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \
10 * Copyright (C) 2006-2007 Thom Johansen
12 * This program is free software; you can redistribute it and/or
13 * modify it under the terms of the GNU General Public License
14 * as published by the Free Software Foundation; either version 2
15 * of the License, or (at your option) any later version.
17 * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY
18 * KIND, either express or implied.
20 ****************************************************************************/
24 #include "fixedpoint.h"
26 #include "replaygain.h"
29 * Calculate first order shelving filter. Filter is not directly usable by the
30 * eq_filter() function.
31 * @param cutoff shelf midpoint frequency. See eq_pk_coefs for format.
32 * @param A decibel value multiplied by ten, describing gain/attenuation of
33 * shelf. Max value is 24 dB.
34 * @param low true for low-shelf filter, false for high-shelf filter.
35 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
37 void filter_shelf_coefs(unsigned long cutoff
, long A
, bool low
, int32_t *c
)
40 int32_t b0
, b1
, a0
, a1
; /* s3.28 */
41 const long g
= get_replaygain_int(A
*5) << 4; /* 10^(db/40), s3.28 */
43 sin
= fsincos(cutoff
/2, &cos
);
45 const int32_t sin_div_g
= DIV64(sin
, g
, 25);
47 b0
= FRACMUL(sin
, g
) + cos
; /* 0.25 .. 4.10 */
48 b1
= FRACMUL(sin
, g
) - cos
; /* -1 .. 3.98 */
49 a0
= sin_div_g
+ cos
; /* 0.25 .. 4.10 */
50 a1
= sin_div_g
- cos
; /* -1 .. 3.98 */
52 const int32_t cos_div_g
= DIV64(cos
, g
, 25);
54 b0
= sin
+ FRACMUL(cos
, g
); /* 0.25 .. 4.10 */
55 b1
= sin
- FRACMUL(cos
, g
); /* -3.98 .. 1 */
56 a0
= sin
+ cos_div_g
; /* 0.25 .. 4.10 */
57 a1
= sin
- cos_div_g
; /* -3.98 .. 1 */
60 const int32_t rcp_a0
= DIV64(1, a0
, 57); /* 0.24 .. 3.98, s2.29 */
61 *c
++ = FRACMUL_SHL(b0
, rcp_a0
, 1); /* 0.063 .. 15.85 */
62 *c
++ = FRACMUL_SHL(b1
, rcp_a0
, 1); /* -15.85 .. 15.85 */
63 *c
++ = -FRACMUL_SHL(a1
, rcp_a0
, 1); /* -1 .. 1 */
66 #ifdef HAVE_SW_TONE_CONTROLS
68 * Calculate second order section filter consisting of one low-shelf and one
70 * @param cutoff_low low-shelf midpoint frequency. See eq_pk_coefs for format.
71 * @param cutoff_high high-shelf midpoint frequency.
72 * @param A_low decibel value multiplied by ten, describing gain/attenuation of
73 * low-shelf part. Max value is 24 dB.
74 * @param A_high decibel value multiplied by ten, describing gain/attenuation of
75 * high-shelf part. Max value is 24 dB.
76 * @param A decibel value multiplied by ten, describing additional overall gain.
77 * @param c pointer to coefficient storage. Coefficients are s4.27 format.
79 void filter_bishelf_coefs(unsigned long cutoff_low
, unsigned long cutoff_high
,
80 long A_low
, long A_high
, long A
, int32_t *c
)
82 const long g
= get_replaygain_int(A
*10) << 7; /* 10^(db/20), s0.31 */
83 int32_t c_ls
[3], c_hs
[3];
85 filter_shelf_coefs(cutoff_low
, A_low
, true, c_ls
);
86 filter_shelf_coefs(cutoff_high
, A_high
, false, c_hs
);
87 c_ls
[0] = FRACMUL(g
, c_ls
[0]);
88 c_ls
[1] = FRACMUL(g
, c_ls
[1]);
90 /* now we cascade the two first order filters to one second order filter
91 * which can be used by eq_filter(). these resulting coefficients have a
92 * really wide numerical range, so we use a fixed point format which will
93 * work for the selected cutoff frequencies (in dsp.c) only.
95 const int32_t b0
= c_ls
[0], b1
= c_ls
[1], b2
= c_hs
[0], b3
= c_hs
[1];
96 const int32_t a0
= c_ls
[2], a1
= c_hs
[2];
97 *c
++ = FRACMUL_SHL(b0
, b2
, 4);
98 *c
++ = FRACMUL_SHL(b0
, b3
, 4) + FRACMUL_SHL(b1
, b2
, 4);
99 *c
++ = FRACMUL_SHL(b1
, b3
, 4);
101 *c
++ = -FRACMUL_SHL(a0
, a1
, 4);
105 /* Coef calculation taken from Audio-EQ-Cookbook.txt by Robert Bristow-Johnson.
106 * Slightly faster calculation can be done by deriving forms which use tan()
107 * instead of cos() and sin(), but the latter are far easier to use when doing
108 * fixed point math, and performance is not a big point in the calculation part.
109 * All the 'a' filter coefficients are negated so we can use only additions
110 * in the filtering equation.
114 * Calculate second order section peaking filter coefficients.
115 * @param cutoff a value from 0 to 0x80000000, where 0 represents 0 Hz and
116 * 0x80000000 represents the Nyquist frequency (samplerate/2).
117 * @param Q Q factor value multiplied by ten. Lower bound is artificially set
119 * @param db decibel value multiplied by ten, describing gain/attenuation at
120 * peak freq. Max value is 24 dB.
121 * @param c pointer to coefficient storage. Coefficients are s3.28 format.
123 void eq_pk_coefs(unsigned long cutoff
, unsigned long Q
, long db
, int32_t *c
)
126 const long one
= 1 << 28; /* s3.28 */
127 const long A
= get_replaygain_int(db
*5) << 5; /* 10^(db/40), s2.29 */
128 const long alpha
= fsincos(cutoff
, &cs
)/(2*Q
)*10 >> 1; /* s1.30 */
129 int32_t a0
, a1
, a2
; /* these are all s3.28 format */
131 const long alphadivA
= DIV64(alpha
, A
, 27);
133 /* possible numerical ranges are in comments by each coef */
134 b0
= one
+ FRACMUL(alpha
, A
); /* [1 .. 5] */
135 b1
= a1
= -2*(cs
>> 3); /* [-2 .. 2] */
136 b2
= one
- FRACMUL(alpha
, A
); /* [-3 .. 1] */
137 a0
= one
+ alphadivA
; /* [1 .. 5] */
138 a2
= one
- alphadivA
; /* [-3 .. 1] */
140 /* range of this is roughly [0.2 .. 1], but we'll never hit 1 completely */
141 const long rcp_a0
= DIV64(1, a0
, 59); /* s0.31 */
142 *c
++ = FRACMUL(b0
, rcp_a0
); /* [0.25 .. 4] */
143 *c
++ = FRACMUL(b1
, rcp_a0
); /* [-2 .. 2] */
144 *c
++ = FRACMUL(b2
, rcp_a0
); /* [-2.4 .. 1] */
145 *c
++ = FRACMUL(-a1
, rcp_a0
); /* [-2 .. 2] */
146 *c
++ = FRACMUL(-a2
, rcp_a0
); /* [-0.6 .. 1] */
150 * Calculate coefficients for lowshelf filter. Parameters are as for
151 * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
153 void eq_ls_coefs(unsigned long cutoff
, unsigned long Q
, long db
, int32_t *c
)
156 const long one
= 1 << 25; /* s6.25 */
157 const long sqrtA
= get_replaygain_int(db
*5/2) << 2; /* 10^(db/80), s5.26 */
158 const long A
= FRACMUL_SHL(sqrtA
, sqrtA
, 8); /* s2.29 */
159 const long alpha
= fsincos(cutoff
, &cs
)/(2*Q
)*10 >> 1; /* s1.30 */
160 const long ap1
= (A
>> 4) + one
;
161 const long am1
= (A
>> 4) - one
;
162 const long twosqrtalpha
= 2*FRACMUL(sqrtA
, alpha
);
163 int32_t a0
, a1
, a2
; /* these are all s6.25 format */
167 b0
= FRACMUL_SHL(A
, ap1
- FRACMUL(am1
, cs
) + twosqrtalpha
, 2);
169 b1
= FRACMUL_SHL(A
, am1
- FRACMUL(ap1
, cs
), 3);
171 b2
= FRACMUL_SHL(A
, ap1
- FRACMUL(am1
, cs
) - twosqrtalpha
, 2);
173 a0
= ap1
+ FRACMUL(am1
, cs
) + twosqrtalpha
;
175 a1
= -2*((am1
+ FRACMUL(ap1
, cs
)));
177 a2
= ap1
+ FRACMUL(am1
, cs
) - twosqrtalpha
;
180 const long rcp_a0
= DIV64(1, a0
, 55); /* s1.30 */
181 *c
++ = FRACMUL_SHL(b0
, rcp_a0
, 2); /* [0.06 .. 15.9] */
182 *c
++ = FRACMUL_SHL(b1
, rcp_a0
, 2); /* [-2 .. 31.7] */
183 *c
++ = FRACMUL_SHL(b2
, rcp_a0
, 2); /* [0 .. 15.9] */
184 *c
++ = FRACMUL_SHL(-a1
, rcp_a0
, 2); /* [-2 .. 2] */
185 *c
++ = FRACMUL_SHL(-a2
, rcp_a0
, 2); /* [0 .. 1] */
189 * Calculate coefficients for highshelf filter. Parameters are as for
190 * eq_pk_coefs, but the coefficient format is s5.26 fixed point.
192 void eq_hs_coefs(unsigned long cutoff
, unsigned long Q
, long db
, int32_t *c
)
195 const long one
= 1 << 25; /* s6.25 */
196 const long sqrtA
= get_replaygain_int(db
*5/2) << 2; /* 10^(db/80), s5.26 */
197 const long A
= FRACMUL_SHL(sqrtA
, sqrtA
, 8); /* s2.29 */
198 const long alpha
= fsincos(cutoff
, &cs
)/(2*Q
)*10 >> 1; /* s1.30 */
199 const long ap1
= (A
>> 4) + one
;
200 const long am1
= (A
>> 4) - one
;
201 const long twosqrtalpha
= 2*FRACMUL(sqrtA
, alpha
);
202 int32_t a0
, a1
, a2
; /* these are all s6.25 format */
206 b0
= FRACMUL_SHL(A
, ap1
+ FRACMUL(am1
, cs
) + twosqrtalpha
, 2);
208 b1
= -FRACMUL_SHL(A
, am1
+ FRACMUL(ap1
, cs
), 3);
210 b2
= FRACMUL_SHL(A
, ap1
+ FRACMUL(am1
, cs
) - twosqrtalpha
, 2);
212 a0
= ap1
- FRACMUL(am1
, cs
) + twosqrtalpha
;
214 a1
= 2*((am1
- FRACMUL(ap1
, cs
)));
216 a2
= ap1
- FRACMUL(am1
, cs
) - twosqrtalpha
;
219 const long rcp_a0
= DIV64(1, a0
, 55); /* s1.30 */
220 *c
++ = FRACMUL_SHL(b0
, rcp_a0
, 2); /* [0 .. 16] */
221 *c
++ = FRACMUL_SHL(b1
, rcp_a0
, 2); /* [-31.7 .. 2] */
222 *c
++ = FRACMUL_SHL(b2
, rcp_a0
, 2); /* [0 .. 16] */
223 *c
++ = FRACMUL_SHL(-a1
, rcp_a0
, 2); /* [-2 .. 2] */
224 *c
++ = FRACMUL_SHL(-a2
, rcp_a0
, 2); /* [0 .. 1] */
227 /* We realise the filters as a second order direct form 1 structure. Direct
228 * form 1 was chosen because of better numerical properties for fixed point
232 #if (!defined(CPU_COLDFIRE) && !defined(CPU_ARM))
233 void eq_filter(int32_t **x
, struct eqfilter
*f
, unsigned num
,
234 unsigned channels
, unsigned shift
)
239 /* Direct form 1 filtering code.
240 y[n] = b0*x[i] + b1*x[i - 1] + b2*x[i - 2] + a1*y[i - 1] + a2*y[i - 2],
241 where y[] is output and x[] is input.
244 for (c
= 0; c
< channels
; c
++) {
245 for (i
= 0; i
< num
; i
++) {
246 acc
= (long long) x
[c
][i
] * f
->coefs
[0];
247 acc
+= (long long) f
->history
[c
][0] * f
->coefs
[1];
248 acc
+= (long long) f
->history
[c
][1] * f
->coefs
[2];
249 acc
+= (long long) f
->history
[c
][2] * f
->coefs
[3];
250 acc
+= (long long) f
->history
[c
][3] * f
->coefs
[4];
251 f
->history
[c
][1] = f
->history
[c
][0];
252 f
->history
[c
][0] = x
[c
][i
];
253 f
->history
[c
][3] = f
->history
[c
][2];
254 x
[c
][i
] = (acc
<< shift
) >> 32;
255 f
->history
[c
][2] = x
[c
][i
];