sem.c 5.64 KB
Newer Older
Philippe Gerum's avatar
Philippe Gerum committed
1
2
3
4
5
6
/*
 * SPDX-License-Identifier: MIT
 *
 * Copyright (C) 2018 Philippe Gerum  <rpm@xenomai.org>
 */

7
#include <stdbool.h>
Philippe Gerum's avatar
Philippe Gerum committed
8
9
10
11
12
13
14
15
16
#include <sys/types.h>
#include <sys/ioctl.h>
#include <time.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdlib.h>
#include <stdio.h>
#include <pthread.h>
17
#include <evl/compiler.h>
18
19
20
21
22
#include <evl/atomic.h>
#include <evl/evl.h>
#include <evl/sem.h>
#include <evl/thread.h>
#include <evl/syscall.h>
Philippe Gerum's avatar
Philippe Gerum committed
23
24
#include "internal.h"

25
26
27
#define __SEM_ACTIVE_MAGIC	0xcb13cb13
#define __SEM_DEAD_MAGIC	0

28
29
int evl_create_sem(struct evl_sem *sem, int clockfd,
		int initval, int flags,
Philippe Gerum's avatar
Philippe Gerum committed
30
31
		const char *fmt, ...)
{
32
	struct evl_monitor_attrs attrs;
Philippe Gerum's avatar
Philippe Gerum committed
33
34
35
36
37
38
	struct evl_element_ids eids;
	int efd, ret;
	va_list ap;
	char *name;

	if (evl_shared_memory == NULL)
39
		return -ENXIO;
Philippe Gerum's avatar
Philippe Gerum committed
40
41
42
43
44
45
46

	va_start(ap, fmt);
	ret = vasprintf(&name, fmt, ap);
	va_end(ap);
	if (ret < 0)
		return -ENOMEM;

47
48
	attrs.type = EVL_MONITOR_EVENT;
	attrs.protocol = EVL_EVENT_COUNT;
Philippe Gerum's avatar
Philippe Gerum committed
49
50
	attrs.clockfd = clockfd;
	attrs.initval = initval;
51
	efd = create_evl_element(EVL_MONITOR_DEV, name, &attrs,	flags, &eids);
Philippe Gerum's avatar
Philippe Gerum committed
52
53
54
55
	free(name);
	if (efd < 0)
		return efd;

56
	sem->u.active.state = evl_shared_memory + eids.state_offset;
57
	/* Force sync the PTE. */
58
59
60
	atomic_set(&sem->u.active.state->u.event.value, initval);
	sem->u.active.fundle = eids.fundle;
	sem->u.active.efd = efd;
Philippe Gerum's avatar
Philippe Gerum committed
61
62
	sem->magic = __SEM_ACTIVE_MAGIC;

63
	return efd;
Philippe Gerum's avatar
Philippe Gerum committed
64
65
66
67
}

int evl_open_sem(struct evl_sem *sem, const char *fmt, ...)
{
68
	struct evl_monitor_binding bind;
Philippe Gerum's avatar
Philippe Gerum committed
69
70
71
72
	int ret, efd;
	va_list ap;

	va_start(ap, fmt);
73
	efd = open_evl_element_vargs(EVL_MONITOR_DEV, fmt, ap);
Philippe Gerum's avatar
Philippe Gerum committed
74
75
76
77
	va_end(ap);
	if (efd < 0)
		return efd;

78
	ret = ioctl(efd, EVL_MONIOC_BIND, &bind);
79
80
81
82
83
84
85
86
87
88
	if (ret) {
		ret = -errno;
		goto fail;
	}

	if (bind.type != EVL_MONITOR_EVENT ||
		bind.protocol != EVL_EVENT_COUNT) {
		ret = -EINVAL;
		goto fail;
	}
Philippe Gerum's avatar
Philippe Gerum committed
89

90
91
92
93
	sem->u.active.state = evl_shared_memory + bind.eids.state_offset;
	__force_read_access(sem->u.active.state->u.event.value);
	sem->u.active.fundle = bind.eids.fundle;
	sem->u.active.efd = efd;
Philippe Gerum's avatar
Philippe Gerum committed
94
95
	sem->magic = __SEM_ACTIVE_MAGIC;

96
	return efd;
97
98
99
100
fail:
	close(efd);

	return ret;
Philippe Gerum's avatar
Philippe Gerum committed
101
102
}

