1 // SPDX-License-Identifier: BSD-3-Clause
2 //
3 // Copyright(c) 2017 Intel Corporation. All rights reserved.
4 //
5 // Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
6 // Liam Girdwood <liam.r.girdwood@linux.intel.com>
7 // Keyon Jie <yang.jie@linux.intel.com>
8
9 #include <stdint.h>
10 #include <stddef.h>
11 #include <errno.h>
12 #include <sof/audio/format.h>
13 #include <sof/math/iir_df2t.h>
14 #include <user/eq.h>
15
16 #if IIR_HIFI3
17
18 #include <xtensa/tie/xt_hifi3.h>
19
20 /*
21 * Direct form II transposed second order filter block (biquad)
22 *
23 * +----+ +---+ +-------+
24 * X(z) ---o--->| b0 |---> + -------------o--->| g |--->| shift |---> Y(z)
25 * | +----+ ^ | +---+ +-------+
26 * | | |
27 * | +------+ |
28 * | | z^-1 | |
29 * | +------+ |
30 * | ^ |
31 * | +----+ | +----+ |
32 * o--->| b1 |---> + <---| a1 |---o
33 * | +----+ ^ +----+ |
34 * | | |
35 * | +------+ |
36 * | | z^-1 | |
37 * | +------+ |
38 * | ^ |
39 * | +----+ | +----+ |
40 * o--->| b2 |---> + <---| a2 |---+
41 * +----+ +----+
42 *
43 */
44
45 /* Series DF2T IIR */
46
47 /* 32 bit data, 32 bit coefficients and 64 bit state variables */
48
iir_df2t(struct iir_state_df2t * iir,int32_t x)49 int32_t iir_df2t(struct iir_state_df2t *iir, int32_t x)
50 {
51 ae_f64 acc;
52 ae_valign align;
53 ae_f32x2 coef_a2a1;
54 ae_f32x2 coef_b2b1;
55 ae_f32x2 coef_b0shift;
56 ae_f32x2 gain;
57 ae_f32 in;
58 ae_f32 tmp;
59 ae_f32x2 *coefp;
60 ae_f64 *delayp;
61 ae_f32 out = 0;
62 int i;
63 int j;
64 int shift;
65 int nseries = iir->biquads_in_series;
66
67 /* Bypass is set with number of biquads set to zero. */
68 if (!iir->biquads)
69 return x;
70
71 /* Coefficients order in coef[] is {a2, a1, b2, b1, b0, shift, gain} */
72 coefp = (ae_f32x2 *)&iir->coef[0];
73 delayp = (ae_f64 *)&iir->delay[0];
74 in = x;
75 for (j = 0; j < iir->biquads; j += nseries) {
76 for (i = 0; i < nseries; i++) {
77 /* Compute output: Delay is kept Q17.47 while multiply
78 * instruction gives Q2.30 x Q1.31 -> Q18.46. Need to
79 * shift delay line values right by one for same align
80 * as MAC. Store to delay line need to be shifted left
81 * by one similarly.
82 */
83 align = AE_LA64_PP(coefp);
84 AE_LA32X2_IP(coef_a2a1, align, coefp);
85 AE_LA32X2_IP(coef_b2b1, align, coefp);
86 AE_LA32X2_IP(coef_b0shift, align, coefp);
87 AE_LA32X2_IP(gain, align, coefp);
88
89 acc = AE_SRAI64(*delayp, 1); /* Convert d0 to Q18.46 */
90 delayp++; /* Point to d1 */
91 AE_MULAF32R_HH(acc, coef_b0shift, in); /* Coef b0 */
92 acc = AE_SLAI64S(acc, 1); /* Convert to Q17.47 */
93 tmp = AE_ROUND32F48SSYM(acc); /* Rount to Q1.31 */
94
95 /* Compute 1st delay d0 */
96 acc = AE_SRAI64(*delayp, 1); /* Convert d1 to Q18.46 */
97 delayp--; /* Point to d0 */
98 AE_MULAF32R_LL(acc, coef_b2b1, in); /* Coef b1 */
99 AE_MULAF32R_LL(acc, coef_a2a1, tmp); /* Coef a1 */
100 acc = AE_SLAI64S(acc, 1); /* Convert to Q17.47 */
101 *delayp = acc; /* Store d0 */
102 delayp++; /* Point to d1 */
103
104 /* Compute delay d1 */
105 acc = AE_MULF32R_HH(coef_b2b1, in); /* Coef b2 */
106 AE_MULAF32R_HH(acc, coef_a2a1, tmp); /* Coef a2 */
107 acc = AE_SLAI64S(acc, 1); /* Convert to Q17.47 */
108 *delayp = acc; /* Store d1 */
109
110 /* Apply gain Q18.14 x Q1.31 -> Q34.30 */
111 acc = AE_MULF32R_HH(gain, tmp); /* Gain */
112 acc = AE_SLAI64S(acc, 17); /* Convert to Q17.47 */
113
114 /* Apply biquad output shift right parameter and then
115 * round and saturate to 32 bits Q1.31.
116 */
117 shift = AE_SEL32_LL(coef_b0shift, coef_b0shift);
118 acc = AE_SRAA64(acc, shift);
119 in = AE_ROUND32F48SSYM(acc);
120
121 /* Proceed to next biquad coefficients and delay
122 * lines. The coefp needs rewind by one int32_t
123 * due to odd number of words in coefficient block.
124 */
125 delayp++;
126 coefp = (ae_f32x2 *)((int32_t *)coefp - 1);
127 }
128 /* Output of previous section is in variable in */
129 out = AE_F32_ADDS_F32(out, in);
130 }
131 return out;
132 }
133
134 #endif
135
