diff options
Diffstat (limited to 'apps/eq.c')
-rw-r--r-- | apps/eq.c | 97 |
1 files changed, 96 insertions, 1 deletions
@@ -21,10 +21,105 @@ | |||
21 | 21 | ||
22 | #include <inttypes.h> | 22 | #include <inttypes.h> |
23 | #include "config.h" | 23 | #include "config.h" |
24 | #include "fixedpoint.h" | 24 | #include "dsp.h" |
25 | #include "eq.h" | 25 | #include "eq.h" |
26 | #include "replaygain.h" | 26 | #include "replaygain.h" |
27 | 27 | ||
28 | /* Inverse gain of circular cordic rotation in s0.31 format. */ | ||
29 | static const long cordic_circular_gain = 0xb2458939; /* 0.607252929 */ | ||
30 | |||
31 | /* Table of values of atan(2^-i) in 0.32 format fractions of pi where pi = 0xffffffff / 2 */ | ||
32 | static const unsigned long atan_table[] = { | ||
33 | 0x1fffffff, /* +0.785398163 (or pi/4) */ | ||
34 | 0x12e4051d, /* +0.463647609 */ | ||
35 | 0x09fb385b, /* +0.244978663 */ | ||
36 | 0x051111d4, /* +0.124354995 */ | ||
37 | 0x028b0d43, /* +0.062418810 */ | ||
38 | 0x0145d7e1, /* +0.031239833 */ | ||
39 | 0x00a2f61e, /* +0.015623729 */ | ||
40 | 0x00517c55, /* +0.007812341 */ | ||
41 | 0x0028be53, /* +0.003906230 */ | ||
42 | 0x00145f2e, /* +0.001953123 */ | ||
43 | 0x000a2f98, /* +0.000976562 */ | ||
44 | 0x000517cc, /* +0.000488281 */ | ||
45 | 0x00028be6, /* +0.000244141 */ | ||
46 | 0x000145f3, /* +0.000122070 */ | ||
47 | 0x0000a2f9, /* +0.000061035 */ | ||
48 | 0x0000517c, /* +0.000030518 */ | ||
49 | 0x000028be, /* +0.000015259 */ | ||
50 | 0x0000145f, /* +0.000007629 */ | ||
51 | 0x00000a2f, /* +0.000003815 */ | ||
52 | 0x00000517, /* +0.000001907 */ | ||
53 | 0x0000028b, /* +0.000000954 */ | ||
54 | 0x00000145, /* +0.000000477 */ | ||
55 | 0x000000a2, /* +0.000000238 */ | ||
56 | 0x00000051, /* +0.000000119 */ | ||
57 | 0x00000028, /* +0.000000060 */ | ||
58 | 0x00000014, /* +0.000000030 */ | ||
59 | 0x0000000a, /* +0.000000015 */ | ||
60 | 0x00000005, /* +0.000000007 */ | ||
61 | 0x00000002, /* +0.000000004 */ | ||
62 | 0x00000001, /* +0.000000002 */ | ||
63 | 0x00000000, /* +0.000000001 */ | ||
64 | 0x00000000, /* +0.000000000 */ | ||
65 | }; | ||
66 | |||
67 | /** | ||
68 | * Implements sin and cos using CORDIC rotation. | ||
69 | * | ||
70 | * @param phase has range from 0 to 0xffffffff, representing 0 and | ||
71 | * 2*pi respectively. | ||
72 | * @param cos return address for cos | ||
73 | * @return sin of phase, value is a signed value from LONG_MIN to LONG_MAX, | ||
74 | * representing -1 and 1 respectively. | ||
75 | */ | ||
76 | static long fsincos(unsigned long phase, long *cos) { | ||
77 | int32_t x, x1, y, y1; | ||
78 | unsigned long z, z1; | ||
79 | int i; | ||
80 | |||
81 | /* Setup initial vector */ | ||
82 | x = cordic_circular_gain; | ||
83 | y = 0; | ||
84 | z = phase; | ||
85 | |||
86 | /* The phase has to be somewhere between 0..pi for this to work right */ | ||
87 | if (z < 0xffffffff / 4) { | ||
88 | /* z in first quadrant, z += pi/2 to correct */ | ||
89 | x = -x; | ||
90 | z += 0xffffffff / 4; | ||
91 | } else if (z < 3 * (0xffffffff / 4)) { | ||
92 | /* z in third quadrant, z -= pi/2 to correct */ | ||
93 | z -= 0xffffffff / 4; | ||
94 | } else { | ||
95 | /* z in fourth quadrant, z -= 3pi/2 to correct */ | ||
96 | x = -x; | ||
97 | z -= 3 * (0xffffffff / 4); | ||
98 | } | ||
99 | |||
100 | /* Each iteration adds roughly 1-bit of extra precision */ | ||
101 | for (i = 0; i < 31; i++) { | ||
102 | x1 = x >> i; | ||
103 | y1 = y >> i; | ||
104 | z1 = atan_table[i]; | ||
105 | |||
106 | /* Decided which direction to rotate vector. Pivot point is pi/2 */ | ||
107 | if (z >= 0xffffffff / 4) { | ||
108 | x -= y1; | ||
109 | y += x1; | ||
110 | z -= z1; | ||
111 | } else { | ||
112 | x += y1; | ||
113 | y -= x1; | ||
114 | z += z1; | ||
115 | } | ||
116 | } | ||
117 | |||
118 | *cos = x; | ||
119 | |||
120 | return y; | ||
121 | } | ||
122 | |||
28 | /** | 123 | /** |
29 | * Calculate first order shelving filter. Filter is not directly usable by the | 124 | * Calculate first order shelving filter. Filter is not directly usable by the |
30 | * eq_filter() function. | 125 | * eq_filter() function. |