103
int evl_close_sem(struct evl_sem *sem)
Philippe Gerum's avatar
Philippe Gerum committed
104
105
106
{
	int ret;

107
108
109
	if (sem->magic == __SEM_UNINIT_MAGIC)
		return 0;

Philippe Gerum's avatar
Philippe Gerum committed
110
111
112
	if (sem->magic != __SEM_ACTIVE_MAGIC)
		return -EINVAL;

113
	ret = close(sem->u.active.efd);
Philippe Gerum's avatar
Philippe Gerum committed
114
115
116
	if (ret)
		return -errno;

117
118
	sem->u.active.fundle = EVL_NO_HANDLE;
	sem->u.active.state = NULL;
Philippe Gerum's avatar
Philippe Gerum committed
119
120
121
122
123
	sem->magic = __SEM_DEAD_MAGIC;

	return 0;
}

124
static int check_sanity(struct evl_sem *sem)
Philippe Gerum's avatar
Philippe Gerum committed
125
{
126
127
128
	int efd;

	if (sem->magic == __SEM_UNINIT_MAGIC) {
129
		efd = evl_create_sem(sem,
130
131
				sem->u.uninit.clockfd,
				sem->u.uninit.initval,
132
				sem->u.uninit.flags,
133
				sem->u.uninit.name);
134
135
		return efd < 0 ? efd : 0;
	}
Philippe Gerum's avatar
Philippe Gerum committed
136
137
138
139

	return sem->magic != __SEM_ACTIVE_MAGIC ? -EINVAL : 0;
}

140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
/*
 * CAUTION: we assume that the implementation of atomic_cmpxchg()
 * which currently relies on GCC's __sync_val_compare_and_swap()
 * built-in does issue proper full memory barrier on successful swap,
 * so we should not have to emit them manually.
 */
static int try_get(struct evl_monitor_state *state)
{
	int val, prev, next;

	val = atomic_read(&state->u.event.value);
	if (val <= 0)
		return -EAGAIN;

	do {
		prev = val;
		next = prev - 1;
		val = atomic_cmpxchg(&state->u.event.value, prev, next);
		/*
		 * If the semaphore's value was strictly positive and
		 * we end up with a negative one after a swap attempt,
		 * then cmpxchg must have failed, and the non-blocking
		 * P operation failed.
		 */
		if (val <= 0)
			return -EAGAIN;
	} while (val != prev);

	return 0;
}

Philippe Gerum's avatar
Philippe Gerum committed
171
int evl_timedget_sem(struct evl_sem *sem, const struct timespec *timeout)
Philippe Gerum's avatar
Philippe Gerum committed
172
{
173
174
	struct evl_monitor_state *state;
	struct evl_monitor_waitreq req;
175
	struct __evl_timespec kts;
Philippe Gerum's avatar
Philippe Gerum committed
176
	fundle_t current;
177
	int ret;
Philippe Gerum's avatar
Philippe Gerum committed
178
179
180
181
182

	current = evl_get_current();
	if (current == EVL_NO_HANDLE)
		return -EPERM;

183
	ret = check_sanity(sem);
Philippe Gerum's avatar
Philippe Gerum committed
184
185
186
	if (ret)
		return ret;

187
	state = sem->u.active.state;
188
189
190
	ret = try_get(state);
	if (ret != -EAGAIN)
		return ret;
Philippe Gerum's avatar
Philippe Gerum committed
191

192
	req.gatefd = -1;
193
	req.timeout_ptr = __evl_ktimespec_ptr64(timeout, kts);
194
	req.status = -EINVAL;
195
	req.value = 0;		/* dummy */
196

197
	ret = oob_ioctl(sem->u.active.efd, EVL_MONIOC_WAIT, &req);
Philippe Gerum's avatar
Philippe Gerum committed
198

199
	return ret ? -errno : req.status;
Philippe Gerum's avatar
Philippe Gerum committed
200
201
}

