forked from thesofproject/sof
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathiir_df1_generic.c
More file actions
166 lines (146 loc) · 5.02 KB
/
iir_df1_generic.c
File metadata and controls
166 lines (146 loc) · 5.02 KB
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
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
// SPDX-License-Identifier: BSD-3-Clause
//
// Copyright(c) 2022 Intel Corporation. All rights reserved.
//
// Author: Andrula Song <andrula.song@intel.com>
#include <stdint.h>
#include <stddef.h>
#include <errno.h>
#include <sof/audio/format.h>
#include <sof/math/iir_df1.h>
#include <user/eq.h>
#include <rtos/symbol.h>
#if SOF_USE_HIFI(NONE, FILTER)
/*
* Direct form I second order filter block (biquad)
*
* +----+ +---+ +-------+
* X(z) ---o--->| b0 |---> + --+-------------o--->| g |--->| shift |---> Y(z)
* | +----+ ^ ^ | +---+ +-------+
* | | | |
* +------+ | | +------+
* | z^-1 | | | | z^-1 |
* +------+ | | +------+
* | +----+ | | +----+ |
* o--->| b1 |---> + + <---| a1 |---o
* | +----+ ^ ^ +----+ |
* | | | |
* +------+ | | +------+
* | z^-1 | | | | z^-1 |
* +------+ | | +------+
* | ^ ^ |
* | +----+ | | +----+ |
* o--->| b2 |---> + +<--- | a2 |---o
* +----+ +----+
*
* y[n] = b0 * x[n] + b1 * x[n-1] + b2 * x[n-2] + a1 * y[n-1] + a2 * y[n-2]
* the a1 a2 has been negated during calculation
*/
/* Series DF1 IIR */
/* 32 bit data, 32 bit coefficients and 32 bit state variables */
int32_t iir_df1(struct iir_state_df1 *iir, int32_t x)
{
int32_t in;
int32_t tmp;
int64_t out = 0;
int64_t acc;
int i;
int j;
int d = 0; /* Index to state */
int c = 0; /* Index to coefficient a2 */
int32_t *coefp = iir->coef;
int32_t *delay = iir->delay;
int nseries = iir->biquads_in_series;
/* Bypass is set with number of biquads set to zero. */
if (!iir->biquads)
return x;
/* Coefficients order in coef[] is {a2, a1, b2, b1, b0, shift, gain} */
/* Delay order in state[] is {y(n - 2), y(n - 1), x(n - 2), x(n - 1)} */
for (j = 0; j < iir->biquads; j += nseries) {
in = x;
for (i = 0; i < nseries; i++) {
/* Compute output: Delay is Q3.61
* Q2.30 x Q1.31 -> Q3.61
* Shift Q3.61 to Q3.31 with rounding, saturate to Q1.31
*/
acc = ((int64_t)coefp[c]) * delay[d]; /* a2 * y(n - 2) */
acc += ((int64_t)coefp[c + 1]) * delay[d + 1]; /* a1 * y(n - 1) */
acc += ((int64_t)coefp[c + 2]) * delay[d + 2]; /* b2 * x(n - 2) */
acc += ((int64_t)coefp[c + 3]) * delay[d + 3]; /* b1 * x(n - 1) */
acc += ((int64_t)coefp[c + 4]) * in; /* b0 * x */
tmp = (int32_t)sat_int32(Q_SHIFT_RND(acc, 61, 31));
/* update the delay value */
delay[d] = delay[d + 1];
delay[d + 1] = tmp;
delay[d + 2] = delay[d + 3];
delay[d + 3] = in;
/* Apply gain Q2.14 x Q1.31 -> Q3.45 */
acc = ((int64_t)coefp[c + 6]) * tmp; /* Gain */
/* Apply biquad output shift right parameter
* simultaneously with Q3.45 to Q3.31 conversion. Then
* saturate to 32 bits Q1.31 and prepare for next
* biquad.
*/
acc = Q_SHIFT_RND(acc, 45 + coefp[c + 5], 31);
in = sat_int32(acc);
/* Proceed to next biquad coefficients and delay
* lines.
*/
c += SOF_EQ_IIR_NBIQUAD;
d += IIR_DF1_NUM_STATE;
}
/* Output of previous section is in variable in */
out += (int64_t)in;
}
return sat_int32(out);
}
EXPORT_SYMBOL(iir_df1);
int32_t iir_df1_4th(struct iir_state_df1 *iir, int32_t x)
{
int32_t in;
int32_t tmp;
int64_t acc;
int i;
int d = 0; /* Index to state */
int c = 0; /* Index to coefficient a2 */
int32_t *coefp = iir->coef;
int32_t *delay = iir->delay;
/* Coefficients order in coef[] is {a2, a1, b2, b1, b0, shift, gain} */
/* Delay order in state[] is {y(n - 2), y(n - 1), x(n - 2), x(n - 1)} */
in = x;
for (i = 0; i < SOF_IIR_DF1_4TH_NUM_BIQUADS; i++) {
/* Compute output: Delay is Q3.61
* Q2.30 x Q1.31 -> Q3.61
* Shift Q3.61 to Q3.31 with rounding, saturate to Q1.31
*/
acc = ((int64_t)coefp[c]) * delay[d]; /* a2 * y(n - 2) */
acc += ((int64_t)coefp[c + 1]) * delay[d + 1]; /* a1 * y(n - 1) */
acc += ((int64_t)coefp[c + 2]) * delay[d + 2]; /* b2 * x(n - 2) */
acc += ((int64_t)coefp[c + 3]) * delay[d + 3]; /* b1 * x(n - 1) */
acc += ((int64_t)coefp[c + 4]) * in; /* b0 * x */
tmp = (int32_t)sat_int32(Q_SHIFT_RND(acc, 61, 31));
/* update the delay value */
delay[d] = delay[d + 1];
delay[d + 1] = tmp;
delay[d + 2] = delay[d + 3];
delay[d + 3] = in;
/* Apply gain Q2.14 x Q1.31 -> Q3.45 */
acc = ((int64_t)coefp[c + 6]) * tmp; /* Gain */
/* Apply biquad output shift right parameter
* simultaneously with Q3.45 to Q3.31 conversion. Then
* saturate to 32 bits Q1.31 and prepare for next
* biquad.
*/
acc = Q_SHIFT_RND(acc, 45 + coefp[c + 5], 31);
in = sat_int32(acc);
/* Proceed to next biquad coefficients and delay
* lines.
*/
c += SOF_EQ_IIR_NBIQUAD;
d += IIR_DF1_NUM_STATE;
}
/* Output of previous section is in variable in */
return in;
}
EXPORT_SYMBOL(iir_df1_4th);
#endif