1 // vim:noexpandtab
2 #include <stdint.h>
3 #include <stdlib.h>
4 #include <stdio.h>
5 #include <stdbool.h>
6 #include <stdarg.h>
7 #include <errno.h>
8 #include <string.h>
9 #include <unistd.h>
10 #include <sys/syscall.h>
11 #include <sys/ulock.h>
12
13 #include "turnstile_multihop_types.h"
14
15 typedef _Atomic(u32) lock_t;
16
17 __inline static void
yield(void)18 yield(void)
19 {
20 #if !defined(__x86_64__) && !defined(__i386__)
21 __asm volatile ("yield");
22 #else
23 __asm volatile ("pause");
24 #endif
25 }
26
27 __inline static void
wfe(void)28 wfe(void)
29 {
30 #if !defined(__x86_64__) && !defined(__i386__)
31 __asm volatile ("wfe");
32 #else
33 __asm volatile ("pause");
34 #endif
35 }
36
37 __inline static void
wfi(void)38 wfi(void)
39 {
40 #if !defined(__x86_64__) && !defined(__i386__)
41 __asm volatile ("wfi");
42 #else
43 __asm volatile ("pause");
44 #endif
45 }
46
47 __inline static void
sev(void)48 sev(void)
49 {
50 #if !defined(__x86_64__) && !defined(__i386__)
51 __asm volatile ("sev");
52 #endif
53 }
54
55 #include <os/tsd.h>
56
57 #ifndef __TSD_MACH_THREAD_SELF
58 #define __TSD_MACH_THREAD_SELF 3
59 #endif
60
61 __inline static mach_port_name_t
_os_get_self(void)62 _os_get_self(void)
63 {
64 mach_port_name_t self = (mach_port_name_t)(uintptr_t)(void *)_os_tsd_get_direct(__TSD_MACH_THREAD_SELF);
65 return self;
66 }
67
68 #define ULL_WAITERS 1U
69
70 static uint32_t lock_no_wait[4] = { 0, 0, 0, 0};
71 static uint32_t lock_wait[4] = { 0, 0, 0, 0};
72
73 static mach_port_name_t main_thread_name = 0;
74
75 __inline static void
ull_lock(lock_t * lock,int id,uint opcode,uint flags)76 ull_lock(lock_t *lock, int id, uint opcode, uint flags)
77 {
78 u32 thread_id = _os_get_self() & ~0x3u;
79 u32 ull_locked = (opcode == UL_UNFAIR_LOCK) ? thread_id : 4u;
80 u32 mach_id = _os_get_self() >> 2;
81 u32 prev;
82 bool succeeded = false;
83 bool waiters = false;
84 bool called_wait = false;
85 u32 count = 0;
86
87 do {
88 count++;
89 if ((count % 100000) == 0) {
90 printf("[%d,%d]%s>top of loop count=%d\n", id, mach_id, __FUNCTION__, count);
91 }
92 u32 new = waiters ? (ULL_WAITERS | ull_locked) : ull_locked;
93 prev = 0;
94 __c11_atomic_compare_exchange_strong(lock, &prev, new, __ATOMIC_ACQUIRE, __ATOMIC_RELAXED);
95 if (prev == 0) {
96 /* Was unlocked, now locked */
97 succeeded = true;
98 break;
99 }
100
101 u32 value = prev;
102 if (!(value & ULL_WAITERS)) {
103 new = value | ULL_WAITERS;
104 __c11_atomic_compare_exchange_strong(lock, &prev, new, __ATOMIC_RELAXED, __ATOMIC_RELAXED);
105 if (prev == value) {
106 /* succeeded in setting ULL_WAITERS */
107 value = new;
108 } else if (prev & ULL_WAITERS) {
109 /* Didn't succeed, but someone else already set ULL_WAITERS */
110 value = prev;
111 } else {
112 /* Something changed under us, so try again */
113 if (count % 100000 == 0) {
114 printf("[%d,%d]%s>Something changed under us, prev=%d\n", id, mach_id, __FUNCTION__, prev);
115 }
116 continue;
117 }
118 }
119 /* Locked with waiters indication, so block */
120 int ret = __ulock_wait(flags | opcode, lock, value, 0);
121 called_wait = true;
122 if (ret < 0) {
123 if (flags & ULF_NO_ERRNO) {
124 errno = -ret;
125 }
126 if (errno == EFAULT) {
127 continue;
128 }
129 printf("[%d,%d]%s>ull_wait() error: %s\n", id, mach_id, __FUNCTION__, strerror(errno));
130 exit(1);
131 }
132 waiters = (ret > 0);
133
134 if (count % 100000 == 0) {
135 printf("[%d,%d]%s>bottom of loop prev=%d\n", id, mach_id, __FUNCTION__, prev);
136 }
137 } while (!succeeded);
138
139 if (called_wait) {
140 lock_wait[id]++;
141 } else {
142 lock_no_wait[id]++;
143 }
144 }
145
146 static uint32_t unlock_no_waiters[4] = { 0, 0, 0, 0};
147 static uint32_t unlock_waiters[4] = { 0, 0, 0, 0 };
148 static uint32_t unlock_waiters_gone[4] = { 0, 0, 0, 0 };
149 static uint32_t unlock_waiters_wake_thread[4] = { 0, 0, 0, 0 };
150
151 __inline static void
ull_unlock(lock_t * lock,int id,uint opcode,uint flags)152 ull_unlock(lock_t *lock, int id, uint opcode, uint flags)
153 {
154 u32 thread_id = _os_get_self() & ~0x3u;
155 u32 ull_locked = (opcode == UL_UNFAIR_LOCK) ? thread_id : 4u;
156 u32 mach_id = _os_get_self() >> 2;
157 u32 prev = ull_locked;
158 __c11_atomic_compare_exchange_strong(lock, &prev, 0, __ATOMIC_RELEASE, __ATOMIC_RELAXED);
159 if (prev == ull_locked) {
160 unlock_no_waiters[id]++;
161 return;
162 }
163
164 if (prev == 0) {
165 printf("%s>already unlocked\n", __FUNCTION__);
166 exit(1);
167 }
168
169 if (prev == (ULL_WAITERS | ull_locked)) {
170 /* locked with waiters */
171 __c11_atomic_store(lock, 0, __ATOMIC_SEQ_CST);
172
173 if ((flags & ULF_WAKE_THREAD) && (_os_get_self() == main_thread_name)) {
174 flags &= ~(uint)ULF_WAKE_THREAD;
175 }
176 int ret = __ulock_wake((flags | opcode), lock, main_thread_name);
177 if ((ret < 0) && (flags & ULF_NO_ERRNO)) {
178 errno = -ret;
179 }
180 if ((flags & ULF_WAKE_THREAD) && (ret < 0) && (errno == EALREADY)) {
181 flags &= ~(uint)ULF_WAKE_THREAD;
182 ret = __ulock_wake((flags | opcode), lock, 0);
183 if ((ret < 0) && (flags & ULF_NO_ERRNO)) {
184 errno = -ret;
185 }
186 } else if ((flags & ULF_WAKE_THREAD) && (ret == 0)) {
187 unlock_waiters_wake_thread[id]++;
188 }
189 if (ret < 0) {
190 if (errno == ENOENT) {
191 unlock_waiters_gone[id]++;
192 } else {
193 printf("[%d,%d]%s>ull_wake() error: %s\n", id, mach_id, __FUNCTION__, strerror(errno));
194 exit(1);
195 }
196 }
197 unlock_waiters[id]++;
198 } else {
199 printf("%s>unexpected lock value %d\n", __FUNCTION__, prev);
200 exit(1);
201 }
202 }
203