chris@162
|
1 /**
|
chris@162
|
2 * Copyright (c) 2014, 2015, Enzien Audio Ltd.
|
chris@162
|
3 *
|
chris@162
|
4 * Permission to use, copy, modify, and/or distribute this software for any
|
chris@162
|
5 * purpose with or without fee is hereby granted, provided that the above
|
chris@162
|
6 * copyright notice and this permission notice appear in all copies.
|
chris@162
|
7 *
|
chris@162
|
8 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES WITH
|
chris@162
|
9 * REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY
|
chris@162
|
10 * AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT,
|
chris@162
|
11 * INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM
|
chris@162
|
12 * LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR
|
chris@162
|
13 * OTHER TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR
|
chris@162
|
14 * PERFORMANCE OF THIS SOFTWARE.
|
chris@162
|
15 */
|
chris@162
|
16
|
chris@162
|
17 #include "SignalBiquad.h"
|
chris@162
|
18
|
chris@162
|
19 // http://reanimator-web.appspot.com/articles/simdiir
|
chris@162
|
20 // http://musicdsp.org/files/Audio-EQ-Cookbook.txt
|
chris@162
|
21
|
chris@162
|
22 hv_size_t sBiquad_init(SignalBiquad *o) {
|
chris@162
|
23 #if HV_SIMD_AVX
|
chris@162
|
24 o->xm1 = _mm256_setzero_ps();
|
chris@162
|
25 o->xm2 = _mm256_setzero_ps();
|
chris@162
|
26 #elif HV_SIMD_SSE
|
chris@162
|
27 o->xm1 = _mm_setzero_ps();
|
chris@162
|
28 o->xm2 = _mm_setzero_ps();
|
chris@162
|
29 #elif HV_SIMD_NEON
|
chris@162
|
30 o->xm1 = vdupq_n_f32(0.0f);
|
chris@162
|
31 o->xm2 = vdupq_n_f32(0.0f);
|
chris@162
|
32 #else // HV_SIMD_NONE
|
chris@162
|
33 o->x1 = 0.0f;
|
chris@162
|
34 o->x2 = 0.0f;
|
chris@162
|
35 #endif
|
chris@162
|
36 o->y1 = 0.0f;
|
chris@162
|
37 o->y2 = 0.0f;
|
chris@162
|
38 return 0;
|
chris@162
|
39 }
|
chris@162
|
40
|
chris@162
|
41 void __hv_biquad_f(SignalBiquad *o, hv_bInf_t bIn, hv_bInf_t bX0, hv_bInf_t bX1, hv_bInf_t bX2, hv_bInf_t bY1, hv_bInf_t bY2, hv_bOutf_t bOut) {
|
chris@162
|
42 #if HV_SIMD_AVX
|
chris@162
|
43 __m256 a = _mm256_mul_ps(bIn, bX0);
|
chris@162
|
44 __m256 b = _mm256_mul_ps(o->xm1, bX1);
|
chris@162
|
45 __m256 c = _mm256_mul_ps(o->xm2, bX2);
|
chris@162
|
46 __m256 d = _mm256_add_ps(a, b);
|
chris@162
|
47 __m256 e = _mm256_add_ps(c, d); // bIn*bX0 + o->x1*bX1 + o->x2*bX2
|
chris@162
|
48 float y0 = e[0] - o->y1*bY1[0] - o->y2*bY2[0];
|
chris@162
|
49 float y1 = e[1] - y0*bY1[1] - o->y1*bY2[1];
|
chris@162
|
50 float y2 = e[2] - y1*bY1[2] - y0*bY2[2];
|
chris@162
|
51 float y3 = e[3] - y2*bY1[3] - y1*bY2[3];
|
chris@162
|
52 float y4 = e[4] - y3*bY1[4] - y2*bY2[4];
|
chris@162
|
53 float y5 = e[5] - y4*bY1[5] - y3*bY2[5];
|
chris@162
|
54 float y6 = e[6] - y5*bY1[6] - y4*bY2[6];
|
chris@162
|
55 float y7 = e[7] - y6*bY1[7] - y5*bY2[7];
|
chris@162
|
56
|
chris@162
|
57 o->xm2 = o->xm1;
|
chris@162
|
58 o->xm1 = bIn;
|
chris@162
|
59 o->y1 = y7;
|
chris@162
|
60 o->y2 = y6;
|
chris@162
|
61
|
chris@162
|
62 *bOut = _mm256_set_ps(y7, y6, y5, y4, y3, y2, y1, y0);
|
chris@162
|
63 #elif HV_SIMD_SSE
|
chris@162
|
64 __m128 a = _mm_mul_ps(bIn, bX0);
|
chris@162
|
65 __m128 b = _mm_mul_ps(o->xm1, bX1);
|
chris@162
|
66 __m128 c = _mm_mul_ps(o->xm2, bX2);
|
chris@162
|
67 __m128 d = _mm_add_ps(a, b);
|
chris@162
|
68 __m128 e = _mm_add_ps(c, d);
|
chris@162
|
69 float y0 = e[0] - o->y1*bY1[0] - o->y2*bY2[0];
|
chris@162
|
70 float y1 = e[1] - y0*bY1[1] - o->y1*bY2[1];
|
chris@162
|
71 float y2 = e[2] - y1*bY1[2] - y0*bY2[2];
|
chris@162
|
72 float y3 = e[3] - y2*bY1[3] - y1*bY2[3];
|
chris@162
|
73
|
chris@162
|
74 o->xm2 = o->xm1;
|
chris@162
|
75 o->xm1 = bIn;
|
chris@162
|
76 o->y1 = y3;
|
chris@162
|
77 o->y2 = y2;
|
chris@162
|
78
|
chris@162
|
79 *bOut = _mm_set_ps(y3, y2, y1, y0);
|
chris@162
|
80 #elif HV_SIMD_NEON
|
chris@162
|
81 float32x4_t a = vmulq_f32(bIn, bX0);
|
chris@162
|
82 float32x4_t b = vmulq_f32(o->xm1, bX1);
|
chris@162
|
83 float32x4_t c = vmulq_f32(o->xm2, bX2);
|
chris@162
|
84 float32x4_t d = vaddq_f32(a, b);
|
chris@162
|
85 float32x4_t e = vaddq_f32(c, d);
|
chris@162
|
86 float y0 = e[0] - o->y1*bY1[0] - o->y2*bY2[0];
|
chris@162
|
87 float y1 = e[1] - y0*bY1[1] - o->y1*bY2[1];
|
chris@162
|
88 float y2 = e[2] - y1*bY1[2] - y0*bY2[2];
|
chris@162
|
89 float y3 = e[3] - y2*bY1[3] - y1*bY2[3];
|
chris@162
|
90
|
chris@162
|
91 o->xm2 = o->xm1;
|
chris@162
|
92 o->xm1 = bIn;
|
chris@162
|
93 o->y1 = y3;
|
chris@162
|
94 o->y2 = y2;
|
chris@162
|
95
|
chris@162
|
96 *bOut = (float32x4_t) {y0, y1, y2, y3};
|
chris@162
|
97 #else
|
chris@162
|
98 const float y = bIn*bX0 + o->x1*bX1 + o->x2*bX2 - o->y1*bY1 - o->y2*bY2;
|
chris@162
|
99 o->x2 = o->x1; o->x1 = bIn;
|
chris@162
|
100 o->y2 = o->y1; o->y1 = y;
|
chris@162
|
101 *bOut = y;
|
chris@162
|
102 #endif
|
chris@162
|
103 }
|
chris@162
|
104
|
chris@162
|
105 static void sBiquad_k_updateCoefficients(SignalBiquad_k *const o) {
|
chris@162
|
106 // calculate all filter coefficients in the double domain
|
chris@162
|
107 #if HV_SIMD_AVX || HV_SIMD_SSE || HV_SIMD_NEON
|
chris@162
|
108 double b0 = (double) o->b0;
|
chris@162
|
109 double b1 = (double) o->b1;
|
chris@162
|
110 double b2 = (double) o->b2;
|
chris@162
|
111 double a1 = (double) -o->a1;
|
chris@162
|
112 double a2 = (double) -o->a2;
|
chris@162
|
113
|
chris@162
|
114 double coeffs[4][8] =
|
chris@162
|
115 {
|
chris@162
|
116 { 0, 0, 0, b0, b1, b2, a1, a2 },
|
chris@162
|
117 { 0, 0, b0, b1, b2, 0, a2, 0 },
|
chris@162
|
118 { 0, b0, b1, b2, 0, 0, 0, 0 },
|
chris@162
|
119 { b0, b1, b2, 0, 0, 0, 0, 0 },
|
chris@162
|
120 };
|
chris@162
|
121
|
chris@162
|
122 for (int i = 0; i < 8; i++) {
|
chris@162
|
123 coeffs[1][i] += a1*coeffs[0][i];
|
chris@162
|
124 coeffs[2][i] += a1*coeffs[1][i] + a2*coeffs[0][i];
|
chris@162
|
125 coeffs[3][i] += a1*coeffs[2][i] + a2*coeffs[1][i];
|
chris@162
|
126 }
|
chris@162
|
127
|
chris@162
|
128 #if HV_SIMD_AVX || HV_SIMD_SSE
|
chris@162
|
129 o->coeff_xp3 = _mm_set_ps((float) coeffs[3][0], (float) coeffs[2][0], (float) coeffs[1][0], (float) coeffs[0][0]);
|
chris@162
|
130 o->coeff_xp2 = _mm_set_ps((float) coeffs[3][1], (float) coeffs[2][1], (float) coeffs[1][1], (float) coeffs[0][1]);
|
chris@162
|
131 o->coeff_xp1 = _mm_set_ps((float) coeffs[3][2], (float) coeffs[2][2], (float) coeffs[1][2], (float) coeffs[0][2]);
|
chris@162
|
132 o->coeff_x0 = _mm_set_ps((float) coeffs[3][3], (float) coeffs[2][3], (float) coeffs[1][3], (float) coeffs[0][3]);
|
chris@162
|
133 o->coeff_xm1 = _mm_set_ps((float) coeffs[3][4], (float) coeffs[2][4], (float) coeffs[1][4], (float) coeffs[0][4]);
|
chris@162
|
134 o->coeff_xm2 = _mm_set_ps((float) coeffs[3][5], (float) coeffs[2][5], (float) coeffs[1][5], (float) coeffs[0][5]);
|
chris@162
|
135 o->coeff_ym1 = _mm_set_ps((float) coeffs[3][6], (float) coeffs[2][6], (float) coeffs[1][6], (float) coeffs[0][6]);
|
chris@162
|
136 o->coeff_ym2 = _mm_set_ps((float) coeffs[3][7], (float) coeffs[2][7], (float) coeffs[1][7], (float) coeffs[0][7]);
|
chris@162
|
137 #else // HV_SIMD_NEON
|
chris@162
|
138 o->coeff_xp3 = (float32x4_t) {(float) coeffs[0][0], (float) coeffs[1][0], (float) coeffs[2][0], (float) coeffs[3][0]};
|
chris@162
|
139 o->coeff_xp2 = (float32x4_t) {(float) coeffs[0][1], (float) coeffs[1][1], (float) coeffs[2][1], (float) coeffs[3][1]};
|
chris@162
|
140 o->coeff_xp1 = (float32x4_t) {(float) coeffs[0][2], (float) coeffs[1][2], (float) coeffs[2][2], (float) coeffs[3][2]};
|
chris@162
|
141 o->coeff_x0 = (float32x4_t) {(float) coeffs[0][3], (float) coeffs[1][3], (float) coeffs[2][3], (float) coeffs[3][3]};
|
chris@162
|
142 o->coeff_xm1 = (float32x4_t) {(float) coeffs[0][4], (float) coeffs[1][4], (float) coeffs[2][4], (float) coeffs[3][4]};
|
chris@162
|
143 o->coeff_xm2 = (float32x4_t) {(float) coeffs[0][5], (float) coeffs[1][5], (float) coeffs[2][5], (float) coeffs[3][5]};
|
chris@162
|
144 o->coeff_ym1 = (float32x4_t) {(float) coeffs[0][6], (float) coeffs[1][6], (float) coeffs[2][6], (float) coeffs[3][6]};
|
chris@162
|
145 o->coeff_ym2 = (float32x4_t) {(float) coeffs[0][7], (float) coeffs[1][7], (float) coeffs[2][7], (float) coeffs[3][7]};
|
chris@162
|
146 #endif
|
chris@162
|
147 #endif
|
chris@162
|
148 // NOTE(mhroth): not necessary to calculate any coefficients for HV_SIMD_NONE case
|
chris@162
|
149 }
|
chris@162
|
150
|
chris@162
|
151 hv_size_t sBiquad_k_init(SignalBiquad_k *o, float b0, float b1, float b2, float a1, float a2) {
|
chris@162
|
152 // initialise filter coefficients
|
chris@162
|
153 o->b0 = b0;
|
chris@162
|
154 o->b1 = b1;
|
chris@162
|
155 o->b2 = b2;
|
chris@162
|
156 o->a1 = a1;
|
chris@162
|
157 o->a2 = a2;
|
chris@162
|
158 sBiquad_k_updateCoefficients(o);
|
chris@162
|
159
|
chris@162
|
160 // clear filter state
|
chris@162
|
161 #if HV_SIMD_AVX || HV_SIMD_SSE
|
chris@162
|
162 o->xm1 = _mm_setzero_ps();
|
chris@162
|
163 o->xm2 = _mm_setzero_ps();
|
chris@162
|
164 o->ym1 = _mm_setzero_ps();
|
chris@162
|
165 o->ym2 = _mm_setzero_ps();
|
chris@162
|
166 #elif HV_SIMD_NEON
|
chris@162
|
167 o->xm1 = vdupq_n_f32(0.0f);
|
chris@162
|
168 o->xm2 = vdupq_n_f32(0.0f);
|
chris@162
|
169 o->ym1 = vdupq_n_f32(0.0f);
|
chris@162
|
170 o->ym2 = vdupq_n_f32(0.0f);
|
chris@162
|
171 #else // HV_SIMD_NONE
|
chris@162
|
172 o->xm1 = 0.0f;
|
chris@162
|
173 o->xm2 = 0.0f;
|
chris@162
|
174 o->ym1 = 0.0f;
|
chris@162
|
175 o->ym2 = 0.0f;
|
chris@162
|
176 #endif
|
chris@162
|
177 return 0;
|
chris@162
|
178 }
|
chris@162
|
179
|
chris@162
|
180 void sBiquad_k_onMessage(SignalBiquad_k *o, int letIn, const HvMessage *const m) {
|
chris@162
|
181 if (msg_isFloat(m,0)) {
|
chris@162
|
182 switch (letIn) {
|
chris@162
|
183 case 1: o->b0 = msg_getFloat(m,0); break;
|
chris@162
|
184 case 2: o->b1 = msg_getFloat(m,0); break;
|
chris@162
|
185 case 3: o->b2 = msg_getFloat(m,0); break;
|
chris@162
|
186 case 4: o->a1 = msg_getFloat(m,0); break;
|
chris@162
|
187 case 5: o->a2 = msg_getFloat(m,0); break;
|
chris@162
|
188 default: return;
|
chris@162
|
189 }
|
chris@162
|
190 sBiquad_k_updateCoefficients(o);
|
chris@162
|
191 }
|
chris@162
|
192 }
|