diff options
-rw-r--r-- | apps/eq.c | 134 |
1 files changed, 96 insertions, 38 deletions
@@ -53,42 +53,99 @@ | |||
53 | /* TODO: replaygain.c has some fixed point routines. perhaps we could reuse | 53 | /* TODO: replaygain.c has some fixed point routines. perhaps we could reuse |
54 | them? */ | 54 | them? */ |
55 | 55 | ||
56 | /* 128 sixteen bit sine samples + guard point */ | 56 | /* Inverse gain of circular cordic rotation in s0.31 format. */ |
57 | short sinetab[] = { | 57 | static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */ |
58 | 0, 1607, 3211, 4807, 6392, 7961, 9511, 11038, 12539, 14009, 15446, 16845, | 58 | |
59 | 18204, 19519, 20787, 22004, 23169, 24278, 25329, 26318, 27244, 28105, 28897, | 59 | /* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */ |
60 | 29621, 30272, 30851, 31356, 31785, 32137, 32412, 32609,32727, 32767, 32727, | 60 | static const unsigned long atan_table[] = { |
61 | 32609, 32412, 32137, 31785, 31356, 30851, 30272, 29621, 28897, 28105, 27244, | 61 | 0x1fffffff, /* +0.785398163 (or pi/4) */ |
62 | 26318, 25329, 24278, 23169, 22004, 20787, 19519, 18204, 16845, 15446, 14009, | 62 | 0x12e4051d, /* +0.463647609 */ |
63 | 12539, 11038, 9511, 7961, 6392, 4807, 3211, 1607, 0, -1607, -3211, -4807, | 63 | 0x09fb385b, /* +0.244978663 */ |
64 | -6392, -7961, -9511, -11038, -12539, -14009, -15446, -16845, -18204, -19519, | 64 | 0x051111d4, /* +0.124354995 */ |
65 | -20787, -22004, -23169, -24278, -25329, -26318, -27244, -28105, -28897, | 65 | 0x028b0d43, /* +0.062418810 */ |
66 | -29621, -30272, -30851, -31356, -31785, -32137, -32412, -32609, -32727, | 66 | 0x0145d7e1, /* +0.031239833 */ |
67 | -32767, -32727, -32609, -32412, -32137, -31785, -31356, -30851, -30272, | 67 | 0x00a2f61e, /* +0.015623729 */ |
68 | -29621, -28897, -28105, -27244, -26318, -25329, -24278, -23169, -22004, | 68 | 0x00517c55, /* +0.007812341 */ |
69 | -20787, -19519, -18204, -16845, -15446, -14009, -12539, -11038, -9511, | 69 | 0x0028be53, /* +0.003906230 */ |
70 | -7961, -6392, -4807, -3211, -1607, 0 | 70 | 0x00145f2e, /* +0.001953123 */ |
71 | 0x000a2f98, /* +0.000976562 */ | ||
72 | 0x000517cc, /* +0.000488281 */ | ||
73 | 0x00028be6, /* +0.000244141 */ | ||
74 | 0x000145f3, /* +0.000122070 */ | ||
75 | 0x0000a2f9, /* +0.000061035 */ | ||
76 | 0x0000517c, /* +0.000030518 */ | ||
77 | 0x000028be, /* +0.000015259 */ | ||
78 | 0x0000145f, /* +0.000007629 */ | ||
79 | 0x00000a2f, /* +0.000003815 */ | ||
80 | 0x00000517, /* +0.000001907 */ | ||
81 | 0x0000028b, /* +0.000000954 */ | ||
82 | 0x00000145, /* +0.000000477 */ | ||
83 | 0x000000a2, /* +0.000000238 */ | ||
84 | 0x00000051, /* +0.000000119 */ | ||
85 | 0x00000028, /* +0.000000060 */ | ||
86 | 0x00000014, /* +0.000000030 */ | ||
87 | 0x0000000a, /* +0.000000015 */ | ||
88 | 0x00000005, /* +0.000000007 */ | ||
89 | 0x00000002, /* +0.000000004 */ | ||
90 | 0x00000001, /* +0.000000002 */ | ||
91 | 0x00000000, /* +0.000000001 */ | ||
92 | 0x00000000, /* +0.000000000 */ | ||
71 | }; | 93 | }; |
72 | 94 | ||
73 | /* Good quality sine calculated by linearly interpolating | 95 | /** |
74 | * a 128 sample sine table. First harmonic has amplitude of about -84 dB. | 96 | * Implements sin and cos using CORDIC rotation. |
75 | * phase has range from 0 to 0xffffffff, representing 0 and | 97 | * |
76 | * 2*pi respectively. | 98 | * @param phase has range from 0 to 0xffffffff, representing 0 and |
77 | * Return value is a signed value from LONG_MIN to LONG_MAX, representing | 99 | * 2*pi respectively. |
78 | * -1 and 1 respectively. | 100 | * @param cos return address for cos |
101 | * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX, | ||
102 | * representing -1 and 1 respectively. | ||
79 | */ | 103 | */ |
80 | static long fsin(unsigned long phase) | 104 | long fsincos(unsigned long phase, long *cos) { |
81 | { | 105 | long x, x1, y, y1; |
82 | unsigned int pos = phase >> 25; | 106 | unsigned long z, z1; |
83 | unsigned short frac = (phase & 0x01ffffff) >> 9; | 107 | int i; |
84 | short diff = sinetab[pos + 1] - sinetab[pos]; | ||
85 | |||
86 | return (sinetab[pos] << 16) + frac*diff; | ||
87 | } | ||
88 | 108 | ||
89 | static inline long fcos(unsigned long phase) | 109 | /* Setup initial vector */ |
90 | { | 110 | x = cordic_circular_gain; |
91 | return fsin(phase + 0xffffffff/4); | 111 | y = 0; |
112 | z = phase; | ||
113 | |||
114 | /* The phase has to be somewhere between 0..pi for this to work right */ | ||
115 | if (z < 0xffffffff / 4) { | ||
116 | /* z in first quadrant, z += pi/2 to correct */ | ||
117 | x = -x; | ||
118 | z += 0xffffffff / 4; | ||
119 | } else if (z < 3 * (0xffffffff / 4)) { | ||
120 | /* z in third quadrant, z -= pi/2 to correct */ | ||
121 | z -= 0xffffffff / 4; | ||
122 | } else { | ||
123 | /* z in fourth quadrant, z -= 3pi/2 to correct */ | ||
124 | x = -x; | ||
125 | z -= 3 * (0xffffffff / 4); | ||
126 | } | ||
127 | |||
128 | /* Each iteration adds roughly 1-bit of extra precision */ | ||
129 | for (i = 0; i < 31; i++) { | ||
130 | x1 = x >> i; | ||
131 | y1 = y >> i; | ||
132 | z1 = atan_table[i]; | ||
133 | |||
134 | /* Decided which direction to rotate vector. Pivot point is pi/2 */ | ||
135 | if (z >= 0xffffffff / 4) { | ||
136 | x -= y1; | ||
137 | y += x1; | ||
138 | z -= z1; | ||
139 | } else { | ||
140 | x += y1; | ||
141 | y -= x1; | ||
142 | z += z1; | ||
143 | } | ||
144 | } | ||
145 | |||
146 | *cos = x; | ||
147 | |||
148 | return y; | ||
92 | } | 149 | } |
93 | 150 | ||
94 | /* Fixed point square root via Newton-Raphson. | 151 | /* Fixed point square root via Newton-Raphson. |
@@ -140,15 +197,16 @@ static long dbtoA(long db) | |||
140 | */ | 197 | */ |
141 | void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | 198 | void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) |
142 | { | 199 | { |
200 | long cc; | ||
143 | const long one = 1 << 28; /* s3.28 */ | 201 | const long one = 1 << 28; /* s3.28 */ |
144 | const long A = dbtoA(db); | 202 | const long A = dbtoA(db); |
145 | const long alpha = DIV64(fsin(cutoff), 2*Q, 15); /* s1.30 */ | 203 | const long alpha = DIV64(fsincos(cutoff, &cc), 2*Q, 15); /* s1.30 */ |
146 | int32_t a0, a1, a2; /* these are all s3.28 format */ | 204 | int32_t a0, a1, a2; /* these are all s3.28 format */ |
147 | int32_t b0, b1, b2; | 205 | int32_t b0, b1, b2; |
148 | 206 | ||
149 | /* possible numerical ranges listed after each coef */ | 207 | /* possible numerical ranges listed after each coef */ |
150 | b0 = one + FRACMUL(alpha, A); /* [1.25..5] */ | 208 | b0 = one + FRACMUL(alpha, A); /* [1.25..5] */ |
151 | b1 = a1 = -2*(fcos(cutoff) >> 3); /* [-2..2] */ | 209 | b1 = a1 = -2*(cc >> 3); /* [-2..2] */ |
152 | b2 = one - FRACMUL(alpha, A); /* [-3..0.75] */ | 210 | b2 = one - FRACMUL(alpha, A); /* [-3..0.75] */ |
153 | a0 = one + DIV64(alpha, A, 27); /* [1.25..5] */ | 211 | a0 = one + DIV64(alpha, A, 27); /* [1.25..5] */ |
154 | a2 = one - DIV64(alpha, A, 27); /* [-3..0.75] */ | 212 | a2 = one - DIV64(alpha, A, 27); /* [-3..0.75] */ |
@@ -163,15 +221,15 @@ void eq_pk_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | |||
163 | /* Calculate coefficients for lowshelf filter */ | 221 | /* Calculate coefficients for lowshelf filter */ |
164 | void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | 222 | void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) |
165 | { | 223 | { |
224 | long cs; | ||
166 | const long one = 1 << 24; /* s7.24 */ | 225 | const long one = 1 << 24; /* s7.24 */ |
167 | const long A = dbtoA(db); | 226 | const long A = dbtoA(db); |
168 | const long alpha = DIV64(fsin(cutoff), 2*Q, 15); /* s1.30 */ | 227 | const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */ |
169 | const long ap1 = (A >> 5) + one; | 228 | const long ap1 = (A >> 5) + one; |
170 | const long am1 = (A >> 5) - one; | 229 | const long am1 = (A >> 5) - one; |
171 | const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); | 230 | const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); |
172 | int32_t a0, a1, a2; /* these are all s7.24 format */ | 231 | int32_t a0, a1, a2; /* these are all s7.24 format */ |
173 | int32_t b0, b1, b2; | 232 | int32_t b0, b1, b2; |
174 | long cs = fcos(cutoff); | ||
175 | 233 | ||
176 | b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2; | 234 | b0 = FRACMUL(A, ap1 - FRACMUL(am1, cs) + twosqrtalpha) << 2; |
177 | b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3; | 235 | b1 = FRACMUL(A, am1 - FRACMUL(ap1, cs)) << 3; |
@@ -190,15 +248,15 @@ void eq_ls_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | |||
190 | /* Calculate coefficients for highshelf filter */ | 248 | /* Calculate coefficients for highshelf filter */ |
191 | void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) | 249 | void eq_hs_coefs(unsigned long cutoff, unsigned long Q, long db, int32_t *c) |
192 | { | 250 | { |
251 | long cs; | ||
193 | const long one = 1 << 24; /* s7.24 */ | 252 | const long one = 1 << 24; /* s7.24 */ |
194 | const long A = dbtoA(db); | 253 | const long A = dbtoA(db); |
195 | const long alpha = DIV64(fsin(cutoff), 2*Q, 15); /* s1.30 */ | 254 | const long alpha = DIV64(fsincos(cutoff, &cs), 2*Q, 15); /* s1.30 */ |
196 | const long ap1 = (A >> 5) + one; | 255 | const long ap1 = (A >> 5) + one; |
197 | const long am1 = (A >> 5) - one; | 256 | const long am1 = (A >> 5) - one; |
198 | const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); | 257 | const long twosqrtalpha = 2*(FRACMUL(fsqrt(A >> 5, 24), alpha) << 1); |
199 | int32_t a0, a1, a2; /* these are all s7.24 format */ | 258 | int32_t a0, a1, a2; /* these are all s7.24 format */ |
200 | int32_t b0, b1, b2; | 259 | int32_t b0, b1, b2; |
201 | long cs = fcos(cutoff); | ||
202 | 260 | ||
203 | b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2; | 261 | b0 = FRACMUL(A, ap1 + FRACMUL(am1, cs) + twosqrtalpha) << 2; |
204 | b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3; | 262 | b1 = -FRACMUL(A, am1 + FRACMUL(ap1, cs)) << 3; |