mqueue.c 21.1 KB
Newer Older
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
1
/*
2
 * Written by Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19
 *
 * 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 <stdarg.h>
20 21
#include <linux/slab.h>
#include <linux/mm.h>
22
#include <linux/sched.h>
23
#include <cobalt/kernel/select.h>
24
#include <rtdm/fd.h>
25 26 27
#include "internal.h"
#include "thread.h"
#include "signal.h"
28
#include "timer.h"
29
#include "mqueue.h"
30
#include "clock.h"
31
#include <trace/events/cobalt-posix.h>
32

33 34
#define COBALT_MSGMAX		65536
#define COBALT_MSGSIZEMAX	(16*1024*1024)
35
#define COBALT_MSGPRIOMAX	32768
36

37
struct cobalt_mq {
38 39 40
	unsigned magic;

	struct list_head link;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
41

42 43
	struct xnsynch receivers;
	struct xnsynch senders;
44 45
	size_t memsize;
	char *mem;
46 47 48
	struct list_head queued;
	struct list_head avail;
	int nrqueued;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
49

50
	/* mq_notify */
51
	struct siginfo si;
52
	mqd_t target_qd;
53
	struct cobalt_thread *target;
54

55
	struct mq_attr attr;
56 57 58 59

	unsigned refs;
	char name[COBALT_MAXNAME];
	xnhandle_t handle;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
60

61 62
	DECLARE_XNSELECT(read_select);
	DECLARE_XNSELECT(write_select);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
63 64
};

65 66 67 68
struct cobalt_mqd {
	struct cobalt_mq *mq;
	struct rtdm_fd fd;
};
69

70 71 72
struct cobalt_msg {
	struct list_head link;
	unsigned int prio;
73 74
	size_t len;
	char data[0];
75
};
76

77 78 79 80 81
struct cobalt_mqwait_context {
	struct xnthread_wait_context wc;
	struct cobalt_msg *msg;
};

Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
82
static struct mq_attr default_attr = {
83 84
      .mq_maxmsg = 10,
      .mq_msgsize = 8192,
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
85 86
};

87
static LIST_HEAD(cobalt_mqq);
88 89

static inline struct cobalt_msg *mq_msg_alloc(struct cobalt_mq *mq)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
90
{
91
	if (list_empty(&mq->avail))
92
		return NULL;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
93

94
	return list_get_entry(&mq->avail, struct cobalt_msg, link);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
95 96
}

97
static inline void mq_msg_free(struct cobalt_mq *mq, struct cobalt_msg * msg)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
98
{
99
	list_add(&msg->link, &mq->avail); /* For earliest re-use of the block. */
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
100 101
}

102
static inline int mq_init(struct cobalt_mq *mq, const struct mq_attr *attr)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
103
{
104 105
	unsigned i, msgsize, memsize;
	char *mem;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
106

107
	if (attr == NULL)
108
		attr = &default_attr;
109 110 111 112 113 114 115 116
	else {
		if (attr->mq_maxmsg <= 0 || attr->mq_msgsize <= 0)
			return -EINVAL;
		if (attr->mq_maxmsg > COBALT_MSGMAX)
			return -EINVAL;
		if (attr->mq_msgsize > COBALT_MSGSIZEMAX)
			return -EINVAL;
	}
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
117

118
	msgsize = attr->mq_msgsize + sizeof(struct cobalt_msg);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
119

120 121 122 123
	/* Align msgsize on natural boundary. */
	if ((msgsize % sizeof(unsigned long)))
		msgsize +=
		    sizeof(unsigned long) - (msgsize % sizeof(unsigned long));
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
124

125 126
	memsize = msgsize * attr->mq_maxmsg;
	memsize = PAGE_ALIGN(memsize);
127 128
	if (get_order(memsize) > MAX_ORDER)
		return -ENOSPC;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
129

130
	mem = xnheap_vmalloc(memsize);
131
	if (mem == NULL)
132
		return -ENOSPC;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
133

134
	mq->memsize = memsize;
135 136
	INIT_LIST_HEAD(&mq->queued);
	mq->nrqueued = 0;
137 138
	xnsynch_init(&mq->receivers, XNSYNCH_PRIO | XNSYNCH_NOPIP, NULL);
	xnsynch_init(&mq->senders, XNSYNCH_PRIO | XNSYNCH_NOPIP, NULL);
139
	mq->mem = mem;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
140

141
	/* Fill the pool. */
142
	INIT_LIST_HEAD(&mq->avail);
143
	for (i = 0; i < attr->mq_maxmsg; i++) {
144
		struct cobalt_msg *msg = (struct cobalt_msg *) (mem + i * msgsize);
145
		mq_msg_free(mq, msg);
146
	}
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
147

148
	mq->attr = *attr;
149
	mq->target = NULL;
150 151
	xnselect_init(&mq->read_select);
	xnselect_init(&mq->write_select);
152 153
	mq->magic = COBALT_MQ_MAGIC;
	mq->refs = 2;
154
	INIT_LIST_HEAD(&mq->link);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
155

156
	return 0;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
157 158
}

