Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/x86/avx512/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_X86_AVX512_CORRELATION_H_
23 #define PRIVATE_DSP_ARCH_X86_AVX512_CORRELATION_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_AVX512_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_AVX512_IMPL */
28
29 namespace lsp
30 {
31 namespace avx512
32 {
33 void corr_init(dsp::correlation_t *corr, const float *a, const float *b, size_t count)
34 {
35 IF_ARCH_X86(
36 size_t off;
37 );
38
39 ARCH_X86_ASM
40 (
41 __ASM_EMIT("xor %[off], %[off]")
42 __ASM_EMIT("vxorps %%zmm0, %%zmm0, %%zmm0") /* xv = 0 */
43 __ASM_EMIT("vxorps %%zmm1, %%zmm1, %%zmm1") /* xa = 0 */
44 __ASM_EMIT("vxorps %%zmm2, %%zmm2, %%zmm2") /* xb = 0 */
45 /* 32x blocks */
46 __ASM_EMIT("sub $32, %[count]")
47 __ASM_EMIT("jb 2f")
48 __ASM_EMIT("1:")
49 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%zmm3") /* zmm3 = a0 */
50 __ASM_EMIT("vmovups 0x40(%[a], %[off]), %%zmm4") /* zmm4 = a1 */
51 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%zmm5") /* zmm5 = b0 */
52 __ASM_EMIT("vmovups 0x40(%[b], %[off]), %%zmm6") /* zmm6 = b1 */
53 __ASM_EMIT("vfmadd231ps %%zmm3, %%zmm5, %%zmm0") /* zmm0 = xv + a0*b0 */
54 __ASM_EMIT("vfmadd231ps %%zmm3, %%zmm3, %%zmm1") /* zmm1 = xa + a0*a0 */
55 __ASM_EMIT("vfmadd231ps %%zmm5, %%zmm5, %%zmm2") /* zmm2 = xb + b0*b0 */
56 __ASM_EMIT("vfmadd231ps %%zmm4, %%zmm6, %%zmm0") /* zmm0 = xv + a1*b1 */
57 __ASM_EMIT("vfmadd231ps %%zmm4, %%zmm4, %%zmm1") /* zmm1 = xa + a1*a1 */
58 __ASM_EMIT("vfmadd231ps %%zmm6, %%zmm6, %%zmm2") /* zmm2 = xb + b1*b1 */
59 __ASM_EMIT("add $0x80, %[off]") /* ++off */
60 __ASM_EMIT("sub $32, %[count]")
61 __ASM_EMIT("jae 1b")
62 __ASM_EMIT("vextractf64x4 $1, %%zmm0, %%ymm4")
63 __ASM_EMIT("vextractf64x4 $1, %%zmm1, %%ymm5")
64 __ASM_EMIT("vextractf64x4 $1, %%zmm2, %%ymm6")
65 __ASM_EMIT("vaddps %%ymm4, %%ymm0, %%ymm0")
66 __ASM_EMIT("vaddps %%ymm5, %%ymm1, %%ymm1")
67 __ASM_EMIT("vaddps %%ymm6, %%ymm2, %%ymm2")
68 __ASM_EMIT("2:")
69 /* 16x blocks */
70 __ASM_EMIT("add $16, %[count]")
71 __ASM_EMIT("jl 4f")
72 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%ymm3") /* ymm3 = a0 */
73 __ASM_EMIT("vmovups 0x20(%[a], %[off]), %%ymm4") /* ymm4 = a1 */
74 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%ymm5") /* ymm5 = b0 */
75 __ASM_EMIT("vmovups 0x20(%[b], %[off]), %%ymm6") /* ymm6 = b1 */
76 __ASM_EMIT("vfmadd231ps %%ymm3, %%ymm5, %%ymm0") /* ymm0 = xv + a0*b0 */
77 __ASM_EMIT("vfmadd231ps %%ymm3, %%ymm3, %%ymm1") /* ymm1 = xa + a0*a0 */
78 __ASM_EMIT("vfmadd231ps %%ymm5, %%ymm5, %%ymm2") /* ymm2 = xb + b0*b0 */
79 __ASM_EMIT("vfmadd231ps %%ymm4, %%ymm6, %%ymm0") /* ymm0 = xv + a1*b1 */
80 __ASM_EMIT("vfmadd231ps %%ymm4, %%ymm4, %%ymm1") /* ymm1 = xa + a1*a1 */
81 __ASM_EMIT("vfmadd231ps %%ymm6, %%ymm6, %%ymm2") /* ymm2 = xb + b1*b1 */
82 __ASM_EMIT("sub $16, %[count]")
83 __ASM_EMIT("add $0x40, %[off]") /* ++off */
84 __ASM_EMIT("4:")
85 __ASM_EMIT("vextractf128 $1, %%ymm0, %%xmm4")
86 __ASM_EMIT("vextractf128 $1, %%ymm1, %%xmm5")
87 __ASM_EMIT("vextractf128 $1, %%ymm2, %%xmm6")
88 __ASM_EMIT("vaddps %%xmm4, %%xmm0, %%xmm0")
89 __ASM_EMIT("vaddps %%xmm5, %%xmm1, %%xmm1")
90 __ASM_EMIT("vaddps %%xmm6, %%xmm2, %%xmm2")
91 /* 8x block */
92 __ASM_EMIT("add $8, %[count]")
93 __ASM_EMIT("jl 6f")
94 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
95 __ASM_EMIT("vmovups 0x10(%[a], %[off]), %%xmm4") /* xmm4 = a1 */
96 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
97 __ASM_EMIT("vmovups 0x10(%[b], %[off]), %%xmm6") /* xmm6 = b1 */
98 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm5, %%xmm0") /* xmm0 = xv + a0*b0 */
99 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm3, %%xmm1") /* xmm1 = xa + a0*a0 */
100 __ASM_EMIT("vfmadd231ps %%xmm5, %%xmm5, %%xmm2") /* xmm2 = xb + b0*b0 */
101 __ASM_EMIT("vfmadd231ps %%xmm4, %%xmm6, %%xmm0") /* xmm0 = xv + a1*b1 */
102 __ASM_EMIT("vfmadd231ps %%xmm4, %%xmm4, %%xmm1") /* xmm1 = xa + a1*a1 */
103 __ASM_EMIT("vfmadd231ps %%xmm6, %%xmm6, %%xmm2") /* xmm2 = xb + b1*b1 */
104 __ASM_EMIT("sub $8, %[count]")
105 __ASM_EMIT("add $0x20, %[off]") /* ++off */
106 __ASM_EMIT("6:")
107 /* 4x block */
108 __ASM_EMIT("add $4, %[count]")
109 __ASM_EMIT("jl 8f")
110 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
111 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
112 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm5, %%xmm0") /* xmm0 = xv + a0*b0 */
113 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm3, %%xmm1") /* xmm1 = xa + a0*a0 */
114 __ASM_EMIT("vfmadd231ps %%xmm5, %%xmm5, %%xmm2") /* xmm2 = xb + b0*b0 */
115 __ASM_EMIT("sub $4, %[count]")
116 __ASM_EMIT("add $0x10, %[off]") /* ++off */
117 __ASM_EMIT("8:")
118 /* Do horizontal sum */
119 __ASM_EMIT("vhaddps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
120 __ASM_EMIT("vhaddps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
121 __ASM_EMIT("vhaddps %%xmm2, %%xmm2, %%xmm2") /* xmm2 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
122 __ASM_EMIT("vhaddps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = xv0+xv1+xv2+xv3 */
123 __ASM_EMIT("vhaddps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = xv0+xv1+xv2+xv3 */
124 __ASM_EMIT("vhaddps %%xmm2, %%xmm2, %%xmm2") /* xmm2 = xv0+xv1+xv2+xv3 */
125 /* 1x blocks */
126 __ASM_EMIT("add $3, %[count]")
127 __ASM_EMIT("jl 10f")
128 __ASM_EMIT("9:")
129 __ASM_EMIT("vmovss 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
130 __ASM_EMIT("vmovss 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
131 __ASM_EMIT("vfmadd231ss %%xmm3, %%xmm5, %%xmm0") /* xmm0 = xv + a0*b0 */
132 __ASM_EMIT("vfmadd231ss %%xmm3, %%xmm3, %%xmm1") /* xmm1 = xa + a0*a0 */
133 __ASM_EMIT("vfmadd231ss %%xmm5, %%xmm5, %%xmm2") /* xmm2 = xb + b0*b0 */
134 __ASM_EMIT("add $0x04, %[off]") /* ++off */
135 __ASM_EMIT("dec %[count]")
136 __ASM_EMIT("jge 9b")
137 __ASM_EMIT("10:")
138 /* Store result */
139 __ASM_EMIT("vaddss 0x00(%[corr]), %%xmm0, %%xmm0")
140 __ASM_EMIT("vaddss 0x04(%[corr]), %%xmm1, %%xmm1")
141 __ASM_EMIT("vaddss 0x08(%[corr]), %%xmm2, %%xmm2")
142 __ASM_EMIT("vmovss %%xmm0, 0x00(%[corr])")
143 __ASM_EMIT("vmovss %%xmm1, 0x04(%[corr])")
144 __ASM_EMIT("vmovss %%xmm2, 0x08(%[corr])")
145
146 : [corr] "+r" (corr), [off] "=&r" (off), [count] "+r" (count)
147 : [a] "r" (a), [b] "r" (b)
148 : "cc", "memory",
149 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
150 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
151 );
152 }
153
154 static const float corr_const[] __lsp_aligned64 =
155 {
156 LSP_DSP_VEC16(1e-18f)
157 };
158
159 static const uint32_t corr_idx[] __lsp_aligned64 =
160 {
161 0, 0, 0, 0, 0, 0, 0, 0, 3, 3, 3, 3, 7, 7, 7, 7, // 0 0 A0 A1
162 0, 0, 0, 0, 3, 3, 3, 3, 7, 7, 7, 7, 11, 11, 11, 11, // 0 A0 A1 A2
163 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15, 15
164 };
165
166 static const uint16_t corr_kmask[] =
167 {
168 0xff00,
169 0xfff0
170 };
171
172 void corr_incr(dsp::correlation_t *corr, float *dst,
173 const float *a_head, const float *b_head,
174 const float *a_tail, const float *b_tail,
175 size_t count)
176 {
177 IF_ARCH_I386(
178 void *ptr;
179 );
180
181 ARCH_X86_ASM
182 (
183 /* 16x blocks */
184 __ASM_EMIT32("subl $16, %[count]")
185 __ASM_EMIT64("sub $16, %[count]")
186 __ASM_EMIT("jb 2f")
187 __ASM_EMIT("kmovw 0x00 + %[CORR_KMASK], %%k1")
188 __ASM_EMIT("kmovw 0x02 + %[CORR_KMASK], %%k2")
189 __ASM_EMIT("1:")
190 __ASM_EMIT("vmovups 0x00(%[a_head]), %%zmm0") /* zmm0 = ah */
191 __ASM_EMIT("vmovups 0x00(%[b_head]), %%zmm1") /* zmm1 = bh */
192 __ASM_EMIT("vmovups 0x00(%[a_tail]), %%zmm3") /* zmm3 = at */
193 __ASM_EMIT("vmovups 0x00(%[b_tail]), %%zmm4") /* zmm4 = bt */
194 __ASM_EMIT("vmulps %%zmm1, %%zmm0, %%zmm2") /* zmm2 = ah*bh */
195 __ASM_EMIT("vmulps %%zmm0, %%zmm0, %%zmm0") /* zmm0 = ah*ah */
196 __ASM_EMIT("vmulps %%zmm1, %%zmm1, %%zmm1") /* zmm1 = bh*bh */
197 __ASM_EMIT("vxorps %%zmm6, %%zmm6, %%zmm6") /* zmm6 = 0 0 0 0 0 0 0 0 */
198 __ASM_EMIT("vfnmadd231ps %%zmm4, %%zmm3, %%zmm2") /* zmm2 = DV = ah*bh - at*bt */
199 __ASM_EMIT("vfnmadd231ps %%zmm3, %%zmm3, %%zmm0") /* zmm0 = DA = ah*ah - at*at */
200 __ASM_EMIT("vfnmadd231ps %%zmm4, %%zmm4, %%zmm1") /* zmm1 = DB = bh*bh - bt*bt */
201
202 __ASM_EMIT("vshufps $0x44, %%zmm0, %%zmm6, %%zmm3") /* zmm3 = 0 0 DA[0] DA[1] ... */
203 __ASM_EMIT("vshufps $0x44, %%zmm1, %%zmm6, %%zmm4") /* zmm4 = 0 0 DB[0] DB[1] ... */
204 __ASM_EMIT("vshufps $0x44, %%zmm2, %%zmm6, %%zmm5") /* zmm5 = 0 0 DV[0] DV[1] ... */
205 __ASM_EMIT("vaddps %%zmm3, %%zmm0, %%zmm0") /* zmm0 = DA[0] DA[1] DA[0]+DA[2] DA[1]+DA[3] ... */
206 __ASM_EMIT("vaddps %%zmm4, %%zmm1, %%zmm1") /* zmm1 = DB[0] DB[1] DB[0]+DB[2] DB[1]+DB[3] ... */
207 __ASM_EMIT("vaddps %%zmm5, %%zmm2, %%zmm2") /* zmm2 = DV[0] DV[1] DV[0]+DV[2] DV[1]+DV[3] ... */
208 __ASM_EMIT("vmovaps 0x00 + %[CORR_IDX], %%zmm6") /* zmm6 = permute mask */
209 __ASM_EMIT("vmovaps 0x40 + %[CORR_IDX], %%zmm7") /* zmm7 = permute mask */
210 __ASM_EMIT("vshufps $0x99, %%zmm0, %%zmm3, %%zmm3") /* zmm3 = 0 DA[0] DA[1] DA[0]+DA[2] ... */
211 __ASM_EMIT("vshufps $0x99, %%zmm1, %%zmm4, %%zmm4") /* zmm4 = 0 DB[0] DB[1] DB[0]+DB[2] ... */
212 __ASM_EMIT("vshufps $0x99, %%zmm2, %%zmm5, %%zmm5") /* zmm5 = 0 DV[0] DV[1] DV[0]+DV[2] ... */
213 __ASM_EMIT("vaddps %%zmm0, %%zmm3, %%zmm3") /* zmm3 = A = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] ... */
214 __ASM_EMIT("vaddps %%zmm1, %%zmm4, %%zmm4") /* zmm4 = B = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] ... */
215 __ASM_EMIT("vaddps %%zmm2, %%zmm5, %%zmm5") /* zmm5 = V = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] ... */
216 __ASM_EMIT("vpermps %%zmm3, %%zmm6, %%zmm0") /* zmm0 = A0 A0 A0 A1 */
217 __ASM_EMIT("vpermps %%zmm4, %%zmm6, %%zmm1") /* zmm1 = B0 B0 B0 B1 */
218 __ASM_EMIT("vpermps %%zmm5, %%zmm6, %%zmm2") /* zmm2 = V0 V0 V0 V1 */
219 __ASM_EMIT("vaddps %%zmm0, %%zmm3, %%zmm3 %{%%k1%}") /* zmm3 = A0 A1 A0+A2 A1+A3 */
220 __ASM_EMIT("vaddps %%zmm1, %%zmm4, %%zmm4 %{%%k1%}") /* zmm4 = B0 B1 B0+B2 B1+B3 */
221 __ASM_EMIT("vaddps %%zmm2, %%zmm5, %%zmm5 %{%%k1%}") /* zmm5 = V0 V1 V0+V2 V1+V3 */
222 __ASM_EMIT("vpermps %%zmm3, %%zmm7, %%zmm0") /* zmm0 = A0 A0 A1 A0+A2 */
223 __ASM_EMIT("vpermps %%zmm4, %%zmm7, %%zmm1") /* zmm1 = B0 B0 B1 B0+B2 */
224 __ASM_EMIT("vpermps %%zmm5, %%zmm7, %%zmm2") /* zmm2 = V0 V0 V1 V0+V2 */
225 __ASM_EMIT("vaddps %%zmm0, %%zmm3, %%zmm3 %{%%k2%}") /* zmm3 = A0 A0+A1 A0+A1+A2 A0+A1+A2+A3 */
226 __ASM_EMIT("vaddps %%zmm1, %%zmm4, %%zmm4 %{%%k2%}") /* zmm4 = B0 B0+B1 B0+B1+B2 B0+B1+B2+B3 */
227 __ASM_EMIT("vaddps %%zmm2, %%zmm5, %%zmm5 %{%%k2%}") /* zmm5 = V0 V0+V1 V0+V1+V2 V0+V1+V2+V3 */
228
229 __ASM_EMIT32("mov %[corr], %[ptr]")
230 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%zmm0")
231 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%zmm1")
232 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%zmm2")
233 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%zmm0")
234 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%zmm1")
235 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%zmm2")
236
237 __ASM_EMIT("vaddps %%zmm3, %%zmm1, %%zmm1") /* zmm1 = BA = xa + A */
238 __ASM_EMIT("vaddps %%zmm4, %%zmm2, %%zmm2") /* zmm2 = BB = xb + B */
239 __ASM_EMIT("vaddps %%zmm5, %%zmm0, %%zmm0") /* zmm0 = T = xv + V */
240 __ASM_EMIT("vmovaps 0x80 + %[CORR_IDX], %%zmm6") /* zmm6 = permute mask */
241 __ASM_EMIT("vmulps %%zmm2, %%zmm1, %%zmm7") /* zmm7 = B = BA*BB */
242 __ASM_EMIT("vpermps %%zmm0, %%zmm6, %%zmm3") /* zmm3 = xv' */
243 __ASM_EMIT("vpermps %%zmm1, %%zmm6, %%zmm4") /* zmm4 = xa' */
244 __ASM_EMIT("vpermps %%zmm2, %%zmm6, %%zmm5") /* zmm5 = xb' */
245
246 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
247 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
248 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
249 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
250 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
251 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
252
253 __ASM_EMIT("vsqrtps %%zmm7, %%zmm6") /* zmm6 = sqrtf(B) */
254 __ASM_EMIT("vcmpps $1, %[CORR_CC], %%zmm7, %%k3") /* k3 = B < threshold */
255 __ASM_EMIT("vdivps %%zmm6, %%zmm0, %%zmm0") /* zmm0 = T/sqrtf(B) */
256 __ASM_EMIT32("mov %[dst], %[ptr]")
257 __ASM_EMIT("vxorps %%zmm0, %%zmm0, %%zmm0 %{%%k3%}") /* zmm0 = (B < threshold) ? 0 : T/sqrtf(B) */
258 __ASM_EMIT("add $0x40, %[a_head]") /* ++a_head */
259 __ASM_EMIT("add $0x40, %[b_head]") /* ++b_head */
260 __ASM_EMIT32("vmovups %%zmm0, 0x00(%[ptr])")
261 __ASM_EMIT64("vmovups %%zmm0, 0x00(%[dst])")
262 __ASM_EMIT("add $0x40, %[a_tail]") /* ++a_tail */
263 __ASM_EMIT("add $0x40, %[b_tail]") /* ++b_tail */
264 __ASM_EMIT32("add $0x40, %[ptr]")
265 __ASM_EMIT64("add $0x40, %[dst]")
266 __ASM_EMIT32("mov %[ptr], %[dst]")
267 __ASM_EMIT32("subl $16, %[count]")
268 __ASM_EMIT64("sub $16, %[count]")
269 __ASM_EMIT("jae 1b")
270 __ASM_EMIT("2:")
271 /* 8x block */
272 __ASM_EMIT32("addl $8, %[count]")
273 __ASM_EMIT64("add $8, %[count]")
274 __ASM_EMIT("jl 4f")
275 __ASM_EMIT("vmovups 0x00(%[a_head]), %%ymm0") /* ymm0 = ah */
276 __ASM_EMIT("vmovups 0x00(%[b_head]), %%ymm1") /* ymm1 = bh */
277 __ASM_EMIT("vmovups 0x00(%[a_tail]), %%ymm3") /* ymm3 = at */
278 __ASM_EMIT("vmovups 0x00(%[b_tail]), %%ymm4") /* ymm4 = bt */
279 __ASM_EMIT("vmulps %%ymm1, %%ymm0, %%ymm2") /* ymm2 = ah*bh */
280 __ASM_EMIT("vmulps %%ymm0, %%ymm0, %%ymm0") /* ymm0 = ah*ah */
281 __ASM_EMIT("vmulps %%ymm1, %%ymm1, %%ymm1") /* ymm1 = bh*bh */
282 __ASM_EMIT("vxorps %%ymm6, %%ymm6, %%ymm6") /* ymm6 = 0 0 0 0 0 0 0 0 */
283 __ASM_EMIT("vfnmadd231ps %%ymm4, %%ymm3, %%ymm2") /* ymm2 = DV = ah*bh - at*bt */
284 __ASM_EMIT("vfnmadd231ps %%ymm3, %%ymm3, %%ymm0") /* ymm0 = DA = ah*ah - at*at */
285 __ASM_EMIT("vfnmadd231ps %%ymm4, %%ymm4, %%ymm1") /* ymm1 = DB = bh*bh - bt*bt */
286
287 __ASM_EMIT("vshufps $0x44, %%ymm0, %%ymm6, %%ymm3") /* ymm3 = 0 0 DA[0] DA[1] 0 0 DA[4] DA[5] */
288 __ASM_EMIT("vshufps $0x44, %%ymm1, %%ymm6, %%ymm4") /* ymm4 = 0 0 DB[0] DB[1] 0 0 DB[4] DB[5] */
289 __ASM_EMIT("vshufps $0x44, %%ymm2, %%ymm6, %%ymm5") /* ymm5 = 0 0 DV[0] DV[1] 0 0 DV[4] DV[5] */
290 __ASM_EMIT("vaddps %%ymm3, %%ymm0, %%ymm0") /* ymm0 = DA[0] DA[1] DA[0]+DA[2] DA[1]+DA[3] DA[4] DA[5] DA[4]+DA[6] DA[5]+DA[7] */
291 __ASM_EMIT("vaddps %%ymm4, %%ymm1, %%ymm1") /* ymm1 = DB[0] DB[1] DB[0]+DB[2] DB[1]+DB[3] DB[4] DB[5] DB[4]+DB[6] DB[5]+DB[7] */
292 __ASM_EMIT("vaddps %%ymm5, %%ymm2, %%ymm2") /* ymm2 = DV[0] DV[1] DV[0]+DV[2] DV[1]+DV[3] DV[4] DV[5] DV[4]+DV[6] DV[5]+DV[7] */
293 __ASM_EMIT("vshufps $0x99, %%ymm0, %%ymm3, %%ymm3") /* ymm3 = 0 DA[0] DA[1] DA[0]+DA[2] 0 DA[4] DA[5] DA[4]+DA[6] */
294 __ASM_EMIT("vshufps $0x99, %%ymm1, %%ymm4, %%ymm4") /* ymm4 = 0 DB[0] DB[1] DB[0]+DB[2] 0 DB[4] DB[5] DB[4]+DB[6] */
295 __ASM_EMIT("vshufps $0x99, %%ymm2, %%ymm5, %%ymm5") /* ymm5 = 0 DV[0] DV[1] DV[0]+DV[2] 0 DV[4] DV[5] DV[4]+DV[6] */
296 __ASM_EMIT("vaddps %%ymm0, %%ymm3, %%ymm3") /* ymm3 = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] DA[4] DA[4]+DA[5] DA[4]+DA[5]+DA[6] DA[4]+DA[5]+DA[6]+DA[7] */
297 __ASM_EMIT("vaddps %%ymm1, %%ymm4, %%ymm4") /* ymm4 = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] DB[4] DB[4]+DB[5] DB[4]+DB[5]+DB[6] DB[4]+DB[5]+DB[6]+DB[7] */
298 __ASM_EMIT("vaddps %%ymm2, %%ymm5, %%ymm5") /* ymm5 = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] DV[4] DV[4]+DV[5] DV[4]+DV[5]+DV[6] DV[4]+DV[5]+DV[6]+DV[7] */
299 __ASM_EMIT("vshufps $0xff, %%xmm3, %%xmm3, %%xmm0") /* xmm0 = DA[0]+DA[1]+DA[2]+DA[3] DA[0]+DA[1]+DA[2]+DA[3] DA[0]+DA[1]+DA[2]+DA[3] DA[0]+DA[1]+DA[2]+DA[3] */
300 __ASM_EMIT("vshufps $0xff, %%xmm4, %%xmm4, %%xmm1") /* xmm1 = DB[0]+DB[1]+DB[2]+DB[3] DB[0]+DB[1]+DB[2]+DB[3] DB[0]+DB[1]+DB[2]+DB[3] DB[0]+DB[1]+DB[2]+DB[3] */
301 __ASM_EMIT("vshufps $0xff, %%xmm5, %%xmm5, %%xmm2") /* xmm2 = DV[0]+DV[1]+DV[2]+DV[3] DV[0]+DV[1]+DV[2]+DV[3] DV[0]+DV[1]+DV[2]+DV[3] DV[0]+DV[1]+DV[2]+DV[3] */
302 __ASM_EMIT("vextractf128 $1, %%ymm3, %%xmm6") /* xmm6 = DA[4] DA[4]+DA[5] DA[4]+DA[5]+DA[6] DA[4]+DA[5]+DA[6]+DA[7] */
303 __ASM_EMIT("vextractf128 $1, %%ymm4, %%xmm7") /* xmm7 = DB[4] DB[4]+DB[5] DB[4]+DB[5]+DB[6] DB[4]+DB[5]+DB[6]+DB[7] */
304 __ASM_EMIT("vaddps %%xmm0, %%xmm6, %%xmm6") /* xmm6 = DA[0]+DA[1]+DA[2]+DA[3]+DA[4] DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5] DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6] DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6]+DA[7] */
305 __ASM_EMIT("vaddps %%xmm1, %%xmm7, %%xmm7") /* xmm7 = DB[0]+DB[1]+DB[2]+DB[3]+DB[4] DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5] DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6] DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6]+DB[7] */
306 __ASM_EMIT("vextractf128 $1, %%ymm5, %%xmm0") /* xmm0 = DV[4] DV[4]+DV[5] DV[4]+DV[5]+DV[6] DV[4]+DV[5]+DV[6]+DV[7] */
307 __ASM_EMIT("vinsertf128 $1, %%xmm6, %%ymm3, %%ymm3") /* ymm3 = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] DA[0]+DA[1]+DA[2]+DA[3]+DA[4] DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5] DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6] DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6]+DA[7] */
308 __ASM_EMIT("vaddps %%xmm2, %%xmm0, %%xmm0") /* xmm0 = DV[0]+DV[1]+DV[2]+DV[3]+DV[4] DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5] DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6] DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6]+DV[7] */
309 __ASM_EMIT("vinsertf128 $1, %%xmm7, %%ymm4, %%ymm4") /* ymm4 = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] DB[0]+DB[1]+DB[2]+DB[3]+DB[4] DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5] DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6] DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6]+DB[7] */
310 __ASM_EMIT("vinsertf128 $1, %%xmm0, %%ymm5, %%ymm5") /* ymm5 = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] DV[0]+DV[1]+DV[2]+DV[3]+DV[4] DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5] DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6] DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6]+DV[7] */
311
312 __ASM_EMIT32("mov %[corr], %[ptr]")
313 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%ymm0")
314 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%ymm1")
315 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%ymm2")
316 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%ymm0")
317 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%ymm1")
318 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%ymm2")
319
320 __ASM_EMIT("vaddps %%ymm3, %%ymm1, %%ymm1") /* ymm1 = BA = xa+DA[0] xa+DA[0]+DA[1] xa+DA[0]+DA[1]+DA[2] xa+DA[0]+DA[1]+DA[2]+DA[3] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6]+DA[7] */
321 __ASM_EMIT("vaddps %%ymm4, %%ymm2, %%ymm2") /* ymm2 = BB = xb+DB[0] xb+DB[0]+DB[1] xb+DB[0]+DB[1]+DB[2] xb+DB[0]+DB[1]+DB[2]+DB[3] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6]+DB[7] */
322 __ASM_EMIT("vaddps %%ymm5, %%ymm0, %%ymm0") /* ymm0 = T = xv+DV[0] xv+DV[0]+DV[1] xv+DV[0]+DV[1]+DV[2] xv+DV[0]+DV[1]+DV[2]+DV[3] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6]+DV[7] */
323 __ASM_EMIT("vmulps %%ymm2, %%ymm1, %%ymm7") /* ymm7 = B = BA*BB */
324 __ASM_EMIT("vextractf128 $1, %%ymm1, %%xmm4") /* xmm4 = BA[4] BA[5] BA[6] BA[7] */
325 __ASM_EMIT("vextractf128 $1, %%ymm0, %%xmm3") /* xmm3 = T[4] T[5] T[6] T[7] */
326 __ASM_EMIT("vextractf128 $1, %%ymm2, %%xmm5") /* xmm5 = BB[4] BB[5] BB[6] BB[7] */
327 __ASM_EMIT("vshufps $0xff, %%xmm3, %%xmm3, %%xmm3") /* xmm3 = T[7] T[7] T[7] T[7] */
328 __ASM_EMIT("vshufps $0xff, %%xmm4, %%xmm4, %%xmm4") /* xmm4 = BA[7] BA[7] BA[7] BA[7] */
329 __ASM_EMIT("vshufps $0xff, %%xmm5, %%xmm5, %%xmm5") /* xmm5 = BB[7] BB[7] BB[7] BB[7] */
330
331 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
332 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
333 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
334 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
335 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
336 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
337
338 __ASM_EMIT("vsqrtps %%ymm7, %%ymm6") /* ymm6 = sqrtf(B) */
339 __ASM_EMIT("vcmpps $5, %[CORR_CC], %%ymm7, %%ymm1") /* ymm1 = B >= threshold */
340 __ASM_EMIT("vdivps %%ymm6, %%ymm0, %%ymm0") /* ymm0 = T/sqrtf(B) */
341 __ASM_EMIT32("mov %[dst], %[ptr]")
342 __ASM_EMIT("vandps %%ymm1, %%ymm0, %%ymm0") /* ymm0 = (B >= threshold) ? T/sqrtf(B) : 0 */
343 __ASM_EMIT("add $0x20, %[a_head]") /* ++a_head */
344 __ASM_EMIT("add $0x20, %[b_head]") /* ++b_head */
345 __ASM_EMIT32("vmovups %%ymm0, 0x00(%[ptr])")
346 __ASM_EMIT64("vmovups %%ymm0, 0x00(%[dst])")
347 __ASM_EMIT("add $0x20, %[a_tail]") /* ++a_tail */
348 __ASM_EMIT("add $0x20, %[b_tail]") /* ++b_tail */
349 __ASM_EMIT32("add $0x20, %[ptr]")
350 __ASM_EMIT64("add $0x20, %[dst]")
351 __ASM_EMIT32("mov %[ptr], %[dst]")
352 __ASM_EMIT32("subl $8, %[count]")
353 __ASM_EMIT64("sub $8, %[count]")
354 __ASM_EMIT("4:")
355 /* 4x block */
356 __ASM_EMIT32("addl $4, %[count]")
357 __ASM_EMIT64("add $4, %[count]")
358 __ASM_EMIT("jl 6f")
359 __ASM_EMIT("vmovups 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
360 __ASM_EMIT("vmovups 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
361 __ASM_EMIT("vmovups 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
362 __ASM_EMIT("vmovups 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
363 __ASM_EMIT("vmulps %%xmm1, %%xmm0, %%xmm2") /* xmm2 = ah*bh */
364 __ASM_EMIT("vmulps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = ah*ah */
365 __ASM_EMIT("vmulps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = bh*bh */
366 __ASM_EMIT("vxorps %%xmm6, %%xmm6, %%xmm6") /* xmm6 = 0 0 0 0 0 0 0 0 */
367 __ASM_EMIT("vfnmadd231ps %%xmm4, %%xmm3, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
368 __ASM_EMIT("vfnmadd231ps %%xmm3, %%xmm3, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
369 __ASM_EMIT("vfnmadd231ps %%xmm4, %%xmm4, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
370
371 __ASM_EMIT("vmovlhps %%xmm0, %%xmm6, %%xmm3") /* xmm3 = 0 0 DA[0] DA[1] 0 0 DA[4] DA[5] */
372 __ASM_EMIT("vmovlhps %%xmm1, %%xmm6, %%xmm4") /* xmm4 = 0 0 DB[0] DB[1] 0 0 DB[4] DB[5] */
373 __ASM_EMIT("vmovlhps %%xmm2, %%xmm6, %%xmm5") /* xmm5 = 0 0 DV[0] DV[1] 0 0 DV[4] DV[5] */
374 __ASM_EMIT("vaddps %%xmm3, %%xmm0, %%xmm0") /* xmm0 = DA[0] DA[1] DA[0]+DA[2] DA[1]+DA[3] DA[4] DA[5] DA[4]+DA[6] DA[5]+DA[7] */
375 __ASM_EMIT("vaddps %%xmm4, %%xmm1, %%xmm1") /* xmm1 = DB[0] DB[1] DB[0]+DB[2] DB[1]+DB[3] DB[4] DB[5] DB[4]+DB[6] DB[5]+DB[7] */
376 __ASM_EMIT("vaddps %%xmm5, %%xmm2, %%xmm2") /* xmm2 = DV[0] DV[1] DV[0]+DV[2] DV[1]+DV[3] DV[4] DV[5] DV[4]+DV[6] DV[5]+DV[7] */
377 __ASM_EMIT("vshufps $0x99, %%xmm0, %%xmm3, %%xmm3") /* xmm3 = 0 DA[0] DA[1] DA[0]+DA[2] 0 DA[4] DA[5] DA[4]+DA[6] */
378 __ASM_EMIT("vshufps $0x99, %%xmm1, %%xmm4, %%xmm4") /* xmm4 = 0 DB[0] DB[1] DB[0]+DB[2] 0 DB[4] DB[5] DB[4]+DB[6] */
379 __ASM_EMIT("vshufps $0x99, %%xmm2, %%xmm5, %%xmm5") /* xmm5 = 0 DV[0] DV[1] DV[0]+DV[2] 0 DV[4] DV[5] DV[4]+DV[6] */
380 __ASM_EMIT("vaddps %%xmm0, %%xmm3, %%xmm3") /* xmm3 = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] DA[4] DA[4]+DA[5] DA[4]+DA[5]+DA[6] DA[4]+DA[5]+DA[6]+DA[7] */
381 __ASM_EMIT("vaddps %%xmm1, %%xmm4, %%xmm4") /* xmm4 = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] DB[4] DB[4]+DB[5] DB[4]+DB[5]+DB[6] DB[4]+DB[5]+DB[6]+DB[7] */
382 __ASM_EMIT("vaddps %%xmm2, %%xmm5, %%xmm5") /* xmm5 = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] DV[4] DV[4]+DV[5] DV[4]+DV[5]+DV[6] DV[4]+DV[5]+DV[6]+DV[7] */
383
384 __ASM_EMIT32("mov %[corr], %[ptr]")
385 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%xmm0")
386 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%xmm1")
387 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%xmm2")
388 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%xmm0")
389 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%xmm1")
390 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%xmm2")
391
392 __ASM_EMIT("vaddps %%xmm3, %%xmm1, %%xmm1") /* xmm1 = BA = xa+DA[0] xa+DA[0]+DA[1] xa+DA[0]+DA[1]+DA[2] xa+DA[0]+DA[1]+DA[2]+DA[3] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6] xa+DA[0]+DA[1]+DA[2]+DA[3]+DA[4]+DA[5]+DA[6]+DA[7] */
393 __ASM_EMIT("vaddps %%xmm4, %%xmm2, %%xmm2") /* xmm2 = BB = xb+DB[0] xb+DB[0]+DB[1] xb+DB[0]+DB[1]+DB[2] xb+DB[0]+DB[1]+DB[2]+DB[3] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6] xb+DB[0]+DB[1]+DB[2]+DB[3]+DB[4]+DB[5]+DB[6]+DB[7] */
394 __ASM_EMIT("vaddps %%xmm5, %%xmm0, %%xmm0") /* xmm0 = T = xv+DV[0] xv+DV[0]+DV[1] xv+DV[0]+DV[1]+DV[2] xv+DV[0]+DV[1]+DV[2]+DV[3] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6] xv+DV[0]+DV[1]+DV[2]+DV[3]+DV[4]+DV[5]+DV[6]+DV[7] */
395 __ASM_EMIT("vmulps %%xmm2, %%xmm1, %%xmm7") /* xmm7 = B = BA*BB */
396 __ASM_EMIT("vshufps $0xff, %%xmm0, %%xmm0, %%xmm3") /* xmm3 = T[7] T[7] T[7] T[7] */
397 __ASM_EMIT("vshufps $0xff, %%xmm1, %%xmm1, %%xmm4") /* xmm4 = BA[7] BA[7] BA[7] BA[7] */
398 __ASM_EMIT("vshufps $0xff, %%xmm2, %%xmm2, %%xmm5") /* xmm5 = BB[7] BB[7] BB[7] BB[7] */
399
400 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
401 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
402 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
403 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
404 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
405 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
406
407 __ASM_EMIT("vsqrtps %%xmm7, %%xmm6") /* xmm6 = sqrtf(B) */
408 __ASM_EMIT("vcmpps $5, %[CORR_CC], %%xmm7, %%xmm1") /* xmm1 = B >= threshold */
409 __ASM_EMIT("vdivps %%xmm6, %%xmm0, %%xmm0") /* xmm0 = T/sqrtf(B) */
410 __ASM_EMIT32("mov %[dst], %[ptr]")
411 __ASM_EMIT("vandps %%xmm1, %%xmm0, %%xmm0") /* xmm0 = (B >= threshold) ? T/sqrtf(B) : 0 */
412 __ASM_EMIT("add $0x10, %[a_head]") /* ++a_head */
413 __ASM_EMIT("add $0x10, %[b_head]") /* ++b_head */
414 __ASM_EMIT32("vmovups %%xmm0, 0x00(%[ptr])")
415 __ASM_EMIT64("vmovups %%xmm0, 0x00(%[dst])")
416 __ASM_EMIT("add $0x10, %[a_tail]") /* ++a_tail */
417 __ASM_EMIT("add $0x10, %[b_tail]") /* ++b_tail */
418 __ASM_EMIT32("add $0x10, %[ptr]")
419 __ASM_EMIT64("add $0x10, %[dst]")
420 __ASM_EMIT32("mov %[ptr], %[dst]")
421 __ASM_EMIT32("subl $4, %[count]")
422 __ASM_EMIT64("sub $4, %[count]")
423 __ASM_EMIT("6:")
424
425 /* 1x blocks */
426 __ASM_EMIT32("addl $3, %[count]")
427 __ASM_EMIT64("add $3, %[count]")
428 __ASM_EMIT("jl 8f")
429 __ASM_EMIT("7:")
430 __ASM_EMIT("vmovss 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
431 __ASM_EMIT("vmovss 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
432 __ASM_EMIT("vmovss 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
433 __ASM_EMIT("vmovss 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
434 __ASM_EMIT("vmulss %%xmm1, %%xmm0, %%xmm2") /* xmm2 = ah*bh */
435 __ASM_EMIT("vmulss %%xmm0, %%xmm0, %%xmm0") /* xmm0 = ah*ah */
436 __ASM_EMIT("vmulss %%xmm1, %%xmm1, %%xmm1") /* xmm1 = bh*bh */
437 __ASM_EMIT("vfnmadd231ss %%xmm4, %%xmm3, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
438 __ASM_EMIT("vfnmadd231ss %%xmm3, %%xmm3, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
439 __ASM_EMIT("vfnmadd231ss %%xmm4, %%xmm4, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
440
441 __ASM_EMIT32("mov %[corr], %[ptr]")
442 __ASM_EMIT32("vmovss 0x00(%[ptr]), %%xmm3")
443 __ASM_EMIT32("vmovss 0x04(%[ptr]), %%xmm4")
444 __ASM_EMIT32("vmovss 0x08(%[ptr]), %%xmm5")
445 __ASM_EMIT64("vmovss 0x00(%[corr]), %%xmm3")
446 __ASM_EMIT64("vmovss 0x04(%[corr]), %%xmm4")
447 __ASM_EMIT64("vmovss 0x08(%[corr]), %%xmm5")
448
449 __ASM_EMIT("vaddss %%xmm3, %%xmm2, %%xmm2") /* xmm2 = T = xv+DV */
450 __ASM_EMIT("vaddss %%xmm4, %%xmm0, %%xmm0") /* xmm0 = BA = xa+DA */
451 __ASM_EMIT("vaddss %%xmm5, %%xmm1, %%xmm1") /* xmm1 = BB = xb+DB */
452 __ASM_EMIT("vmulss %%xmm1, %%xmm0, %%xmm7") /* xmm7 = B = BA*BB */
453
454 __ASM_EMIT32("vmovss %%xmm2, 0x00(%[ptr])")
455 __ASM_EMIT32("vmovss %%xmm0, 0x04(%[ptr])")
456 __ASM_EMIT32("vmovss %%xmm1, 0x08(%[ptr])")
457 __ASM_EMIT64("vmovss %%xmm2, 0x00(%[corr])")
458 __ASM_EMIT64("vmovss %%xmm0, 0x04(%[corr])")
459 __ASM_EMIT64("vmovss %%xmm1, 0x08(%[corr])")
460
461 __ASM_EMIT("vsqrtss %%xmm7, %%xmm7, %%xmm6") /* xmm6 = sqrtf(B) */
462 __ASM_EMIT("vcmpss $5, %[CORR_CC], %%xmm7, %%xmm1") /* xmm1 = B >= threshold */
463 __ASM_EMIT("vdivss %%xmm6, %%xmm2, %%xmm0") /* xmm0 = T/sqrtf(B) */
464 __ASM_EMIT32("mov %[dst], %[ptr]")
465 __ASM_EMIT("vandps %%xmm1, %%xmm0, %%xmm0") /* xmm0 = (B >= threshold) ? T/sqrtf(B) : 0 */
466 __ASM_EMIT("add $0x04, %[a_head]") /* ++a_head */
467 __ASM_EMIT("add $0x04, %[b_head]") /* ++b_head */
468 __ASM_EMIT32("vmovss %%xmm0, 0x00(%[ptr])")
469 __ASM_EMIT64("vmovss %%xmm0, 0x00(%[dst])")
470 __ASM_EMIT("add $0x04, %[a_tail]") /* ++a_tail */
471 __ASM_EMIT("add $0x04, %[b_tail]") /* ++b_tail */
472 __ASM_EMIT32("add $0x04, %[ptr]")
473 __ASM_EMIT64("add $0x04, %[dst]")
474 __ASM_EMIT32("mov %[ptr], %[dst]")
475 __ASM_EMIT32("decl %[count]")
476 __ASM_EMIT64("dec %[count]")
477 __ASM_EMIT("jge 7b")
478 __ASM_EMIT("8:")
479
480 : __IF_32(
481 [ptr] "=&r" (ptr),
482 [corr] "+m" (corr), [dst] "+m" (dst),
483 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
484 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
485 [count] "+m" (count)
486 )
487 __IF_64(
488 [dst] "+r" (dst),
489 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
490 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
491 [count] "+r" (count)
492 )
493 : __IF_64( [corr] "r" (corr), )
494 [CORR_CC] "o" (corr_const),
495 [CORR_IDX] "o" (corr_idx),
496 [CORR_KMASK] "o" (corr_kmask)
497 : "cc", "memory",
498 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
499 "%xmm4", "%xmm5", "%xmm6", "%xmm7",
500 "%k1", "%k2", "%k3"
501 );
502 }
503
504 } /* namespace avx512 */
505 } /* namespace lsp */
506
507
508
509
510
511 #endif /* PRIVATE_DSP_ARCH_X86_AVX512_CORRELATION_H_ */