Philippe Gerum's avatar
Philippe Gerum committed
202
int evl_get_sem(struct evl_sem *sem)
203
204
205
{
	struct timespec timeout = { .tv_sec = 0, .tv_nsec = 0 };

Philippe Gerum's avatar
Philippe Gerum committed
206
	return evl_timedget_sem(sem, &timeout);
207
208
}

Philippe Gerum's avatar
Philippe Gerum committed
209
int evl_tryget_sem(struct evl_sem *sem)
Philippe Gerum's avatar
Philippe Gerum committed
210
211
212
{
	int ret;

213
	ret = check_sanity(sem);
Philippe Gerum's avatar
Philippe Gerum committed
214
215
216
	if (ret)
		return ret;

217
	return try_get(sem->u.active.state);
Philippe Gerum's avatar
Philippe Gerum committed
218
219
}

220
221
222
223
224
static inline bool is_polled(struct evl_monitor_state *state)
{
	return !!atomic_read(&state->u.event.pollrefs);
}

Philippe Gerum's avatar
Philippe Gerum committed
225
int evl_put_sem(struct evl_sem *sem)
Philippe Gerum's avatar
Philippe Gerum committed
226
{
227
228
	struct evl_monitor_state *state;
	int val, prev, next, ret;
229
	__s32 sigval = 1;
Philippe Gerum's avatar
Philippe Gerum committed
230

231
	ret = check_sanity(sem);
Philippe Gerum's avatar
Philippe Gerum committed
232
233
234
	if (ret)
		return ret;

235
	state = sem->u.active.state;
236
	val = atomic_read(&state->u.event.value);
237
	if (val < 0 || is_polled(state)) {
Philippe Gerum's avatar
Philippe Gerum committed
238
239
	slow_path:
		if (evl_get_current())
240
241
			ret = oob_ioctl(sem->u.active.efd,
					EVL_MONIOC_SIGNAL, &sigval);
Philippe Gerum's avatar
Philippe Gerum committed
242
243
		else
			/* In-band threads may post pended sema4s. */
244
245
			ret = ioctl(sem->u.active.efd,
				EVL_MONIOC_SIGNAL, &sigval);
Philippe Gerum's avatar
Philippe Gerum committed
246
247
248
249
		return ret ? -errno : 0;
	}

	do {
250
251
252
		prev = val;
		next = prev + 1;
		val = atomic_cmpxchg(&state->u.event.value, prev, next);
253
254
255
256
257
		/*
		 * If somebody sneaked in the wait queue or started
		 * polling us in the meantime, we have to perform a
		 * kernel entry.
		 */
258
		if (val < 0)
Philippe Gerum's avatar
Philippe Gerum committed
259
			goto slow_path;
260
261
262
263
264
265
		if (is_polled(state)) {
			/* If swap happened, just trigger a wakeup. */
			if (val == prev)
				sigval = 0;
			goto slow_path;
		}
266
	} while (val != prev);
Philippe Gerum's avatar
Philippe Gerum committed
267
268
269
270

	return 0;
}

271
int evl_peek_sem(struct evl_sem *sem, int *r_val)
Philippe Gerum's avatar
Philippe Gerum committed
272
273
274
275
{
	if (sem->magic != __SEM_ACTIVE_MAGIC)
		return -EINVAL;

276
	*r_val = atomic_read(&sem->u.active.state->u.event.value);
277
278

	return 0;
Philippe Gerum's avatar
Philippe Gerum committed
279
}