timerfd.c 7.5 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
/*
 * Copyright (C) 2013 Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
 */

#include <linux/timerfd.h>
20
#include <linux/err.h>
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
#include <cobalt/kernel/timer.h>
#include <cobalt/kernel/select.h>
#include <rtdm/fd.h>
#include "internal.h"
#include "clock.h"
#include "timer.h"
#include "timerfd.h"

struct cobalt_tfd {
	int flags;
	clockid_t clockid;
	struct rtdm_fd fd;
	struct xntimer timer;
	DECLARE_XNSELECT(read_select);
	struct itimerspec value;
	struct xnsynch readers;
	struct xnthread *target;
};

#define COBALT_TFD_TICKED	(1 << 2)

#define COBALT_TFD_SETTIME_FLAGS (TFD_TIMER_ABSTIME | TFD_WAKEUP)

static ssize_t timerfd_read(struct rtdm_fd *fd, void __user *buf, size_t size)
{
	struct cobalt_tfd *tfd;
47
48
	__u64 __user *u_ticks;
	__u64 ticks = 0;
49
50
51
52
53
54
55
56
	bool aligned;
	spl_t s;
	int err;

	if (size < sizeof(ticks))
		return -EINVAL;

	u_ticks = buf;
57
58
59
	if (!access_wok(u_ticks, sizeof(*u_ticks)))
		return -EFAULT;

60
61
62
63
64
65
66
67
68
	aligned = (((unsigned long)buf) & (sizeof(ticks) - 1)) == 0;

	tfd = container_of(fd, struct cobalt_tfd, fd);

	xnlock_get_irqsave(&nklock, s);
	if (tfd->flags & COBALT_TFD_TICKED) {
		err = 0;
		goto out;
	}
69
	if (rtdm_fd_flags(fd) & O_NONBLOCK) {
70
71
72
73
74
75
76
77
78
79
80
81
82
83
		err = -EAGAIN;
		goto out;
	}

	do {
		err = xnsynch_sleep_on(&tfd->readers, XN_INFINITE, XN_RELATIVE);
	} while (err == 0 && (tfd->flags & COBALT_TFD_TICKED) == 0);

	if (err & XNBREAK)
		err = -EINTR;
  out:
	if (err == 0) {
		xnticks_t now;

84
		if (xntimer_periodic_p(&tfd->timer)) {
85
86
87
88
89
90
91
92
93
94
95
			now = xnclock_read_raw(xntimer_clock(&tfd->timer));
			ticks = 1 + xntimer_get_overruns(&tfd->timer, now);
		} else
			ticks = 1;

		tfd->flags &= ~COBALT_TFD_TICKED;
		xnselect_signal(&tfd->read_select, 0);
	}
	xnlock_put_irqrestore(&nklock, s);

	if (err == 0) {
96
97
98
99
		err = aligned ? __xn_put_user(ticks, u_ticks) :
			__xn_copy_to_user(buf, &ticks, sizeof(ticks));
		if (err)
			err =-EFAULT;
100
101
102
103
104
105
	}

	return err ?: sizeof(ticks);
}

static int
106
107
timerfd_select(struct rtdm_fd *fd, struct xnselector *selector,
	       unsigned type, unsigned index)
108
109
110
111
112
113
114
115
116
117
118
119
120
121
{
	struct cobalt_tfd *tfd = container_of(fd, struct cobalt_tfd, fd);
	struct xnselect_binding *binding;
	spl_t s;
	int err;

	if (type != XNSELECT_READ)
		return -EBADF;

	binding = xnmalloc(sizeof(*binding));
	if (binding == NULL)
		return -ENOMEM;

	xnlock_get_irqsave(&nklock, s);
122
	xntimer_set_affinity(&tfd->timer, xnthread_current()->sched);
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
	err = xnselect_bind(&tfd->read_select, binding, selector, type,
			index, tfd->flags & COBALT_TFD_TICKED);
	xnlock_put_irqrestore(&nklock, s);

	return err;
}

static void timerfd_close(struct rtdm_fd *fd)
{
	struct cobalt_tfd *tfd = container_of(fd, struct cobalt_tfd, fd);
	int resched;
	spl_t s;

	xnlock_get_irqsave(&nklock, s);
	xntimer_destroy(&tfd->timer);
	resched = xnsynch_destroy(&tfd->readers) == XNSYNCH_RESCHED;
	xnlock_put_irqrestore(&nklock, s);
	xnselect_destroy(&tfd->read_select);
	xnfree(tfd);

	if (resched)
		xnsched_run();
}

static struct rtdm_fd_ops timerfd_ops = {
	.read_rt = timerfd_read,
149
	.select = timerfd_select,
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
	.close = timerfd_close,
};

static void timerfd_handler(struct xntimer *xntimer)
{
	struct cobalt_tfd *tfd;

	tfd = container_of(xntimer, struct cobalt_tfd, timer);
	tfd->flags |= COBALT_TFD_TICKED;
	xnselect_signal(&tfd->read_select, 1);
	xnsynch_wakeup_one_sleeper(&tfd->readers);
	if (tfd->target)
		xnthread_unblock(tfd->target);
}