159
static inline void mq_destroy(struct cobalt_mq *mq)
160
{
161
	int resched;
162 163 164
	spl_t s;

	xnlock_get_irqsave(&nklock, s);
165 166
	resched = (xnsynch_destroy(&mq->receivers) == XNSYNCH_RESCHED);
	resched = (xnsynch_destroy(&mq->senders) == XNSYNCH_RESCHED) || resched;
167
	list_del(&mq->link);
168
	xnlock_put_irqrestore(&nklock, s);
169 170
	xnselect_destroy(&mq->read_select);
	xnselect_destroy(&mq->write_select);
171
	xnregistry_remove(mq->handle);
172
	xnheap_vfree(mq->mem);
173
	kfree(mq);
174 175

	if (resched)
176
		xnsched_run();
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
177 178
}

179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209
static int mq_unref_inner(struct cobalt_mq *mq, spl_t s)
{
	int destroy;

	destroy = --mq->refs == 0;
	xnlock_put_irqrestore(&nklock, s);

	if (destroy)
		mq_destroy(mq);

	return destroy;
}

static int mq_unref(struct cobalt_mq *mq)
{
	spl_t s;

	xnlock_get_irqsave(&nklock, s);
	return mq_unref_inner(mq, s);
}

static void mqd_close(struct rtdm_fd *fd)
{
	struct cobalt_mqd *mqd = container_of(fd, struct cobalt_mqd, fd);
	struct cobalt_mq *mq = mqd->mq;

	kfree(mqd);
	mq_unref(mq);
}

int
210 211
mqd_select(struct rtdm_fd *fd, struct xnselector *selector,
	   unsigned type, unsigned index)
212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231
{
	struct cobalt_mqd *mqd = container_of(fd, struct cobalt_mqd, fd);
	struct xnselect_binding *binding;
	struct cobalt_mq *mq;
	int err;
	spl_t s;

	if (type == XNSELECT_READ || type == XNSELECT_WRITE) {
		binding = xnmalloc(sizeof(*binding));
		if (!binding)
			return -ENOMEM;
	} else
		return -EBADF;

	xnlock_get_irqsave(&nklock, s);
	mq = mqd->mq;

	switch(type) {
	case XNSELECT_READ:
		err = -EBADF;
232
		if ((rtdm_fd_flags(fd) & COBALT_PERMS_MASK) == O_WRONLY)
233 234 235 236 237 238 239 240 241 242 243
			goto unlock_and_error;

		err = xnselect_bind(&mq->read_select, binding,
				selector, type, index,
				!list_empty(&mq->queued));
		if (err)
			goto unlock_and_error;
		break;

	case XNSELECT_WRITE:
		err = -EBADF;
244
		if ((rtdm_fd_flags(fd) & COBALT_PERMS_MASK) == O_RDONLY)
245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263
			goto unlock_and_error;

		err = xnselect_bind(&mq->write_select, binding,
				selector, type, index,
				!list_empty(&mq->avail));
		if (err)
			goto unlock_and_error;
		break;
	}
	xnlock_put_irqrestore(&nklock, s);
	return 0;

      unlock_and_error:
	xnlock_put_irqrestore(&nklock, s);
	xnfree(binding);
	return err;
}

static struct rtdm_fd_ops mqd_ops = {
264 265
	.close = mqd_close,
	.select = mqd_select,
266 267 268 269 270
};

