Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/x86/sse4/3dmath.h

raw
1 /*
2 * Copyright (C) 2020 Linux Studio Plugins Project <https://lsp-plug.in/>
3 * (C) 2020 Vladimir Sadovnikov <[email protected]>
4 *
5 * This file is part of lsp-dsp-lib
6 * Created on: 31 мар. 2020 г.
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_SSE4_3DMATH_H_
23 #define PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_SSE4_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_SSE4_IMPL */
28
29 namespace lsp
30 {
31 namespace sse4
32 {
33 using namespace dsp;
34
35 IF_ARCH_X86(
36 static const uint32_t X_MASK0111[] __lsp_aligned16 = { 0xffffffff, 0xffffffff, 0xffffffff, 0 };
37 static const float X_3DPOINT[] __lsp_aligned16 = { 0.0f, 0.0f, 0.0f, 1.0f };
38 static const float X_3D_TOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(DSP_3D_TOLERANCE) };
39 static const float X_3D_MTOLERANCE[] __lsp_aligned16 = { LSP_DSP_VEC4(-DSP_3D_TOLERANCE) };
40 static const uint32_t IONE[] __lsp_aligned16 = { LSP_DSP_VEC4(1) };
41 )
42
43 /* Input:
44 * x0 = a0 a1 a2 a3
45 * x1 = b0 b1 b2 b3
46 *
47 * Output:
48 * x0 = b0 b1 b2 b3
49 * x1 = a0 a1 a2 a3
50 */
51 #define VECTOR_XCHG(x0, x1) \
52 __ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = b ^ a */ \
53 __ASM_EMIT("xorps %" x1 ", %" x0) /* xmm0 = a ^ ( b ^ a) = b */ \
54 __ASM_EMIT("xorps %" x0 ", %" x1) /* xmm1 = (b ^ a) ^ b = a */
55
56 /* 3x vector multiplication
57 * Input:
58 * x0 = vector1 [dx dy dz ? ]
59 * x1 = vector2 [dx dy dz ? ]
60 * x2 = vector3 [dx dy dz ? ]
61 *
62 * Output:
63 * x0 = vector1 * vector2 [ vz vx vy ? ]
64 * x1 = vector2 * vector3 [ vz vx vy ? ]
65 * x2 = vector3 * vector1 [ vz vx vy ? ]
66 */
67 #define VECTOR_MUL3(x0, x1, x2, x3, x4, x5, x6, x7) \
68 __ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = dx0 dy0 dz0 dw0 */ \
69 __ASM_EMIT("movaps %" x1 ", %" x4) /* xmm4 = dx1 dy1 dz1 dw1 */ \
70 __ASM_EMIT("movaps %" x2 ", %" x5) /* xmm5 = dx2 dy2 dz2 dw2 */ \
71 __ASM_EMIT("shufps $0xc9, %" x3 ", %" x3) /* xmm3 = dy0 dz0 dx0 dw0 */ \
72 __ASM_EMIT("shufps $0xc9, %" x4 ", %" x4) /* xmm4 = dy1 dz1 dx1 dw1 */ \
73 __ASM_EMIT("shufps $0xc9, %" x5 ", %" x5) /* xmm5 = dy2 dz2 dx2 dw2 */ \
74 __ASM_EMIT("movaps %" x0 ", %" x6) /* xmm6 = dx0 dy0 dz0 dw0 */ \
75 __ASM_EMIT("movaps %" x3 ", %" x7) /* xmm7 = dy0 dz0 dx0 dw0 */ \
76 \
77 __ASM_EMIT("mulps %" x4 ", %" x0) /* xmm0 = dx0*dy1 dy0*dz1 dz0*dx1 dw0*dw1 */ \
78 __ASM_EMIT("mulps %" x1 ", %" x3) /* xmm3 = dy0*dx1 dz0*dy1 dx0*dz1 dw0*dw1 */ \
79 __ASM_EMIT("mulps %" x5 ", %" x1) /* xmm1 = dx1*dy2 dy1*dz2 dz1*dx2 dw1*dw2 */ \
80 __ASM_EMIT("mulps %" x2 ", %" x4) /* xmm4 = dy1*dx2 dz1*dy2 dx1*dz2 dw1*dw2 */ \
81 __ASM_EMIT("mulps %" x7 ", %" x2) /* xmm2 = dx2*dy0 dy2*dz0 dz2*dx0 dw2*dw0 */ \
82 __ASM_EMIT("mulps %" x6 ", %" x5) /* xmm5 = dy2*dx0 dz2*dy0 dx2*dz0 dw2*dw0 */ \
83 __ASM_EMIT("subps %" x3 ", %" x0) /* xmm0 = nz0 nx0 ny0 nw0 */ \
84 __ASM_EMIT("subps %" x4 ", %" x1) /* xmm1 = nz1 nx1 ny1 nw1 */ \
85 __ASM_EMIT("subps %" x5 ", %" x2) /* xmm2 = nz2 nx2 ny2 nw2 */
86
87 /* 3x scalar multiplication
88 * Input:
89 * x0 = vector1 [dx dy dz ? ]
90 * x1 = vector2 [dx dy dz ? ]
91 * x2 = vector3 [dx dy dz ? ]
92 *
93 * Output:
94 * x0 = vector1 * vector2 [ S0 ? ? ? ]
95 * x1 = vector2 * vector3 [ S1 ? ? ? ]
96 * x2 = vector3 * vector1 [ S2 ? ? ? ]
97 */
98 #define SCALAR_MUL3(x0, x1, x2, x3) \
99 __ASM_EMIT("movaps %" x0 ", %" x3) \
100 __ASM_EMIT("dpps $0x71, %" x1 ", %" x0) /* xmm0 = dx0*dx1+dy0*dy1+dz0*dz1 */ \
101 __ASM_EMIT("dpps $0x71, %" x2 ", %" x1) /* xmm1 = dx0*dx1+dy0*dy1+dz0*dz1 */ \
102 __ASM_EMIT("dpps $0x71, %" x3 ", %" x2) /* xmm2 = dx0*dx1+dy0*dy1+dz0*dz1 */
103
104 /*
105 * Input:
106 * x0 = vector 1 = dx1 dy1 dz1 dw1
107 * x1 = vector 2 = dx2 dy2 dz2 dw2
108 *
109 * Output:
110 * x0 = dx1*dx2 + dy1*dy2 + dz1*dz2
111 */
112 #define SCALAR_MUL(x0, x1, x2) \
113 __ASM_EMIT("dpps $0x71, %" x1 ", %" x0) /* xmm0 = dx1*dx2 + dy1*dy2 + dz1*dz2 */
114
115 /*
116 * Input:
117 * x0 = vector/point to normalize
118 * Output:
119 * x0 = normalized vector
120 */
121 #define NORMALIZE(x0, x1, x2) \
122 __ASM_EMIT("movaps %" x0 ", %" x1) /* xmm1 = dx dy dz dw */ \
123 __ASM_EMIT("xorps %" x2 ", %" x2) /* xmm2 = 0 */ \
124 __ASM_EMIT("dpps $0x7f, %" x1 ", %" x1) /* xmm1 = dx*dx+dy*dy+dz*dz */ \
125 __ASM_EMIT("sqrtps %" x1", %" x1) /* xmm1 = sqrt(x*x + y*y + z*z) = w */ \
126 __ASM_EMIT("ucomiss %" x2 ", %" x1) /* xmm2 =?= xmm1 */ \
127 __ASM_EMIT("jle 1000000f") \
128 __ASM_EMIT("divps %" x1 ", %" x0) /* xmm0 = dx/w dy/w dz/w dw/w */ \
129 __ASM_EMIT("1000000:")
130
131 /*
132 * Input:
133 * x0 = vector/point to scale
134 * x1 = radius-vector to match length
135 *
136 * Output:
137 * x0 = scaled vector
138 */
139 #define SCALE(x0, x1, x2, x3) \
140 __ASM_EMIT("movaps %" x0 ", %" x3) /* xmm3 = x y z w */ \
141 __ASM_EMIT("xorps %" x2 ", %" x2) /* xmm2 = 0 */ \
142 __ASM_EMIT("dpps $0x7f, %" x3 ", %" x3) /* xmm3 = dx*dx+dy*dy+dz*dz */ \
143 __ASM_EMIT("shufps $0x00, %" x1 ", %" x1) /* xmm1 = r r r r */ \
144 __ASM_EMIT("sqrtps %" x3", %" x3) /* xmm3 = sqrt(x*x + y*y + z*z) = W W W W */ \
145 __ASM_EMIT("ucomiss %" x2 ", %" x3) /* xmm3 =?= xmm2 */ \
146 __ASM_EMIT("jle 1000000f") \
147 __ASM_EMIT("divps %" x3 ", %" x0) /* xmm0 = r/W r/W r/W r/W */ \
148 __ASM_EMIT("mulps %" x1 ", %" x0) /* xmm0 = x*r/W y*r/W z*r/W w*r/W */ \
149 __ASM_EMIT("1000000:")
150
151 void normalize_point(point3d_t *p)
152 {
153 float x0, x1, x2;
154
155 ARCH_X86_ASM
156 (
157 __ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w
158 NORMALIZE("[x0]", "[x1]", "[x2]")
159 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
160 __ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
161 __ASM_EMIT("movups %[x0], (%[p])")
162
163 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
164 : [p] "r" (p),
165 [zmask] "m" (X_MASK0111),
166 [omask] "m" (X_3DPOINT)
167 : "cc", "memory"
168 );
169 }
170
171 void scale_point1(point3d_t *p, float r)
172 {
173 float x0, x1, x2;
174
175 ARCH_X86_ASM
176 (
177 __ASM_EMIT("movups (%[p]), %[x0]") // xmm0 = x y z w
178 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
179 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
180 __ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
181 __ASM_EMIT("movups %[x0], (%[p])")
182
183 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
184 : [p] "r" (p),
185 [zmask] "m" (X_MASK0111),
186 [omask] "m" (X_3DPOINT)
187 : "cc", "memory"
188 );
189 }
190
191 void scale_point2(point3d_t *p, const point3d_t *s, float r)
192 {
193 float x0, x1, x2;
194
195 ARCH_X86_ASM
196 (
197 __ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = x y z w
198 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
199 __ASM_EMIT("andps %[zmask], %[x0]") // xmm0 = x y z 0
200 __ASM_EMIT("orps %[omask], %[x0]") // xmm0 = x y z 1
201 __ASM_EMIT("movups %[x0], (%[p])")
202
203 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
204 : [p] "r" (p), [s] "r" (s),
205 [zmask] "m" (X_MASK0111),
206 [omask] "m" (X_3DPOINT)
207 : "cc", "memory"
208 );
209 }
210
211 void normalize_vector(vector3d_t *v)
212 {
213 float x0, x1, x2;
214
215 ARCH_X86_ASM
216 (
217 __ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw
218 NORMALIZE("[x0]", "[x1]", "[x2]")
219 __ASM_EMIT("movups %[x0], (%[v])")
220
221 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2)
222 : [v] "r" (v)
223 : "cc", "memory"
224 );
225 }
226
227 void scale_vector1(vector3d_t *v, float r)
228 {
229 float x0, x1, x2;
230
231 ARCH_X86_ASM
232 (
233 __ASM_EMIT("movups (%[v]), %[x0]") // xmm0 = dx dy dz dw
234 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
235 __ASM_EMIT("andps %[zmask], %[x0]")
236 __ASM_EMIT("movups %[x0], (%[v])")
237
238 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
239 : [v] "r" (v),
240 [zmask] "m" (X_MASK0111)
241 : "cc", "memory"
242 );
243 }
244
245 void scale_vector2(vector3d_t *v, const vector3d_t *s, float r)
246 {
247 float x0, x1, x2;
248
249 ARCH_X86_ASM
250 (
251 __ASM_EMIT("movups (%[s]), %[x0]") // xmm0 = dx dy dz dw
252 SCALE("[x0]", "[x3]", "[x1]", "[x2]")
253 __ASM_EMIT("andps %[zmask], %[x0]")
254 __ASM_EMIT("movups %[x0], (%[v])")
255
256 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "+x" (r)
257 : [v] "r" (v), [s] "r" (s),
258 [zmask] "m" (X_MASK0111)
259 : "cc", "memory"
260 );
261 }
262
263 float check_point3d_on_triangle_pvp(const point3d_t *pv, const point3d_t *p)
264 {
265 float x0, x1, x2, x3, x4, x5, x6, x7;
266
267 ARCH_X86_ASM
268 (
269 /* Load vectors */
270 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */
271 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = x0 y0 z0 w0 */
272 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = x1 y1 z1 w1 */
273 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = x2 y2 z2 w2 */
274 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */
275 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */
276 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */
277 /* 3x vector multiplications */
278 VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
279 /* 3x scalar multiplications */
280 SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
281 /* Compare with zeros */
282 __ASM_EMIT("xorps %[x4], %[x4]")
283 __ASM_EMIT("ucomiss %[x4], %[x0]")
284 __ASM_EMIT("jb 110f")
285 __ASM_EMIT("ucomiss %[x4], %[x1]")
286 __ASM_EMIT("jb 109f")
287 __ASM_EMIT("ucomiss %[x4], %[x2]")
288 __ASM_EMIT("jb 108f")
289 __ASM_EMIT("mulss %[x1], %[x0]")
290 __ASM_EMIT("mulss %[x2], %[x0]")
291 __ASM_EMIT("ucomiss %[x4], %[x0]")
292 __ASM_EMIT("jne 110f")
293 /* There is somewhere zero, need additional check */
294 /* Load vectors */
295 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */
296 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = x0 y0 z0 w0 */
297 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = x1 y1 z1 w1 */
298 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = x2 y2 z2 w2 */
299 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */
300 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */
301 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */
302 /* Do 3x scalar multiplications */
303 SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
304 __ASM_EMIT("mulss %[x1], %[x0]")
305 __ASM_EMIT("mulss %[x2], %[x0]")
306 __ASM_EMIT("jmp 110f")
307
308 __ASM_EMIT("108:")
309 __ASM_EMIT("movss %[x2], %[x0]")
310 __ASM_EMIT("jmp 110f")
311 __ASM_EMIT("109:")
312 __ASM_EMIT("movss %[x1], %[x0]")
313 __ASM_EMIT("110:")
314 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
315 [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7)
316 : [pv] "r" (pv), [p] "r" (p)
317 );
318
319 return x0;
320 }
321
322 float check_point3d_on_triangle_p3p(const point3d_t *p1, const point3d_t *p2, const point3d_t *p3, const point3d_t *p)
323 {
324 float x0, x1, x2, x3, x4, x5, x6, x7;
325
326 ARCH_X86_ASM
327 (
328 /* Load vectors */
329 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */
330 __ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x0 y0 z0 w0 */
331 __ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x1 y1 z1 w1 */
332 __ASM_EMIT("movups (%[p3]), %[x2]") /* xmm2 = x2 y2 z2 w2 */
333 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */
334 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */
335 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */
336 /* 3x vector multiplications */
337 VECTOR_MUL3("[x0]", "[x1]", "[x2]", "[x3]", "[x4]", "[x5]", "[x6]", "[x7]")
338 /* 3x scalar multiplications */
339 SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
340 /* Compare with zeros */
341 __ASM_EMIT("xorps %[x4], %[x4]")
342 __ASM_EMIT("ucomiss %[x4], %[x0]")
343 __ASM_EMIT("jb 110f")
344 __ASM_EMIT("ucomiss %[x4], %[x1]")
345 __ASM_EMIT("jb 109f")
346 __ASM_EMIT("ucomiss %[x4], %[x2]")
347 __ASM_EMIT("jb 108f")
348 __ASM_EMIT("mulss %[x1], %[x0]")
349 __ASM_EMIT("mulss %[x2], %[x0]")
350 __ASM_EMIT("ucomiss %[x4], %[x0]")
351 __ASM_EMIT("jne 110f")
352 /* There is somewhere zero, need additional check */
353 /* Load vectors */
354 __ASM_EMIT("movups (%[p]), %[x3]") /* xmm3 = px py pz pw */
355 __ASM_EMIT("movups (%[p1]), %[x0]") /* xmm0 = x0 y0 z0 w0 */
356 __ASM_EMIT("movups (%[p2]), %[x1]") /* xmm1 = x1 y1 z1 w1 */
357 __ASM_EMIT("movups (%[p3]), %[x2]") /* xmm2 = x2 y2 z2 w2 */
358 __ASM_EMIT("subps %[x3], %[x0]") /* xmm0 = dx0 dy0 dz0 dw0 */
359 __ASM_EMIT("subps %[x3], %[x1]") /* xmm1 = dx1 dy1 dz1 dw1 */
360 __ASM_EMIT("subps %[x3], %[x2]") /* xmm2 = dx2 dy2 dz2 dw2 */
361 /* Do 3x scalar multiplications */
362 SCALAR_MUL3("[x0]", "[x1]", "[x2]", "[x3]")
363 __ASM_EMIT("mulss %[x1], %[x0]")
364 __ASM_EMIT("mulss %[x2], %[x0]")
365 __ASM_EMIT("jmp 110f")
366
367 __ASM_EMIT("108:")
368 __ASM_EMIT("movss %[x2], %[x0]")
369 __ASM_EMIT("jmp 110f")
370 __ASM_EMIT("109:")
371 __ASM_EMIT("movss %[x1], %[x0]")
372 __ASM_EMIT("110:")
373 : [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
374 [x4] "=&x" (x4), [x5] "=&x" (x5), [x6] "=&x" (x6), [x7] "=&x" (x7)
375 : [p1] "r" (p1), [p2] "r" (p2), [p3] "r" (p3), [p] "r" (p)
376 );
377
378 return x0;
379 }
380
381 size_t colocation_v1p3(const vector3d_t *pl, const point3d_t *p0, const point3d_t *p1, const point3d_t *p2)
382 {
383 float x0, x1, x2, x3, x4;
384 float res[4] __lsp_aligned16;
385
386 ARCH_X86_ASM
387 (
388 __ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */
389 __ASM_EMIT("movups (%[p0]), %[x0]") /* xmm0 = p0 */
390 __ASM_EMIT("movups (%[p1]), %[x1]") /* xmm1 = p1 */
391 __ASM_EMIT("movups (%[p2]), %[x2]") /* xmm2 = p2 */
392
393 __ASM_EMIT("dpps $0xf1, %[x3], %[x0]") /* xmm0 = p0 * pl */
394 __ASM_EMIT("dpps $0xf2, %[x3], %[x1]") /* xmm1 = p1 * pl */
395 __ASM_EMIT("dpps $0xf4, %[x3], %[x2]") /* xmm2 = p2 * pl */
396 __ASM_EMIT("orps %[x1], %[x0]") /* xmm0 = k0 k1 0 0 */
397 __ASM_EMIT("orps %[x2], %[x0]") /* xmm0 = k0 k1 k2 0 */
398 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 0 */
399
400 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
401 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
402 __ASM_EMIT("andps %[IONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
403 __ASM_EMIT("andps %[IONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
404 __ASM_EMIT("paddd %[x1], %[x0]")
405 __ASM_EMIT("movdqa %[x0], (%[res])")
406 __ASM_EMIT32("movl 0x00(%[res]), %[p0]")
407 __ASM_EMIT32("movl 0x04(%[res]), %[p1]")
408 __ASM_EMIT32("movl 0x08(%[res]), %[p2]")
409 __ASM_EMIT64("movl 0x00(%[res]), %k[p0]")
410 __ASM_EMIT64("movl 0x04(%[res]), %k[p1]")
411 __ASM_EMIT64("movl 0x08(%[res]), %k[p2]")
412 __ASM_EMIT("lea (%[p1], %[p2], 4), %[p1]")
413 __ASM_EMIT("lea (%[p0], %[p1], 4), %[p0]")
414 : [p0] "+r" (p0), [p1] "+r" (p1), [p2] "+r" (p2),
415 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
416 [x4] "=&x" (x4)
417 : [pl] "r" (pl),
418 [res] "r" (res),
419 [PTOL] "m" (X_3D_TOLERANCE),
420 [MTOL] "m" (X_3D_MTOLERANCE),
421 [IONE] "m" (IONE)
422 : "cc"
423 );
424
425 return size_t(p0);
426 }
427
428 size_t colocation_v1pv(const vector3d_t *pl, const point3d_t *pv)
429 {
430 float x0, x1, x2, x3, x4;
431 float res[4] __lsp_aligned16;
432 size_t pt;
433
434 ARCH_X86_ASM
435 (
436 __ASM_EMIT("movups (%[pl]), %[x3]") /* xmm3 = pl */
437 __ASM_EMIT("movups 0x00(%[pv]), %[x0]") /* xmm0 = p0 */
438 __ASM_EMIT("movups 0x10(%[pv]), %[x1]") /* xmm1 = p1 */
439 __ASM_EMIT("movups 0x20(%[pv]), %[x2]") /* xmm2 = p2 */
440
441 __ASM_EMIT("dpps $0xf1, %[x3], %[x0]") /* xmm0 = p0 * pl */
442 __ASM_EMIT("dpps $0xf2, %[x3], %[x1]") /* xmm1 = p1 * pl */
443 __ASM_EMIT("dpps $0xf4, %[x3], %[x2]") /* xmm2 = p2 * pl */
444 __ASM_EMIT("orps %[x1], %[x0]") /* xmm0 = k0 k1 0 0 */
445 __ASM_EMIT("orps %[x2], %[x0]") /* xmm0 = k0 k1 k2 0 */
446 __ASM_EMIT("movaps %[x0], %[x1]") /* xmm1 = k0 k1 k2 0 */
447
448 __ASM_EMIT("cmpps $2, %[PTOL], %[x0]") /* xmm0 = k0 <= +TOL k1 <= +TOL k2 <= +TOL ? */
449 __ASM_EMIT("cmpps $1, %[MTOL], %[x1]") /* xmm1 = k0 < -TOL k1 < -TOL k2 < -TOL ? */
450 __ASM_EMIT("andps %[IONE], %[x0]") /* xmm0 = 1*[k0 <= +TOL] 1*[k1 <= +TOL] 1*[k2 <= +TOL] ? */
451 __ASM_EMIT("andps %[IONE], %[x1]") /* xmm1 = 1*[k0 < -TOL] 1*[k1 < -TOL] 1*[k2 < -TOL] ? */
452 __ASM_EMIT("paddd %[x1], %[x0]")
453 __ASM_EMIT("movdqa %[x0], (%[res])")
454 __ASM_EMIT32("movl 0x00(%[res]), %[pt]")
455 __ASM_EMIT32("movl 0x04(%[res]), %[pl]")
456 __ASM_EMIT32("movl 0x08(%[res]), %[pv]")
457 __ASM_EMIT64("movl 0x00(%[res]), %k[pt]")
458 __ASM_EMIT64("movl 0x04(%[res]), %k[pl]")
459 __ASM_EMIT64("movl 0x08(%[res]), %k[pv]")
460 __ASM_EMIT("lea (%[pl], %[pv], 4), %[pl]")
461 __ASM_EMIT("lea (%[pt], %[pl], 4), %[pt]")
462 : [pl] "+r" (pl), [pv] "+r" (pv), [pt] "=&r" (pt),
463 [x0] "=&x" (x0), [x1] "=&x" (x1), [x2] "=&x" (x2), [x3] "=&x" (x3),
464 [x4] "=&x" (x4)
465 : [res] "r" (res),
466 [PTOL] "m" (X_3D_TOLERANCE),
467 [MTOL] "m" (X_3D_MTOLERANCE),
468 [IONE] "m" (IONE)
469 : "cc"
470 );
471
472 return pt;
473 }
474 }
475 }
476
477 #endif /* PRIVATE_DSP_ARCH_X86_SSE4_3DMATH_H_ */