1//------------------------------------------------------------------------------
2// Copyright (c) 2016 by Lukasz Janyst <lukasz@jany.st>
3//------------------------------------------------------------------------------
4// This file is part of thread-bites.
5//
6// thread-bites is free software: you can redistribute it and/or modify
7// it under the terms of the GNU General Public License as published by
8// the Free Software Foundation, either version 3 of the License, or
9// (at your option) any later version.
10//
11// thread-bites is distributed in the hope that it will be useful,
12// but WITHOUT ANY WARRANTY; without even the implied warranty of
13// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14// GNU General Public License for more details.
15//
16// You should have received a copy of the GNU General Public License
17// along with thread-bites. If not, see <http://www.gnu.org/licenses/>.
18//------------------------------------------------------------------------------
19
20#include "tb.h"
21#include "tb-private.h"
22
23#include <linux/futex.h>
24#include <string.h>
25
26//------------------------------------------------------------------------------
27// Lock function prototypes
28//------------------------------------------------------------------------------
29static int lock_normal(tbthread_mutex_t *mutex);
30static int trylock_normal(tbthread_mutex_t *mutex);
31static int unlock_normal(tbthread_mutex_t *mutex);
32
33static int lock_errorcheck(tbthread_mutex_t *mutex);
34static int trylock_errorcheck(tbthread_mutex_t *mutex);
35static int unlock_errorcheck(tbthread_mutex_t *mutex);
36
37static int lock_recursive(tbthread_mutex_t *mutex);
38static int trylock_recursive(tbthread_mutex_t *mutex);
39static int unlock_recursive(tbthread_mutex_t *mutex);
40
41static int lock_prio_none(tbthread_mutex_t *mutex);
42static int trylock_prio_none(tbthread_mutex_t *mutex);
43static int unlock_prio_none(tbthread_mutex_t *mutex);
44
45static int lock_prio_inherit(tbthread_mutex_t *mutex);
46static int trylock_prio_inherit(tbthread_mutex_t *mutex);
47static int unlock_prio_inherit(tbthread_mutex_t *mutex);
48
49static int lock_prio_protect(tbthread_mutex_t *mutex);
50static int trylock_prio_protect(tbthread_mutex_t *mutex);
51static int unlock_prio_protect(tbthread_mutex_t *mutex);
52
53//------------------------------------------------------------------------------
54// Mutex function tables
55//------------------------------------------------------------------------------
56static int (*lockers[])(tbthread_mutex_t *) = {
57 lock_normal,
58 lock_errorcheck,
59 lock_recursive,
60 lock_prio_none,
61 lock_prio_inherit,
62 lock_prio_protect
63};
64
65static int (*trylockers[])(tbthread_mutex_t *) = {
66 trylock_normal,
67 trylock_errorcheck,
68 trylock_recursive,
69 trylock_prio_none,
70 trylock_prio_inherit,
71 trylock_prio_protect
72};
73
74static int (*unlockers[])(tbthread_mutex_t *) = {
75 unlock_normal,
76 unlock_errorcheck,
77 unlock_recursive,
78 unlock_prio_none,
79 unlock_prio_inherit,
80 unlock_prio_protect
81};
82
83//------------------------------------------------------------------------------
84// Low level locking
85//------------------------------------------------------------------------------
86void tb_futex_lock(int *futex)
87{
88 while(1) {
89 if(__sync_bool_compare_and_swap(futex, 0, 1))
90 return;
91 SYSCALL3(__NR_futex, futex, FUTEX_WAIT, 1);
92 }
93}
94
95int tb_futex_trylock(int *futex)
96{
97 if(__sync_bool_compare_and_swap(futex, 0, 1))
98 return 0;
99 return -EBUSY;
100}
101
102void tb_futex_unlock(int *futex)
103{
104 if(__sync_bool_compare_and_swap(futex, 1, 0))
105 SYSCALL3(__NR_futex, futex, FUTEX_WAKE, 1);
106}
107
108//------------------------------------------------------------------------------
109// Normal mutex
110//------------------------------------------------------------------------------
111static int lock_normal(tbthread_mutex_t *mutex)
112{
113 return (*lockers[mutex->protocol])(mutex);
114}
115
116static int trylock_normal(tbthread_mutex_t *mutex)
117{
118 return (*trylockers[mutex->protocol])(mutex);
119}
120
121static int unlock_normal(tbthread_mutex_t *mutex)
122{
123 return (*unlockers[mutex->protocol])(mutex);
124}
125
126//------------------------------------------------------------------------------
127// Errorcheck mutex
128//------------------------------------------------------------------------------
129static int lock_errorcheck(tbthread_mutex_t *mutex)
130{
131 tbthread_t self = tbthread_self();
132 if(mutex->owner == self)
133 return -EDEADLK;
134 (*lockers[mutex->protocol])(mutex);
135 return 0;
136}
137
138static int trylock_errorcheck(tbthread_mutex_t *mutex)
139{
140 return (*trylockers[mutex->protocol])(mutex);
141}
142
143static int unlock_errorcheck(tbthread_mutex_t *mutex)
144{
145 if(mutex->owner != tbthread_self() || mutex->futex == 0)
146 return -EPERM;
147 (*unlockers[mutex->protocol])(mutex);
148 return 0;
149}
150
151//------------------------------------------------------------------------------
152// Recursive mutex
153//------------------------------------------------------------------------------
154static int lock_recursive(tbthread_mutex_t *mutex)
155{
156 tbthread_t self = tbthread_self();
157 if(mutex->owner != self) {
158 (*lockers[mutex->protocol])(mutex);
159 mutex->owner = self;
160 }
161 if(mutex->counter == (uint64_t)-1)
162 return -EAGAIN;
163 ++mutex->counter;
164 return 0;
165}
166
167static int trylock_recursive(tbthread_mutex_t *mutex)
168{
169 tbthread_t self = tbthread_self();
170 if(mutex->owner != self && (*trylockers[mutex->protocol])(mutex))
171 return -EBUSY;
172
173 if(mutex->owner != self) {
174 mutex->owner = self;
175 mutex->counter = 1;
176 return 0;
177 }
178
179 if(mutex->counter == (uint64_t)-1)
180 return -EAGAIN;
181
182 ++mutex->counter;
183 return 0;
184}
185
186static int unlock_recursive(tbthread_mutex_t *mutex)
187{
188 if(mutex->owner != tbthread_self())
189 return -EPERM;
190 --mutex->counter;
191 if(mutex->counter == 0) {
192 mutex->owner = 0;
193 return (*unlockers[mutex->protocol])(mutex);
194 }
195 return 0;
196}
197
198//------------------------------------------------------------------------------
199// Priority none
200//------------------------------------------------------------------------------
201static int lock_prio_none(tbthread_mutex_t *mutex)
202{
203 tb_futex_lock(&mutex->futex);
204 mutex->owner = tbthread_self();
205 return 0;
206}
207
208static int trylock_prio_none(tbthread_mutex_t *mutex)
209{
210 int ret = tb_futex_trylock(&mutex->futex);
211 if(ret == 0)
212 mutex->owner = tbthread_self();
213 return ret;
214}
215
216static int unlock_prio_none(tbthread_mutex_t *mutex)
217{
218 mutex->owner = 0;
219 tb_futex_unlock(&mutex->futex);
220 return 0;
221}
222
223//------------------------------------------------------------------------------
224// Priority inherit
225//------------------------------------------------------------------------------
226static int lock_prio_inherit(tbthread_mutex_t *mutex)
227{
228 tbthread_t self = tbthread_self();
229
230 while(1) {
231 int locked = 0;
232 tb_futex_lock(&mutex->internal_futex);
233 if(mutex->futex == 0) {
234 locked = 1;
235 mutex->owner = self;
236 mutex->futex = 1;
237 tb_inherit_mutex_add(mutex);
238 }
239 else
240 tb_inherit_mutex_sched(mutex, self);
241 tb_futex_unlock(&mutex->internal_futex);
242 if(locked)
243 return 0;
244 SYSCALL3(__NR_futex, &mutex->futex, FUTEX_WAIT, 1);
245 }
246}
247
248static int trylock_prio_inherit(tbthread_mutex_t *mutex)
249{
250 tbthread_t self = tbthread_self();
251
252 int locked = 0;
253 tb_futex_lock(&mutex->internal_futex);
254 if(mutex->futex == 0) {
255 locked = 1;
256 mutex->owner = self;
257 mutex->futex = 1;
258 tb_inherit_mutex_add(mutex);
259 }
260 tb_futex_unlock(&mutex->internal_futex);
261 if(locked)
262 return 0;
263 return -EBUSY;
264}
265
266static int unlock_prio_inherit(tbthread_mutex_t *mutex)
267{
268 tb_futex_lock(&mutex->internal_futex);
269 tb_inherit_mutex_unsched(mutex);
270 mutex->owner = 0;
271 mutex->futex = 0;
272 SYSCALL3(__NR_futex, &mutex->futex, FUTEX_WAKE, 1);
273 tb_futex_unlock(&mutex->internal_futex);
274 return 0;
275}
276
277//------------------------------------------------------------------------------
278// Priority protect
279//------------------------------------------------------------------------------
280static int lock_prio_protect(tbthread_mutex_t *mutex)
281{
282 lock_prio_none(mutex);
283 tb_protect_mutex_sched(mutex);
284 return 0;
285}
286
287static int trylock_prio_protect(tbthread_mutex_t *mutex)
288{
289 int ret = trylock_prio_none(mutex);
290 if(ret == 0)
291 tb_protect_mutex_sched(mutex);
292 return ret;
293}
294
295static int unlock_prio_protect(tbthread_mutex_t *mutex)
296{
297 tbthread_t self = tbthread_self();
298 tb_protect_mutex_unsched(mutex);
299 unlock_prio_none(mutex);
300 return 0;
301}
302
303//------------------------------------------------------------------------------
304// Init attributes
305//------------------------------------------------------------------------------
306int tbthread_mutexattr_init(tbthread_mutexattr_t *attr)
307{
308 memset(attr, 0, sizeof(tbthread_mutexattr_t));
309 attr->type = TBTHREAD_MUTEX_DEFAULT;
310 attr->protocol = TBTHREAD_PRIO_NONE;
311 return 0;
312}
313
314//------------------------------------------------------------------------------
315// Destroy attributes - no op
316//------------------------------------------------------------------------------
317int tbthread_mutexattr_destroy(tbthread_mutexattr_t *attr)
318{
319 return 0;
320}
321
322//------------------------------------------------------------------------------
323// Set type
324//------------------------------------------------------------------------------
325int tbthread_mutexattr_gettype(const tbthread_mutexattr_t *attr, int *type)
326{
327 *type = attr->type;
328 return 0;
329}
330
331//------------------------------------------------------------------------------
332// Get type
333//------------------------------------------------------------------------------
334int tbthread_mutexattr_settype(tbthread_mutexattr_t *attr, int type)
335{
336 if(type < TBTHREAD_MUTEX_NORMAL || type > TBTHREAD_MUTEX_RECURSIVE)
337 return -EINVAL;
338 attr->type = type;
339}
340
341//------------------------------------------------------------------------------
342// Initialize the mutex
343//------------------------------------------------------------------------------
344int tbthread_mutex_init(tbthread_mutex_t *mutex,
345 const tbthread_mutexattr_t *attr)
346{
347 memset(mutex, 0, sizeof(tbthread_mutex_t));
348 uint8_t type = TBTHREAD_MUTEX_DEFAULT;
349 uint8_t protocol = TBTHREAD_PRIO_NONE;
350 uint16_t sched_info = 0;
351 if(attr) {
352 type = attr->type;
353 protocol = attr->protocol;
354 if(protocol == TBTHREAD_PRIO_PROTECT && attr->prioceiling != 0)
355 sched_info = SCHED_INFO_PACK(SCHED_FIFO, attr->prioceiling);
356 }
357 mutex->type = type;
358 mutex->protocol = protocol;
359 mutex->sched_info = sched_info;
360}
361
362//------------------------------------------------------------------------------
363// Destroy the mutex - no op
364//------------------------------------------------------------------------------
365int tbthread_mutex_destroy(tbthread_mutex_t *mutex)
366{
367 return 0;
368}
369
370//------------------------------------------------------------------------------
371// Lock the mutex
372//------------------------------------------------------------------------------
373int tbthread_mutex_lock(tbthread_mutex_t *mutex)
374{
375 return (*lockers[mutex->type])(mutex);
376}
377
378//------------------------------------------------------------------------------
379// Try locking the mutex
380//------------------------------------------------------------------------------
381int tbthread_mutex_trylock(tbthread_mutex_t *mutex)
382{
383 return (*trylockers[mutex->type])(mutex);
384}
385
386//------------------------------------------------------------------------------
387// Unlock the mutex
388//------------------------------------------------------------------------------
389int tbthread_mutex_unlock(tbthread_mutex_t *mutex)
390{
391 return (*unlockers[mutex->type])(mutex);;
392}
393
394//------------------------------------------------------------------------------
395// Set priority ceiling
396//------------------------------------------------------------------------------
397int tbthread_mutexattr_setprioceiling(tbthread_mutexattr_t *attr, int ceiling)
398{
399 if(ceiling < 0 || ceiling > 99)
400 return -EINVAL;
401 attr->prioceiling = ceiling;
402 return 0;
403}
404
405//------------------------------------------------------------------------------
406// Set protocol
407//------------------------------------------------------------------------------
408int tbthread_mutexattr_setprotocol(tbthread_mutexattr_t *attr, int protocol)
409{
410 if(protocol != TBTHREAD_PRIO_NONE && protocol != TBTHREAD_PRIO_INHERIT &&
411 protocol != TBTHREAD_PRIO_PROTECT)
412 return -EINVAL;
413 attr->protocol = protocol;
414 return 0;
415}
416
417//------------------------------------------------------------------------------
418// Get priority ceiling
419//------------------------------------------------------------------------------
420int tbthread_mutex_getprioceiling(const tbthread_mutex_t *mutex, int *ceiling)
421{
422 if(!mutex)
423 return -EINVAL;
424 *ceiling = SCHED_INFO_PRIORITY(mutex->sched_info);
425 return 0;
426}
427
428//------------------------------------------------------------------------------
429// Set priority ceiling
430//------------------------------------------------------------------------------
431int tbthread_mutex_setprioceiling(tbthread_mutex_t *mutex, int ceiling,
432 int *old_ceiling)
433{
434 if(!mutex)
435 return -EINVAL;
436 if(mutex->protocol != TBTHREAD_PRIO_PROTECT)
437 return -EINVAL;
438 if(ceiling < 0 || ceiling > 99)
439 return -EINVAL;
440
441
442 tbthread_t self = tbthread_self();
443 int locked = 0;
444 if(mutex->owner != self) {
445 lock_normal(mutex);
446 locked = 1;
447 }
448 if(old_ceiling)
449 *old_ceiling = SCHED_INFO_PRIORITY(mutex->sched_info);
450 mutex->sched_info = SCHED_INFO_PACK(SCHED_FIFO, ceiling);
451
452 if(locked)
453 unlock_normal(mutex);
454 return 0;
455}
456