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