1 /* Copyright (c) 2013 The Chromium OS Authors. All rights reserved.
2 * Use of this source code is governed by a BSD-style license that can be
3 * found in the LICENSE file.
4 */
5
6 #include "crossover.h"
7 #include "biquad.h"
8
lr4_set(struct lr4 * lr4,enum biquad_type type,float freq)9 static void lr4_set(struct lr4 *lr4, enum biquad_type type, float freq)
10 {
11 struct biquad q;
12 biquad_set(&q, type, freq, 0, 0);
13 lr4->b0 = q.b0;
14 lr4->b1 = q.b1;
15 lr4->b2 = q.b2;
16 lr4->a1 = q.a1;
17 lr4->a2 = q.a2;
18 lr4->x1 = 0;
19 lr4->x2 = 0;
20 lr4->y1 = 0;
21 lr4->y2 = 0;
22 lr4->z1 = 0;
23 lr4->z2 = 0;
24 }
25
26 /* Split input data using two LR4 filters, put the result into the input array
27 * and another array.
28 *
29 * data0 --+-- lp --> data0
30 * |
31 * \-- hp --> data1
32 */
lr4_split(struct lr4 * lp,struct lr4 * hp,int count,float * data0,float * data1)33 static void lr4_split(struct lr4 *lp, struct lr4 *hp, int count, float *data0,
34 float *data1)
35 {
36 float lx1 = lp->x1;
37 float lx2 = lp->x2;
38 float ly1 = lp->y1;
39 float ly2 = lp->y2;
40 float lz1 = lp->z1;
41 float lz2 = lp->z2;
42 float lb0 = lp->b0;
43 float lb1 = lp->b1;
44 float lb2 = lp->b2;
45 float la1 = lp->a1;
46 float la2 = lp->a2;
47
48 float hx1 = hp->x1;
49 float hx2 = hp->x2;
50 float hy1 = hp->y1;
51 float hy2 = hp->y2;
52 float hz1 = hp->z1;
53 float hz2 = hp->z2;
54 float hb0 = hp->b0;
55 float hb1 = hp->b1;
56 float hb2 = hp->b2;
57 float ha1 = hp->a1;
58 float ha2 = hp->a2;
59
60 int i;
61 for (i = 0; i < count; i++) {
62 float x, y, z;
63 x = data0[i];
64 y = lb0*x + lb1*lx1 + lb2*lx2 - la1*ly1 - la2*ly2;
65 z = lb0*y + lb1*ly1 + lb2*ly2 - la1*lz1 - la2*lz2;
66 lx2 = lx1;
67 lx1 = x;
68 ly2 = ly1;
69 ly1 = y;
70 lz2 = lz1;
71 lz1 = z;
72 data0[i] = z;
73
74 y = hb0*x + hb1*hx1 + hb2*hx2 - ha1*hy1 - ha2*hy2;
75 z = hb0*y + hb1*hy1 + hb2*hy2 - ha1*hz1 - ha2*hz2;
76 hx2 = hx1;
77 hx1 = x;
78 hy2 = hy1;
79 hy1 = y;
80 hz2 = hz1;
81 hz1 = z;
82 data1[i] = z;
83 }
84
85 lp->x1 = lx1;
86 lp->x2 = lx2;
87 lp->y1 = ly1;
88 lp->y2 = ly2;
89 lp->z1 = lz1;
90 lp->z2 = lz2;
91
92 hp->x1 = hx1;
93 hp->x2 = hx2;
94 hp->y1 = hy1;
95 hp->y2 = hy2;
96 hp->z1 = hz1;
97 hp->z2 = hz2;
98 }
99
100 /* Split input data using two LR4 filters and sum them back to the original
101 * data array.
102 *
103 * data --+-- lp --+--> data
104 * | |
105 * \-- hp --/
106 */
lr4_merge(struct lr4 * lp,struct lr4 * hp,int count,float * data)107 static void lr4_merge(struct lr4 *lp, struct lr4 *hp, int count, float *data)
108 {
109 float lx1 = lp->x1;
110 float lx2 = lp->x2;
111 float ly1 = lp->y1;
112 float ly2 = lp->y2;
113 float lz1 = lp->z1;
114 float lz2 = lp->z2;
115 float lb0 = lp->b0;
116 float lb1 = lp->b1;
117 float lb2 = lp->b2;
118 float la1 = lp->a1;
119 float la2 = lp->a2;
120
121 float hx1 = hp->x1;
122 float hx2 = hp->x2;
123 float hy1 = hp->y1;
124 float hy2 = hp->y2;
125 float hz1 = hp->z1;
126 float hz2 = hp->z2;
127 float hb0 = hp->b0;
128 float hb1 = hp->b1;
129 float hb2 = hp->b2;
130 float ha1 = hp->a1;
131 float ha2 = hp->a2;
132
133 int i;
134 for (i = 0; i < count; i++) {
135 float x, y, z;
136 x = data[i];
137 y = lb0*x + lb1*lx1 + lb2*lx2 - la1*ly1 - la2*ly2;
138 z = lb0*y + lb1*ly1 + lb2*ly2 - la1*lz1 - la2*lz2;
139 lx2 = lx1;
140 lx1 = x;
141 ly2 = ly1;
142 ly1 = y;
143 lz2 = lz1;
144 lz1 = z;
145
146 y = hb0*x + hb1*hx1 + hb2*hx2 - ha1*hy1 - ha2*hy2;
147 z = hb0*y + hb1*hy1 + hb2*hy2 - ha1*hz1 - ha2*hz2;
148 hx2 = hx1;
149 hx1 = x;
150 hy2 = hy1;
151 hy1 = y;
152 hz2 = hz1;
153 hz1 = z;
154 data[i] = z + lz1;
155 }
156
157 lp->x1 = lx1;
158 lp->x2 = lx2;
159 lp->y1 = ly1;
160 lp->y2 = ly2;
161 lp->z1 = lz1;
162 lp->z2 = lz2;
163
164 hp->x1 = hx1;
165 hp->x2 = hx2;
166 hp->y1 = hy1;
167 hp->y2 = hy2;
168 hp->z1 = hz1;
169 hp->z2 = hz2;
170 }
171
crossover_init(struct crossover * xo,float freq1,float freq2)172 void crossover_init(struct crossover *xo, float freq1, float freq2)
173 {
174 int i;
175 for (i = 0; i < 3; i++) {
176 float f = (i == 0) ? freq1 : freq2;
177 lr4_set(&xo->lp[i], BQ_LOWPASS, f);
178 lr4_set(&xo->hp[i], BQ_HIGHPASS, f);
179 }
180 }
181
crossover_process(struct crossover * xo,int count,float * data0,float * data1,float * data2)182 void crossover_process(struct crossover *xo, int count, float *data0,
183 float *data1, float *data2)
184 {
185 lr4_split(&xo->lp[0], &xo->hp[0], count, data0, data1);
186 lr4_merge(&xo->lp[1], &xo->hp[1], count, data0);
187 lr4_split(&xo->lp[2], &xo->hp[2], count, data1, data2);
188 }
189