165
COBALT_SYSCALL(timerfd_create, lostage, (int clockid, int flags))
166
167
{
	struct cobalt_tfd *tfd;
168
	struct xnthread *curr;
169
	struct xnclock *clock;
170
	int ret, ufd;
171

172
	if (flags & ~TFD_CREATE_FLAGS)
173
174
		return -EINVAL;

175
	clock = cobalt_clock_find(clockid);
176
177
	if (IS_ERR(clock))
		return PTR_ERR(clock);
178
179
180
181
182

	tfd = xnmalloc(sizeof(*tfd));
	if (tfd == NULL)
		return -ENOMEM;

183
184
	ufd = __rtdm_anon_getfd("[cobalt-timerfd]",
				O_RDWR | (flags & TFD_SHARED_FCNTL_FLAGS));
185
186
187
188
189
	if (ufd < 0) {
		ret = ufd;
		goto fail_getfd;
	}

190
191
	tfd->flags = flags & ~TFD_NONBLOCK;
	tfd->fd.oflags = (flags & TFD_NONBLOCK) ? O_NONBLOCK : 0;
192
	tfd->clockid = clockid;
193
	curr = xnthread_current();
194
	xntimer_init(&tfd->timer, clock, timerfd_handler,
195
		     curr ? curr->sched : NULL, XNTIMER_UGRAVITY);
196
197
198
199
	xnsynch_init(&tfd->readers, XNSYNCH_PRIO | XNSYNCH_NOPIP, NULL);
	xnselect_init(&tfd->read_select);
	tfd->target = NULL;

200
	ret = rtdm_fd_enter(&tfd->fd, ufd, COBALT_TIMERFD_MAGIC, &timerfd_ops);
201
202
203
	if (ret < 0)
		goto fail;

204
205
206
207
	ret = rtdm_fd_register(&tfd->fd, ufd);
	if (ret < 0)
		goto fail;

208
209
210
211
212
	return ufd;
fail:
	xnselect_destroy(&tfd->read_select);
	xnsynch_destroy(&tfd->readers);
	xntimer_destroy(&tfd->timer);
213
	__rtdm_anon_putfd(ufd);
214
215
216
217
fail_getfd:
	xnfree(tfd);

	return ret;
218
219
220
221
222
223
}

static inline struct cobalt_tfd *tfd_get(int ufd)
{
	struct rtdm_fd *fd;

224
	fd = rtdm_fd_get(ufd, COBALT_TIMERFD_MAGIC);
225
226
	if (IS_ERR(fd)) {
		int err = PTR_ERR(fd);
227
		if (err == -EBADF && cobalt_current_process() == NULL)
228
229
230
231
232
233
234
235
236
237
238
239
			err = -EPERM;
		return ERR_PTR(err);
	}

	return container_of(fd, struct cobalt_tfd, fd);
}

static inline void tfd_put(struct cobalt_tfd *tfd)
{
	rtdm_fd_put(&tfd->fd);
}

240
241
242
int __cobalt_timerfd_settime(int fd, int flags,
			     const struct itimerspec *value,
			     struct itimerspec *ovalue)
243
244
{
	struct cobalt_tfd *tfd;
245
	int cflag, ret;
246
247
248
249
250
251
252
253
254
255
256
257
258
	spl_t s;

	if (flags & ~COBALT_TFD_SETTIME_FLAGS)
		return -EINVAL;

	tfd = tfd_get(fd);
	if (IS_ERR(tfd))
		return PTR_ERR(tfd);

	cflag = (flags & TFD_TIMER_ABSTIME) ? TIMER_ABSTIME : 0;

	xnlock_get_irqsave(&nklock, s);

259
	tfd->target = NULL;
260
	if (flags & TFD_WAKEUP) {
261
		tfd->target = xnthread_current();
262
		if (tfd->target == NULL) {
263
264
			ret = -EPERM;
			goto out;
265
		}
266
	}
267

268
269
	if (ovalue)
		__cobalt_timer_getval(&tfd->timer, ovalue);
270

271
	xntimer_set_affinity(&tfd->timer, xnthread_current()->sched);
272

273
274
275
	ret = __cobalt_timer_setval(&tfd->timer,
				    clock_flag(cflag, tfd->clockid), value);
out:
276
277
	xnlock_put_irqrestore(&nklock, s);

278
	tfd_put(tfd);
279

280
281
	return ret;
}
282

283
COBALT_SYSCALL(timerfd_settime, primary,
284
285
286
	       (int fd, int flags,
		const struct itimerspec __user *new_value,
		struct itimerspec __user *old_value))
287
288
289
{
	struct itimerspec ovalue, value;
	int ret;
290

291
	ret = cobalt_copy_from_user(&value, new_value, sizeof(value));
292
293
294
295
296
297
298
299
	if (ret)
		return ret;

	ret = __cobalt_timerfd_settime(fd, flags, &value, &ovalue);
	if (ret)
		return ret;

	if (old_value) {
300
		ret = cobalt_copy_to_user(old_value, &ovalue, sizeof(ovalue));
301
302
303
304
305
306
		value.it_value.tv_sec = 0;
		value.it_value.tv_nsec = 0;
		__cobalt_timerfd_settime(fd, flags, &value, NULL);
	}

	return ret;
307
308
}

309
int __cobalt_timerfd_gettime(int fd, struct itimerspec *value)
310
311
312
313
314
315
316
317
318
{
	struct cobalt_tfd *tfd;
	spl_t s;

	tfd = tfd_get(fd);
	if (IS_ERR(tfd))
		return PTR_ERR(tfd);

	xnlock_get_irqsave(&nklock, s);
319
	__cobalt_timer_getval(&tfd->timer, value);
320
321
322
323
	xnlock_put_irqrestore(&nklock, s);

	tfd_put(tfd);

324
325
326
327
	return 0;
}

COBALT_SYSCALL(timerfd_gettime, current,
328
	       (int fd, struct itimerspec __user *curr_value))
329
330
331
332
333
334
{
	struct itimerspec value;
	int ret;

	ret = __cobalt_timerfd_gettime(fd, &value);

335
	return ret ?: cobalt_copy_to_user(curr_value, &value, sizeof(value));
336
}