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