1 /*
2 * Copyright (c) 2019 Intel Corporation
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/sys/sem.h>
8 #include <zephyr/syscall_handler.h>
9
10 #ifdef CONFIG_USERSPACE
11 #define SYS_SEM_MINIMUM 0
12 #define SYS_SEM_CONTENDED (SYS_SEM_MINIMUM - 1)
13
bounded_dec(atomic_t * val,atomic_t minimum)14 static inline atomic_t bounded_dec(atomic_t *val, atomic_t minimum)
15 {
16 atomic_t old_value, new_value;
17
18 do {
19 old_value = atomic_get(val);
20 if (old_value < minimum) {
21 break;
22 }
23
24 new_value = old_value - 1;
25 } while (atomic_cas(val, old_value, new_value) == 0);
26
27 return old_value;
28 }
29
bounded_inc(atomic_t * val,atomic_t minimum,atomic_t maximum)30 static inline atomic_t bounded_inc(atomic_t *val, atomic_t minimum,
31 atomic_t maximum)
32 {
33 atomic_t old_value, new_value;
34
35 do {
36 old_value = atomic_get(val);
37 if (old_value >= maximum) {
38 break;
39 }
40
41 new_value = old_value < minimum ?
42 minimum + 1 : old_value + 1;
43 } while (atomic_cas(val, old_value, new_value) == 0U);
44
45 return old_value;
46 }
47
sys_sem_init(struct sys_sem * sem,unsigned int initial_count,unsigned int limit)48 int sys_sem_init(struct sys_sem *sem, unsigned int initial_count,
49 unsigned int limit)
50 {
51 if (sem == NULL || limit == SYS_SEM_MINIMUM ||
52 initial_count > limit || limit > INT_MAX) {
53 return -EINVAL;
54 }
55
56 atomic_set(&sem->futex.val, initial_count);
57 sem->limit = limit;
58
59 return 0;
60 }
61
sys_sem_give(struct sys_sem * sem)62 int sys_sem_give(struct sys_sem *sem)
63 {
64 int ret = 0;
65 atomic_t old_value;
66
67 old_value = bounded_inc(&sem->futex.val,
68 SYS_SEM_MINIMUM, sem->limit);
69 if (old_value < 0) {
70 ret = k_futex_wake(&sem->futex, true);
71
72 if (ret > 0) {
73 return 0;
74 }
75 } else if (old_value >= sem->limit) {
76 return -EAGAIN;
77 } else {
78 ;
79 }
80 return ret;
81 }
82
sys_sem_take(struct sys_sem * sem,k_timeout_t timeout)83 int sys_sem_take(struct sys_sem *sem, k_timeout_t timeout)
84 {
85 int ret = 0;
86 atomic_t old_value;
87
88 do {
89 old_value = bounded_dec(&sem->futex.val,
90 SYS_SEM_MINIMUM);
91 if (old_value > 0) {
92 return 0;
93 }
94
95 ret = k_futex_wait(&sem->futex,
96 SYS_SEM_CONTENDED, timeout);
97 } while (ret == 0 || ret == -EAGAIN);
98
99 return ret;
100 }
101
sys_sem_count_get(struct sys_sem * sem)102 unsigned int sys_sem_count_get(struct sys_sem *sem)
103 {
104 int value = atomic_get(&sem->futex.val);
105
106 return value > SYS_SEM_MINIMUM ? value : SYS_SEM_MINIMUM;
107 }
108 #else
sys_sem_init(struct sys_sem * sem,unsigned int initial_count,unsigned int limit)109 int sys_sem_init(struct sys_sem *sem, unsigned int initial_count,
110 unsigned int limit)
111 {
112 k_sem_init(&sem->kernel_sem, initial_count, limit);
113
114 return 0;
115 }
116
sys_sem_give(struct sys_sem * sem)117 int sys_sem_give(struct sys_sem *sem)
118 {
119 k_sem_give(&sem->kernel_sem);
120
121 return 0;
122 }
123
sys_sem_take(struct sys_sem * sem,k_timeout_t timeout)124 int sys_sem_take(struct sys_sem *sem, k_timeout_t timeout)
125 {
126 int ret_value = 0;
127
128 ret_value = k_sem_take(&sem->kernel_sem, timeout);
129 if (ret_value == -EAGAIN || ret_value == -EBUSY) {
130 ret_value = -ETIMEDOUT;
131 }
132
133 return ret_value;
134 }
135
sys_sem_count_get(struct sys_sem * sem)136 unsigned int sys_sem_count_get(struct sys_sem *sem)
137 {
138 return k_sem_count_get(&sem->kernel_sem);
139 }
140 #endif
141
