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 =
60 b0 * x + b1 * x1 + b2 * x2 - a1 * y1 - a2 * y2;
61 data[j] = y;
62 x2 = x1;
63 x1 = x;
64 y2 = y1;
65 y1 = y;
66 }
67 q->x1 = x1;
68 q->x2 = x2;
69 q->y1 = y1;
70 q->y2 = y2;
71 }
72 }
73
74 /* This is the actual processing loop used. It is the unrolled version of the
75 * above prototype. */
eq_process(struct eq * eq,float * data,int count)76 void eq_process(struct eq *eq, float *data, int count)
77 {
78 int i, j;
79 for (i = 0; i < eq->n; i += 2) {
80 if (i + 1 == eq->n) {
81 struct biquad *q = &eq->biquad[i];
82 float x1 = q->x1;
83 float x2 = q->x2;
84 float y1 = q->y1;
85 float y2 = q->y2;
86 float b0 = q->b0;
87 float b1 = q->b1;
88 float b2 = q->b2;
89 float a1 = q->a1;
90 float a2 = q->a2;
91 for (j = 0; j < count; j++) {
92 float x = data[j];
93 float y = b0 * x + b1 * x1 + b2 * x2 - a1 * y1 -
94 a2 * y2;
95 data[j] = y;
96 x2 = x1;
97 x1 = x;
98 y2 = y1;
99 y1 = y;
100 }
101 q->x1 = x1;
102 q->x2 = x2;
103 q->y1 = y1;
104 q->y2 = y2;
105 } else {
106 struct biquad *q = &eq->biquad[i];
107 struct biquad *r = &eq->biquad[i + 1];
108 float x1 = q->x1;
109 float x2 = q->x2;
110 float y1 = q->y1;
111 float y2 = q->y2;
112 float qb0 = q->b0;
113 float qb1 = q->b1;
114 float qb2 = q->b2;
115 float qa1 = q->a1;
116 float qa2 = q->a2;
117
118 float z1 = r->y1;
119 float z2 = r->y2;
120 float rb0 = r->b0;
121 float rb1 = r->b1;
122 float rb2 = r->b2;
123 float ra1 = r->a1;
124 float ra2 = r->a2;
125
126 for (j = 0; j < count; j++) {
127 float x = data[j];
128 float y = qb0 * x + qb1 * x1 + qb2 * x2 -
129 qa1 * y1 - qa2 * y2;
130 float z = rb0 * y + rb1 * y1 + rb2 * y2 -
131 ra1 * z1 - ra2 * z2;
132 data[j] = z;
133 x2 = x1;
134 x1 = x;
135 y2 = y1;
136 y1 = y;
137 z2 = z1;
138 z1 = z;
139 }
140 q->x1 = x1;
141 q->x2 = x2;
142 q->y1 = y1;
143 q->y2 = y2;
144 r->y1 = z1;
145 r->y2 = z2;
146 }
147 }
148 }
149