static inline int mqd_create(struct cobalt_mq *mq, unsigned long flags, int ufd)
{
	struct cobalt_mqd *mqd;
271
	int ret;
272

273
	if (cobalt_ppd_get(0) == &cobalt_kernel_ppd)
274 275 276 277 278 279
		return -EPERM;

	mqd = kmalloc(sizeof(*mqd), GFP_KERNEL);
	if (mqd == NULL)
		return -ENOSPC;

280
	mqd->fd.oflags = flags;
281 282
	mqd->mq = mq;

283 284 285 286 287
	ret = rtdm_fd_enter(&mqd->fd, ufd, COBALT_MQD_MAGIC, &mqd_ops);
	if (ret < 0)
		return ret;

	return rtdm_fd_register(&mqd->fd, ufd);
288 289
}

290 291
static int mq_open(int uqd, const char *name, int oflags,
		   int mode, struct mq_attr *attr)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
292
{
293 294
	struct cobalt_mq *mq;
	xnhandle_t handle;
295
	spl_t s;
296 297
	int err;

298 299
	if (name[0] != '/' || name[1] == '\0')
		return -EINVAL;
300

301 302 303 304 305 306 307
  retry_bind:
	err = xnregistry_bind(&name[1], XN_NONBLOCK, XN_RELATIVE, &handle);
	switch (err) {
	case 0:
		/* Found */
		if ((oflags & (O_CREAT | O_EXCL)) == (O_CREAT | O_EXCL))
			return -EEXIST;
308

309 310 311 312 313 314
		xnlock_get_irqsave(&nklock, s);
		mq = xnregistry_lookup(handle, NULL);
		if (mq && mq->magic != COBALT_MQ_MAGIC) {
			xnlock_put_irqrestore(&nklock, s);
			return -EINVAL;
		}
315

316 317 318 319 320 321 322
		if (mq) {
			++mq->refs;
			xnlock_put_irqrestore(&nklock, s);
		} else {
			xnlock_put_irqrestore(&nklock, s);
			goto retry_bind;
		}
323

324 325 326 327 328 329 330
		err = mqd_create(mq, oflags & (O_NONBLOCK | COBALT_PERMS_MASK),
				uqd);
		if (err < 0) {
			mq_unref(mq);
			return err;
		}
		break;
331

332 333 334 335
	case -EWOULDBLOCK:
		/* Not found */
		if ((oflags & O_CREAT) == 0)
			return (mqd_t)-ENOENT;
336

337 338 339
		mq = kmalloc(sizeof(*mq), GFP_KERNEL);
		if (mq == NULL)
			return -ENOSPC;
340

341 342
		err = mq_init(mq, attr);
		if (err) {
343
			kfree(mq);
344 345
			return err;
		}
346

347
		snprintf(mq->name, sizeof(mq->name), "%s", &name[1]);
348

349 350 351 352 353 354
		err = mqd_create(mq, oflags & (O_NONBLOCK | COBALT_PERMS_MASK),
				uqd);
		if (err < 0) {
			mq_destroy(mq);
			return err;
		}
355

356 357 358 359 360 361 362 363
		xnlock_get_irqsave(&nklock, s);
		err = xnregistry_enter(mq->name, mq, &mq->handle, NULL);
		if (err < 0)
			--mq->refs;
		else
			list_add_tail(&mq->link, &cobalt_mqq);
		xnlock_put_irqrestore(&nklock, s);
		if (err < 0) {
364
			rtdm_fd_close(uqd, COBALT_MQD_MAGIC);
365 366 367 368 369
			if (err == -EEXIST)
				goto retry_bind;
			return err;
		}
		break;
370

371 372 373
	default:
		return err;
	}
374

375
	return 0;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
376 377
}

378
static inline int mq_close(mqd_t fd)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
379
{
380
	return rtdm_fd_close(fd, COBALT_MQD_MAGIC);
Gilles Chanteperdrix's avatar
sync  
Gilles Chanteperdrix committed
381 382
}

