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 <stdlib.h>
7 #include "eq.h"
8
9 struct eq {
10 int n;
11 struct biquad biquad[MAX_BIQUADS_PER_EQ];
12 };
13
eq_new()14 struct eq *eq_new()
15 {
16 struct eq *eq = (struct eq *)calloc(1, sizeof(*eq));
17 return eq;
18 }
19
eq_free(struct eq * eq)20 void eq_free(struct eq *eq)
21 {
22 free(eq);
23 }
24
eq_append_biquad(struct eq * eq,enum biquad_type type,float freq,float Q,float gain)25 int eq_append_biquad(struct eq *eq, enum biquad_type type, float freq, float Q,
26 float gain)
27 {
28 if (eq->n >= MAX_BIQUADS_PER_EQ)
29 return -1;
30 biquad_set(&eq->biquad[eq->n++], type, freq, Q, gain);
31 return 0;
32 }
33
eq_append_biquad_direct(struct eq * eq,const struct biquad * biquad)34 int eq_append_biquad_direct(struct eq *eq, const struct biquad *biquad)
35 {
36 if (eq->n >= MAX_BIQUADS_PER_EQ)
37 return -1;
38 eq->biquad[eq->n++] = *biquad;
39 return 0;
40 }
41
42 /* This is the prototype of the processing loop. */
eq_process1(struct eq * eq,float * data,int count)43 void eq_process1(struct eq *eq, float *data, int count)
44 {
45 int i, j;
46 for (i = 0; i < eq->n; i++) {
47 struct biquad *q = &eq->biquad[i];
48 float x1 = q->x1;
49 float x2 = q->x2;
50 float y1 = q->y1;
51 float y2 = q->y2;
52 float b0 = q->b0;
53 float b1 = q->b1;
54 float b2 = q->b2;
55 float a1 = q->a1;
56 float a2 = q->a2;
57 for (j = 0; j < count; j++) {
58 float x = data[j];
59 float y = b0*x
60 + b1*x1 + b2*x2
61 - a1*y1 - a2*y2;
62 data[j] = y;
63 x2 = x1;
64 x1 = x;
65 y2 = y1;
66 y1 = y;
67 }
68 q->x1 = x1;
69 q->x2 = x2;
70 q->y1 = y1;
71 q->y2 = y2;
72 }
73 }
74
75 /* This is the actual processing loop used. It is the unrolled version of the
76 * above prototype. */
eq_process(struct eq * eq,float * data,int count)77 void eq_process(struct eq *eq, float *data, int count)
78 {
79 int i, j;
80 for (i = 0; i < eq->n; i += 2) {
81 if (i + 1 == eq->n) {
82 struct biquad *q = &eq->biquad[i];
83 float x1 = q->x1;
84 float x2 = q->x2;
85 float y1 = q->y1;
86 float y2 = q->y2;
87 float b0 = q->b0;
88 float b1 = q->b1;
89 float b2 = q->b2;
90 float a1 = q->a1;
91 float a2 = q->a2;
92 for (j = 0; j < count; j++) {
93 float x = data[j];
94 float y = b0*x
95 + b1*x1 + b2*x2
96 - a1*y1 - a2*y2;
97 data[j] = y;
98 x2 = x1;
99 x1 = x;
100 y2 = y1;
101 y1 = y;
102 }
103 q->x1 = x1;
104 q->x2 = x2;
105 q->y1 = y1;
106 q->y2 = y2;
107 } else {
108 struct biquad *q = &eq->biquad[i];
109 struct biquad *r = &eq->biquad[i+1];
110 float x1 = q->x1;
111 float x2 = q->x2;
112 float y1 = q->y1;
113 float y2 = q->y2;
114 float qb0 = q->b0;
115 float qb1 = q->b1;
116 float qb2 = q->b2;
117 float qa1 = q->a1;
118 float qa2 = q->a2;
119
120 float z1 = r->y1;
121 float z2 = r->y2;
122 float rb0 = r->b0;
123 float rb1 = r->b1;
124 float rb2 = r->b2;
125 float ra1 = r->a1;
126 float ra2 = r->a2;
127
128 for (j = 0; j < count; j++) {
129 float x = data[j];
130 float y = qb0*x
131 + qb1*x1 + qb2*x2
132 - qa1*y1 - qa2*y2;
133 float z = rb0*y
134 + rb1*y1 + rb2*y2
135 - ra1*z1 - ra2*z2;
136 data[j] = z;
137 x2 = x1;
138 x1 = x;
139 y2 = y1;
140 y1 = y;
141 z2 = z1;
142 z1 = z;
143 }
144 q->x1 = x1;
145 q->x2 = x2;
146 q->y1 = y1;
147 q->y2 = y2;
148 r->y1 = z1;
149 r->y2 = z2;
150 }
151 }
152 }
153