forked from thesofproject/sof
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathiir_df1_hifi5.c
More file actions
119 lines (103 loc) · 3.69 KB
/
Copy pathiir_df1_hifi5.c
File metadata and controls
119 lines (103 loc) · 3.69 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
// SPDX-License-Identifier: BSD-3-Clause
//
// Copyright(c) 2022-2024 Intel Corporation.
//
// Author: Seppo Ingalsuo <seppo.ingalsuo@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 <sof/common.h>
#include <rtos/symbol.h>
#if SOF_USE_MIN_HIFI(5, 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)
{
ae_valignx2 coef_align;
ae_valignx2 data_r_align;
ae_valignx2 data_w_align = AE_ZALIGN128();
ae_f64 acc;
ae_int32x2 delay_y2y1;
ae_int32x2 delay_x2x1;
ae_int32x2 coef_a2a1;
ae_int32x2 coef_b2b1;
ae_int32x2 coef_b0;
ae_int32x2 gain;
ae_int32x2 shift;
ae_int32 in;
ae_int32 out = 0;
ae_int32x4 *coefp = (ae_int32x4 *)iir->coef;
ae_int32x4 *delay_r = (ae_int32x4 *)iir->delay;
ae_int32x4 *delay_w = delay_r;
int i;
int j;
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)} */
data_r_align = AE_LA128_PP(delay_r);
for (j = 0; j < iir->biquads; j += nseries) {
in = x;
for (i = 0; i < nseries; i++) {
/* Load data */
AE_LA32X2X2_IP(delay_y2y1, delay_x2x1, data_r_align, delay_r);
/* Load coefficients */
coef_align = AE_LA128_PP(coefp);
AE_LA32X2X2_IP(coef_a2a1, coef_b2b1, coef_align, coefp);
AE_L32_IP(coef_b0, (ae_int32 *)coefp, 4);
AE_L32_IP(shift, (ae_int32 *)coefp, 4);
AE_L32_IP(gain, (ae_int32 *)coefp, 4);
acc = AE_MULF32RA_HH(coef_b0, in); /* acc = b0 * in */
AE_MULAAFD32RA_HH_LL(acc, coef_a2a1, delay_y2y1); /* + a2 * y2 + a1 * y1 */
AE_MULAAFD32RA_HH_LL(acc, coef_b2b1, delay_x2x1); /* + b2 * x2 + b1 * x1 */
AE_PKSR32(delay_y2y1, acc, 1); /* y2 = y1, y1 = acc(q1.31) */
delay_x2x1 = AE_SEL32_LL(delay_x2x1, in); /* x2 = x1, x1 = in */
/* Store data */
AE_SA32X2X2_IP(delay_y2y1, delay_x2x1, data_w_align, delay_w);
/* Apply gain */
acc = AE_MULF32R_LL(gain, delay_y2y1); /* acc = gain * y1 */
acc = AE_SLAI64S(acc, 17); /* Convert to Q17.47 */
/* Apply biquad output shift right parameter and then
* round and saturate to 32 bits Q1.31.
*/
acc = AE_SRAA64(acc, shift);
in = AE_ROUND32F48SSYM(acc);
}
/* Output of previous section is in variable in */
out = AE_F32_ADDS_F32(out, in);
}
AE_SA128POS_FP(data_w_align, delay_w);
return out;
}
EXPORT_SYMBOL(iir_df1);
#endif