383
static inline int mq_unlink(const char *name)
Gilles Chanteperdrix's avatar
sync  
Gilles Chanteperdrix committed
384
{
385 386
	struct cobalt_mq *mq;
	xnhandle_t handle;
387 388 389
	spl_t s;
	int err;

390 391
	if (name[0] != '/' || name[1] == '\0')
		return -EINVAL;
392

393 394 395 396 397
	err = xnregistry_bind(&name[1], XN_NONBLOCK, XN_RELATIVE, &handle);
	if (err == -EWOULDBLOCK)
		return -ENOENT;
	if (err)
		return err;
398

399 400 401 402 403 404 405 406 407
	xnlock_get_irqsave(&nklock, s);
	mq = xnregistry_lookup(handle, NULL);
	if (!mq) {
		err = -ENOENT;
		goto err_unlock;
	}
	if (mq->magic != COBALT_MQ_MAGIC) {
		err = -EINVAL;
	  err_unlock:
408 409
		xnlock_put_irqrestore(&nklock, s);

410 411 412 413 414
		return err;
	}
	if (mq_unref_inner(mq, s) == 0)
		xnregistry_unlink(&name[1]);
	return 0;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
415 416
}

417
static inline struct cobalt_msg *
418
mq_trysend(struct cobalt_mqd *mqd, size_t len)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
419
{
420
	struct cobalt_msg *msg;
421
	struct cobalt_mq *mq;
422 423
	unsigned flags;

424
	mq = mqd->mq;
425
	flags = rtdm_fd_flags(&mqd->fd) & COBALT_PERMS_MASK;
426 427

	if (flags != O_WRONLY && flags != O_RDWR)
428
		return ERR_PTR(-EBADF);
429 430

	if (len > mq->attr.mq_msgsize)
431
		return ERR_PTR(-EMSGSIZE);
432

433 434
	msg = mq_msg_alloc(mq);
	if (msg == NULL)
435 436
		return ERR_PTR(-EAGAIN);

437
	if (list_empty(&mq->avail))
438 439
		xnselect_signal(&mq->write_select, 0);

440
	return msg;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
441 442
}

443
static inline struct cobalt_msg *
444
mq_tryrcv(struct cobalt_mqd *mqd, size_t len)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
445
{
446 447
	struct cobalt_msg *msg;
	unsigned int flags;
448
	struct cobalt_mq *mq;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
449

450
	mq = mqd->mq;
451
	flags = rtdm_fd_flags(&mqd->fd) & COBALT_PERMS_MASK;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
452

453
	if (flags != O_RDONLY && flags != O_RDWR)
454
		return ERR_PTR(-EBADF);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
455

456
	if (len < mq->attr.mq_msgsize)
457
		return ERR_PTR(-EMSGSIZE);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
458

459
	if (list_empty(&mq->queued))
460
		return ERR_PTR(-EAGAIN);
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
461

462 463 464 465
	msg = list_get_entry(&mq->queued, struct cobalt_msg, link);
	mq->nrqueued--;

	if (list_empty(&mq->queued))
466 467
		xnselect_signal(&mq->read_select, 0);

468
	return msg;
469
}
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
470

471
static struct cobalt_msg *
472
mq_timedsend_inner(struct cobalt_mqd *mqd,
473 474 475
		   size_t len, const void __user *u_ts,
		   int (*fetch_timeout)(struct timespec *ts,
					const void __user *u_ts))
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
476
{
477
	struct cobalt_mqwait_context mwc;
478
	struct cobalt_msg *msg;
479
	struct cobalt_mq *mq;
480
	struct timespec ts;
481 482
	xntmode_t tmode;
	xnticks_t to;
483
	spl_t s;
484
	int ret;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
485

486 487 488
	to = XN_INFINITE;
	tmode = XN_RELATIVE;
redo:
489
	xnlock_get_irqsave(&nklock, s);
490
	msg = mq_trysend(mqd, len);
491 492
	if (msg != ERR_PTR(-EAGAIN))
		goto out;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
493

494
	if (rtdm_fd_flags(&mqd->fd) & O_NONBLOCK)
495
		goto out;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
496

497 498 499 500 501 502 503 504
	if (fetch_timeout) {
		xnlock_put_irqrestore(&nklock, s);
		ret = fetch_timeout(&ts, u_ts);
		if (ret)
			return ERR_PTR(ret);
		if ((unsigned long)ts.tv_nsec >= ONE_BILLION)
			return ERR_PTR(-EINVAL);
		to = ts2ns(&ts) + 1;
505
		tmode = XN_REALTIME;
506 507
		fetch_timeout = NULL;
		goto redo;
508
	}
509

510
	mq = mqd->mq;
511 512 513 514
	xnthread_prepare_wait(&mwc.wc);
	ret = xnsynch_sleep_on(&mq->senders, to, tmode);
	if (ret) {
		if (ret & XNBREAK)
515
			msg = ERR_PTR(-EINTR);
516
		else if (ret & XNTIMEO)
517
			msg = ERR_PTR(-ETIMEDOUT);
518
		else if (ret & XNRMID)
519
			msg = ERR_PTR(-EBADF);
520 521 522
	} else
		msg = mwc.msg;
out:
523 524 525
	xnlock_put_irqrestore(&nklock, s);

	return msg;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
526 527
}

