Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/aarch64/asimd/correlation.h

raw
1 /*
2 * Copyright (C) 2024 Linux Studio Plugins Project <https://lsp-plug.in/>
3 * (C) 2024 Vladimir Sadovnikov <[email protected]>
4 *
5 * This file is part of lsp-dsp-lib
6 * Created on: 9 мар. 2024 г.
7 *
8 * lsp-dsp-lib is free software: you can redistribute it and/or modify
9 * it under the terms of the GNU Lesser General Public License as published by
10 * the Free Software Foundation, either version 3 of the License, or
11 * any later version.
12 *
13 * lsp-dsp-lib is distributed in the hope that it will be useful,
14 * but WITHOUT ANY WARRANTY; without even the implied warranty of
15 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 * GNU Lesser General Public License for more details.
17 *
18 * You should have received a copy of the GNU Lesser General Public License
19 * along with lsp-dsp-lib. If not, see <https://www.gnu.org/licenses/>.
20 */
21
22 #ifndef PRIVATE_DSP_ARCH_AARCH64_ASIMD_CORRELATION_H_
23 #define PRIVATE_DSP_ARCH_AARCH64_ASIMD_CORRELATION_H_
24
25 #ifndef PRIVATE_DSP_ARCH_AARCH64_ASIMD_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_AARCH64_ASIMD_IMPL */
28
29 namespace lsp
30 {
31 namespace asimd
32 {
33
34 void corr_init(dsp::correlation_t *corr, const float *a, const float *b, size_t count)
35 {
36 ARCH_AARCH64_ASM(
37 __ASM_EMIT("eor v0.16b, v0.16b, v0.16b") /* xv = 0 */
38 __ASM_EMIT("eor v1.16b, v1.16b, v1.16b") /* xa = 0 */
39 __ASM_EMIT("eor v2.16b, v2.16b, v2.16b") /* xb = 0 */
40 /* 16x blocks */
41 __ASM_EMIT("subs %[count], %[count], #16")
42 __ASM_EMIT("b.lo 2f")
43 __ASM_EMIT("eor v3.16b, v3.16b, v3.16b") /* xv = 0 */
44 __ASM_EMIT("eor v4.16b, v4.16b, v4.16b") /* xa = 0 */
45 __ASM_EMIT("eor v5.16b, v5.16b, v5.16b") /* xb = 0 */
46 __ASM_EMIT("1:")
47 __ASM_EMIT("ldp q8, q9, [%[a], 0x00]") /* v8 = a0, v9 = a1 */
48 __ASM_EMIT("ldp q10, q11, [%[a], 0x20]") /* v10 = a2, v11 = a3 */
49 __ASM_EMIT("ldp q12, q13, [%[b], 0x00]") /* v12 = b0, v13 = b1 */
50 __ASM_EMIT("ldp q14, q15, [%[b], 0x20]") /* v14 = b2, v15 = b3 */
51 __ASM_EMIT("fmla v0.4s, v8.4s, v12.4s") /* v0 = xv + a0*b0 */
52 __ASM_EMIT("fmla v3.4s, v9.4s, v13.4s") /* v3 = xv + a1*b1 */
53 __ASM_EMIT("fmla v1.4s, v8.4s, v8.4s") /* v1 = xa + a0*a0 */
54 __ASM_EMIT("fmla v4.4s, v9.4s, v9.4s") /* v4 = xa + a1*a1 */
55 __ASM_EMIT("fmla v2.4s, v12.4s, v12.4s") /* v2 = xb + b0*b0 */
56 __ASM_EMIT("fmla v5.4s, v13.4s, v13.4s") /* v5 = xb + b1*b1 */
57 __ASM_EMIT("fmla v0.4s, v10.4s, v14.4s") /* v0 = xv + a0*b0 */
58 __ASM_EMIT("fmla v3.4s, v11.4s, v15.4s") /* v3 = xv + a1*b1 */
59 __ASM_EMIT("fmla v1.4s, v10.4s, v10.4s") /* v1 = xa + a0*a0 */
60 __ASM_EMIT("fmla v4.4s, v11.4s, v11.4s") /* v4 = xa + a1*a1 */
61 __ASM_EMIT("fmla v2.4s, v14.4s, v14.4s") /* v2 = xb + b0*b0 */
62 __ASM_EMIT("fmla v5.4s, v15.4s, v15.4s") /* v5 = xb + b1*b1 */
63 __ASM_EMIT("subs %[count], %[count], #16")
64 __ASM_EMIT("add %[a], %[a], #0x40")
65 __ASM_EMIT("add %[b], %[b], #0x40")
66 __ASM_EMIT("b.hs 1b")
67 __ASM_EMIT("fadd v0.4s, v0.4s, v3.4s")
68 __ASM_EMIT("fadd v1.4s, v1.4s, v4.4s")
69 __ASM_EMIT("fadd v2.4s, v2.4s, v5.4s")
70 __ASM_EMIT("2:")
71 /* 8x block */
72 __ASM_EMIT("adds %[count], %[count], #8")
73 __ASM_EMIT("b.lt 4f")
74 __ASM_EMIT("ldp q8, q9, [%[a], 0x00]") /* v8 = a0, v9 = a1 */
75 __ASM_EMIT("ldp q12, q13, [%[b], 0x00]") /* v12 = b0, v13 = b1 */
76 __ASM_EMIT("fmla v0.4s, v8.4s, v12.4s") /* v0 = xv + a0*b0 */
77 __ASM_EMIT("fmla v1.4s, v8.4s, v8.4s") /* v1 = xa + a0*a0 */
78 __ASM_EMIT("fmla v2.4s, v12.4s, v12.4s") /* v2 = xb + b0*b0 */
79 __ASM_EMIT("fmla v0.4s, v9.4s, v13.4s") /* v0 = xv + a1*b1 */
80 __ASM_EMIT("fmla v1.4s, v9.4s, v9.4s") /* v1 = xa + a1*a1 */
81 __ASM_EMIT("fmla v2.4s, v13.4s, v13.4s") /* v2 = xb + b1*b1 */
82 __ASM_EMIT("sub %[count], %[count], #8")
83 __ASM_EMIT("add %[a], %[a], #0x20")
84 __ASM_EMIT("add %[b], %[b], #0x20")
85 __ASM_EMIT("4:")
86 /* 4x block */
87 __ASM_EMIT("adds %[count], %[count], #4")
88 __ASM_EMIT("b.lt 6f")
89 __ASM_EMIT("ldr q8, [%[a], 0x00]") /* v8 = a0, v9 = a1 */
90 __ASM_EMIT("ldr q12, [%[b], 0x00]") /* v12 = b0, v13 = b1 */
91 __ASM_EMIT("fmla v0.4s, v8.4s, v12.4s") /* v0 = xv + a0*b0 */
92 __ASM_EMIT("fmla v1.4s, v8.4s, v8.4s") /* v1 = xa + a0*a0 */
93 __ASM_EMIT("fmla v2.4s, v12.4s, v12.4s") /* v2 = xb + b0*b0 */
94 __ASM_EMIT("sub %[count], %[count], #4")
95 __ASM_EMIT("add %[a], %[a], #0x10")
96 __ASM_EMIT("add %[b], %[b], #0x10")
97 __ASM_EMIT("6:")
98 __ASM_EMIT("eor v6.16b, v6.16b, v6.16b") /* v6 = 0 */
99 __ASM_EMIT("ext v3.16b, v0.16b, v6.16b, #8")/* v3 = xv2 xv3 0 0 */
100 __ASM_EMIT("ext v4.16b, v1.16b, v6.16b, #8")/* v4 = xa2 xb3 0 0 */
101 __ASM_EMIT("ext v5.16b, v2.16b, v6.16b, #8")/* v5 = xa2 xb3 0 0 */
102 __ASM_EMIT("fadd v0.4s, v0.4s, v3.4s") /* v0 = xv0+xv2 xv1+xv3 xv2 xv3 */
103 __ASM_EMIT("fadd v1.4s, v1.4s, v4.4s") /* v1 = xa0+xa2 xa1+xa3 xa2 xa3 */
104 __ASM_EMIT("fadd v2.4s, v2.4s, v5.4s") /* v2 = xb0+xb2 xb1+xb3 xb2 xb3 */
105 __ASM_EMIT("ext v3.16b, v0.16b, v6.16b, #4")/* v3 = xv1+xv3 xv2 xv3 0 */
106 __ASM_EMIT("ext v4.16b, v1.16b, v6.16b, #4")/* v4 = xa1+xa3 xa2 xa3 0 */
107 __ASM_EMIT("ext v5.16b, v2.16b, v6.16b, #4")/* v5 = xb1+xb3 xb2 xb3 0 */
108 __ASM_EMIT("fadd v0.4s, v0.4s, v3.4s") /* v0 = xv0+xv1+xv2+xv3 xv1+xv2+xv3 xv2+xv3 xv3 */
109 __ASM_EMIT("fadd v1.4s, v1.4s, v4.4s") /* v1 = xa0+xa1+xa2+xa3 xa1+xa2+xa3 xa2+xa3 xa3 */
110 __ASM_EMIT("fadd v2.4s, v2.4s, v5.4s") /* v2 = xb0+xb1+xb2+xb3 xb1+xb2+xb3 xb2+xb3 xb3 */
111 /* 1x blocks */
112 __ASM_EMIT("adds %[count], %[count], #3")
113 __ASM_EMIT("b.lt 8f")
114 __ASM_EMIT("7:")
115 __ASM_EMIT("ld1r {v8.4s}, [%[a]]") /* v8 = a0 */
116 __ASM_EMIT("ld1r {v12.4s}, [%[b]]") /* q12 = b0 */
117 __ASM_EMIT("fmla v0.4s, v8.4s, v12.4s") /* v0 = xv + a0*b0 */
118 __ASM_EMIT("fmla v1.4s, v8.4s, v8.4s") /* v1 = xa + a0*a0 */
119 __ASM_EMIT("fmla v2.4s, v12.4s, v12.4s") /* v2 = xb + b0*b0 */
120 __ASM_EMIT("subs %[count], %[count], #1")
121 __ASM_EMIT("add %[a], %[a], #0x04")
122 __ASM_EMIT("add %[b], %[b], #0x04")
123 __ASM_EMIT("b.ge 7b")
124 __ASM_EMIT("8:")
125 /* Store result */
126 __ASM_EMIT("ld3r {v3.4s, v4.4s, v5.4s}, [%[corr]]") /* v3 = v, v4 = a, v5 = b */
127 __ASM_EMIT("fadd v0.4s, v0.4s, v3.4s")
128 __ASM_EMIT("fadd v1.4s, v1.4s, v4.4s")
129 __ASM_EMIT("fadd v2.4s, v2.4s, v5.4s")
130 __ASM_EMIT("st3 {v0.s, v1.s, v2.s}[0], [%[corr]]")
131
132 : [a] "+r" (a), [b] "+r" (b), [count] "+r" (count)
133 : [corr] "r" (corr)
134 : "cc", "memory",
135 "v0", "v1", "v2", "v3", "v4", "v5", "v6",
136 "v8", "v9", "v10", "v11", "v12", "v13", "v14", "v15"
137 );
138 }
139
140 static const float corr_const[] __lsp_aligned16 =
141 {
142 LSP_DSP_VEC8(1e-18f)
143 };
144
145 void corr_incr(dsp::correlation_t *corr, float *dst,
146 const float *a_head, const float *b_head,
147 const float *a_tail, const float *b_tail,
148 size_t count)
149 {
150 ARCH_AARCH64_ASM(
151 __ASM_EMIT("ld3r {v0.4s, v1.4s, v2.4s}, [%[corr]]") /* v0 = xv, v1 = xa, v2 = xb */
152 __ASM_EMIT("eor v3.16b, v3.16b, v3.16b") /* v3 = 0 */
153 /* 8x blocks */
154 __ASM_EMIT("subs %[count], %[count], #8")
155 __ASM_EMIT("b.lo 2f")
156 __ASM_EMIT("1:")
157 __ASM_EMIT("ldp q4, q5, [%[a_head], 0x00]") /* v4 = ah0, v5 = ah1 */
158 __ASM_EMIT("ldp q6, q7, [%[b_head], 0x00]") /* v6 = ah0, v7 = ah1 */
159 __ASM_EMIT("ldp q8, q9, [%[a_tail], 0x00]") /* v8 = ah0, v9 = ah1 */
160 __ASM_EMIT("ldp q10, q11, [%[b_tail], 0x00]") /* v10 = ah0, v11 = ah1 */
161 __ASM_EMIT("fmul v12.4s, v4.4s, v6.4s") /* v12 = ah0*bh0 */
162 __ASM_EMIT("fmul v13.4s, v5.4s, v7.4s")
163 __ASM_EMIT("fmul v4.4s, v4.4s, v4.4s") /* v4 = ah0*ah0 */
164 __ASM_EMIT("fmul v5.4s, v5.4s, v5.4s")
165 __ASM_EMIT("fmul v6.4s, v6.4s, v6.4s") /* v6 = bh0*bh0 */
166 __ASM_EMIT("fmul v7.4s, v7.4s, v7.4s")
167 __ASM_EMIT("fmls v12.4s, v8.4s, v10.4s") /* v12 = DV = ah0*bh0 - at0*bt0 */
168 __ASM_EMIT("fmls v13.4s, v9.4s, v11.4s")
169 __ASM_EMIT("fmls v4.4s, v8.4s, v8.4s") /* v4 = DA = ah0*ah0 - at0*at0 */
170 __ASM_EMIT("fmls v5.4s, v9.4s, v9.4s")
171 __ASM_EMIT("fmls v6.4s, v10.4s, v10.4s") /* v6 = DB = bh0*bh0 - bt0*bt0 */
172 __ASM_EMIT("fmls v7.4s, v11.4s, v11.4s")
173
174 __ASM_EMIT("ext v14.16b, v3.16b, v12.16b, #8") /* v14 = 0 0 DV[0] DV[1] */
175 __ASM_EMIT("ext v15.16b, v3.16b, v13.16b, #8") /* v15 = 0 0 DV[4] DV[5] */
176 __ASM_EMIT("fadd v12.4s, v12.4s, v14.4s") /* v12 = DV[0] DV[1] DV[0]+DV[2] DV[1]+DV[3] */
177 __ASM_EMIT("fadd v13.4s, v13.4s, v15.4s") /* v13 = DV[4] DV[5] DV[4]+DV[6] DV[5]+DV[7] */
178 __ASM_EMIT("ext v14.16b, v3.16b, v12.16b, #12") /* v14 = 0 DV[0] DV[1] DV[0]+DV[2] */
179 __ASM_EMIT("ext v15.16b, v3.16b, v13.16b, #12") /* v15 = 0 DV[4] DV[5] DV[4]+DV[6] */
180 __ASM_EMIT("fadd v12.4s, v12.4s, v14.4s") /* v12 = V[0..3] = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] */
181 __ASM_EMIT("fadd v13.4s, v13.4s, v15.4s") /* v13 = V[4..7] = DV[4] DV[4]+DV[5] DV[4]+DV[5]+DV[6] DV[4]+DV[5]+DV[6]+DV[7] */
182 __ASM_EMIT("ext v14.16b, v3.16b, v4.16b, #8") /* v14 = 0 0 DA[0] DA[1] */
183 __ASM_EMIT("ext v15.16b, v3.16b, v5.16b, #8") /* v15 = 0 0 DA[4] DA[5] */
184 __ASM_EMIT("fadd v4.4s, v4.4s, v14.4s") /* v4 = DA[0] DA[1] DA[0]+DA[2] DA[1]+DA[3] */
185 __ASM_EMIT("fadd v5.4s, v5.4s, v15.4s") /* v5 = DA[4] DA[5] DA[4]+DA[6] DA[5]+DA[7] */
186 __ASM_EMIT("ext v14.16b, v3.16b, v4.16b, #12") /* v14 = 0 DA[0] DA[1] DA[0]+DA[2] */
187 __ASM_EMIT("ext v15.16b, v3.16b, v5.16b, #12") /* v15 = 0 DA[4] DA[5] DA[4]+DA[6] */
188 __ASM_EMIT("fadd v4.4s, v4.4s, v14.4s") /* v4 = A[0..3] = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] */
189 __ASM_EMIT("fadd v5.4s, v5.4s, v15.4s") /* v5 = A[4..7] = DA[4] DA[4]+DA[5] DA[4]+DA[5]+DA[6] DA[4]+DA[5]+DA[6]+DA[7] */
190 __ASM_EMIT("ext v14.16b, v3.16b, v6.16b, #8") /* v14 = 0 0 DB[0] DB[1] */
191 __ASM_EMIT("ext v15.16b, v3.16b, v7.16b, #8") /* v15 = 0 0 DB[4] DB[5] */
192 __ASM_EMIT("fadd v6.4s, v6.4s, v14.4s") /* v6 = DB[0] DB[1] DB[0]+DB[2] DB[1]+DB[3] */
193 __ASM_EMIT("fadd v7.4s, v7.4s, v15.4s") /* v7 = DB[4] DB[5] DB[4]+DB[6] DB[5]+DB[7] */
194 __ASM_EMIT("ext v14.16b, v3.16b, v6.16b, #12") /* v14 = 0 DB[0] DB[1] DB[0]+DB[2] */
195 __ASM_EMIT("ext v15.16b, v3.16b, v7.16b, #12") /* v15 = 0 DB[4] DB[5] DB[4]+DB[6] */
196 __ASM_EMIT("fadd v6.4s, v6.4s, v14.4s") /* v6 = B[0..3] = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] */
197 __ASM_EMIT("fadd v7.4s, v7.4s, v15.4s") /* v7 = B[4..7] = DB[4] DB[4]+DB[5] DB[4]+DB[5]+DB[6] DB[4]+DB[5]+DB[6]+DB[7] */
198 __ASM_EMIT("dup v8.4s, v12.s[3]") /* v8 = V[3] V[3] V[3] V[3] */
199 __ASM_EMIT("dup v9.4s, v4.s[3]") /* v9 = A[3] A[3] A[3] A[3] */
200 __ASM_EMIT("dup v10.4s, v6.s[3]") /* v10 = B[3] B[3] B[3] B[3] */
201 __ASM_EMIT("fadd v13.4s, v13.4s, v8.4s") /* v13 = V[3]+V[4] V[3]+V[5] V[3]+V[6] V[3]+V[7] */
202 __ASM_EMIT("fadd v5.4s, v5.4s, v9.4s") /* v5 = A[3]+A[4] A[3]+A[5] A[3]+A[6] A[3]+A[7] */
203 __ASM_EMIT("fadd v7.4s, v7.4s, v10.4s") /* v7 = B[3]+B[4] B[3]+B[5] B[3]+B[6] B[3]+B[7] */
204
205 __ASM_EMIT("fadd v4.4s, v4.4s, v1.4s") /* v4 = BA = xa + A */
206 __ASM_EMIT("fadd v5.4s, v5.4s, v1.4s")
207 __ASM_EMIT("fadd v6.4s, v6.4s, v2.4s") /* v6 = BB = xb + B */
208 __ASM_EMIT("fadd v7.4s, v7.4s, v2.4s")
209 __ASM_EMIT("fadd v8.4s, v12.4s, v0.4s") /* v8 = T = xv + V */
210 __ASM_EMIT("fadd v9.4s, v13.4s, v0.4s")
211 __ASM_EMIT("fmul v10.4s, v4.4s, v6.4s") /* v10 = B = BA * BB */
212 __ASM_EMIT("fmul v11.4s, v5.4s, v7.4s")
213 __ASM_EMIT("dup v0.4s, v9.s[3]") /* v0 = xv' = T[7] */
214 __ASM_EMIT("dup v1.4s, v5.s[3]") /* v1 = xa' = BA[7] */
215 __ASM_EMIT("dup v2.4s, v7.s[3]") /* v2 = xb' = BB[7] */
216 __ASM_EMIT("ldp q14, q15, [%[CORR_CC]]") /* v14 = threshold, v15 = threshold */
217
218 __ASM_EMIT("fcmge v14.4s, v8.4s, v14.4s") /* v14 = T >= threshold */
219 __ASM_EMIT("fcmge v15.4s, v9.4s, v15.4s")
220 __ASM_EMIT("frsqrte v4.4s, v10.4s") /* v4 = x0 */
221 __ASM_EMIT("frsqrte v5.4s, v11.4s")
222 __ASM_EMIT("fmul v6.4s, v4.4s, v10.4s") /* v6 = R * x0 */
223 __ASM_EMIT("fmul v7.4s, v5.4s, v11.4s")
224 __ASM_EMIT("frsqrts v12.4s, v6.4s, v4.4s") /* v12 = (3 - R * x0 * x0) / 2 */
225 __ASM_EMIT("frsqrts v13.4s, v7.4s, v5.4s")
226 __ASM_EMIT("fmul v4.4s, v4.4s, v12.4s") /* v4 = x1 = x0 * (3 - R * x0 * x0) / 2 */
227 __ASM_EMIT("fmul v5.4s, v5.4s, v13.4s")
228 __ASM_EMIT("fmul v6.4s, v4.4s, v10.4s") /* v6 = R * x1 */
229 __ASM_EMIT("fmul v7.4s, v5.4s, v11.4s")
230 __ASM_EMIT("frsqrts v12.4s, v6.4s, v4.4s") /* v12 = (3 - R * x1 * x1) / 2 */
231 __ASM_EMIT("frsqrts v13.4s, v7.4s, v5.4s")
232 __ASM_EMIT("fmul v10.4s, v4.4s, v12.4s") /* v10 = 1/svrtf(B) = x2 = x1 * (3 - R * x1 * x1) / 2 */
233 __ASM_EMIT("fmul v11.4s, v5.4s, v13.4s")
234 __ASM_EMIT("fmul v10.4s, v8.4s, v10.4s") /* v10 = T/svrtf(B) */
235 __ASM_EMIT("fmul v11.4s, v9.4s, v11.4s")
236 __ASM_EMIT("and v10.16b, v10.16b, v14.16b") /* v10 = (T >= threshold) ? T/svrt(B) : 0 */
237 __ASM_EMIT("and v11.16b, v11.16b, v15.16b")
238 __ASM_EMIT("add %[a_head], %[a_head], #0x20")
239 __ASM_EMIT("add %[b_head], %[b_head], #0x20")
240 __ASM_EMIT("subs %[count], %[count], #8")
241 __ASM_EMIT("stp q10, q11, [%[dst], 0x00]")
242 __ASM_EMIT("add %[a_tail], %[a_tail], #0x20")
243 __ASM_EMIT("add %[b_tail], %[b_tail], #0x20")
244 __ASM_EMIT("add %[dst], %[dst], #0x20")
245 __ASM_EMIT("b.hs 1b")
246 __ASM_EMIT("2:")
247 /* 4x block */
248 __ASM_EMIT("adds %[count], %[count], #4")
249 __ASM_EMIT("b.lt 4f")
250 __ASM_EMIT("ldr q4, [%[a_head], 0x00]") /* v4 = ah0 */
251 __ASM_EMIT("ldr q6, [%[b_head], 0x00]") /* v6 = ah0 */
252 __ASM_EMIT("ldr q8, [%[a_tail], 0x00]") /* v8 = ah0 */
253 __ASM_EMIT("ldr q10, [%[b_tail], 0x00]") /* v10 = ah0 */
254 __ASM_EMIT("fmul v12.4s, v4.4s, v6.4s") /* v12 = ah0*bh0 */
255 __ASM_EMIT("fmul v4.4s, v4.4s, v4.4s") /* v4 = ah0*ah0 */
256 __ASM_EMIT("fmul v6.4s, v6.4s, v6.4s") /* v6 = bh0*bh0 */
257 __ASM_EMIT("fmls v12.4s, v8.4s, v10.4s") /* v12 = DV = ah0*bh0 - at0*bt0 */
258 __ASM_EMIT("fmls v4.4s, v8.4s, v8.4s") /* v4 = DA = ah0*ah0 - at0*at0 */
259 __ASM_EMIT("fmls v6.4s, v10.4s, v10.4s") /* v6 = DB = bh0*bh0 - bt0*bt0 */
260
261 __ASM_EMIT("ext v13.16b, v3.16b, v12.16b, #8") /* v13 = 0 0 DV[0] DV[1] */
262 __ASM_EMIT("ext v5.16b, v3.16b, v4.16b, #8") /* v5 = 0 0 DA[0] DA[1] */
263 __ASM_EMIT("ext v7.16b, v3.16b, v6.16b, #8") /* v7 = 0 0 DB[0] DB[1] */
264 __ASM_EMIT("fadd v12.4s, v12.4s, v13.4s") /* v12 = DV[0] DV[1] DV[0]+DV[2] DV[1]+DV[3] */
265 __ASM_EMIT("fadd v4.4s, v4.4s, v5.4s") /* v4 = DA[0] DA[1] DA[0]+DA[2] DA[1]+DA[3] */
266 __ASM_EMIT("fadd v6.4s, v6.4s, v7.4s") /* v6 = DB[0] DB[1] DB[0]+DB[2] DB[1]+DB[3] */
267 __ASM_EMIT("ext v13.16b, v3.16b, v12.16b, #12") /* v13 = 0 DV[0] DV[1] DV[0]+DV[2] */
268 __ASM_EMIT("ext v5.16b, v3.16b, v4.16b, #12") /* v5 = 0 DA[0] DA[1] DA[0]+DA[2] */
269 __ASM_EMIT("ext v7.16b, v3.16b, v6.16b, #12") /* v7 = 0 DB[0] DB[1] DB[0]+DB[2] */
270 __ASM_EMIT("fadd v12.4s, v12.4s, v13.4s") /* v12 = V[0..3] = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] */
271 __ASM_EMIT("fadd v4.4s, v4.4s, v5.4s") /* v4 = A[0..3] = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] */
272 __ASM_EMIT("fadd v6.4s, v6.4s, v7.4s") /* v6 = B[0..3] = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] */
273
274 __ASM_EMIT("fadd v4.4s, v4.4s, v1.4s") /* v4 = BA = xa + A */
275 __ASM_EMIT("fadd v6.4s, v6.4s, v2.4s") /* v6 = BB = xb + B */
276 __ASM_EMIT("fadd v8.4s, v12.4s, v0.4s") /* v8 = T = xv + V */
277 __ASM_EMIT("fmul v10.4s, v4.4s, v6.4s") /* v10 = B = BA * BB */
278 __ASM_EMIT("dup v1.4s, v4.s[3]") /* v1 = xa' = BA[7] */
279 __ASM_EMIT("dup v2.4s, v6.s[3]") /* v2 = xb' = BB[7] */
280 __ASM_EMIT("dup v0.4s, v8.s[3]") /* v0 = xv' = T[7] */
281 __ASM_EMIT("ldr q14, [%[CORR_CC]]") /* v14 = threshold */
282
283 __ASM_EMIT("fcmge v14.4s, v8.4s, v14.4s") /* v14 = T >= threshold */
284 __ASM_EMIT("frsqrte v4.4s, v10.4s") /* v4 = x0 */
285 __ASM_EMIT("fmul v6.4s, v4.4s, v10.4s") /* v6 = R * x0 */
286 __ASM_EMIT("frsqrts v12.4s, v6.4s, v4.4s") /* v12 = (3 - R * x0 * x0) / 2 */
287 __ASM_EMIT("fmul v4.4s, v4.4s, v12.4s") /* v4 = x1 = x0 * (3 - R * x0 * x0) / 2 */
288 __ASM_EMIT("fmul v6.4s, v4.4s, v10.4s") /* v6 = R * x1 */
289 __ASM_EMIT("frsqrts v12.4s, v6.4s, v4.4s") /* v12 = (3 - R * x1 * x1) / 2 */
290 __ASM_EMIT("fmul v10.4s, v4.4s, v12.4s") /* v10 = 1/svrtf(B) = x2 = x1 * (3 - R * x1 * x1) / 2 */
291 __ASM_EMIT("fmul v10.4s, v8.4s, v10.4s") /* v10 = T/svrtf(B) */
292 __ASM_EMIT("and v10.16b, v10.16b, v14.16b") /* v10 = (T >= threshold) ? T/svrt(B) : 0 */
293 __ASM_EMIT("add %[a_head], %[a_head], #0x10")
294 __ASM_EMIT("add %[b_head], %[b_head], #0x10")
295 __ASM_EMIT("sub %[count], %[count], #4")
296 __ASM_EMIT("str q10, [%[dst], 0x00]")
297 __ASM_EMIT("add %[a_tail], %[a_tail], #0x10")
298 __ASM_EMIT("add %[b_tail], %[b_tail], #0x10")
299 __ASM_EMIT("add %[dst], %[dst], #0x10")
300 __ASM_EMIT("4:")
301 /* 1x blocks */
302 __ASM_EMIT("adds %[count], %[count], #3")
303 __ASM_EMIT("blt 6f")
304 __ASM_EMIT("ldr q3, [%[CORR_CC]]") /* v3 = threshold */
305 __ASM_EMIT("5:")
306 __ASM_EMIT("ld1r {v4.4s}, [%[a_head]]") /* v4 = ah0 */
307 __ASM_EMIT("ld1r {v6.4s}, [%[b_head]]") /* v6 = bh0 */
308 __ASM_EMIT("ld1r {v8.4s}, [%[a_tail]]") /* v8 = at0 */
309 __ASM_EMIT("ld1r {v10.4s}, [%[b_tail]]") /* v10 = bt0 */
310 __ASM_EMIT("fmul v12.4s, v4.4s, v6.4s") /* v12 = ah0*bh0 */
311 __ASM_EMIT("fmul v4.4s, v4.4s, v4.4s") /* v4 = ah0*ah0 */
312 __ASM_EMIT("fmul v6.4s, v6.4s, v6.4s") /* v6 = bh0*bh0 */
313 __ASM_EMIT("fmls v12.4s, v8.4s, v10.4s") /* v12 = DV = ah0*bh0 - at0*bt0 */
314 __ASM_EMIT("fmls v4.4s, v8.4s, v8.4s") /* v4 = DA = ah0*ah0 - at0*at0 */
315 __ASM_EMIT("fmls v6.4s, v10.4s, v10.4s") /* v6 = DB = bh0*bh0 - bt0*bt0 */
316
317 __ASM_EMIT("fadd v1.4s, v4.4s, v1.4s") /* v1 = BA = xa + DA */
318 __ASM_EMIT("fadd v2.4s, v6.4s, v2.4s") /* v2 = BB = xb + DB */
319 __ASM_EMIT("fadd v0.4s, v12.4s, v0.4s") /* v0 = T = xv + DV */
320 __ASM_EMIT("fmul v10.4s, v1.4s, v2.4s") /* v10 = B = BA * BB */
321
322 __ASM_EMIT("fcmge v14.4s, v0.4s, v3.4s") /* v14 = T >= threshold */
323 __ASM_EMIT("frsqrte v4.4s, v10.4s") /* v4 = x0 */
324 __ASM_EMIT("fmul v6.4s, v4.4s, v10.4s") /* v6 = R * x0 */
325 __ASM_EMIT("frsqrts v12.4s, v6.4s, v4.4s") /* v12 = (3 - R * x0 * x0) / 2 */
326 __ASM_EMIT("fmul v4.4s, v4.4s, v12.4s") /* v4 = x1 = x0 * (3 - R * x0 * x0) / 2 */
327 __ASM_EMIT("fmul v6.4s, v4.4s, v10.4s") /* v6 = R * x1 */
328 __ASM_EMIT("frsqrts v12.4s, v6.4s, v4.4s") /* v12 = (3 - R * x1 * x1) / 2 */
329 __ASM_EMIT("fmul v10.4s, v4.4s, v12.4s") /* v10 = 1/svrtf(B) = x2 = x1 * (3 - R * x1 * x1) / 2 */
330 __ASM_EMIT("fmul v10.4s, v0.4s, v10.4s") /* v10 = T/svrtf(B) */
331 __ASM_EMIT("and v10.16b, v10.16b, v14.16b") /* v10 = (T >= threshold) ? T/svrt(B) : 0 */
332 __ASM_EMIT("add %[a_head], %[a_head], #0x04")
333 __ASM_EMIT("add %[b_head], %[b_head], #0x04")
334 __ASM_EMIT("subs %[count], %[count], #1")
335 __ASM_EMIT("st1 {v10.s}[0], [%[dst]]")
336 __ASM_EMIT("add %[a_tail], %[a_tail], #0x04")
337 __ASM_EMIT("add %[b_tail], %[b_tail], #0x04")
338 __ASM_EMIT("add %[dst], %[dst], #0x04")
339 __ASM_EMIT("b.ge 5b")
340 __ASM_EMIT("6:")
341 /* Store state */
342 __ASM_EMIT("st3 {v0.s, v1.s, v2.s}[0], [%[corr]]")
343
344 : [dst] "+r" (dst),
345 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
346 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
347 [count] "+r" (count)
348 : [corr] "r" (corr),
349 [CORR_CC] "r" (&corr_const[0])
350 : "cc", "memory",
351 "v0", "v1", "v2", "v3", "v4", "v5", "v6", "v7",
352 "v8", "v9", "v10", "v11", "v12", "v13", "v14", "v15"
353 );
354 }
355
356 } /* namespace asimd */
357 } /* namespace lsp */
358
359
360
361 #endif /* PRIVATE_DSP_ARCH_AARCH64_ASIMD_CORRELATION_H_ */