1/*
2 * SPDX-License-Identifier: BSD-2-Clause
3 *
4 * Copyright (c) 2021 Warner Losh
5 * Copyright (c) 2023 Stormshield
6 * Copyright (c) 2023 Klara, Inc.
7 */
8
9#include <sys/syscall.h>
10
11#define	STDOUT_FILENO	1
12
13#define	MUTEX_LOCKED	0x01
14#define	MUTEX_UNLOCKED	0x00
15
16#define	STACK_SIZE	4096
17#define	TLS_SIZE	4096
18
19	.text
20	.file "swp_test.S"
21	.syntax unified
22	.globl main
23	.p2align 2
24	.type main,%function
25	.code 32
26
27main:
28	/*
29	 * Stack slots:
30	 * 0 - Sync word
31	 * 1 - Thread id
32	 * 2 - Shared word
33	 */
34	sub sp, sp, #12
35
36	/* Print a message */
37	movw r0, :lower16:.L.mainmsg
38	movt r0, :upper16:.L.mainmsg
39	ldr r1, =(.L.mainmsgEnd - .L.mainmsg - 1)
40	bl print
41
42	/* Create two secondary threads */
43	mov r0, #1
44	str r0, [sp, #4]	/* Thread ID */
45	movw r0, :lower16:secondary_thread
46	movt r0, :upper16:secondary_thread
47	mov r1, sp
48	movw r2, :lower16:stack1
49	movt r2, :upper16:stack1
50	movw r3, :lower16:tls1
51	movt r3, :upper16:tls1
52	bl create_thr
53
541:
55	/*
56	 * Wait for the first new thread to ack its existence by
57	 * incrementing the thread id.
58	 */
59	ldr r0, [sp, #4]
60	cmp r0, #1
61	bne 2f
62	ldr r7, =SYS_sched_yield
63	swi 0
64	b 1b
65
662:
67	/* Create thread #2 */
68	movw r0, :lower16:secondary_thread
69	movt r0, :upper16:secondary_thread
70	mov r1, sp
71	movw r2, :lower16:stack2
72	movt r2, :upper16:stack2
73	movw r3, :lower16:tls2
74	movt r3, :upper16:tls2
75	bl create_thr
76
773:
78	/*
79	 * Wait for the first new thread to ack its existence by
80	 * incrementing the thread id.
81	 */
82	ldr r0, [sp, #4]
83	cmp r0, #2
84	bne 4f
85	ldr r7, =SYS_sched_yield
86	swi 0
87	b 3b
88
89	/* Loop */
904:
91	mov r0, sp
92	mov r1, #0	/* Thread loop */
93	add r2, sp, #8
94	bl thread_loop
95	b 4b
96
97	/* UNREACHABLE */
98	mov r0, #0
99	ldr r7, =SYS_exit
100	swi 0
101
102	.p2align 2
103	.type secondary_thread,%function
104	.code 32
105secondary_thread:
106	/*
107	 * On entry, r0 is where we stashed our sync word and
108	 * ack word (thread ID).
109	 *
110	 * Stash the sync word in r4, thread ID in r5.
111	 */
112	mov r4, r0
113	ldr r5, [r0, #4]
114
115	/* Print a message */
116	movw r0, :lower16:.L.secondarymsg
117	movt r0, :upper16:.L.secondarymsg
118	ldr r1, =(.L.secondarymsgEnd - .L.secondarymsg - 1)
119	bl print
120
121	/* Acknowledge that we started */
122	add r0, r5, #1
123	str r0, [r4, #4]
124
1251:
126	mov r0, r4
127	mov r1, r5
128	add r2, r4, #8
129	bl thread_loop
130	b 1b
131
132	.p2align 2
133	.type thread_loop,%function
134	.code 32
135thread_loop:
136	push {r4, r5, r6, r7, r8, lr}
137
138	/*
139	 * r0 == sync word
140	 * r1 == thread ID
141	 * r2 == shared word
142	 */
143	mov r4, r0
144	mov r5, r1
145	mov r6, r2
146	bl lock_mutex_swp
147	str r5, [r6] /* Write the thread ID */
148	bl random_cycles
149
150	# Save off the now cycle count */
151	mov r8, r0
152
153	/* Print the thread ID and cycle count */
154	mov r0, r5
155	mov r1, #0
156	bl printnum
157
158	/* Separator */
159	movw r0, :lower16:.L.idsep
160	movt r0, :upper16:.L.idsep
161	ldr r1, =(.L.idsepEnd - .L.idsep - 1)
162	bl print
163
164	/* Cycle count */
165	mov r0, r8
166	mov r1, #1
167	bl printnum
168
1691:
170	ldr r0, [r6]
171	cmp r0, r5	/* Check against the thread ID */
172	bne 2f
173	str r5, [r6]
174
175	/*
176	 * Check if the count hit 0, otherwise go again.
177	 */
178	cmp r8, #0
179	beq 3f
180	sub r8, r8, #1
181	b 1b
182
1832:
184	/* exit(1) */
185	mov r0, #1
186	ldr r7, =SYS_exit
187	swi 0
188
1893:
190	mov r0, r4
191	bl unlock_mutex_swp
192
193	/*
194	 * Yield to lower the chance that we end up re-acquiring, the other two
195	 * threads are still actively trying to acquire the lock.
196	 */
197	ldr r7, =SYS_sched_yield
198	swi 0
199
200	pop {r4, r5, r6, r7, r8, lr}
201	bx lr
202
203	.p2align 2
204	.type random_cycles,%function
205	.code 32
206random_cycles:
207	/* Return a random number < 4k */
208	sub sp, sp, #4
209	mov r0, sp
210	mov r1, #4
211	mov r2, #0
212	ldr r7, =SYS_getrandom
213	swi 0
214
215	/*
216	 * Just truncate the result of getrandom(2)
217	 * to put us within range.  Naive, but functional.
218	 */
219	ldr r0, [sp]
220	mov r1, #0xfff
221	and r0, r0, r1
222	add sp, sp, #4
223	bx lr
224
225	/*
226	 * lock_mutex_swp and unlock_mutex_swp lifted from
227	 * ARM documentation on SWP/SWPB.
228	 */
229	.p2align 2
230	.type lock_mutex_swp,%function
231	.code 32
232lock_mutex_swp:
233	mov r2, #MUTEX_LOCKED
234	swp r1, r2, [r0]	/* Swap in lock value. */
235	cmp r1, r2		/* Check if we were locked already. */
236	beq lock_mutex_swp	/* Retry if so */
237	bx lr			/* Return locked */
238
239	.p2align 2
240	.type unlock_mutex_swp,%function
241	.code 32
242unlock_mutex_swp:
243	mov r1, #MUTEX_UNLOCKED
244	str r1, [r0]		/* Move in unlocked */
245	bx lr
246
247	.p2align 2
248	.type create_thr,%function
249	.code 32
250create_thr:
251	/*
252	 * r0 == start_func
253	 * r1 == arg
254	 * r2 == stack_base
255	 * r3 == tls_base
256	 */
257	sub sp, sp, #56
258	str r0, [sp, #4] 	/* start_func */
259	str r1, [sp, #8]	/* arg */
260	str r2, [sp, #12]	/* stack_base */
261	mov r0, #STACK_SIZE
262	str r0, [sp, #16]	/* stack_size */
263	str r3, [sp, #20]	/* tls_base */
264	mov r0, #TLS_SIZE
265	str r0, [sp, #24]	/* tls_size */
266	mov r0, #0
267	str r0, [sp, #28]
268	str r0, [sp, #32]
269	str r0, [sp, #36]
270	str r0, [sp, #40]
271
272	add r0, sp, #4	/* &thrp */
273	mov r1, #52	/* sizeof(thrp) */
274	ldr r7, =SYS_thr_new
275	swi 0
276
277	add sp, sp, #56
278	bx lr
279
280	.p2align 2
281	.type printnum,%function
282	.code 32
283printnum:
284	push {r4, r5, r6, r7, r8, r10, lr}
285	sub sp, #4
286
287	/* 1000000000 */
288	movw r6, #0xca00
289	movt r6, #0x3b9a
290
291	udiv r5, r0, r6
292	cmp r5, #9
293	bhi abort
294
295	/* r4 is our accumulator */
296	mov r4, r0
297	/* r5 to be used as our "significant bit" */
298	mov r5, #0
299	/* r10 is "output_newline" */
300	mov r10, r1
301
3021:
303	cmp r6, #0
304	beq 4f
305
306	/* Divide by current place */
307	udiv r0, r4, r6
308	/* Significant already? print anyways */
309	cmp r5, #0
310	bne 2f
311
312	/*
313	 * Not significant, maybe print.  If we made it all the way to 1, we
314	 * need to just print the 0 anyways.
315	 */
316	cmp r6, #1
317	beq 2f
318
319	cmp r0, #0
320	bne 2f
321	b 3f	/* Proceed */
322
323	/* Print */
3242:
325	mov r5, #1
326	mov r8, r0
327	add r0, r0, #0x30
328	str r0, [sp]
329	mov r0, sp
330	mov r1, #1
331	bl print
332
333	/* Multiply back into place and subtract from accumulator */
334	mul r0, r8, r6
335	sub r4, r4, r0
336
3373:
338	mov r3, #10
339	udiv r6, r6, r3
340	b 1b
341
3424:
343	cmp r10, #0
344	beq 5f
345
346	/* newline */
347	mov r0, #0x0a
348	str r0, [sp]
349	mov r0, sp
350	mov r1, #1
351	bl print
352
3535:
354	add sp, sp, #4
355	pop {r4, r5, r6, r7, r8, r10, lr}
356	bx lr
357
358abort:
359	movw r0, :lower16:.L.badnum
360	movt r0, :upper16:.L.badnum
361	ldr r1, =(.L.badnumEnd - .L.badnum - 1)
362	bl print
363
364	mov r0, #1
365	ldr r7, =SYS_exit
366	swi 0
367
368	.p2align 2
369	.type print,%function
370	.code 32
371print:
372	/* r0 - string, r1 = size */
373	mov r2, r1
374	mov r1, r0
375	ldr r0, =STDOUT_FILENO
376	ldr r7, =SYS_write
377	swi 0
378
379	bx lr
380
381.L.mainmsg:
382	.asciz "Main thread\n"
383.L.mainmsgEnd:
384	.size .L.mainmsg, .L.mainmsgEnd - .L.mainmsg
385.L.secondarymsg:
386	.asciz "Secondary thread\n"
387.L.secondarymsgEnd:
388	.size .L.secondarymsg, .L.secondarymsgEnd - .L.secondarymsg
389.L.badnum:
390	.asciz "Bad number\n"
391.L.badnumEnd:
392	.size .L.badnum, .L.badnumEnd - .L.badnum
393.L.idsep:
394	.asciz " - cycles "
395.L.idsepEnd:
396	.size .L.idsep, .L.idsepEnd - .L.idsep
397
398	.type stack1,%object
399	.local stack1
400	.comm stack1,STACK_SIZE,1
401	.type tls1,%object
402	.local tls1
403	.comm tls1,TLS_SIZE,1
404
405	.type stack2,%object
406	.local stack2
407	.comm stack2,STACK_SIZE,1
408	.type tls2,%object
409	.local tls2
410	.comm tls2,TLS_SIZE,1
411