1 /******************************************************************************
2 *
3 * Copyright 2022 Google LLC
4 *
5 * Licensed under the Apache License, Version 2.0 (the "License");
6 * you may not use this file except in compliance with the License.
7 * You may obtain a copy of the License at:
8 *
9 * http://www.apache.org/licenses/LICENSE-2.0
10 *
11 * Unless required by applicable law or agreed to in writing, software
12 * distributed under the License is distributed on an "AS IS" BASIS,
13 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
14 * See the License for the specific language governing permissions and
15 * limitations under the License.
16 *
17 ******************************************************************************/
18
19 #if __ARM_NEON && __ARM_ARCH_ISA_A64 && \
20 !defined(TEST_ARM) || defined(TEST_NEON)
21
22 #ifndef TEST_NEON
23 #include <arm_neon.h>
24 #endif /* TEST_NEON */
25
26
27 /**
28 * Import
29 */
30
31 static inline int32_t filter_hp50(struct lc3_ltpf_hp50_state *, int32_t);
32
33
34 /**
35 * Resample from 16 Khz to 12.8 KHz
36 */
37 #ifndef resample_16k_12k8
38
neon_resample_16k_12k8(struct lc3_ltpf_hp50_state * hp50,const int16_t * x,int16_t * y,int n)39 LC3_HOT static void neon_resample_16k_12k8(
40 struct lc3_ltpf_hp50_state *hp50, const int16_t *x, int16_t *y, int n)
41 {
42 static const int16_t h[4][20] = {
43
44 { -61, 214, -398, 417, 0, -1052, 2686, -4529, 5997, 26233,
45 5997, -4529, 2686, -1052, 0, 417, -398, 214, -61, 0 },
46
47 { -79, 180, -213, 0, 598, -1522, 2389, -2427, 0, 24506,
48 13068, -5289, 1873, 0, -752, 763, -457, 156, 0, -28 },
49
50 { -61, 92, 0, -323, 861, -1361, 1317, 0, -3885, 19741,
51 19741, -3885, 0, 1317, -1361, 861, -323, 0, 92, -61 },
52
53 { -28, 0, 156, -457, 763, -752, 0, 1873, -5289, 13068,
54 24506, 0, -2427, 2389, -1522, 598, 0, -213, 180, -79 },
55
56 };
57
58 x -= 20 - 1;
59
60 for (int i = 0; i < 5*n; i += 5) {
61 const int16_t *hn = h[i & 3];
62 const int16_t *xn = x + (i >> 2);
63 int32x4_t un;
64
65 un = vmull_s16( vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
66 un = vmlal_s16(un, vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
67 un = vmlal_s16(un, vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
68 un = vmlal_s16(un, vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
69 un = vmlal_s16(un, vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
70
71 int32_t yn = filter_hp50(hp50, vaddvq_s32(un));
72 *(y++) = (yn + (1 << 15)) >> 16;
73 }
74 }
75
76 #ifndef TEST_NEON
77 #define resample_16k_12k8 neon_resample_16k_12k8
78 #endif
79
80 #endif /* resample_16k_12k8 */
81
82 /**
83 * Resample from 32 Khz to 12.8 KHz
84 */
85 #ifndef resample_32k_12k8
86
neon_resample_32k_12k8(struct lc3_ltpf_hp50_state * hp50,const int16_t * x,int16_t * y,int n)87 LC3_HOT static void neon_resample_32k_12k8(
88 struct lc3_ltpf_hp50_state *hp50, const int16_t *x, int16_t *y, int n)
89 {
90 x -= 40 - 1;
91
92 static const int16_t h[2][40] = {
93
94 { -30, -31, 46, 107, 0, -199, -162, 209, 430, 0,
95 -681, -526, 658, 1343, 0, -2264, -1943, 2999, 9871, 13116,
96 9871, 2999, -1943, -2264, 0, 1343, 658, -526, -681, 0,
97 430, 209, -162, -199, 0, 107, 46, -31, -30, 0 },
98
99 { -14, -39, 0, 90, 78, -106, -229, 0, 382, 299,
100 -376, -761, 0, 1194, 937, -1214, -2644, 0, 6534, 12253,
101 12253, 6534, 0, -2644, -1214, 937, 1194, 0, -761, -376,
102 299, 382, 0, -229, -106, 78, 90, 0, -39, -14 },
103
104 };
105
106 for (int i = 0; i < 5*n; i += 5) {
107 const int16_t *hn = h[i & 1];
108 const int16_t *xn = x + (i >> 1);
109
110 int32x4_t un = vmull_s16(vld1_s16(xn), vld1_s16(hn));
111 xn += 4, hn += 4;
112
113 for (int i = 1; i < 10; i++)
114 un = vmlal_s16(un, vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
115
116 int32_t yn = filter_hp50(hp50, vaddvq_s32(un));
117 *(y++) = (yn + (1 << 15)) >> 16;
118 }
119 }
120
121 #ifndef TEST_NEON
122 #define resample_32k_12k8 neon_resample_32k_12k8
123 #endif
124
125 #endif /* resample_32k_12k8 */
126
127 /**
128 * Resample from 48 Khz to 12.8 KHz
129 */
130 #ifndef resample_48k_12k8
131
neon_resample_48k_12k8(struct lc3_ltpf_hp50_state * hp50,const int16_t * x,int16_t * y,int n)132 LC3_HOT static void neon_resample_48k_12k8(
133 struct lc3_ltpf_hp50_state *hp50, const int16_t *x, int16_t *y, int n)
134 {
135 static const int16_t alignas(16) h[4][64] = {
136
137 { -13, -25, -20, 10, 51, 71, 38, -47, -133, -145,
138 -42, 139, 277, 242, 0, -329, -511, -351, 144, 698,
139 895, 450, -535, -1510, -1697, -521, 1999, 5138, 7737, 8744,
140 7737, 5138, 1999, -521, -1697, -1510, -535, 450, 895, 698,
141 144, -351, -511, -329, 0, 242, 277, 139, -42, -145,
142 -133, -47, 38, 71, 51, 10, -20, -25, -13, 0 },
143
144 { -9, -23, -24, 0, 41, 71, 52, -23, -115, -152,
145 -78, 92, 254, 272, 76, -251, -493, -427, 0, 576,
146 900, 624, -262, -1309, -1763, -954, 1272, 4356, 7203, 8679,
147 8169, 5886, 2767, 0, -1542, -1660, -809, 240, 848, 796,
148 292, -252, -507, -398, -82, 199, 288, 183, 0, -130,
149 -145, -71, 20, 69, 60, 20, -15, -26, -17, -3 },
150
151 { -6, -20, -26, -8, 31, 67, 62, 0, -94, -152,
152 -108, 45, 223, 287, 143, -167, -454, -480, -134, 439,
153 866, 758, 0, -1071, -1748, -1295, 601, 3559, 6580, 8485,
154 8485, 6580, 3559, 601, -1295, -1748, -1071, 0, 758, 866,
155 439, -134, -480, -454, -167, 143, 287, 223, 45, -108,
156 -152, -94, 0, 62, 67, 31, -8, -26, -20, -6 },
157
158 { -3, -17, -26, -15, 20, 60, 69, 20, -71, -145,
159 -130, 0, 183, 288, 199, -82, -398, -507, -252, 292,
160 796, 848, 240, -809, -1660, -1542, 0, 2767, 5886, 8169,
161 8679, 7203, 4356, 1272, -954, -1763, -1309, -262, 624, 900,
162 576, 0, -427, -493, -251, 76, 272, 254, 92, -78,
163 -152, -115, -23, 52, 71, 41, 0, -24, -23, -9 },
164
165 };
166
167 x -= 60 - 1;
168
169 for (int i = 0; i < 15*n; i += 15) {
170 const int16_t *hn = h[i & 3];
171 const int16_t *xn = x + (i >> 2);
172
173 int32x4_t un = vmull_s16(vld1_s16(xn), vld1_s16(hn));
174 xn += 4, hn += 4;
175
176 for (int i = 1; i < 15; i++)
177 un = vmlal_s16(un, vld1_s16(xn), vld1_s16(hn)), xn += 4, hn += 4;
178
179 int32_t yn = filter_hp50(hp50, vaddvq_s32(un));
180 *(y++) = (yn + (1 << 15)) >> 16;
181 }
182 }
183
184 #ifndef TEST_NEON
185 #define resample_48k_12k8 neon_resample_48k_12k8
186 #endif
187
188 #endif /* resample_48k_12k8 */
189
190 /**
191 * Return dot product of 2 vectors
192 */
193 #ifndef dot
194
neon_dot(const int16_t * a,const int16_t * b,int n)195 LC3_HOT static inline float neon_dot(const int16_t *a, const int16_t *b, int n)
196 {
197 int64x2_t v = vmovq_n_s64(0);
198
199 for (int i = 0; i < (n >> 4); i++) {
200 int32x4_t u;
201
202 u = vmull_s16( vld1_s16(a), vld1_s16(b)), a += 4, b += 4;
203 u = vmlal_s16(u, vld1_s16(a), vld1_s16(b)), a += 4, b += 4;
204 v = vpadalq_s32(v, u);
205
206 u = vmull_s16( vld1_s16(a), vld1_s16(b)), a += 4, b += 4;
207 u = vmlal_s16(u, vld1_s16(a), vld1_s16(b)), a += 4, b += 4;
208 v = vpadalq_s32(v, u);
209 }
210
211 int32_t v32 = (vaddvq_s64(v) + (1 << 5)) >> 6;
212 return (float)v32;
213 }
214
215 #ifndef TEST_NEON
216 #define dot neon_dot
217 #endif
218
219 #endif /* dot */
220
221 /**
222 * Return vector of correlations
223 */
224 #ifndef correlate
225
neon_correlate(const int16_t * a,const int16_t * b,int n,float * y,int nc)226 LC3_HOT static void neon_correlate(
227 const int16_t *a, const int16_t *b, int n, float *y, int nc)
228 {
229 for ( ; nc >= 4; nc -= 4, b -= 4) {
230 const int16_t *an = (const int16_t *)a;
231 const int16_t *bn = (const int16_t *)b;
232
233 int64x2_t v0 = vmovq_n_s64(0), v1 = v0, v2 = v0, v3 = v0;
234 int16x4_t ax, b0, b1;
235
236 b0 = vld1_s16(bn-4);
237
238 for (int i=0; i < (n >> 4); i++ )
239 for (int j = 0; j < 2; j++) {
240 int32x4_t u0, u1, u2, u3;
241
242 b1 = b0;
243 b0 = vld1_s16(bn), bn += 4;
244 ax = vld1_s16(an), an += 4;
245
246 u0 = vmull_s16(ax, b0);
247 u1 = vmull_s16(ax, vext_s16(b1, b0, 3));
248 u2 = vmull_s16(ax, vext_s16(b1, b0, 2));
249 u3 = vmull_s16(ax, vext_s16(b1, b0, 1));
250
251 b1 = b0;
252 b0 = vld1_s16(bn), bn += 4;
253 ax = vld1_s16(an), an += 4;
254
255 u0 = vmlal_s16(u0, ax, b0);
256 u1 = vmlal_s16(u1, ax, vext_s16(b1, b0, 3));
257 u2 = vmlal_s16(u2, ax, vext_s16(b1, b0, 2));
258 u3 = vmlal_s16(u3, ax, vext_s16(b1, b0, 1));
259
260 v0 = vpadalq_s32(v0, u0);
261 v1 = vpadalq_s32(v1, u1);
262 v2 = vpadalq_s32(v2, u2);
263 v3 = vpadalq_s32(v3, u3);
264 }
265
266 *(y++) = (float)((int32_t)((vaddvq_s64(v0) + (1 << 5)) >> 6));
267 *(y++) = (float)((int32_t)((vaddvq_s64(v1) + (1 << 5)) >> 6));
268 *(y++) = (float)((int32_t)((vaddvq_s64(v2) + (1 << 5)) >> 6));
269 *(y++) = (float)((int32_t)((vaddvq_s64(v3) + (1 << 5)) >> 6));
270 }
271
272 for ( ; nc > 0; nc--)
273 *(y++) = neon_dot(a, b--, n);
274 }
275 #endif /* correlate */
276
277 #ifndef TEST_NEON
278 #define correlate neon_correlate
279 #endif
280
281 #endif /* __ARM_NEON && __ARM_ARCH_ISA_A64 */
282