528
static void mq_release_msg(struct cobalt_mq *mq, struct cobalt_msg *msg)
529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550
{
	struct cobalt_mqwait_context *mwc;
	struct xnthread_wait_context *wc;
	struct xnthread *thread;

	/*
	 * Try passing the free message slot to a waiting sender, link
	 * it to the free queue otherwise.
	 */
	if (xnsynch_pended_p(&mq->senders)) {
		thread = xnsynch_wakeup_one_sleeper(&mq->senders);
		wc = xnthread_get_wait_context(thread);
		mwc = container_of(wc, struct cobalt_mqwait_context, wc);
		mwc->msg = msg;
		xnthread_complete_wait(wc);
	} else {
		mq_msg_free(mq, msg);
		if (list_is_singular(&mq->avail))
			xnselect_signal(&mq->write_select, 1);
	}
}

551
static int
552
mq_finish_send(struct cobalt_mqd *mqd, struct cobalt_msg *msg)
553
{
554 555
	struct cobalt_mqwait_context *mwc;
	struct xnthread_wait_context *wc;
556
	struct cobalt_sigpending *sigp;
557
	struct xnthread *thread;
558
	struct cobalt_mq *mq;
559
	spl_t s;
560

561
	mq = mqd->mq;
562

563
	xnlock_get_irqsave(&nklock, s);
564 565 566 567 568 569
	/* Can we do pipelined sending? */
	if (xnsynch_pended_p(&mq->receivers)) {
		thread = xnsynch_wakeup_one_sleeper(&mq->receivers);
		wc = xnthread_get_wait_context(thread);
		mwc = container_of(wc, struct cobalt_mqwait_context, wc);
		mwc->msg = msg;
570
		xnthread_complete_wait(wc);
571 572 573 574
	} else {
		/* Nope, have to go through the queue. */
		list_add_priff(msg, &mq->queued, prio, link);
		mq->nrqueued++;
575

576
		/*
577 578
		 * If first message and no pending reader, send a
		 * signal if notification was enabled via mq_notify().
579
		 */
580 581 582 583 584 585
		if (list_is_singular(&mq->queued)) {
			xnselect_signal(&mq->read_select, 1);
			if (mq->target) {
				sigp = cobalt_signal_alloc();
				if (sigp) {
					cobalt_copy_siginfo(SI_MESGQ, &sigp->si, &mq->si);
586 587
					if (cobalt_signal_send(mq->target, sigp, 0) <= 0)
						cobalt_signal_free(sigp);
588 589 590
				}
				mq->target = NULL;
			}
591
		}
592
	}
593
	xnsched_run();
594
	xnlock_put_irqrestore(&nklock, s);
595

596
	return 0;
597 598
}

