Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/x86/avx/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_AVX_CORRELATION_H_
23 #define PRIVATE_DSP_ARCH_X86_AVX_CORRELATION_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_AVX_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_AVX_IMPL */
28
29 namespace lsp
30 {
31 namespace avx
32 {
33
34 void corr_init(dsp::correlation_t *corr, const float *a, const float *b, size_t count)
35 {
36 IF_ARCH_X86(
37 size_t off;
38 );
39
40 ARCH_X86_ASM
41 (
42 __ASM_EMIT("xor %[off], %[off]")
43 __ASM_EMIT("vxorps %%ymm0, %%ymm0, %%ymm0") /* xv = 0 */
44 __ASM_EMIT("vxorps %%ymm1, %%ymm1, %%ymm1") /* xa = 0 */
45 __ASM_EMIT("vxorps %%ymm2, %%ymm2, %%ymm2") /* xb = 0 */
46 /* 16x blocks */
47 __ASM_EMIT("sub $16, %[count]")
48 __ASM_EMIT("jb 2f")
49 __ASM_EMIT("1:")
50 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%ymm3") /* ymm3 = a0 */
51 __ASM_EMIT("vmovups 0x20(%[a], %[off]), %%ymm4") /* ymm4 = a1 */
52 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%ymm5") /* ymm5 = b0 */
53 __ASM_EMIT("vmovups 0x20(%[b], %[off]), %%ymm6") /* ymm6 = b1 */
54 __ASM_EMIT("vmulps %%ymm3, %%ymm5, %%ymm7") /* ymm7 = a0*b0 */
55 __ASM_EMIT("vmulps %%ymm3, %%ymm3, %%ymm3") /* ymm3 = a0*a0 */
56 __ASM_EMIT("vmulps %%ymm5, %%ymm5, %%ymm5") /* ymm5 = b0*b0 */
57 __ASM_EMIT("vaddps %%ymm7, %%ymm0, %%ymm0") /* ymm0 = xv + a0*b0 */
58 __ASM_EMIT("vaddps %%ymm3, %%ymm1, %%ymm1") /* ymm1 = xa + a0*a0 */
59 __ASM_EMIT("vaddps %%ymm5, %%ymm2, %%ymm2") /* ymm2 = xb + b0*b0 */
60 __ASM_EMIT("vmulps %%ymm4, %%ymm6, %%ymm7") /* ymm7 = a0*b0 */
61 __ASM_EMIT("vmulps %%ymm4, %%ymm4, %%ymm4") /* ymm4 = a0*a0 */
62 __ASM_EMIT("vmulps %%ymm6, %%ymm6, %%ymm6") /* ymm6 = b0*b0 */
63 __ASM_EMIT("vaddps %%ymm7, %%ymm0, %%ymm0") /* ymm0 = xv + a1*b1 */
64 __ASM_EMIT("vaddps %%ymm4, %%ymm1, %%ymm1") /* ymm1 = xa + a1*a1 */
65 __ASM_EMIT("vaddps %%ymm6, %%ymm2, %%ymm2") /* ymm2 = xb + b1*b1 */
66 __ASM_EMIT("add $0x40, %[off]") /* ++off */
67 __ASM_EMIT("sub $16, %[count]")
68 __ASM_EMIT("jae 1b")
69 __ASM_EMIT("vextractf128 $1, %%ymm0, %%xmm4")
70 __ASM_EMIT("vextractf128 $1, %%ymm1, %%xmm5")
71 __ASM_EMIT("vextractf128 $1, %%ymm2, %%xmm6")
72 __ASM_EMIT("vaddps %%xmm4, %%xmm0, %%xmm0")
73 __ASM_EMIT("vaddps %%xmm5, %%xmm1, %%xmm1")
74 __ASM_EMIT("vaddps %%xmm6, %%xmm2, %%xmm2")
75 __ASM_EMIT("2:")
76 /* 8x block */
77 __ASM_EMIT("add $8, %[count]")
78 __ASM_EMIT("jl 4f")
79 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
80 __ASM_EMIT("vmovups 0x10(%[a], %[off]), %%xmm4") /* xmm4 = a1 */
81 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
82 __ASM_EMIT("vmovups 0x10(%[b], %[off]), %%xmm6") /* xmm6 = b1 */
83 __ASM_EMIT("vmulps %%xmm3, %%xmm5, %%xmm7") /* xmm7 = a0*b0 */
84 __ASM_EMIT("vmulps %%xmm3, %%xmm3, %%xmm3") /* xmm3 = a0*a0 */
85 __ASM_EMIT("vmulps %%xmm5, %%xmm5, %%xmm5") /* xmm5 = b0*b0 */
86 __ASM_EMIT("vaddps %%xmm7, %%xmm0, %%xmm0") /* xmm0 = xv + a0*b0 */
87 __ASM_EMIT("vaddps %%xmm3, %%xmm1, %%xmm1") /* xmm1 = xa + a0*a0 */
88 __ASM_EMIT("vaddps %%xmm5, %%xmm2, %%xmm2") /* xmm2 = xb + b0*b0 */
89 __ASM_EMIT("vmulps %%xmm4, %%xmm6, %%xmm7") /* xmm7 = a0*b0 */
90 __ASM_EMIT("vmulps %%xmm4, %%xmm4, %%xmm4") /* xmm4 = a0*a0 */
91 __ASM_EMIT("vmulps %%xmm6, %%xmm6, %%xmm6") /* xmm6 = b0*b0 */
92 __ASM_EMIT("vaddps %%xmm7, %%xmm0, %%xmm0") /* xmm0 = xv + a1*b1 */
93 __ASM_EMIT("vaddps %%xmm4, %%xmm1, %%xmm1") /* xmm1 = xa + a1*a1 */
94 __ASM_EMIT("vaddps %%xmm6, %%xmm2, %%xmm2") /* xmm2 = xb + b1*b1 */
95 __ASM_EMIT("sub $8, %[count]")
96 __ASM_EMIT("add $0x20, %[off]") /* ++off */
97 __ASM_EMIT("4:")
98 /* 4x block */
99 __ASM_EMIT("add $4, %[count]")
100 __ASM_EMIT("jl 6f")
101 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
102 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
103 __ASM_EMIT("vmulps %%xmm3, %%xmm5, %%xmm7") /* xmm7 = a0*b0 */
104 __ASM_EMIT("vmulps %%xmm3, %%xmm3, %%xmm3") /* xmm3 = a0*a0 */
105 __ASM_EMIT("vmulps %%xmm5, %%xmm5, %%xmm5") /* xmm5 = b0*b0 */
106 __ASM_EMIT("vaddps %%xmm7, %%xmm0, %%xmm0") /* xmm0 = xv + a0*b0 */
107 __ASM_EMIT("vaddps %%xmm3, %%xmm1, %%xmm1") /* xmm1 = xa + a0*a0 */
108 __ASM_EMIT("vaddps %%xmm5, %%xmm2, %%xmm2") /* xmm2 = xb + b0*b0 */
109 __ASM_EMIT("sub $4, %[count]")
110 __ASM_EMIT("add $0x10, %[off]") /* ++off */
111 __ASM_EMIT("6:")
112 /* Do horizontal sum */
113 __ASM_EMIT("vhaddps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
114 __ASM_EMIT("vhaddps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
115 __ASM_EMIT("vhaddps %%xmm2, %%xmm2, %%xmm2") /* xmm2 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
116 __ASM_EMIT("vhaddps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = xv0+xv1+xv2+xv3 */
117 __ASM_EMIT("vhaddps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = xv0+xv1+xv2+xv3 */
118 __ASM_EMIT("vhaddps %%xmm2, %%xmm2, %%xmm2") /* xmm2 = xv0+xv1+xv2+xv3 */
119 /* 1x blocks */
120 __ASM_EMIT("add $3, %[count]")
121 __ASM_EMIT("jl 8f")
122 __ASM_EMIT("7:")
123 __ASM_EMIT("vmovss 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
124 __ASM_EMIT("vmovss 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
125 __ASM_EMIT("vmulss %%xmm3, %%xmm5, %%xmm7") /* xmm7 = a0*b0 */
126 __ASM_EMIT("vmulss %%xmm3, %%xmm3, %%xmm3") /* xmm3 = a0*a0 */
127 __ASM_EMIT("vmulss %%xmm5, %%xmm5, %%xmm5") /* xmm5 = b0*b0 */
128 __ASM_EMIT("vaddss %%xmm7, %%xmm0, %%xmm0") /* xmm0 = xv + a0*b0 */
129 __ASM_EMIT("vaddss %%xmm3, %%xmm1, %%xmm1") /* xmm1 = xa + a0*a0 */
130 __ASM_EMIT("vaddss %%xmm5, %%xmm2, %%xmm2") /* xmm2 = xb + b0*b0 */
131 __ASM_EMIT("add $0x04, %[off]") /* ++off */
132 __ASM_EMIT("dec %[count]")
133 __ASM_EMIT("jge 7b")
134 __ASM_EMIT("8:")
135 /* Store result */
136 __ASM_EMIT("vaddss 0x00(%[corr]), %%xmm0, %%xmm0")
137 __ASM_EMIT("vaddss 0x04(%[corr]), %%xmm1, %%xmm1")
138 __ASM_EMIT("vaddss 0x08(%[corr]), %%xmm2, %%xmm2")
139 __ASM_EMIT("vmovss %%xmm0, 0x00(%[corr])")
140 __ASM_EMIT("vmovss %%xmm1, 0x04(%[corr])")
141 __ASM_EMIT("vmovss %%xmm2, 0x08(%[corr])")
142
143 : [corr] "+r" (corr), [off] "=&r" (off), [count] "+r" (count)
144 : [a] "r" (a), [b] "r" (b)
145 : "cc", "memory",
146 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
147 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
148 );
149 }
150
151 void corr_init_fma3(dsp::correlation_t *corr, const float *a, const float *b, size_t count)
152 {
153 IF_ARCH_X86(
154 size_t off;
155 );
156
157 ARCH_X86_ASM
158 (
159 __ASM_EMIT("xor %[off], %[off]")
160 __ASM_EMIT("vxorps %%ymm0, %%ymm0, %%ymm0") /* xv = 0 */
161 __ASM_EMIT("vxorps %%ymm1, %%ymm1, %%ymm1") /* xa = 0 */
162 __ASM_EMIT("vxorps %%ymm2, %%ymm2, %%ymm2") /* xb = 0 */
163 /* 16x blocks */
164 __ASM_EMIT("sub $16, %[count]")
165 __ASM_EMIT("jb 2f")
166 __ASM_EMIT("1:")
167 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%ymm3") /* ymm3 = a0 */
168 __ASM_EMIT("vmovups 0x20(%[a], %[off]), %%ymm4") /* ymm4 = a1 */
169 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%ymm5") /* ymm5 = b0 */
170 __ASM_EMIT("vmovups 0x20(%[b], %[off]), %%ymm6") /* ymm6 = b1 */
171 __ASM_EMIT("vfmadd231ps %%ymm3, %%ymm5, %%ymm0") /* ymm0 = xv + a0*b0 */
172 __ASM_EMIT("vfmadd231ps %%ymm3, %%ymm3, %%ymm1") /* ymm1 = xa + a0*a0 */
173 __ASM_EMIT("vfmadd231ps %%ymm5, %%ymm5, %%ymm2") /* ymm2 = xv + b0*b0 */
174 __ASM_EMIT("vfmadd231ps %%ymm4, %%ymm6, %%ymm0") /* ymm0 = xv + a1*b1 */
175 __ASM_EMIT("vfmadd231ps %%ymm4, %%ymm4, %%ymm1") /* ymm1 = xa + a1*a1 */
176 __ASM_EMIT("vfmadd231ps %%ymm6, %%ymm6, %%ymm2") /* ymm2 = xv + b1*b1 */
177 __ASM_EMIT("add $0x40, %[off]") /* ++off */
178 __ASM_EMIT("sub $16, %[count]")
179 __ASM_EMIT("jae 1b")
180 __ASM_EMIT("vextractf128 $1, %%ymm0, %%xmm4")
181 __ASM_EMIT("vextractf128 $1, %%ymm1, %%xmm5")
182 __ASM_EMIT("vextractf128 $1, %%ymm2, %%xmm6")
183 __ASM_EMIT("vaddps %%xmm4, %%xmm0, %%xmm0")
184 __ASM_EMIT("vaddps %%xmm5, %%xmm1, %%xmm1")
185 __ASM_EMIT("vaddps %%xmm6, %%xmm2, %%xmm2")
186 __ASM_EMIT("2:")
187 /* 8x block */
188 __ASM_EMIT("add $8, %[count]")
189 __ASM_EMIT("jl 4f")
190 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
191 __ASM_EMIT("vmovups 0x10(%[a], %[off]), %%xmm4") /* xmm4 = a1 */
192 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
193 __ASM_EMIT("vmovups 0x10(%[b], %[off]), %%xmm6") /* xmm6 = b1 */
194 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm5, %%xmm0") /* xmm0 = xv + a0*b0 */
195 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm3, %%xmm1") /* xmm1 = xa + a0*a0 */
196 __ASM_EMIT("vfmadd231ps %%xmm5, %%xmm5, %%xmm2") /* xmm2 = xv + b0*b0 */
197 __ASM_EMIT("vfmadd231ps %%xmm4, %%xmm6, %%xmm0") /* xmm0 = xv + a1*b1 */
198 __ASM_EMIT("vfmadd231ps %%xmm4, %%xmm4, %%xmm1") /* xmm1 = xa + a1*a1 */
199 __ASM_EMIT("vfmadd231ps %%xmm6, %%xmm6, %%xmm2") /* xmm2 = xv + b1*b1 */
200 __ASM_EMIT("sub $8, %[count]")
201 __ASM_EMIT("add $0x20, %[off]") /* ++off */
202 __ASM_EMIT("4:")
203 /* 4x block */
204 __ASM_EMIT("add $4, %[count]")
205 __ASM_EMIT("jl 6f")
206 __ASM_EMIT("vmovups 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
207 __ASM_EMIT("vmovups 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
208 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm5, %%xmm0") /* xmm0 = xv + a0*b0 */
209 __ASM_EMIT("vfmadd231ps %%xmm3, %%xmm3, %%xmm1") /* xmm1 = xa + a0*a0 */
210 __ASM_EMIT("vfmadd231ps %%xmm5, %%xmm5, %%xmm2") /* xmm2 = xv + b0*b0 */
211 __ASM_EMIT("sub $4, %[count]")
212 __ASM_EMIT("add $0x10, %[off]") /* ++off */
213 __ASM_EMIT("6:")
214 /* Do horizontal sum */
215 __ASM_EMIT("vhaddps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
216 __ASM_EMIT("vhaddps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
217 __ASM_EMIT("vhaddps %%xmm2, %%xmm2, %%xmm2") /* xmm2 = xv0+xv1 xv2+xv3 xv0+xv1 xv2+xv3 */
218 __ASM_EMIT("vhaddps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = xv0+xv1+xv2+xv3 */
219 __ASM_EMIT("vhaddps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = xv0+xv1+xv2+xv3 */
220 __ASM_EMIT("vhaddps %%xmm2, %%xmm2, %%xmm2") /* xmm2 = xv0+xv1+xv2+xv3 */
221 /* 1x blocks */
222 __ASM_EMIT("add $3, %[count]")
223 __ASM_EMIT("jl 8f")
224 __ASM_EMIT("7:")
225 __ASM_EMIT("vmovss 0x00(%[a], %[off]), %%xmm3") /* xmm3 = a0 */
226 __ASM_EMIT("vmovss 0x00(%[b], %[off]), %%xmm5") /* xmm5 = b0 */
227 __ASM_EMIT("vfmadd231ss %%xmm3, %%xmm5, %%xmm0") /* xmm0 = xv + a0*b0 */
228 __ASM_EMIT("vfmadd231ss %%xmm3, %%xmm3, %%xmm1") /* xmm1 = xa + a0*a0 */
229 __ASM_EMIT("vfmadd231ss %%xmm5, %%xmm5, %%xmm2") /* xmm2 = xv + b0*b0 */
230 __ASM_EMIT("add $0x04, %[off]") /* ++off */
231 __ASM_EMIT("dec %[count]")
232 __ASM_EMIT("jge 7b")
233 __ASM_EMIT("8:")
234 /* Store result */
235 __ASM_EMIT("vaddss 0x00(%[corr]), %%xmm0, %%xmm0")
236 __ASM_EMIT("vaddss 0x04(%[corr]), %%xmm1, %%xmm1")
237 __ASM_EMIT("vaddss 0x08(%[corr]), %%xmm2, %%xmm2")
238 __ASM_EMIT("vmovss %%xmm0, 0x00(%[corr])")
239 __ASM_EMIT("vmovss %%xmm1, 0x04(%[corr])")
240 __ASM_EMIT("vmovss %%xmm2, 0x08(%[corr])")
241
242 : [corr] "+r" (corr), [off] "=&r" (off), [count] "+r" (count)
243 : [a] "r" (a), [b] "r" (b)
244 : "cc", "memory",
245 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
246 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
247 );
248 }
249
250 static const float corr_const[] __lsp_aligned32 =
251 {
252 LSP_DSP_VEC8(1e-18f)
253 };
254
255 void corr_incr(dsp::correlation_t *corr, float *dst,
256 const float *a_head, const float *b_head,
257 const float *a_tail, const float *b_tail,
258 size_t count)
259 {
260 IF_ARCH_I386(
261 void *ptr;
262 );
263
264 ARCH_X86_ASM
265 (
266 /* 8x blocks */
267 __ASM_EMIT32("subl $8, %[count]")
268 __ASM_EMIT64("sub $8, %[count]")
269 __ASM_EMIT("jb 2f")
270 __ASM_EMIT("1:")
271 __ASM_EMIT("vmovups 0x00(%[a_head]), %%ymm0") /* ymm0 = ah */
272 __ASM_EMIT("vmovups 0x00(%[b_head]), %%ymm1") /* ymm1 = bh */
273 __ASM_EMIT("vmovups 0x00(%[a_tail]), %%ymm3") /* ymm3 = at */
274 __ASM_EMIT("vmovups 0x00(%[b_tail]), %%ymm4") /* ymm4 = bt */
275 __ASM_EMIT("vmulps %%ymm1, %%ymm0, %%ymm2") /* ymm2 = ah*bh */
276 __ASM_EMIT("vmulps %%ymm4, %%ymm3, %%ymm5") /* ymm5 = at*bt */
277 __ASM_EMIT("vmulps %%ymm0, %%ymm0, %%ymm0") /* ymm0 = ah*ah */
278 __ASM_EMIT("vmulps %%ymm1, %%ymm1, %%ymm1") /* ymm1 = bh*bh */
279 __ASM_EMIT("vmulps %%ymm3, %%ymm3, %%ymm3") /* ymm3 = at*at */
280 __ASM_EMIT("vmulps %%ymm4, %%ymm4, %%ymm4") /* ymm4 = bt*bt */
281 __ASM_EMIT("vxorps %%ymm6, %%ymm6, %%ymm6") /* ymm6 = 0 0 0 0 0 0 0 0 */
282 __ASM_EMIT("vsubps %%ymm5, %%ymm2, %%ymm2") /* ymm2 = DV = ah*bh - at*bt */
283 __ASM_EMIT("vsubps %%ymm3, %%ymm0, %%ymm0") /* ymm0 = DA = ah*ah - at*at */
284 __ASM_EMIT("vsubps %%ymm4, %%ymm1, %%ymm1") /* ymm1 = DB = bh*bh - bt*bt */
285
286 __ASM_EMIT("vshufps $0x44, %%ymm0, %%ymm6, %%ymm3") /* ymm3 = 0 0 DA[0] DA[1] 0 0 DA[4] DA[5] */
287 __ASM_EMIT("vshufps $0x44, %%ymm1, %%ymm6, %%ymm4") /* ymm4 = 0 0 DB[0] DB[1] 0 0 DB[4] DB[5] */
288 __ASM_EMIT("vshufps $0x44, %%ymm2, %%ymm6, %%ymm5") /* ymm5 = 0 0 DV[0] DV[1] 0 0 DV[4] DV[5] */
289 __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] */
290 __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] */
291 __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] */
292 __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] */
293 __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] */
294 __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] */
295 __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] */
296 __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] */
297 __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] */
298 __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] */
299 __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] */
300 __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] */
301 __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] */
302 __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] */
303 __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] */
304 __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] */
305 __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] */
306 __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] */
307 __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] */
308 __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] */
309 __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] */
310
311 __ASM_EMIT32("mov %[corr], %[ptr]")
312 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%ymm0")
313 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%ymm1")
314 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%ymm2")
315 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%ymm0")
316 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%ymm1")
317 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%ymm2")
318
319 __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] */
320 __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] */
321 __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] */
322 __ASM_EMIT("vmulps %%ymm2, %%ymm1, %%ymm7") /* ymm7 = B = BA*BB */
323 __ASM_EMIT("vextractf128 $1, %%ymm1, %%xmm4") /* xmm4 = BA[4] BA[5] BA[6] BA[7] */
324 __ASM_EMIT("vextractf128 $1, %%ymm0, %%xmm3") /* xmm3 = T[4] T[5] T[6] T[7] */
325 __ASM_EMIT("vextractf128 $1, %%ymm2, %%xmm5") /* xmm5 = BB[4] BB[5] BB[6] BB[7] */
326 __ASM_EMIT("vshufps $0xff, %%xmm3, %%xmm3, %%xmm3") /* xmm3 = T[7] T[7] T[7] T[7] */
327 __ASM_EMIT("vshufps $0xff, %%xmm4, %%xmm4, %%xmm4") /* xmm4 = BA[7] BA[7] BA[7] BA[7] */
328 __ASM_EMIT("vshufps $0xff, %%xmm5, %%xmm5, %%xmm5") /* xmm5 = BB[7] BB[7] BB[7] BB[7] */
329
330 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
331 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
332 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
333 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
334 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
335 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
336
337 __ASM_EMIT("vsqrtps %%ymm7, %%ymm6") /* ymm6 = sqrtf(B) */
338 __ASM_EMIT("vcmpps $5, %[CORR_CC], %%ymm7, %%ymm1")/* ymm1 = B >= threshold */
339 __ASM_EMIT("vdivps %%ymm6, %%ymm0, %%ymm0") /* ymm0 = T/sqrtf(B) */
340 __ASM_EMIT32("mov %[dst], %[ptr]")
341 __ASM_EMIT("vandps %%ymm1, %%ymm0, %%ymm0") /* ymm0 = (B >= threshold) ? T/sqrtf(B) : 0 */
342 __ASM_EMIT("add $0x20, %[a_head]") /* ++a_head */
343 __ASM_EMIT("add $0x20, %[b_head]") /* ++b_head */
344 __ASM_EMIT32("vmovups %%ymm0, 0x00(%[ptr])")
345 __ASM_EMIT64("vmovups %%ymm0, 0x00(%[dst])")
346 __ASM_EMIT("add $0x20, %[a_tail]") /* ++a_tail */
347 __ASM_EMIT("add $0x20, %[b_tail]") /* ++b_tail */
348 __ASM_EMIT32("add $0x20, %[ptr]")
349 __ASM_EMIT64("add $0x20, %[dst]")
350 __ASM_EMIT32("mov %[ptr], %[dst]")
351 __ASM_EMIT32("subl $8, %[count]")
352 __ASM_EMIT64("sub $8, %[count]")
353 __ASM_EMIT("jae 1b")
354 __ASM_EMIT("2:")
355 /* 4x block */
356 __ASM_EMIT32("addl $4, %[count]")
357 __ASM_EMIT64("add $4, %[count]")
358 __ASM_EMIT("jl 4f")
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 %%xmm4, %%xmm3, %%xmm5") /* xmm5 = at*bt */
365 __ASM_EMIT("vmulps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = ah*ah */
366 __ASM_EMIT("vmulps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = bh*bh */
367 __ASM_EMIT("vmulps %%xmm3, %%xmm3, %%xmm3") /* xmm3 = at*at */
368 __ASM_EMIT("vmulps %%xmm4, %%xmm4, %%xmm4") /* xmm4 = bt*bt */
369 __ASM_EMIT("vxorps %%xmm6, %%xmm6, %%xmm6") /* xmm6 = 0 0 0 0 0 0 0 0 */
370 __ASM_EMIT("vsubps %%xmm5, %%xmm2, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
371 __ASM_EMIT("vsubps %%xmm3, %%xmm0, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
372 __ASM_EMIT("vsubps %%xmm4, %%xmm1, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
373
374 __ASM_EMIT("vmovlhps %%xmm0, %%xmm6, %%xmm3") /* xmm3 = 0 0 DA[0] DA[1] 0 0 DA[4] DA[5] */
375 __ASM_EMIT("vmovlhps %%xmm1, %%xmm6, %%xmm4") /* xmm4 = 0 0 DB[0] DB[1] 0 0 DB[4] DB[5] */
376 __ASM_EMIT("vmovlhps %%xmm2, %%xmm6, %%xmm5") /* xmm5 = 0 0 DV[0] DV[1] 0 0 DV[4] DV[5] */
377 __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] */
378 __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] */
379 __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] */
380 __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] */
381 __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] */
382 __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] */
383 __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] */
384 __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] */
385 __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] */
386
387 __ASM_EMIT32("mov %[corr], %[ptr]")
388 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%xmm0")
389 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%xmm1")
390 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%xmm2")
391 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%xmm0")
392 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%xmm1")
393 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%xmm2")
394
395 __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] */
396 __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] */
397 __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] */
398 __ASM_EMIT("vmulps %%xmm2, %%xmm1, %%xmm7") /* xmm7 = B = BA*BB */
399 __ASM_EMIT("vshufps $0xff, %%xmm0, %%xmm0, %%xmm3") /* xmm3 = T[7] T[7] T[7] T[7] */
400 __ASM_EMIT("vshufps $0xff, %%xmm1, %%xmm1, %%xmm4") /* xmm4 = BA[7] BA[7] BA[7] BA[7] */
401 __ASM_EMIT("vshufps $0xff, %%xmm2, %%xmm2, %%xmm5") /* xmm5 = BB[7] BB[7] BB[7] BB[7] */
402
403 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
404 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
405 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
406 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
407 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
408 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
409
410 __ASM_EMIT("vsqrtps %%xmm7, %%xmm6") /* xmm6 = sqrtf(B) */
411 __ASM_EMIT("vcmpps $5, %[CORR_CC], %%xmm7, %%xmm1")/* xmm1 = B >= threshold */
412 __ASM_EMIT("vdivps %%xmm6, %%xmm0, %%xmm0") /* xmm0 = T/sqrtf(B) */
413 __ASM_EMIT32("mov %[dst], %[ptr]")
414 __ASM_EMIT("vandps %%xmm1, %%xmm0, %%xmm0") /* xmm0 = (B >= threshold) ? T/sqrtf(B) : 0 */
415 __ASM_EMIT("add $0x10, %[a_head]") /* ++a_head */
416 __ASM_EMIT("add $0x10, %[b_head]") /* ++b_head */
417 __ASM_EMIT32("vmovups %%xmm0, 0x00(%[ptr])")
418 __ASM_EMIT64("vmovups %%xmm0, 0x00(%[dst])")
419 __ASM_EMIT("add $0x10, %[a_tail]") /* ++a_tail */
420 __ASM_EMIT("add $0x10, %[b_tail]") /* ++b_tail */
421 __ASM_EMIT32("add $0x10, %[ptr]")
422 __ASM_EMIT64("add $0x10, %[dst]")
423 __ASM_EMIT32("mov %[ptr], %[dst]")
424 __ASM_EMIT32("subl $4, %[count]")
425 __ASM_EMIT64("sub $4, %[count]")
426 __ASM_EMIT("4:")
427
428 /* 1x blocks */
429 __ASM_EMIT32("addl $3, %[count]")
430 __ASM_EMIT64("add $3, %[count]")
431 __ASM_EMIT("jl 6f")
432 __ASM_EMIT("5:")
433 __ASM_EMIT("vmovss 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
434 __ASM_EMIT("vmovss 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
435 __ASM_EMIT("vmovss 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
436 __ASM_EMIT("vmovss 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
437 __ASM_EMIT("vmulss %%xmm1, %%xmm0, %%xmm2") /* xmm2 = ah*bh */
438 __ASM_EMIT("vmulss %%xmm4, %%xmm3, %%xmm5") /* xmm5 = at*bt */
439 __ASM_EMIT("vmulss %%xmm0, %%xmm0, %%xmm0") /* xmm0 = ah*ah */
440 __ASM_EMIT("vmulss %%xmm1, %%xmm1, %%xmm1") /* xmm1 = bh*bh */
441 __ASM_EMIT("vmulss %%xmm3, %%xmm3, %%xmm3") /* xmm3 = at*at */
442 __ASM_EMIT("vmulss %%xmm4, %%xmm4, %%xmm4") /* xmm4 = bt*bt */
443 __ASM_EMIT("vsubss %%xmm5, %%xmm2, %%xmm5") /* xmm5 = DV = ah*bh - at*bt */
444 __ASM_EMIT("vsubss %%xmm3, %%xmm0, %%xmm3") /* xmm3 = DA = ah*ah - at*at */
445 __ASM_EMIT("vsubss %%xmm4, %%xmm1, %%xmm4") /* xmm4 = DB = bh*bh - bt*bt */
446
447 __ASM_EMIT32("mov %[corr], %[ptr]")
448 __ASM_EMIT32("vmovss 0x00(%[ptr]), %%xmm0")
449 __ASM_EMIT32("vmovss 0x04(%[ptr]), %%xmm1")
450 __ASM_EMIT32("vmovss 0x08(%[ptr]), %%xmm2")
451 __ASM_EMIT64("vmovss 0x00(%[corr]), %%xmm0")
452 __ASM_EMIT64("vmovss 0x04(%[corr]), %%xmm1")
453 __ASM_EMIT64("vmovss 0x08(%[corr]), %%xmm2")
454
455 __ASM_EMIT("vaddss %%xmm3, %%xmm1, %%xmm1") /* xmm1 = BA = xa+DA */
456 __ASM_EMIT("vaddss %%xmm4, %%xmm2, %%xmm2") /* xmm2 = BB = xb+DB */
457 __ASM_EMIT("vaddss %%xmm5, %%xmm0, %%xmm0") /* xmm0 = T = xv+DV */
458 __ASM_EMIT("vmulss %%xmm2, %%xmm1, %%xmm7") /* xmm7 = B = BA*BB */
459
460 __ASM_EMIT32("vmovss %%xmm0, 0x00(%[ptr])")
461 __ASM_EMIT32("vmovss %%xmm1, 0x04(%[ptr])")
462 __ASM_EMIT32("vmovss %%xmm2, 0x08(%[ptr])")
463 __ASM_EMIT64("vmovss %%xmm0, 0x00(%[corr])")
464 __ASM_EMIT64("vmovss %%xmm1, 0x04(%[corr])")
465 __ASM_EMIT64("vmovss %%xmm2, 0x08(%[corr])")
466
467 __ASM_EMIT("vsqrtss %%xmm7, %%xmm7, %%xmm6") /* xmm6 = sqrtf(B) */
468 __ASM_EMIT("vcmpss $5, %[CORR_CC], %%xmm7, %%xmm1")/* xmm1 = B >= threshold */
469 __ASM_EMIT("vdivss %%xmm6, %%xmm0, %%xmm0") /* xmm0 = T/sqrtf(B) */
470 __ASM_EMIT32("mov %[dst], %[ptr]")
471 __ASM_EMIT("vandps %%xmm1, %%xmm0, %%xmm0") /* xmm0 = (B >= threshold) ? T/sqrtf(B) : 0 */
472 __ASM_EMIT("add $0x04, %[a_head]") /* ++a_head */
473 __ASM_EMIT("add $0x04, %[b_head]") /* ++b_head */
474 __ASM_EMIT32("vmovss %%xmm0, 0x00(%[ptr])")
475 __ASM_EMIT64("vmovss %%xmm0, 0x00(%[dst])")
476 __ASM_EMIT("add $0x04, %[a_tail]") /* ++a_tail */
477 __ASM_EMIT("add $0x04, %[b_tail]") /* ++b_tail */
478 __ASM_EMIT32("add $0x04, %[ptr]")
479 __ASM_EMIT64("add $0x04, %[dst]")
480 __ASM_EMIT32("mov %[ptr], %[dst]")
481 __ASM_EMIT32("decl %[count]")
482 __ASM_EMIT64("dec %[count]")
483 __ASM_EMIT("jge 5b")
484 __ASM_EMIT("6:")
485
486 : __IF_32(
487 [ptr] "=&r" (ptr),
488 [corr] "+m" (corr), [dst] "+m" (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] "+m" (count)
492 )
493 __IF_64(
494 [dst] "+r" (dst),
495 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
496 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
497 [count] "+r" (count)
498 )
499 : __IF_64( [corr] "r" (corr), )
500 [CORR_CC] "o" (corr_const)
501 : "cc", "memory",
502 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
503 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
504 );
505 }
506
507 void corr_incr_fma3(dsp::correlation_t *corr, float *dst,
508 const float *a_head, const float *b_head,
509 const float *a_tail, const float *b_tail,
510 size_t count)
511 {
512 IF_ARCH_I386(
513 void *ptr;
514 );
515
516 ARCH_X86_ASM
517 (
518 /* 8x blocks */
519 __ASM_EMIT32("subl $8, %[count]")
520 __ASM_EMIT64("sub $8, %[count]")
521 __ASM_EMIT("jb 2f")
522 __ASM_EMIT("1:")
523 __ASM_EMIT("vmovups 0x00(%[a_head]), %%ymm0") /* ymm0 = ah */
524 __ASM_EMIT("vmovups 0x00(%[b_head]), %%ymm1") /* ymm1 = bh */
525 __ASM_EMIT("vmovups 0x00(%[a_tail]), %%ymm3") /* ymm3 = at */
526 __ASM_EMIT("vmovups 0x00(%[b_tail]), %%ymm4") /* ymm4 = bt */
527 __ASM_EMIT("vmulps %%ymm1, %%ymm0, %%ymm2") /* ymm2 = ah*bh */
528 __ASM_EMIT("vmulps %%ymm0, %%ymm0, %%ymm0") /* ymm0 = ah*ah */
529 __ASM_EMIT("vmulps %%ymm1, %%ymm1, %%ymm1") /* ymm1 = bh*bh */
530 __ASM_EMIT("vxorps %%ymm6, %%ymm6, %%ymm6") /* ymm6 = 0 0 0 0 0 0 0 0 */
531 __ASM_EMIT("vfnmadd231ps %%ymm4, %%ymm3, %%ymm2") /* ymm2 = DV = ah*bh - at*bt */
532 __ASM_EMIT("vfnmadd231ps %%ymm3, %%ymm3, %%ymm0") /* ymm0 = DA = ah*ah - at*at */
533 __ASM_EMIT("vfnmadd231ps %%ymm4, %%ymm4, %%ymm1") /* ymm1 = DB = bh*bh - bt*bt */
534
535 __ASM_EMIT("vshufps $0x44, %%ymm0, %%ymm6, %%ymm3") /* ymm3 = 0 0 DA[0] DA[1] 0 0 DA[4] DA[5] */
536 __ASM_EMIT("vshufps $0x44, %%ymm1, %%ymm6, %%ymm4") /* ymm4 = 0 0 DB[0] DB[1] 0 0 DB[4] DB[5] */
537 __ASM_EMIT("vshufps $0x44, %%ymm2, %%ymm6, %%ymm5") /* ymm5 = 0 0 DV[0] DV[1] 0 0 DV[4] DV[5] */
538 __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] */
539 __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] */
540 __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] */
541 __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] */
542 __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] */
543 __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] */
544 __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] */
545 __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] */
546 __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] */
547 __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] */
548 __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] */
549 __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] */
550 __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] */
551 __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] */
552 __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] */
553 __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] */
554 __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] */
555 __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] */
556 __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] */
557 __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] */
558 __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] */
559
560 __ASM_EMIT32("mov %[corr], %[ptr]")
561 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%ymm0")
562 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%ymm1")
563 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%ymm2")
564 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%ymm0")
565 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%ymm1")
566 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%ymm2")
567
568 __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] */
569 __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] */
570 __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] */
571 __ASM_EMIT("vmulps %%ymm2, %%ymm1, %%ymm7") /* ymm7 = B = BA*BB */
572 __ASM_EMIT("vextractf128 $1, %%ymm1, %%xmm4") /* xmm4 = BA[4] BA[5] BA[6] BA[7] */
573 __ASM_EMIT("vextractf128 $1, %%ymm0, %%xmm3") /* xmm3 = T[4] T[5] T[6] T[7] */
574 __ASM_EMIT("vextractf128 $1, %%ymm2, %%xmm5") /* xmm5 = BB[4] BB[5] BB[6] BB[7] */
575 __ASM_EMIT("vshufps $0xff, %%xmm3, %%xmm3, %%xmm3") /* xmm3 = T[7] T[7] T[7] T[7] */
576 __ASM_EMIT("vshufps $0xff, %%xmm4, %%xmm4, %%xmm4") /* xmm4 = BA[7] BA[7] BA[7] BA[7] */
577 __ASM_EMIT("vshufps $0xff, %%xmm5, %%xmm5, %%xmm5") /* xmm5 = BB[7] BB[7] BB[7] BB[7] */
578
579 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
580 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
581 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
582 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
583 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
584 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
585
586 __ASM_EMIT("vsqrtps %%ymm7, %%ymm6") /* ymm6 = sqrtf(B) */
587 __ASM_EMIT("vcmpps $5, %[CORR_CC], %%ymm7, %%ymm1")/* ymm1 = B >= 1e-10f */
588 __ASM_EMIT("vdivps %%ymm6, %%ymm0, %%ymm0") /* ymm0 = T/sqrtf(B) */
589 __ASM_EMIT32("mov %[dst], %[ptr]")
590 __ASM_EMIT("vandps %%ymm1, %%ymm0, %%ymm0") /* ymm0 = (B >= 1e-10f) ? T/sqrtf(B) : 0 */
591 __ASM_EMIT("add $0x20, %[a_head]") /* ++a_head */
592 __ASM_EMIT("add $0x20, %[b_head]") /* ++b_head */
593 __ASM_EMIT32("vmovups %%ymm0, 0x00(%[ptr])")
594 __ASM_EMIT64("vmovups %%ymm0, 0x00(%[dst])")
595 __ASM_EMIT("add $0x20, %[a_tail]") /* ++a_tail */
596 __ASM_EMIT("add $0x20, %[b_tail]") /* ++b_tail */
597 __ASM_EMIT32("add $0x20, %[ptr]")
598 __ASM_EMIT64("add $0x20, %[dst]")
599 __ASM_EMIT32("mov %[ptr], %[dst]")
600 __ASM_EMIT32("subl $8, %[count]")
601 __ASM_EMIT64("sub $8, %[count]")
602 __ASM_EMIT("jae 1b")
603 __ASM_EMIT("2:")
604 /* 4x block */
605 __ASM_EMIT32("addl $4, %[count]")
606 __ASM_EMIT64("add $4, %[count]")
607 __ASM_EMIT("jl 4f")
608 __ASM_EMIT("vmovups 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
609 __ASM_EMIT("vmovups 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
610 __ASM_EMIT("vmovups 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
611 __ASM_EMIT("vmovups 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
612 __ASM_EMIT("vmulps %%xmm1, %%xmm0, %%xmm2") /* xmm2 = ah*bh */
613 __ASM_EMIT("vmulps %%xmm0, %%xmm0, %%xmm0") /* xmm0 = ah*ah */
614 __ASM_EMIT("vmulps %%xmm1, %%xmm1, %%xmm1") /* xmm1 = bh*bh */
615 __ASM_EMIT("vxorps %%xmm6, %%xmm6, %%xmm6") /* xmm6 = 0 0 0 0 0 0 0 0 */
616 __ASM_EMIT("vfnmadd231ps %%xmm4, %%xmm3, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
617 __ASM_EMIT("vfnmadd231ps %%xmm3, %%xmm3, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
618 __ASM_EMIT("vfnmadd231ps %%xmm4, %%xmm4, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
619
620 __ASM_EMIT("vmovlhps %%xmm0, %%xmm6, %%xmm3") /* xmm3 = 0 0 DA[0] DA[1] 0 0 DA[4] DA[5] */
621 __ASM_EMIT("vmovlhps %%xmm1, %%xmm6, %%xmm4") /* xmm4 = 0 0 DB[0] DB[1] 0 0 DB[4] DB[5] */
622 __ASM_EMIT("vmovlhps %%xmm2, %%xmm6, %%xmm5") /* xmm5 = 0 0 DV[0] DV[1] 0 0 DV[4] DV[5] */
623 __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] */
624 __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] */
625 __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] */
626 __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] */
627 __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] */
628 __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] */
629 __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] */
630 __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] */
631 __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] */
632
633 __ASM_EMIT32("mov %[corr], %[ptr]")
634 __ASM_EMIT32("vbroadcastss 0x00(%[ptr]), %%xmm0")
635 __ASM_EMIT32("vbroadcastss 0x04(%[ptr]), %%xmm1")
636 __ASM_EMIT32("vbroadcastss 0x08(%[ptr]), %%xmm2")
637 __ASM_EMIT64("vbroadcastss 0x00(%[corr]), %%xmm0")
638 __ASM_EMIT64("vbroadcastss 0x04(%[corr]), %%xmm1")
639 __ASM_EMIT64("vbroadcastss 0x08(%[corr]), %%xmm2")
640
641 __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] */
642 __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] */
643 __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] */
644 __ASM_EMIT("vmulps %%xmm2, %%xmm1, %%xmm7") /* xmm7 = B = BA*BB */
645 __ASM_EMIT("vshufps $0xff, %%xmm0, %%xmm0, %%xmm3") /* xmm3 = T[7] T[7] T[7] T[7] */
646 __ASM_EMIT("vshufps $0xff, %%xmm1, %%xmm1, %%xmm4") /* xmm4 = BA[7] BA[7] BA[7] BA[7] */
647 __ASM_EMIT("vshufps $0xff, %%xmm2, %%xmm2, %%xmm5") /* xmm5 = BB[7] BB[7] BB[7] BB[7] */
648
649 __ASM_EMIT32("vmovss %%xmm3, 0x00(%[ptr])")
650 __ASM_EMIT32("vmovss %%xmm4, 0x04(%[ptr])")
651 __ASM_EMIT32("vmovss %%xmm5, 0x08(%[ptr])")
652 __ASM_EMIT64("vmovss %%xmm3, 0x00(%[corr])")
653 __ASM_EMIT64("vmovss %%xmm4, 0x04(%[corr])")
654 __ASM_EMIT64("vmovss %%xmm5, 0x08(%[corr])")
655
656 __ASM_EMIT("vsqrtps %%xmm7, %%xmm6") /* xmm6 = sqrtf(B) */
657 __ASM_EMIT("vcmpps $5, %[CORR_CC], %%xmm7, %%xmm1")/* xmm1 = B >= 1e-10f */
658 __ASM_EMIT("vdivps %%xmm6, %%xmm0, %%xmm0") /* xmm0 = T/sqrtf(B) */
659 __ASM_EMIT32("mov %[dst], %[ptr]")
660 __ASM_EMIT("vandps %%xmm1, %%xmm0, %%xmm0") /* xmm0 = (B >= 1e-10f) ? T/sqrtf(B) : 0 */
661 __ASM_EMIT("add $0x10, %[a_head]") /* ++a_head */
662 __ASM_EMIT("add $0x10, %[b_head]") /* ++b_head */
663 __ASM_EMIT32("vmovups %%xmm0, 0x00(%[ptr])")
664 __ASM_EMIT64("vmovups %%xmm0, 0x00(%[dst])")
665 __ASM_EMIT("add $0x10, %[a_tail]") /* ++a_tail */
666 __ASM_EMIT("add $0x10, %[b_tail]") /* ++b_tail */
667 __ASM_EMIT32("add $0x10, %[ptr]")
668 __ASM_EMIT64("add $0x10, %[dst]")
669 __ASM_EMIT32("mov %[ptr], %[dst]")
670 __ASM_EMIT32("subl $4, %[count]")
671 __ASM_EMIT64("sub $4, %[count]")
672 __ASM_EMIT("4:")
673
674 /* 1x blocks */
675 __ASM_EMIT32("addl $3, %[count]")
676 __ASM_EMIT64("add $3, %[count]")
677 __ASM_EMIT("jl 6f")
678 __ASM_EMIT("5:")
679 __ASM_EMIT("vmovss 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
680 __ASM_EMIT("vmovss 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
681 __ASM_EMIT("vmovss 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
682 __ASM_EMIT("vmovss 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
683 __ASM_EMIT("vmulss %%xmm1, %%xmm0, %%xmm2") /* xmm2 = ah*bh */
684 __ASM_EMIT("vmulss %%xmm0, %%xmm0, %%xmm0") /* xmm0 = ah*ah */
685 __ASM_EMIT("vmulss %%xmm1, %%xmm1, %%xmm1") /* xmm1 = bh*bh */
686 __ASM_EMIT("vfnmadd231ss %%xmm4, %%xmm3, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
687 __ASM_EMIT("vfnmadd231ss %%xmm3, %%xmm3, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
688 __ASM_EMIT("vfnmadd231ss %%xmm4, %%xmm4, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
689
690 __ASM_EMIT32("mov %[corr], %[ptr]")
691 __ASM_EMIT32("vmovss 0x00(%[ptr]), %%xmm3")
692 __ASM_EMIT32("vmovss 0x04(%[ptr]), %%xmm4")
693 __ASM_EMIT32("vmovss 0x08(%[ptr]), %%xmm5")
694 __ASM_EMIT64("vmovss 0x00(%[corr]), %%xmm3")
695 __ASM_EMIT64("vmovss 0x04(%[corr]), %%xmm4")
696 __ASM_EMIT64("vmovss 0x08(%[corr]), %%xmm5")
697
698 __ASM_EMIT("vaddss %%xmm3, %%xmm2, %%xmm2") /* xmm2 = T = xv+DV */
699 __ASM_EMIT("vaddss %%xmm4, %%xmm0, %%xmm0") /* xmm0 = BA = xa+DA */
700 __ASM_EMIT("vaddss %%xmm5, %%xmm1, %%xmm1") /* xmm1 = BB = xb+DB */
701 __ASM_EMIT("vmulss %%xmm1, %%xmm0, %%xmm7") /* xmm7 = B = BA*BB */
702
703 __ASM_EMIT32("vmovss %%xmm2, 0x00(%[ptr])")
704 __ASM_EMIT32("vmovss %%xmm0, 0x04(%[ptr])")
705 __ASM_EMIT32("vmovss %%xmm1, 0x08(%[ptr])")
706 __ASM_EMIT64("vmovss %%xmm2, 0x00(%[corr])")
707 __ASM_EMIT64("vmovss %%xmm0, 0x04(%[corr])")
708 __ASM_EMIT64("vmovss %%xmm1, 0x08(%[corr])")
709
710 __ASM_EMIT("vsqrtss %%xmm7, %%xmm7, %%xmm6") /* xmm6 = sqrtf(B) */
711 __ASM_EMIT("vcmpss $5, %[CORR_CC], %%xmm7, %%xmm1")/* xmm1 = B >= 1e-10f */
712 __ASM_EMIT("vdivss %%xmm6, %%xmm2, %%xmm0") /* xmm0 = T/sqrtf(B) */
713 __ASM_EMIT32("mov %[dst], %[ptr]")
714 __ASM_EMIT("vandps %%xmm1, %%xmm0, %%xmm0") /* xmm0 = (B >= 1e-10f) ? T/sqrtf(B) : 0 */
715 __ASM_EMIT("add $0x04, %[a_head]") /* ++a_head */
716 __ASM_EMIT("add $0x04, %[b_head]") /* ++b_head */
717 __ASM_EMIT32("vmovss %%xmm0, 0x00(%[ptr])")
718 __ASM_EMIT64("vmovss %%xmm0, 0x00(%[dst])")
719 __ASM_EMIT("add $0x04, %[a_tail]") /* ++a_tail */
720 __ASM_EMIT("add $0x04, %[b_tail]") /* ++b_tail */
721 __ASM_EMIT32("add $0x04, %[ptr]")
722 __ASM_EMIT64("add $0x04, %[dst]")
723 __ASM_EMIT32("mov %[ptr], %[dst]")
724 __ASM_EMIT32("decl %[count]")
725 __ASM_EMIT64("dec %[count]")
726 __ASM_EMIT("jge 5b")
727 __ASM_EMIT("6:")
728
729 : __IF_32(
730 [ptr] "=&r" (ptr),
731 [corr] "+m" (corr), [dst] "+m" (dst),
732 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
733 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
734 [count] "+m" (count)
735 )
736 __IF_64(
737 [dst] "+r" (dst),
738 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
739 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
740 [count] "+r" (count)
741 )
742 : __IF_64( [corr] "r" (corr), )
743 [CORR_CC] "o" (corr_const)
744 : "cc", "memory",
745 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
746 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
747 );
748 }
749
750 } /* namespace avx */
751 } /* namespace lsp */
752
753
754
755
756 #endif /* PRIVATE_DSP_ARCH_X86_AVX_CORRELATION_H_ */