diff options
Diffstat (limited to 'firmware/kernel')
-rw-r--r-- | firmware/kernel/include/mrsw_lock.h | 54 | ||||
-rw-r--r-- | firmware/kernel/mrsw_lock.c | 343 |
2 files changed, 397 insertions, 0 deletions
diff --git a/firmware/kernel/include/mrsw_lock.h b/firmware/kernel/include/mrsw_lock.h new file mode 100644 index 0000000000..fbfe1d405d --- /dev/null +++ b/firmware/kernel/include/mrsw_lock.h | |||
@@ -0,0 +1,54 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2014 by Michael Sevakis | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #ifndef MRSW_LOCK_H | ||
22 | #define MRSW_LOCK_H | ||
23 | |||
24 | /* Multi-reader, single-writer object that allows mutltiple readers or a | ||
25 | * single writer thread access to a critical section. | ||
26 | * | ||
27 | * Readers and writers, as with a mutex, may claim the lock recursively for | ||
28 | * the same type of access they were already granted. | ||
29 | * | ||
30 | * Writers are trivially granted read access by ignoring the request; the | ||
31 | * object is not changed. | ||
32 | * | ||
33 | * Reader promotion to writer is NOT supported and a reader attempting write | ||
34 | * access will result in a deadlock. That could change but for now, be warned. | ||
35 | * | ||
36 | * Full priority inheritance is implemented. | ||
37 | */ | ||
38 | struct mrsw_lock | ||
39 | { | ||
40 | int volatile count; /* rd/wr counter; >0 = reader(s), <0 = writer */ | ||
41 | struct thread_entry *queue; | ||
42 | struct blocker_splay splay; /* priority inheritance info | ||
43 | for waiters */ | ||
44 | uint8_t rdrecursion[MAXTHREADS]; /* per-thread reader recursion counts */ | ||
45 | IF_COP( struct corelock cl; ) | ||
46 | }; | ||
47 | |||
48 | void mrsw_init(struct mrsw_lock *mrsw); | ||
49 | void mrsw_read_acquire(struct mrsw_lock *mrsw); | ||
50 | void mrsw_read_release(struct mrsw_lock *mrsw); | ||
51 | void mrsw_write_acquire(struct mrsw_lock *mrsw); | ||
52 | void mrsw_write_release(struct mrsw_lock *mrsw); | ||
53 | |||
54 | #endif /* MRSW_LOCK_H */ | ||
diff --git a/firmware/kernel/mrsw_lock.c b/firmware/kernel/mrsw_lock.c new file mode 100644 index 0000000000..42f43caec3 --- /dev/null +++ b/firmware/kernel/mrsw_lock.c | |||
@@ -0,0 +1,343 @@ | |||
1 | /*************************************************************************** | ||
2 | * __________ __ ___. | ||
3 | * Open \______ \ ____ ____ | | _\_ |__ _______ ___ | ||
4 | * Source | _// _ \_/ ___\| |/ /| __ \ / _ \ \/ / | ||
5 | * Jukebox | | ( <_> ) \___| < | \_\ ( <_> > < < | ||
6 | * Firmware |____|_ /\____/ \___ >__|_ \|___ /\____/__/\_ \ | ||
7 | * \/ \/ \/ \/ \/ | ||
8 | * $Id$ | ||
9 | * | ||
10 | * Copyright (C) 2014 by Michael Sevakis | ||
11 | * | ||
12 | * This program is free software; you can redistribute it and/or | ||
13 | * modify it under the terms of the GNU General Public License | ||
14 | * as published by the Free Software Foundation; either version 2 | ||
15 | * of the License, or (at your option) any later version. | ||
16 | * | ||
17 | * This software is distributed on an "AS IS" basis, WITHOUT WARRANTY OF ANY | ||
18 | * KIND, either express or implied. | ||
19 | * | ||
20 | ****************************************************************************/ | ||
21 | #include <string.h> | ||
22 | #include "config.h" | ||
23 | #include "system.h" | ||
24 | #include "thread.h" | ||
25 | #include "kernel.h" | ||
26 | #include "kernel-internal.h" | ||
27 | |||
28 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
29 | |||
30 | static FORCE_INLINE void | ||
31 | mrsw_reader_claim(struct mrsw_lock *mrsw, struct thread_entry *current, | ||
32 | int count, unsigned int slotnum) | ||
33 | { | ||
34 | /* no need to lock this; if a reader can claim, noone is in the queue */ | ||
35 | threadbit_set_bit(&mrsw->splay.mask, slotnum); | ||
36 | mrsw->splay.blocker.thread = count == 1 ? current : NULL; | ||
37 | } | ||
38 | |||
39 | static FORCE_INLINE void | ||
40 | mrsw_reader_relinquish(struct mrsw_lock *mrsw, struct thread_entry *current, | ||
41 | int count, unsigned int slotnum) | ||
42 | { | ||
43 | /* If no writer is queued or has ownership then noone is queued; | ||
44 | if a writer owns it, then the reader would be blocked instead. | ||
45 | Therefore, if the queue has threads, then the next after the | ||
46 | owning readers is a writer and this is not the last reader. */ | ||
47 | if (mrsw->queue) | ||
48 | { | ||
49 | corelock_lock(&mrsw->splay.cl); | ||
50 | } | ||
51 | |||
52 | threadbit_clear_bit(&mrsw->splay.mask, slotnum); | ||
53 | |||
54 | if (count == 0) | ||
55 | { | ||
56 | /* There is noone waiting; we'd be calling mrsw_wakeup_writer() | ||
57 | at this time instead */ | ||
58 | mrsw->splay.blocker.thread = NULL; | ||
59 | return; | ||
60 | } | ||
61 | |||
62 | if (count == 1) | ||
63 | { | ||
64 | KERNEL_ASSERT(threadbit_popcount(&mrsw->splay.mask) == 1, | ||
65 | "mrsw_reader_relinquish() - " | ||
66 | "threadbits has wrong popcount: %d\n", | ||
67 | threadbit_popcount(&mrsw->splay.mask)); | ||
68 | /* switch owner to sole remaining reader */ | ||
69 | slotnum = threadbit_ffs(&mrsw->splay.mask); | ||
70 | mrsw->splay.blocker.thread = thread_id_entry(slotnum); | ||
71 | } | ||
72 | |||
73 | if (mrsw->queue) | ||
74 | { | ||
75 | priority_disinherit(current, &mrsw->splay.blocker); | ||
76 | corelock_unlock(&mrsw->splay.cl); | ||
77 | } | ||
78 | } | ||
79 | |||
80 | static FORCE_INLINE unsigned int | ||
81 | mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw, unsigned int slotnum) | ||
82 | { | ||
83 | threadbit_clear_bit(&mrsw->splay.mask, slotnum); | ||
84 | return wakeup_thread(&mrsw->queue, WAKEUP_TRANSFER); | ||
85 | } | ||
86 | |||
87 | static FORCE_INLINE unsigned int | ||
88 | mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw) | ||
89 | { | ||
90 | return wakeup_thread(&mrsw->queue, WAKEUP_TRANSFER); | ||
91 | } | ||
92 | |||
93 | static FORCE_INLINE unsigned int | ||
94 | mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) | ||
95 | { | ||
96 | unsigned int result = wakeup_thread(&mrsw->queue, WAKEUP_TRANSFER_MULTI); | ||
97 | mrsw->count = thread_self_entry()->retval; | ||
98 | return result; | ||
99 | } | ||
100 | |||
101 | #else /* !HAVE_PRIORITY_SCHEDULING */ | ||
102 | |||
103 | #define mrsw_reader_claim(mrsw, current, count, slotnum) \ | ||
104 | do {} while (0) | ||
105 | |||
106 | #define mrsw_reader_relinquish(mrsw, current, count, slotnum) \ | ||
107 | do {} while (0) | ||
108 | |||
109 | static FORCE_INLINE unsigned int | ||
110 | mrsw_reader_wakeup_writer(struct mrsw_lock *mrsw) | ||
111 | { | ||
112 | mrsw->splay.blocker.thread = mrsw->queue; | ||
113 | return wakeup_thread(&mrsw->queue); | ||
114 | } | ||
115 | |||
116 | static FORCE_INLINE unsigned int | ||
117 | mrsw_writer_wakeup_writer(struct mrsw_lock *mrsw) | ||
118 | { | ||
119 | mrsw->splay.blocker.thread = mrsw->queue; | ||
120 | return wakeup_thread(&mrsw->queue); | ||
121 | } | ||
122 | |||
123 | static FORCE_INLINE unsigned int | ||
124 | mrsw_writer_wakeup_readers(struct mrsw_lock *mrsw) | ||
125 | { | ||
126 | mrsw->splay.blocker.thread = NULL; | ||
127 | for (int count = 0; mrsw->queue && mrsw->queue->retval != 0; count++) | ||
128 | wakeup_thread(&mrsw->queue); | ||
129 | return THREAD_OK; | ||
130 | } | ||
131 | |||
132 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
133 | |||
134 | /** Public interface **/ | ||
135 | |||
136 | /* Initializes a multi-reader, single-writer object */ | ||
137 | void mrsw_init(struct mrsw_lock *mrsw) | ||
138 | { | ||
139 | mrsw->count = 0; | ||
140 | mrsw->queue = NULL; | ||
141 | mrsw->splay.blocker.thread = NULL; | ||
142 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
143 | mrsw->splay.blocker.priority = PRIORITY_IDLE; | ||
144 | threadbit_clear(&mrsw->splay.mask); | ||
145 | corelock_init(&mrsw->splay.cl); | ||
146 | memset(mrsw->rdrecursion, 0, sizeof (mrsw->rdrecursion)); | ||
147 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
148 | corelock_init(&mrsw->cl); | ||
149 | } | ||
150 | |||
151 | /* Request reader thread lock. Any number of reader threads may enter which | ||
152 | * also locks-out all writer threads. Same thread may safely acquire read | ||
153 | * access recursively. The current writer is ignored and gets access. */ | ||
154 | void mrsw_read_acquire(struct mrsw_lock *mrsw) | ||
155 | { | ||
156 | struct thread_entry *current = thread_self_entry(); | ||
157 | |||
158 | if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 )) | ||
159 | return; /* Read request while holding write access; pass */ | ||
160 | |||
161 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
162 | /* Track recursion counts for each thread: | ||
163 | IF_PRIO, mrsw->count just tracks unique owners */ | ||
164 | unsigned int slotnum = THREAD_ID_SLOT(current->id); | ||
165 | if (threadbit_test_bit(&mrsw->splay.mask, slotnum)) | ||
166 | { | ||
167 | KERNEL_ASSERT(mrsw->rdrecursion[slotnum] < UINT8_MAX, | ||
168 | "mrsw_read_acquire() - " | ||
169 | "Thread %s did too many claims\n", | ||
170 | current->name); | ||
171 | mrsw->rdrecursion[slotnum]++; | ||
172 | return; | ||
173 | } | ||
174 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
175 | |||
176 | corelock_lock(&mrsw->cl); | ||
177 | |||
178 | int count = mrsw->count; | ||
179 | |||
180 | if (LIKELY(count >= 0 && !mrsw->queue)) | ||
181 | { | ||
182 | /* Lock open to readers: | ||
183 | IFN_PRIO, mrsw->count tracks reader recursion */ | ||
184 | mrsw->count = ++count; | ||
185 | mrsw_reader_claim(mrsw, current, count, slotnum); | ||
186 | corelock_unlock(&mrsw->cl); | ||
187 | return; | ||
188 | } | ||
189 | |||
190 | /* A writer owns it or is waiting; block... */ | ||
191 | IF_COP( current->obj_cl = &mrsw->cl; ) | ||
192 | IF_PRIO( current->blocker = &mrsw->splay.blocker; ) | ||
193 | current->bqp = &mrsw->queue; | ||
194 | current->retval = 1; /* indicate multi-wake candidate */ | ||
195 | |||
196 | disable_irq(); | ||
197 | block_thread(current, TIMEOUT_BLOCK); | ||
198 | |||
199 | corelock_unlock(&mrsw->cl); | ||
200 | |||
201 | /* ...and turn control over to next thread */ | ||
202 | switch_thread(); | ||
203 | } | ||
204 | |||
205 | /* Release reader thread lockout of writer thread. The last reader to | ||
206 | * leave opens up access to writer threads. The current writer is ignored. */ | ||
207 | void mrsw_read_release(struct mrsw_lock *mrsw) | ||
208 | { | ||
209 | struct thread_entry *current = thread_self_entry(); | ||
210 | |||
211 | if (current == mrsw->splay.blocker.thread IF_PRIO( && mrsw->count < 0 )) | ||
212 | return; /* Read release while holding write access; ignore */ | ||
213 | |||
214 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
215 | unsigned int slotnum = THREAD_ID_SLOT(current->id); | ||
216 | KERNEL_ASSERT(threadbit_test_bit(&mrsw->splay.mask, slotnum), | ||
217 | "mrsw_read_release() -" | ||
218 | " thread '%s' not reader\n", | ||
219 | current->name); | ||
220 | |||
221 | uint8_t *rdcountp = &mrsw->rdrecursion[slotnum]; | ||
222 | unsigned int rdcount = *rdcountp; | ||
223 | if (rdcount > 0) | ||
224 | { | ||
225 | /* Reader is releasing recursive claim */ | ||
226 | *rdcountp = rdcount - 1; | ||
227 | return; | ||
228 | } | ||
229 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
230 | |||
231 | corelock_lock(&mrsw->cl); | ||
232 | int count = mrsw->count; | ||
233 | |||
234 | KERNEL_ASSERT(count > 0, "mrsw_read_release() - no readers!\n"); | ||
235 | |||
236 | unsigned int result = THREAD_NONE; | ||
237 | const int oldlevel = disable_irq_save(); | ||
238 | |||
239 | if (--count == 0 && mrsw->queue) | ||
240 | { | ||
241 | /* No readers remain and a writer is waiting */ | ||
242 | mrsw->count = -1; | ||
243 | result = mrsw_reader_wakeup_writer(mrsw IF_PRIO(, slotnum)); | ||
244 | } | ||
245 | else | ||
246 | { | ||
247 | /* Giving up readership; we may be the last, or not */ | ||
248 | mrsw->count = count; | ||
249 | mrsw_reader_relinquish(mrsw, current, count, slotnum); | ||
250 | } | ||
251 | |||
252 | restore_irq(oldlevel); | ||
253 | corelock_unlock(&mrsw->cl); | ||
254 | |||
255 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
256 | if (result & THREAD_SWITCH) | ||
257 | switch_thread(); | ||
258 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
259 | (void)result; | ||
260 | } | ||
261 | |||
262 | /* Acquire writer thread lock which provides exclusive access. If a thread | ||
263 | * that is holding read access calls this it will deadlock. The writer may | ||
264 | * safely call recursively. */ | ||
265 | void mrsw_write_acquire(struct mrsw_lock *mrsw) | ||
266 | { | ||
267 | struct thread_entry *current = thread_self_entry(); | ||
268 | |||
269 | if (current == mrsw->splay.blocker.thread) | ||
270 | { | ||
271 | /* Current thread already has write access */ | ||
272 | mrsw->count--; | ||
273 | return; | ||
274 | } | ||
275 | |||
276 | corelock_lock(&mrsw->cl); | ||
277 | |||
278 | int count = mrsw->count; | ||
279 | |||
280 | if (LIKELY(count == 0)) | ||
281 | { | ||
282 | /* Lock is open to a writer */ | ||
283 | mrsw->count = -1; | ||
284 | mrsw->splay.blocker.thread = current; | ||
285 | corelock_unlock(&mrsw->cl); | ||
286 | return; | ||
287 | } | ||
288 | |||
289 | /* Readers present or a writer owns it - block... */ | ||
290 | IF_COP( current->obj_cl = &mrsw->cl; ) | ||
291 | IF_PRIO( current->blocker = &mrsw->splay.blocker; ) | ||
292 | current->bqp = &mrsw->queue; | ||
293 | current->retval = 0; /* indicate single-wake candidate */ | ||
294 | |||
295 | disable_irq(); | ||
296 | block_thread(current, TIMEOUT_BLOCK); | ||
297 | |||
298 | corelock_unlock(&mrsw->cl); | ||
299 | |||
300 | /* ...and turn control over to next thread */ | ||
301 | switch_thread(); | ||
302 | } | ||
303 | |||
304 | /* Release writer thread lock and open the lock to readers and writers */ | ||
305 | void mrsw_write_release(struct mrsw_lock *mrsw) | ||
306 | { | ||
307 | KERNEL_ASSERT(thread_self_entry() == mrsw->splay.blocker.thread, | ||
308 | "mrsw_write_release->wrong thread (%s != %s)\n", | ||
309 | thread_self_entry()->name, | ||
310 | mrsw->splay.blocker.thread->name); | ||
311 | |||
312 | int count = mrsw->count; | ||
313 | if (count < -1) | ||
314 | { | ||
315 | /* This thread still owns write lock */ | ||
316 | mrsw->count = count + 1; | ||
317 | return; | ||
318 | } | ||
319 | |||
320 | unsigned int result = THREAD_NONE; | ||
321 | |||
322 | corelock_lock(&mrsw->cl); | ||
323 | const int oldlevel = disable_irq_save(); | ||
324 | |||
325 | if (mrsw->queue == NULL) /* 'count' becomes zero */ | ||
326 | { | ||
327 | mrsw->splay.blocker.thread = NULL; | ||
328 | mrsw->count = 0; | ||
329 | } | ||
330 | else if (mrsw->queue->retval == 0) /* 'count' stays -1 */ | ||
331 | result = mrsw_writer_wakeup_writer(mrsw); | ||
332 | else /* 'count' becomes # of readers */ | ||
333 | result = mrsw_writer_wakeup_readers(mrsw); | ||
334 | |||
335 | restore_irq(oldlevel); | ||
336 | corelock_unlock(&mrsw->cl); | ||
337 | |||
338 | #ifdef HAVE_PRIORITY_SCHEDULING | ||
339 | if (result & THREAD_SWITCH) | ||
340 | switch_thread(); | ||
341 | #endif /* HAVE_PRIORITY_SCHEDULING */ | ||
342 | (void)result; | ||
343 | } | ||