599
static struct cobalt_msg *
600
mq_timedrcv_inner(struct cobalt_mqd *mqd,
601 602 603 604
		  size_t len,
		  const void __user *u_ts,
		  int (*fetch_timeout)(struct timespec *ts,
				       const void __user *u_ts))
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
605
{
606
	struct cobalt_mqwait_context mwc;
607
	struct cobalt_msg *msg;
608
	struct cobalt_mq *mq;
609
	struct timespec ts;
610 611
	xntmode_t tmode;
	xnticks_t to;
612
	spl_t s;
613
	int ret;
614

615 616 617
	to = XN_INFINITE;
	tmode = XN_RELATIVE;
redo:
618
	xnlock_get_irqsave(&nklock, s);
619
	msg = mq_tryrcv(mqd, len);
620 621
	if (msg != ERR_PTR(-EAGAIN))
		goto out;
622

623
	if (rtdm_fd_flags(&mqd->fd) & O_NONBLOCK)
624
		goto out;
625

626 627 628 629 630 631 632 633
	if (fetch_timeout) {
		xnlock_put_irqrestore(&nklock, s);
		ret = fetch_timeout(&ts, u_ts);
		if (ret)
			return ERR_PTR(ret);
		if (ts.tv_nsec >= ONE_BILLION)
			return ERR_PTR(-EINVAL);
		to = ts2ns(&ts) + 1;
634
		tmode = XN_REALTIME;
635 636
		fetch_timeout = NULL;
		goto redo;
637
	}
638

639
	mq = mqd->mq;
640 641
	xnthread_prepare_wait(&mwc.wc);
	ret = xnsynch_sleep_on(&mq->receivers, to, tmode);
642 643 644
	if (ret == 0)
		msg = mwc.msg;
	else if (ret & XNRMID)
645 646 647 648 649 650
		msg = ERR_PTR(-EBADF);
	else if (ret & XNTIMEO)
		msg = ERR_PTR(-ETIMEDOUT);
	else
		msg = ERR_PTR(-EINTR);
out:
651 652 653
	xnlock_put_irqrestore(&nklock, s);

	return msg;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
654 655
}

656
static int
657
mq_finish_rcv(struct cobalt_mqd *mqd, struct cobalt_msg *msg)
658
{
659
	spl_t s;
660

661
	xnlock_get_irqsave(&nklock, s);
662
	mq_release_msg(mqd->mq, msg);
663
	xnsched_run();
664 665
	xnlock_put_irqrestore(&nklock, s);

666
	return 0;
667 668
}

669
static inline int mq_getattr(struct cobalt_mqd *mqd, struct mq_attr *attr)
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
670
{
671
	struct cobalt_mq *mq;
672 673
	spl_t s;

674
	mq = mqd->mq;
675
	*attr = mq->attr;
676
	xnlock_get_irqsave(&nklock, s);
677
	attr->mq_flags = rtdm_fd_flags(&mqd->fd);
678
	attr->mq_curmsgs = mq->nrqueued;
679 680 681
	xnlock_put_irqrestore(&nklock, s);

	return 0;
Gilles Chanteperdrix's avatar
Gilles Chanteperdrix committed
682 683
}

684 685
static inline int
mq_notify(struct cobalt_mqd *mqd, unsigned index, const struct sigevent *evp)
686
{
687
	struct cobalt_thread *thread = cobalt_current_thread();
688
	struct cobalt_mq *mq;
689 690 691 692 693 694 695 696
	int err;
	spl_t s;

	if (evp && ((evp->sigev_notify != SIGEV_SIGNAL &&
		     evp->sigev_notify != SIGEV_NONE) ||
		    (unsigned int)(evp->sigev_signo - 1) > SIGRTMAX - 1))
		return -EINVAL;

697
	if (xnsched_interrupt_p() || thread == NULL)
698 699 700
		return -EPERM;

	xnlock_get_irqsave(&nklock, s);
701
	mq = mqd->mq;
702 703 704 705 706 707 708 709 710 711
	if (mq->target && mq->target != thread) {
		err = -EBUSY;
		goto unlock_and_error;
	}

	if (evp == NULL || evp->sigev_notify == SIGEV_NONE)
		/* Here, mq->target == cobalt_current_thread() or NULL. */
		mq->target = NULL;
	else {
		mq->target = thread;
712
		mq->target_qd = index;
713
		mq->si.si_signo = evp->sigev_signo;
714
		mq->si.si_errno = 0;
715 716
		mq->si.si_code = SI_MESGQ;
		mq->si.si_value = evp->sigev_value;
717 718 719 720 721 722
		/*
		 * XXX: we differ from the regular kernel here, which
		 * passes the sender's pid/uid data into the
		 * receiver's namespaces. We pass the receiver's creds
		 * into the init namespace instead.
		 */
723
		mq->si.si_pid = task_pid_nr(current);
724
		mq->si.si_uid = get_current_uuid();
725 726 727 728 729 730 731 732 733 734
	}

	xnlock_put_irqrestore(&nklock, s);
	return 0;

      unlock_and_error:
	xnlock_put_irqrestore(&nklock, s);
	return err;
}

