Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/x86/sse3/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: 10 мар. 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_SSE3_CORRELATION_H_
23 #define PRIVATE_DSP_ARCH_X86_SSE3_CORRELATION_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_SSE3_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_SSE3_IMPL */
28
29 namespace lsp
30 {
31 namespace sse3
32 {
33 static const float corr_const[] __lsp_aligned16 =
34 {
35 LSP_DSP_VEC4(1e-10f)
36 };
37
38 void x64_corr_incr(dsp::correlation_t *corr, float *dst,
39 const float *a_head, const float *b_head,
40 const float *a_tail, const float *b_tail,
41 size_t count)
42 {
43 ARCH_X86_64_ASM
44 (
45 /* load data */
46 __ASM_EMIT("movss 0x00(%[corr]), %%xmm8") /* xmm8 = xv */
47 __ASM_EMIT("movss 0x04(%[corr]), %%xmm9") /* xmm9 = xa */
48 __ASM_EMIT("movss 0x08(%[corr]), %%xmm10") /* xmm10 = xb */
49 __ASM_EMIT("movaps %[CORR_CC], %%xmm11") /* xmm11 = 1e-10f */
50 /* 4x blocks */
51 __ASM_EMIT("sub $4, %[count]")
52 __ASM_EMIT("jb 2f")
53 __ASM_EMIT("shufps $0x00, %%xmm8, %%xmm8")
54 __ASM_EMIT("shufps $0x00, %%xmm9, %%xmm9")
55 __ASM_EMIT("shufps $0x00, %%xmm10, %%xmm10")
56 __ASM_EMIT(".align 0x10")
57 __ASM_EMIT("1:")
58 __ASM_EMIT("movups 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
59 __ASM_EMIT("movups 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
60 __ASM_EMIT("movaps %%xmm0, %%xmm2") /* xmm2 = ah */
61 __ASM_EMIT("movups 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
62 __ASM_EMIT("movups 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
63 __ASM_EMIT("movaps %%xmm3, %%xmm5") /* xmm5 = at */
64 __ASM_EMIT("mulps %%xmm1, %%xmm2") /* xmm2 = ah*bh */
65 __ASM_EMIT("mulps %%xmm0, %%xmm0") /* xmm0 = ah*ah */
66 __ASM_EMIT("mulps %%xmm1, %%xmm1") /* xmm1 = bh*bh */
67 __ASM_EMIT("mulps %%xmm4, %%xmm5") /* xmm5 = at*bt */
68 __ASM_EMIT("mulps %%xmm3, %%xmm3") /* xmm3 = at*at */
69 __ASM_EMIT("mulps %%xmm4, %%xmm4") /* xmm4 = bt*bt */
70 __ASM_EMIT("subps %%xmm3, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
71 __ASM_EMIT("subps %%xmm4, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
72 __ASM_EMIT("subps %%xmm5, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
73
74 __ASM_EMIT("xorps %%xmm3, %%xmm3") /* xmm3 = 0 0 0 0 */
75 __ASM_EMIT("xorps %%xmm4, %%xmm4") /* xmm4 = 0 0 0 0 */
76 __ASM_EMIT("xorps %%xmm5, %%xmm5") /* xmm5 = 0 0 0 0 */
77 __ASM_EMIT("movlhps %%xmm0, %%xmm3") /* xmm3 = 0 0 DA[0] DA[1] */
78 __ASM_EMIT("movlhps %%xmm1, %%xmm4") /* xmm4 = 0 0 DB[0] DB[1] */
79 __ASM_EMIT("movlhps %%xmm2, %%xmm5") /* xmm5 = 0 0 DV[0] DV[1] */
80 __ASM_EMIT("addps %%xmm3, %%xmm0") /* xmm0 = DA[0] DA[1] DA[0]+DA[2] DA[1]+DA[3] */
81 __ASM_EMIT("addps %%xmm4, %%xmm1") /* xmm1 = DB[0] DB[1] DB[0]+DB[2] DB[1]+DB[3] */
82 __ASM_EMIT("addps %%xmm5, %%xmm2") /* xmm2 = DV[0] DV[1] DV[0]+DV[2] DV[1]+DV[3] */
83 __ASM_EMIT("shufps $0x99, %%xmm0, %%xmm3") /* xmm3 = 0 DA[0] DA[1] DA[0]+DA[2] */
84 __ASM_EMIT("shufps $0x99, %%xmm1, %%xmm4") /* xmm4 = 0 DB[0] DB[1] DB[0]+DB[2] */
85 __ASM_EMIT("shufps $0x99, %%xmm2, %%xmm5") /* xmm5 = 0 DV[0] DV[1] DV[0]+DV[2] */
86 __ASM_EMIT("addps %%xmm0, %%xmm3") /* xmm3 = DA[0] DA[0]+DA[1] DA[0]+DA[1]+DA[2] DA[0]+DA[1]+DA[2]+DA[3] */
87 __ASM_EMIT("addps %%xmm1, %%xmm4") /* xmm4 = DB[0] DB[0]+DB[1] DB[0]+DB[1]+DB[2] DB[0]+DB[1]+DB[2]+DB[3] */
88 __ASM_EMIT("addps %%xmm2, %%xmm5") /* xmm5 = DV[0] DV[0]+DV[1] DV[0]+DV[1]+DV[2] DV[0]+DV[1]+DV[2]+DV[3] */
89
90 __ASM_EMIT("addps %%xmm3, %%xmm9") /* xmm9 = 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] */
91 __ASM_EMIT("addps %%xmm4, %%xmm10") /* xmm10 = BB = xb+DV[0] xb+DV[0]+DV[1] xb+DV[0]+DV[1]+DV[2] xb+DV[0]+DV[1]+DV[2]+DV[3] */
92 __ASM_EMIT("movaps %%xmm9, %%xmm6") /* xmm6 = BA */
93 __ASM_EMIT("addps %%xmm5, %%xmm8") /* xmm8 = 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] */
94 __ASM_EMIT("mulps %%xmm10, %%xmm6") /* xmm6 = B = BA*BB */
95 __ASM_EMIT("movaps %%xmm8, %%xmm5") /* xmm5 = T */
96 __ASM_EMIT("shufps $0xff, %%xmm8, %%xmm8") /* xmm8 = T[3] T[3] T[3] T[3] */
97 __ASM_EMIT("shufps $0xff, %%xmm9, %%xmm9") /* xmm9 = BA[3] BA[3] BA[3] BA[3] */
98 __ASM_EMIT("shufps $0xff, %%xmm10, %%xmm10") /* xmm10 = BB[3] BB[3] BB[3] BB[3] */
99
100 __ASM_EMIT("sqrtps %%xmm6, %%xmm7") /* xmm7 = sqrtf(B) */
101 __ASM_EMIT("cmpps $5, %%xmm11, %%xmm6") /* xmm6 = B >= 1e-10f */
102 __ASM_EMIT("divps %%xmm7, %%xmm5") /* xmm5 = T/sqrtf(B) */
103 __ASM_EMIT("andps %%xmm6, %%xmm5") /* xmm5 = (B >= 1e-10f) ? T/sqrtf(B) : 0 */
104 __ASM_EMIT("add $0x10, %[a_head]") /* ++a_head */
105 __ASM_EMIT("add $0x10, %[b_head]") /* ++b_head */
106 __ASM_EMIT("movups %%xmm5, 0x00(%[dst])")
107 __ASM_EMIT("add $0x10, %[a_tail]") /* ++a_tail */
108 __ASM_EMIT("add $0x10, %[b_tail]") /* ++b_tail */
109 __ASM_EMIT("add $0x10, %[dst]")
110 __ASM_EMIT("sub $4, %[count]")
111 __ASM_EMIT("jae 1b")
112 __ASM_EMIT("2:")
113 /* 1x blocks */
114 __ASM_EMIT("add $3, %[count]")
115 __ASM_EMIT("jl 4f")
116 __ASM_EMIT("3:")
117 __ASM_EMIT("movss 0x00(%[a_head]), %%xmm0") /* xmm0 = ah */
118 __ASM_EMIT("movss 0x00(%[b_head]), %%xmm1") /* xmm1 = bh */
119 __ASM_EMIT("movss %%xmm0, %%xmm2") /* xmm2 = ah */
120 __ASM_EMIT("movss 0x00(%[a_tail]), %%xmm3") /* xmm3 = at */
121 __ASM_EMIT("movss 0x00(%[b_tail]), %%xmm4") /* xmm4 = bt */
122 __ASM_EMIT("movss %%xmm3, %%xmm5") /* xmm5 = at */
123 __ASM_EMIT("mulss %%xmm1, %%xmm2") /* xmm2 = ah*bh */
124 __ASM_EMIT("mulss %%xmm0, %%xmm0") /* xmm0 = ah*ah */
125 __ASM_EMIT("mulss %%xmm1, %%xmm1") /* xmm1 = bh*bh */
126 __ASM_EMIT("mulss %%xmm4, %%xmm5") /* xmm5 = at*bt */
127 __ASM_EMIT("mulss %%xmm3, %%xmm3") /* xmm3 = at*at */
128 __ASM_EMIT("mulss %%xmm4, %%xmm4") /* xmm4 = bt*bt */
129 __ASM_EMIT("subss %%xmm3, %%xmm0") /* xmm0 = DA = ah*ah - at*at */
130 __ASM_EMIT("subss %%xmm4, %%xmm1") /* xmm1 = DB = bh*bh - bt*bt */
131 __ASM_EMIT("subss %%xmm5, %%xmm2") /* xmm2 = DV = ah*bh - at*bt */
132
133 __ASM_EMIT("addss %%xmm0, %%xmm9") /* xmm9 = BA = xa+DA */
134 __ASM_EMIT("addss %%xmm1, %%xmm10") /* xmm10 = BB = xb+DB */
135 __ASM_EMIT("movaps %%xmm9, %%xmm3") /* xmm3 = BA */
136 __ASM_EMIT("addss %%xmm2, %%xmm8") /* xmm8 = T = xv+DV */
137 __ASM_EMIT("mulss %%xmm10, %%xmm3") /* xmm3 = B = BA*BB */
138 __ASM_EMIT("movaps %%xmm8, %%xmm0") /* xmm0 = T */
139
140 __ASM_EMIT("sqrtss %%xmm3, %%xmm7") /* xmm7 = sqrtf(B) */
141 __ASM_EMIT("cmpss $5, %[CORR_CC], %%xmm3") /* xmm3 = B >= 1e-10f */
142 __ASM_EMIT("divss %%xmm7, %%xmm0") /* xmm2 = T/sqrtf(B) */
143 __ASM_EMIT32("mov %[dst], %[ptr]")
144 __ASM_EMIT("andps %%xmm3, %%xmm0") /* xmm0 = (B >= 1e-10f) ? T/sqrtf(B) : 0 */
145 __ASM_EMIT("add $0x04, %[a_head]") /* ++a_head */
146 __ASM_EMIT("add $0x04, %[b_head]") /* ++b_head */
147 __ASM_EMIT("movss %%xmm0, 0x00(%[dst])")
148 __ASM_EMIT("add $0x04, %[a_tail]") /* ++a_tail */
149 __ASM_EMIT("add $0x04, %[b_tail]") /* ++b_tail */
150 __ASM_EMIT("add $0x04, %[dst]")
151 __ASM_EMIT("dec %[count]")
152 __ASM_EMIT("jge 3b")
153 __ASM_EMIT("4:")
154
155 /* Store result */
156 __ASM_EMIT("movss %%xmm8, 0x00(%[corr])")
157 __ASM_EMIT("movss %%xmm9, 0x04(%[corr])")
158 __ASM_EMIT("movss %%xmm10, 0x08(%[corr])")
159
160 : [dst] "+r" (dst),
161 [a_head] "+r" (a_head), [b_head] "+r" (b_head),
162 [a_tail] "+r" (a_tail), [b_tail] "+r" (b_tail),
163 [count] "+r" (count)
164 : [corr] "r" (corr),
165 [CORR_CC] "o" (corr_const)
166 : "cc", "memory",
167 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
168 "%xmm4", "%xmm5", "%xmm6", "%xmm7",
169 "%xmm8", "%xmm9", "%xmm9", "%xmm10"
170 );
171 }
172
173 } /* namespace sse3 */
174 } /* namespace lsp */
175
176
177
178
179 #endif /* PRIVATE_DSP_ARCH_X86_SSE3_CORRELATION_H_ */