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
9static 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 */
33static 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 */
107static 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
172void 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
182void 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