735
static inline struct cobalt_mqd *cobalt_mqd_get(mqd_t ufd)
736
{
737 738
	struct rtdm_fd *fd;

739
	fd = rtdm_fd_get(ufd, COBALT_MQD_MAGIC);
740 741
	if (IS_ERR(fd)) {
		int err = PTR_ERR(fd);
742
		if (err == -EBADF && cobalt_current_process() == NULL)
743 744 745
			err = -EPERM;
		return ERR_PTR(err);
	}
746

747 748
	return container_of(fd, struct cobalt_mqd, fd);
}
749

750 751 752
static inline void cobalt_mqd_put(struct cobalt_mqd *mqd)
{
	rtdm_fd_put(&mqd->fd);
753 754
}

755
int __cobalt_mq_notify(mqd_t fd, const struct sigevent *evp)
756
{
757
	struct cobalt_mqd *mqd;
758
	int ret;
759

760
	mqd = cobalt_mqd_get(fd);
761 762 763 764 765 766
	if (IS_ERR(mqd))
		ret = PTR_ERR(mqd);
	else {
		trace_cobalt_mq_notify(fd, evp);
		ret = mq_notify(mqd, fd, evp);
		cobalt_mqd_put(mqd);
767 768
	}

769 770
	return ret;
}
771

772
COBALT_SYSCALL(mq_notify, primary,
773
	       (mqd_t fd, const struct sigevent *__user evp))
774 775
{
	struct sigevent sev;
776

777
	if (evp && cobalt_copy_from_user(&sev, evp, sizeof(sev)))
778
		return -EFAULT;
779

780
	return __cobalt_mq_notify(fd, evp ? &sev : NULL);
781
}
782

783 784
int __cobalt_mq_open(const char __user *u_name, int oflags,
		     mode_t mode, struct mq_attr *attr)
785 786
{
	char name[COBALT_MAXNAME];
787 788 789
	unsigned int len;
	mqd_t uqd;
	int ret;
790

791
	len = cobalt_strncpy_from_user(name, u_name, sizeof(name));
792 793 794 795 796
	if (len < 0)
		return -EFAULT;

	if (len >= sizeof(name))
		return -ENAMETOOLONG;
797

798 799 800
	if (len == 0)
		return -EINVAL;

801 802
	trace_cobalt_mq_open(name, oflags, mode);

803
	uqd = __rtdm_anon_getfd("[cobalt-mq]", oflags);
804 805 806 807 808
	if (uqd < 0)
		return uqd;

	ret = mq_open(uqd, name, oflags, mode, attr);
	if (ret < 0) {
809
		__rtdm_anon_putfd(uqd);
810 811
		return ret;
	}
812

813
	return uqd;
814 815
}

816
COBALT_SYSCALL(mq_open, lostage,
817 818
	       (const char __user *u_name, int oflags,
		mode_t mode, struct mq_attr __user *u_attr))
819 820 821 822
{
	struct mq_attr _attr, *attr = &_attr;

	if ((oflags & O_CREAT) && u_attr) {
823
		if (cobalt_copy_from_user(&_attr, u_attr, sizeof(_attr)))
824 825 826 827 828 829 830
			return -EFAULT;
	} else
		attr = NULL;

	return __cobalt_mq_open(u_name, oflags, mode, attr);
}

831
COBALT_SYSCALL(mq_close, lostage, (mqd_t uqd))
832
{
833 834
	trace_cobalt_mq_close(uqd);

835
	return mq_close(uqd);
836 837
}

838
COBALT_SYSCALL(mq_unlink, lostage, (const char __user *u_name))
839 840 841 842
{
	char name[COBALT_MAXNAME];
	unsigned len;

843
	len = cobalt_strncpy_from_user(name, u_name, sizeof(name));
844 845 846 847 848
	if (len < 0)
		return -EFAULT;
	if (len >= sizeof(name))
		return -ENAMETOOLONG;

849 850
	trace_cobalt_mq_unlink(name);

851
	return mq_unlink(name);
852 853
}

854
int __cobalt_mq_getattr(mqd_t uqd, struct mq_attr *attr)
855
{
856
	struct cobalt_mqd *mqd;
857
	int ret;
858