Debian Codegraph @

modules/lsp-dsp-lib/include/private/dsp/arch/aarch64/asimd/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_AARCH64_ASIMD_PMATH_COS_H_
23 #define PRIVATE_DSP_ARCH_AARCH64_ASIMD_PMATH_COS_H_
24
25 #ifndef PRIVATE_DSP_ARCH_AARCH64_ASIMD_IMPL
26 #error "This header should not be included directly"
27 #endif /* PRIVATE_DSP_ARCH_AARCH64_ASIMD_IMPL */
28
29 #include <private/dsp/arch/aarch64/asimd/pmath/sin.h>
30
31 namespace lsp
32 {
33 namespace asimd
34 {
35 void cosf1(float *dst, size_t count)
36 {
37 ARCH_AARCH64_ASM(
38 __ASM_EMIT("ldp q16, q17, [%[S2C], #0x00]") /* v16 = PI/2, v17 = PI */
39 __ASM_EMIT("ldp q18, q19, [%[S2C], #0x20]") /* v18 = 1/(2*PI), v19 = C0 */
40 __ASM_EMIT("ldp q20, q21, [%[S2C], #0x40]") /* v20 = C1, v21 = C2 */
41 __ASM_EMIT("ldp q22, q23, [%[S2C], #0x60]") /* v22 = C3, v23 = C4 */
42 // x16 blocks
43 __ASM_EMIT("subs %[count], %[count], #16")
44 __ASM_EMIT("b.lo 2f")
45 __ASM_EMIT("1:")
46 __ASM_EMIT("ldp q0, q1, [%[dst], #0x00]") // v0 = x0, v1 = x1
47 __ASM_EMIT("ldp q2, q3, [%[dst], #0x20]") // v2 = x2, v3 = x3
48 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
49 __ASM_EMIT("fsub v1.4s, v17.4s, v1.4s") // v1 = PI/2 - x1 + PI/2
50 __ASM_EMIT("fsub v2.4s, v17.4s, v2.4s") // v2 = PI/2 - x2 + PI/2
51 __ASM_EMIT("fsub v3.4s, v17.4s, v3.4s") // v3 = PI/2 - x3 + PI/2
52 SINF_X_PLUS_PI_2_CORE_X16
53 __ASM_EMIT("subs %[count], %[count], #16")
54 __ASM_EMIT("stp q0, q1, [%[dst], #0x00]")
55 __ASM_EMIT("stp q2, q3, [%[dst], #0x20]")
56 __ASM_EMIT("add %[dst], %[dst], #0x40")
57 __ASM_EMIT("b.hs 1b")
58 __ASM_EMIT("2:")
59 // x8 block
60 __ASM_EMIT("adds %[count], %[count], #8")
61 __ASM_EMIT("b.lt 4f")
62 __ASM_EMIT("ldp q0, q1, [%[dst], #0x00]") // v0 = x0, v1 = x1
63 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
64 __ASM_EMIT("fsub v1.4s, v17.4s, v1.4s") // v1 = PI/2 - x1 + PI/2
65 SINF_X_PLUS_PI_2_CORE_X8
66 __ASM_EMIT("sub %[count], %[count], #8")
67 __ASM_EMIT("stp q0, q1, [%[dst], #0x00]")
68 __ASM_EMIT("add %[dst], %[dst], #0x20")
69 __ASM_EMIT("4:")
70 // x4 block
71 __ASM_EMIT("adds %[count], %[count], #4")
72 __ASM_EMIT("b.lt 6f")
73 __ASM_EMIT("ldr q0, [%[dst], #0x00]") // v0 = x0
74 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
75 SINF_X_PLUS_PI_2_CORE_X4
76 __ASM_EMIT("sub %[count], %[count], #4")
77 __ASM_EMIT("str q0, [%[dst], #0x00]")
78 __ASM_EMIT("add %[dst], %[dst], #0x10")
79 __ASM_EMIT("6:")
80 // Tail: 1x-3x block
81 __ASM_EMIT("adds %[count], %[count], #4")
82 __ASM_EMIT("b.ls 14f")
83 __ASM_EMIT("tst %[count], #1")
84 __ASM_EMIT("b.eq 8f")
85 __ASM_EMIT("ld1 {v0.s}[0], [%[dst]]")
86 __ASM_EMIT("add %[dst], %[dst], #0x04")
87 __ASM_EMIT("8:")
88 __ASM_EMIT("tst %[count], #2")
89 __ASM_EMIT("b.eq 10f")
90 __ASM_EMIT("ld1 {v0.d}[1], [%[dst]]")
91 __ASM_EMIT("10:")
92 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
93 SINF_X_PLUS_PI_2_CORE_X4
94 __ASM_EMIT("tst %[count], #1")
95 __ASM_EMIT("b.eq 12f")
96 __ASM_EMIT("sub %[dst], %[dst], #0x04")
97 __ASM_EMIT("st1 {v0.s}[0], [%[dst]]")
98 __ASM_EMIT("add %[dst], %[dst], #0x04")
99 __ASM_EMIT("12:")
100 __ASM_EMIT("tst %[count], #2")
101 __ASM_EMIT("b.eq 14f")
102 __ASM_EMIT("st1 {v0.d}[1], [%[dst]]")
103 // End
104 __ASM_EMIT("14:")
105
106 : [dst] "+r" (dst), [count] "+r" (count)
107 : [S2C] "r" (&sinf_const[0])
108 : "cc", "memory",
109 "v0", "v1", "v2", "v3",
110 "v4", "v5", "v6", "v7",
111 "v8", "v9", "v10", "v11",
112 "v12", "v13", "v14", "v15",
113 "v16", "v17", "v18", "v19",
114 "v20", "v21", "v22", "v23"
115 );
116 }
117
118 void cosf2(float *dst, const float *src, size_t count)
119 {
120 ARCH_AARCH64_ASM(
121 __ASM_EMIT("ldp q16, q17, [%[S2C], #0x00]") /* v16 = PI/2, v17 = PI */
122 __ASM_EMIT("ldp q18, q19, [%[S2C], #0x20]") /* v18 = 1/(2*PI), v19 = C0 */
123 __ASM_EMIT("ldp q20, q21, [%[S2C], #0x40]") /* v20 = C1, v21 = C2 */
124 __ASM_EMIT("ldp q22, q23, [%[S2C], #0x60]") /* v22 = C3, v23 = C4 */
125 // x16 blocks
126 __ASM_EMIT("subs %[count], %[count], #16")
127 __ASM_EMIT("b.lo 2f")
128 __ASM_EMIT("1:")
129 __ASM_EMIT("ldp q0, q1, [%[src], #0x00]") // v0 = x0, v1 = x1
130 __ASM_EMIT("ldp q2, q3, [%[src], #0x20]") // v2 = x2, v3 = x3
131 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
132 __ASM_EMIT("fsub v1.4s, v17.4s, v1.4s") // v1 = PI/2 - x1 + PI/2
133 __ASM_EMIT("fsub v2.4s, v17.4s, v2.4s") // v2 = PI/2 - x2 + PI/2
134 __ASM_EMIT("fsub v3.4s, v17.4s, v3.4s") // v3 = PI/2 - x3 + PI/2
135 SINF_X_PLUS_PI_2_CORE_X16
136 __ASM_EMIT("subs %[count], %[count], #16")
137 __ASM_EMIT("stp q0, q1, [%[dst], #0x00]")
138 __ASM_EMIT("stp q2, q3, [%[dst], #0x20]")
139 __ASM_EMIT("add %[src], %[src], #0x40")
140 __ASM_EMIT("add %[dst], %[dst], #0x40")
141 __ASM_EMIT("b.hs 1b")
142 __ASM_EMIT("2:")
143 // x8 block
144 __ASM_EMIT("adds %[count], %[count], #8")
145 __ASM_EMIT("b.lt 4f")
146 __ASM_EMIT("ldp q0, q1, [%[src], #0x00]") // v0 = x0, v1 = x1
147 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
148 __ASM_EMIT("fsub v1.4s, v17.4s, v1.4s") // v1 = PI/2 - x1 + PI/2
149 SINF_X_PLUS_PI_2_CORE_X8
150 __ASM_EMIT("sub %[count], %[count], #8")
151 __ASM_EMIT("stp q0, q1, [%[dst], #0x00]")
152 __ASM_EMIT("add %[src], %[src], #0x20")
153 __ASM_EMIT("add %[dst], %[dst], #0x20")
154 __ASM_EMIT("4:")
155 // x4 block
156 __ASM_EMIT("adds %[count], %[count], #4")
157 __ASM_EMIT("b.lt 6f")
158 __ASM_EMIT("ldr q0, [%[src], #0x00]") // v0 = x0
159 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
160 SINF_X_PLUS_PI_2_CORE_X4
161 __ASM_EMIT("sub %[count], %[count], #4")
162 __ASM_EMIT("str q0, [%[dst], #0x00]")
163 __ASM_EMIT("add %[src], %[src], #0x10")
164 __ASM_EMIT("add %[dst], %[dst], #0x10")
165 __ASM_EMIT("6:")
166 // Tail: 1x-3x block
167 __ASM_EMIT("adds %[count], %[count], #4")
168 __ASM_EMIT("b.ls 14f")
169 __ASM_EMIT("tst %[count], #1")
170 __ASM_EMIT("b.eq 8f")
171 __ASM_EMIT("ld1 {v0.s}[0], [%[src]]")
172 __ASM_EMIT("add %[src], %[src], #0x04")
173 __ASM_EMIT("8:")
174 __ASM_EMIT("tst %[count], #2")
175 __ASM_EMIT("b.eq 10f")
176 __ASM_EMIT("ld1 {v0.d}[1], [%[src]]")
177 __ASM_EMIT("10:")
178 __ASM_EMIT("fsub v0.4s, v17.4s, v0.4s") // v0 = PI/2 - x0 + PI/2
179 SINF_X_PLUS_PI_2_CORE_X4
180 __ASM_EMIT("tst %[count], #1")
181 __ASM_EMIT("b.eq 12f")
182 __ASM_EMIT("st1 {v0.s}[0], [%[dst]]")
183 __ASM_EMIT("add %[dst], %[dst], #0x04")
184 __ASM_EMIT("12:")
185 __ASM_EMIT("tst %[count], #2")
186 __ASM_EMIT("b.eq 14f")
187 __ASM_EMIT("st1 {v0.d}[1], [%[dst]]")
188 // End
189 __ASM_EMIT("14:")
190
191 : [dst] "+r" (dst), [src] "+r" (src), [count] "+r" (count)
192 : [S2C] "r" (&sinf_const[0])
193 : "cc", "memory",
194 "v0", "v1", "v2", "v3",
195 "v4", "v5", "v6", "v7",
196 "v8", "v9", "v10", "v11",
197 "v12", "v13", "v14", "v15",
198 "v16", "v17", "v18", "v19",
199 "v20", "v21", "v22", "v23"
200 );
201 }
202
203 void cosf_kp1(float *dst, float k, float p, size_t count)
204 {
205 ARCH_AARCH64_ASM(
206 __ASM_EMIT("ldp q16, q17, [%[S2C], #0x00]") /* v16 = PI/2, v17 = PI */
207 __ASM_EMIT("ldp q18, q19, [%[S2C], #0x20]") /* v18 = 1/(2*PI), v19 = C0 */
208 __ASM_EMIT("ldp q20, q21, [%[S2C], #0x40]") /* v20 = C1, v21 = C2 */
209 __ASM_EMIT("ldp q22, q23, [%[S2C], #0x60]") /* v22 = C3, v23 = C4 */
210 __ASM_EMIT("ld1r {v28.4s}, [%[k]]") /* v28 = k k k k */
211 __ASM_EMIT("ld1r {v29.4s}, [%[p]]") /* v29 = p p p p */
212 __ASM_EMIT("ldp q24, q25, [%[S2KP], #0x00]") /* v24 = 0 1 2 3, v25 = 4 5 6 7 */
213 __ASM_EMIT("ldp q26, q27, [%[S2KP], #0x20]") /* v26 = 8 8 8 8, v27 = 4 4 4 4 */
214 __ASM_EMIT("fsub v29.4s, v17.4s, v29.4s") /* v29 = PI/2 - p + PI/2 ... */
215 // x16 blocks
216 __ASM_EMIT("subs %[count], %[count], #16")
217 __ASM_EMIT("b.lo 2f")
218 __ASM_EMIT("1:")
219 __ASM_EMIT("fmul v0.4s, v28.4s, v24.4s") // v0 = k*x0
220 __ASM_EMIT("fmul v1.4s, v28.4s, v25.4s") // v1 = k*x1
221 __ASM_EMIT("fadd v24.4s, v24.4s, v26.4s") // v24 = i0 + 8
222 __ASM_EMIT("fadd v25.4s, v25.4s, v26.4s") // v25 = i1 + 8
223 __ASM_EMIT("fsub v0.4s, v29.4s, v0.4s") // v0 = PI/2 - (k*x0 + p) + PI/2
224 __ASM_EMIT("fsub v1.4s, v29.4s, v1.4s") // v1 = PI/2 - (k*x1 + p) + PI/2
225 __ASM_EMIT("fmul v2.4s, v28.4s, v24.4s") // v2 = k*x2
226 __ASM_EMIT("fmul v3.4s, v28.4s, v25.4s") // v3 = k*x3
227 __ASM_EMIT("fadd v24.4s, v24.4s, v26.4s") // v24 = i0 + 8
228 __ASM_EMIT("fadd v25.4s, v25.4s, v26.4s") // v25 = i1 + 8
229 __ASM_EMIT("fsub v2.4s, v29.4s, v2.4s") // v2 = PI/2 - (k*x2 + p) + PI/2
230 __ASM_EMIT("fsub v3.4s, v29.4s, v3.4s") // v3 = PI/2 - (k*x3 + p) + PI/2
231 SINF_X_PLUS_PI_2_CORE_X16
232 __ASM_EMIT("subs %[count], %[count], #16")
233 __ASM_EMIT("stp q0, q1, [%[dst], #0x00]")
234 __ASM_EMIT("stp q2, q3, [%[dst], #0x20]")
235 __ASM_EMIT("add %[dst], %[dst], #0x40")
236 __ASM_EMIT("b.hs 1b")
237 __ASM_EMIT("2:")
238 // x8 block
239 __ASM_EMIT("adds %[count], %[count], #8")
240 __ASM_EMIT("b.lt 4f")
241 __ASM_EMIT("fmul v0.4s, v28.4s, v24.4s") // v0 = k*x0
242 __ASM_EMIT("fmul v1.4s, v28.4s, v25.4s") // v1 = k*x1
243 __ASM_EMIT("fadd v24.4s, v24.4s, v26.4s") // v24 = i0 + 8
244 __ASM_EMIT("fadd v25.4s, v25.4s, v26.4s") // v25 = i1 + 8
245 __ASM_EMIT("fsub v0.4s, v29.4s, v0.4s") // v0 = PI/2 - (k*x0 + p) + PI/2
246 __ASM_EMIT("fsub v1.4s, v29.4s, v1.4s") // v1 = PI/2 - (k*x1 + p) + PI/2
247 SINF_X_PLUS_PI_2_CORE_X8
248 __ASM_EMIT("sub %[count], %[count], #8")
249 __ASM_EMIT("stp q0, q1, [%[dst], #0x00]")
250 __ASM_EMIT("add %[dst], %[dst], #0x20")
251 __ASM_EMIT("4:")
252 // x4 block
253 __ASM_EMIT("adds %[count], %[count], #4")
254 __ASM_EMIT("b.lt 6f")
255 __ASM_EMIT("fmul v0.4s, v28.4s, v24.4s") // v0 = k*x0
256 __ASM_EMIT("fadd v24.4s, v24.4s, v27.4s") // v24 = i0 + 4
257 __ASM_EMIT("fsub v0.4s, v29.4s, v0.4s") // v0 = PI/2 - (k*x0 + p) + PI/2
258 SINF_X_PLUS_PI_2_CORE_X4
259 __ASM_EMIT("sub %[count], %[count], #4")
260 __ASM_EMIT("str q0, [%[dst], #0x00]")
261 __ASM_EMIT("add %[dst], %[dst], #0x10")
262 __ASM_EMIT("6:")
263 // Tail: 1x-3x block
264 __ASM_EMIT("adds %[count], %[count], #4")
265 __ASM_EMIT("b.ls 10f")
266 __ASM_EMIT("fmul v0.4s, v28.4s, v24.4s") // v0 = k*x0
267 __ASM_EMIT("fsub v0.4s, v29.4s, v0.4s") // v0 = PI/2 - (k*x0 + p) + PI/2
268 SINF_X_PLUS_PI_2_CORE_X4
269 __ASM_EMIT("tst %[count], #1")
270 __ASM_EMIT("b.eq 8f")
271 __ASM_EMIT("st1 {v0.s}[0], [%[dst]]")
272 __ASM_EMIT("add %[dst], %[dst], #0x04")
273 __ASM_EMIT("ext v0.16b, v0.16b, v0.16b, #4") // v0 = S1 S2 S3 S0
274 __ASM_EMIT("8:")
275 __ASM_EMIT("tst %[count], #2")
276 __ASM_EMIT("b.eq 10f")
277 __ASM_EMIT("st1 {v0.d}[0], [%[dst]]")
278 // End
279 __ASM_EMIT("10:")
280
281 : [dst] "+r" (dst), [count] "+r" (count)
282 : [k] "r" (&k),
283 [p] "r" (&p),
284 [S2C] "r" (&sinf_const[0]),
285 [S2KP] "r" (&kp_gen_const[0])
286 : "cc", "memory",
287 "v0", "v1", "v2", "v3",
288 "v4", "v5", "v6", "v7",
289 "v8", "v9", "v10", "v11",
290 "v12", "v13", "v14", "v15",
291 "v16", "v17", "v18", "v19",
292 "v20", "v21", "v22", "v23",
293 "v24", "v25", "v26", "v27",
294 "v28", "v29"
295 );
296 }
297
298 } /* namespace asimd */
299 } /* namespace lsp */
300
301 #endif /* PRIVATE_DSP_ARCH_AARCH64_ASIMD_PMATH_COS_H_ */