Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/x86/sse2/pmath/cos.h

raw
1 /*
2 * Copyright (C) 2025 Linux Studio Plugins Project <https://lsp-plug.in/>
3 * (C) 2025 Vladimir Sadovnikov <[email protected]>
4 *
5 * This file is part of lsp-dsp-lib
6 * Created on: 14 апр. 2025 г.
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_SSE2_PMATH_COS_H_
23 #define PRIVATE_DSP_ARCH_X86_SSE2_PMATH_COS_H_
24
25 #ifndef PRIVATE_DSP_ARCH_X86_SSE2_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_X86_SSE2_IMPL */
28
29 #include <private/dsp/arch/x86/sse2/pmath/sin.h>
30
31 namespace lsp
32 {
33 namespace sse2
34 {
35 void cosf1(float *dst, size_t count)
36 {
37 IF_ARCH_X86(float *src);
38
39 ARCH_X86_ASM(
40 // x8 blocks
41 __ASM_EMIT("sub $8, %[count]")
42 __ASM_EMIT("jb 2f")
43 __ASM_EMIT("1:")
44 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm0") // xmm0 = PI
45 __ASM_EMIT("movups 0x00(%[dst]), %%xmm1") // xmm1 = x1
46 __ASM_EMIT("movaps %%xmm0, %%xmm4") // xmm4 = PI
47 __ASM_EMIT("movups 0x10(%[dst]), %%xmm5") // xmm5 = x2
48 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - x1 + PI/2
49 __ASM_EMIT("subps %%xmm5, %%xmm4") // xmm4 = PI/2 - x2 + PI/2
50 SINF_X_PLUS_PI_2_CORE_X8
51 __ASM_EMIT("movups %%xmm0, 0x00(%[dst])")
52 __ASM_EMIT("movups %%xmm4, 0x10(%[dst])")
53 __ASM_EMIT("add $0x20, %[dst]")
54 __ASM_EMIT("sub $8, %[count]")
55 __ASM_EMIT("jae 1b")
56 __ASM_EMIT("2:")
57 // x4 block
58 __ASM_EMIT("add $4, %[count]")
59 __ASM_EMIT("jl 4f")
60 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm0") // xmm0 = PI
61 __ASM_EMIT("movups 0x00(%[dst]), %%xmm1") // xmm1 = x
62 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - x + PI/2
63 SINF_X_PLUS_PI_2_CORE_X4
64 __ASM_EMIT("movups %%xmm0, 0x00(%[dst])")
65 __ASM_EMIT("add $0x10, %[dst]")
66 __ASM_EMIT("sub $4, %[count]")
67 __ASM_EMIT("4:")
68 // Tail: 1x-3x block
69 __ASM_EMIT("add $4, %[count]")
70 __ASM_EMIT("jle 12f")
71 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm0") // xmm0 = PI
72 __ASM_EMIT("mov %[dst], %[src]")
73 __ASM_EMIT("test $1, %[count]")
74 __ASM_EMIT("jz 6f")
75 __ASM_EMIT("movss 0x00(%[src]), %%xmm1")
76 __ASM_EMIT("add $4, %[src]")
77 __ASM_EMIT("6:")
78 __ASM_EMIT("test $2, %[count]")
79 __ASM_EMIT("jz 8f")
80 __ASM_EMIT("movhps 0x00(%[src]), %%xmm1")
81 __ASM_EMIT("8:")
82 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - x + PI/2
83 SINF_X_PLUS_PI_2_CORE_X4
84 __ASM_EMIT("test $1, %[count]")
85 __ASM_EMIT("jz 10f")
86 __ASM_EMIT("movss %%xmm0, 0x00(%[dst])")
87 __ASM_EMIT("add $4, %[dst]")
88 __ASM_EMIT("10:")
89 __ASM_EMIT("test $2, %[count]")
90 __ASM_EMIT("jz 12f")
91 __ASM_EMIT("movhps %%xmm0, 0x00(%[dst])")
92 // End
93 __ASM_EMIT("12:")
94
95 : [dst] "+r" (dst), [src] "=&r" (src), [count] "+r" (count)
96 : [S2C] "o" (sinf_const)
97 : "cc", "memory",
98 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
99 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
100 );
101 }
102
103 void cosf2(float *dst, const float *src, size_t count)
104 {
105 ARCH_X86_ASM(
106 // x8 blocks
107 __ASM_EMIT("sub $8, %[count]")
108 __ASM_EMIT("jb 2f")
109 __ASM_EMIT("1:")
110 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm0") // xmm0 = PI
111 __ASM_EMIT("movups 0x00(%[src]), %%xmm1") // xmm1 = x1
112 __ASM_EMIT("movaps %%xmm0, %%xmm4") // xmm4 = PI
113 __ASM_EMIT("movups 0x10(%[src]), %%xmm5") // xmm5 = x2
114 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - x1 + PI/2
115 __ASM_EMIT("subps %%xmm5, %%xmm4") // xmm4 = PI/2 - x2 + PI/2
116 SINF_X_PLUS_PI_2_CORE_X8
117 __ASM_EMIT("movups %%xmm0, 0x00(%[dst])")
118 __ASM_EMIT("movups %%xmm4, 0x10(%[dst])")
119 __ASM_EMIT("add $0x20, %[src]")
120 __ASM_EMIT("add $0x20, %[dst]")
121 __ASM_EMIT("sub $8, %[count]")
122 __ASM_EMIT("jae 1b")
123 __ASM_EMIT("2:")
124 // x4 block
125 __ASM_EMIT("add $4, %[count]")
126 __ASM_EMIT("jl 4f")
127 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm0") // xmm0 = PI
128 __ASM_EMIT("movups 0x00(%[src]), %%xmm1") // xmm1 = x1
129 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - x1 + PI/2
130 SINF_X_PLUS_PI_2_CORE_X4
131 __ASM_EMIT("movups %%xmm0, 0x00(%[dst])")
132 __ASM_EMIT("add $0x10, %[src]")
133 __ASM_EMIT("add $0x10, %[dst]")
134 __ASM_EMIT("sub $4, %[count]")
135 __ASM_EMIT("4:")
136 // Tail: 1x-3x block
137 __ASM_EMIT("add $4, %[count]")
138 __ASM_EMIT("jle 12f")
139 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm0") // xmm0 = PI
140 __ASM_EMIT("test $1, %[count]")
141 __ASM_EMIT("jz 6f")
142 __ASM_EMIT("movss 0x00(%[src]), %%xmm1")
143 __ASM_EMIT("add $4, %[src]")
144 __ASM_EMIT("6:")
145 __ASM_EMIT("test $2, %[count]")
146 __ASM_EMIT("jz 8f")
147 __ASM_EMIT("movhps 0x00(%[src]), %%xmm1")
148 __ASM_EMIT("8:")
149 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - x1 + PI/2
150 SINF_X_PLUS_PI_2_CORE_X4
151 __ASM_EMIT("test $1, %[count]")
152 __ASM_EMIT("jz 10f")
153 __ASM_EMIT("movss %%xmm0, 0x00(%[dst])")
154 __ASM_EMIT("add $4, %[dst]")
155 __ASM_EMIT("10:")
156 __ASM_EMIT("test $2, %[count]")
157 __ASM_EMIT("jz 12f")
158 __ASM_EMIT("movhps %%xmm0, 0x00(%[dst])")
159 // End
160 __ASM_EMIT("12:")
161
162 : [dst] "+r" (dst), [src] "+r" (src), [count] "+r" (count)
163 : [S2C] "o" (sinf_const)
164 : "cc", "memory",
165 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
166 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
167 );
168 }
169
170 void cosf_kp1(float *dst, float k, float p, size_t count)
171 {
172 ARCH_X86_ASM(
173 // Prepare
174 __ASM_EMIT("movss %[k], %%xmm6") // xmm6 = k
175 __ASM_EMIT("movss %[p], %%xmm0") // xmm7 = p
176 __ASM_EMIT("shufps $0x00, %%xmm6, %%xmm6") // xmm6 = k k k k
177 __ASM_EMIT("shufps $0x00, %%xmm0, %%xmm0") // xmm0 = p p p p
178 __ASM_EMIT("movaps 0x10 + %[S2C], %%xmm7") // xmm7 = PI PI PI PI
179 __ASM_EMIT("movaps 0x00 + %[S2KP], %%xmm4") // xmm4 = i = 0 1 2 3
180 __ASM_EMIT("movaps 0x10 + %[S2KP], %%xmm5") // xmm5 = step = 4 4 4 4
181 __ASM_EMIT("subps %%xmm0, %%xmm7") // xmm7 = PI/2 - p + PI/2
182 // x4 blocks
183 __ASM_EMIT("sub $4, %[count]")
184 __ASM_EMIT("jb 2f")
185 __ASM_EMIT("1:")
186 __ASM_EMIT("movaps %%xmm4, %%xmm1") // xmm1 = i
187 __ASM_EMIT("movaps %%xmm7, %%xmm0") // xmm0 = PI/2 - p + PI/2
188 __ASM_EMIT("mulps %%xmm6, %%xmm1") // xmm1 = k*i
189 __ASM_EMIT("addps %%xmm5, %%xmm4") // xmm4 = i + step
190 __ASM_EMIT("subps %%xmm1, %%xmm0") // xmm0 = PI/2 - (p + k*i) + PI/2
191 SINF_X_PLUS_PI_2_CORE_X4
192 __ASM_EMIT("movups %%xmm0, 0x00(%[dst])")
193 __ASM_EMIT("add $0x10, %[dst]")
194 __ASM_EMIT("sub $4, %[count]")
195 __ASM_EMIT("jae 1b")
196 __ASM_EMIT("2:")
197 // Tail: 1x-3x block
198 __ASM_EMIT("add $4, %[count]")
199 __ASM_EMIT("jle 6f")
200 __ASM_EMIT("movaps %%xmm7, %%xmm0") // xmm0 = PI/2 - p + PI/2
201 __ASM_EMIT("mulps %%xmm6, %%xmm4") // xmm4 = k*i
202 __ASM_EMIT("subps %%xmm4, %%xmm0") // xmm0 = PI/2 - (p + k*i) + PI/2
203 SINF_X_PLUS_PI_2_CORE_X4
204 __ASM_EMIT("test $1, %[count]")
205 __ASM_EMIT("jz 4f")
206 __ASM_EMIT("movss %%xmm0, 0x00(%[dst])")
207 __ASM_EMIT("shufps $0x39, %%xmm0, %%xmm0") // xmm0 = S1 S2 S3 S0
208 __ASM_EMIT("add $4, %[dst]")
209 __ASM_EMIT("4:")
210 __ASM_EMIT("test $2, %[count]")
211 __ASM_EMIT("jz 6f")
212 __ASM_EMIT("movlps %%xmm0, 0x00(%[dst])")
213 // End
214 __ASM_EMIT("6:")
215
216 : [dst] "+r" (dst), [count] "+r" (count)
217 : [S2C] "o" (sinf_const),
218 [S2KP] "o" (kp_gen_const),
219 [k] "m" (k),
220 [p] "m" (p)
221 : "cc", "memory",
222 "%xmm0", "%xmm1", "%xmm2", "%xmm3",
223 "%xmm4", "%xmm5", "%xmm6", "%xmm7"
224 );
225 }
226 } /* namespace sse2 */
227 } /* namespace lsp */
228
229
230 #endif /* PRIVATE_DSP_ARCH_X86_SSE2_PMATH_COS_H_ */