/*
|
* Written by Gilles Chanteperdrix <gilles.chanteperdrix@xenomai.org>.
|
* Copyright (C) 2014,2015 Philippe Gerum <rpm@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/stddef.h>
|
#include <linux/err.h>
|
#include <cobalt/kernel/time.h>
|
#include "internal.h"
|
#include "thread.h"
|
#include "clock.h"
|
#include "sem.h"
|
#include <trace/events/cobalt-posix.h>
|
|
#ifdef CONFIG_XENO_OPT_VFILE
|
|
static int sem_vfile_show(struct xnvfile_regular_iterator *it, void *data)
|
{
|
return 0;
|
}
|
|
static struct xnvfile_regular_ops sem_vfile_ops = {
|
.show = sem_vfile_show,
|
};
|
|
static struct xnpnode_regular __sem_pnode = {
|
.node = {
|
.dirname = "sem",
|
.root = &posix_ptree,
|
.ops = &xnregistry_vfreg_ops,
|
},
|
.vfile = {
|
.ops = &sem_vfile_ops,
|
},
|
};
|
|
#else /* !CONFIG_XENO_OPT_VFILE */
|
|
static struct xnpnode_link __sem_pnode = {
|
.node = {
|
.dirname = "sem",
|
}
|
};
|
|
#endif /* !CONFIG_XENO_OPT_VFILE */
|
|
static inline struct cobalt_resources *sem_kqueue(struct cobalt_sem *sem)
|
{
|
int pshared = !!(sem->flags & SEM_PSHARED);
|
return cobalt_current_resources(pshared);
|
}
|
|
static inline int sem_check(struct cobalt_sem *sem)
|
{
|
if (sem == NULL || sem->magic != COBALT_SEM_MAGIC)
|
return -EINVAL;
|
|
if (sem->resnode.scope && sem->resnode.scope != sem_kqueue(sem))
|
return -EPERM;
|
|
return 0;
|
}
|
|
int __cobalt_sem_destroy(xnhandle_t handle)
|
{
|
struct cobalt_sem *sem;
|
int ret = 0;
|
spl_t s;
|
|
xnlock_get_irqsave(&nklock, s);
|
sem = xnregistry_lookup(handle, NULL);
|
if (!cobalt_obj_active(sem, COBALT_SEM_MAGIC, typeof(*sem))) {
|
ret = -EINVAL;
|
goto fail;
|
}
|
|
if (--sem->refs) {
|
ret = -EBUSY;
|
goto fail;
|
}
|
|
cobalt_mark_deleted(sem);
|
if (!sem->pathname)
|
cobalt_del_resource(&sem->resnode);
|
if (xnsynch_destroy(&sem->synchbase) == XNSYNCH_RESCHED) {
|
xnsched_run();
|
ret = 1;
|
}
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
xnregistry_remove(sem->resnode.handle);
|
if (sem->pathname)
|
putname(sem->pathname);
|
|
cobalt_umm_free(&cobalt_ppd_get(!!(sem->flags & SEM_PSHARED))->umm,
|
sem->state);
|
|
xnfree(sem);
|
|
return ret;
|
fail:
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
struct cobalt_sem *
|
__cobalt_sem_init(const char *name, struct cobalt_sem_shadow *sm,
|
int flags, unsigned int value)
|
{
|
struct cobalt_sem_state *state;
|
struct cobalt_sem *sem, *osem;
|
struct cobalt_ppd *sys_ppd;
|
int ret, sflags, pshared;
|
struct list_head *semq;
|
spl_t s;
|
|
if ((flags & SEM_PULSE) != 0 && value > 0) {
|
ret = -EINVAL;
|
goto out;
|
}
|
|
sem = xnmalloc(sizeof(*sem));
|
if (sem == NULL) {
|
ret = -ENOMEM;
|
goto out;
|
}
|
|
pshared = !!(flags & SEM_PSHARED);
|
sys_ppd = cobalt_ppd_get(pshared);
|
state = cobalt_umm_alloc(&sys_ppd->umm, sizeof(*state));
|
if (state == NULL) {
|
ret = -EAGAIN;
|
goto err_free_sem;
|
}
|
|
xnlock_get_irqsave(&nklock, s);
|
|
semq = &cobalt_current_resources(pshared)->semq;
|
if ((sm->magic == COBALT_SEM_MAGIC && !list_empty(semq)) ||
|
sm->magic == COBALT_NAMED_SEM_MAGIC) {
|
osem = xnregistry_lookup(sm->handle, NULL);
|
if (cobalt_obj_active(osem, COBALT_SEM_MAGIC, typeof(*osem))) {
|
ret = -EBUSY;
|
goto err_lock_put;
|
}
|
}
|
|
if (value > (unsigned)SEM_VALUE_MAX) {
|
ret = -EINVAL;
|
goto err_lock_put;
|
}
|
|
ret = xnregistry_enter(name ?: "", sem, &sem->resnode.handle,
|
name ? &__sem_pnode.node : NULL);
|
if (ret < 0)
|
goto err_lock_put;
|
|
sem->magic = COBALT_SEM_MAGIC;
|
if (!name)
|
cobalt_add_resource(&sem->resnode, sem, pshared);
|
else
|
sem->resnode.scope = NULL;
|
sflags = flags & SEM_FIFO ? 0 : XNSYNCH_PRIO;
|
xnsynch_init(&sem->synchbase, sflags, NULL);
|
|
sem->state = state;
|
atomic_set(&state->value, value);
|
state->flags = flags;
|
sem->flags = flags;
|
sem->refs = name ? 2 : 1;
|
sem->pathname = NULL;
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
__cobalt_sem_shadow_init(sem,
|
name ? COBALT_NAMED_SEM_MAGIC : COBALT_SEM_MAGIC, sm);
|
|
trace_cobalt_psem_init(name ?: "anon",
|
sem->resnode.handle, flags, value);
|
|
return sem;
|
|
err_lock_put:
|
xnlock_put_irqrestore(&nklock, s);
|
cobalt_umm_free(&sys_ppd->umm, state);
|
err_free_sem:
|
xnfree(sem);
|
out:
|
trace_cobalt_psem_init_failed(name ?: "anon", flags, value, ret);
|
|
return ERR_PTR(ret);
|
}
|
|
void __cobalt_sem_shadow_init(struct cobalt_sem *sem, __u32 magic,
|
struct cobalt_sem_shadow *sm)
|
{
|
__u32 flags = sem->state->flags;
|
struct cobalt_ppd *sys_ppd;
|
|
sys_ppd = cobalt_ppd_get(!!(flags & SEM_PSHARED));
|
|
sm->magic = magic;
|
sm->handle = sem->resnode.handle;
|
sm->state_offset = cobalt_umm_offset(&sys_ppd->umm, sem->state);
|
if (sem->state->flags & SEM_PSHARED)
|
sm->state_offset = -sm->state_offset;
|
}
|
|
static int sem_destroy(struct cobalt_sem_shadow *sm)
|
{
|
struct cobalt_sem *sem;
|
int warn, ret;
|
spl_t s;
|
|
xnlock_get_irqsave(&nklock, s);
|
|
if (sm->magic != COBALT_SEM_MAGIC) {
|
ret = -EINVAL;
|
goto fail;
|
}
|
|
sem = xnregistry_lookup(sm->handle, NULL);
|
ret = sem_check(sem);
|
if (ret)
|
goto fail;
|
|
if ((sem->flags & SEM_NOBUSYDEL) != 0 &&
|
xnsynch_pended_p(&sem->synchbase)) {
|
ret = -EBUSY;
|
goto fail;
|
}
|
|
warn = sem->flags & SEM_WARNDEL;
|
cobalt_mark_deleted(sm);
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
ret = __cobalt_sem_destroy(sem->resnode.handle);
|
|
return warn ? ret : 0;
|
fail:
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
static inline int do_trywait(struct cobalt_sem *sem)
|
{
|
int ret;
|
|
ret = sem_check(sem);
|
if (ret)
|
return ret;
|
|
if (atomic_sub_return(1, &sem->state->value) < 0)
|
return -EAGAIN;
|
|
return 0;
|
}
|
|
static int sem_wait(xnhandle_t handle)
|
{
|
struct cobalt_sem *sem;
|
int ret, info;
|
spl_t s;
|
|
xnlock_get_irqsave(&nklock, s);
|
|
sem = xnregistry_lookup(handle, NULL);
|
ret = do_trywait(sem);
|
if (ret != -EAGAIN)
|
goto out;
|
|
ret = 0;
|
info = xnsynch_sleep_on(&sem->synchbase, XN_INFINITE, XN_RELATIVE);
|
if (info & XNRMID) {
|
ret = -EINVAL;
|
} else if (info & XNBREAK) {
|
atomic_inc(&sem->state->value); /* undo do_trywait() */
|
ret = -EINTR;
|
}
|
out:
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
int __cobalt_sem_timedwait(struct cobalt_sem_shadow __user *u_sem,
|
const struct timespec64 *ts)
|
{
|
int ret, info;
|
bool validate_ts = true;
|
struct cobalt_sem *sem;
|
xnhandle_t handle;
|
xntmode_t tmode;
|
spl_t s;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
trace_cobalt_psem_timedwait(handle);
|
|
xnlock_get_irqsave(&nklock, s);
|
|
for (;;) {
|
sem = xnregistry_lookup(handle, NULL);
|
ret = do_trywait(sem);
|
if (ret != -EAGAIN)
|
break;
|
|
/*
|
* POSIX states that the validity of the timeout spec
|
* _need_ not be checked if the semaphore can be
|
* locked immediately, we show this behavior despite
|
* it's actually more complex, to keep some
|
* applications ported to Linux happy.
|
*/
|
if (validate_ts) {
|
atomic_inc(&sem->state->value);
|
if (!ts) {
|
ret = -EFAULT;
|
break;
|
}
|
if (!timespec64_valid(ts)) {
|
ret = -EINVAL;
|
break;
|
}
|
validate_ts = false;
|
continue;
|
}
|
|
ret = 0;
|
tmode = sem->flags & SEM_RAWCLOCK ? XN_ABSOLUTE : XN_REALTIME;
|
info = xnsynch_sleep_on(&sem->synchbase, ts2ns(ts) + 1, tmode);
|
if (info & XNRMID)
|
ret = -EINVAL;
|
else if (info & (XNBREAK|XNTIMEO)) {
|
ret = (info & XNBREAK) ? -EINTR : -ETIMEDOUT;
|
atomic_inc(&sem->state->value);
|
}
|
break;
|
}
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
int __cobalt_sem_timedwait64(struct cobalt_sem_shadow __user *u_sem,
|
const struct __kernel_timespec __user *u_ts)
|
{
|
int ret = 1;
|
struct timespec64 ts64;
|
|
if (u_ts)
|
ret = cobalt_get_timespec64(&ts64, u_ts);
|
|
return __cobalt_sem_timedwait(u_sem, ret ? NULL : &ts64);
|
}
|
|
static int sem_post(xnhandle_t handle)
|
{
|
struct cobalt_sem *sem;
|
int ret;
|
spl_t s;
|
|
xnlock_get_irqsave(&nklock, s);
|
|
sem = xnregistry_lookup(handle, NULL);
|
ret = sem_check(sem);
|
if (ret)
|
goto out;
|
|
if (atomic_read(&sem->state->value) == SEM_VALUE_MAX) {
|
ret = -EINVAL;
|
goto out;
|
}
|
|
if (atomic_inc_return(&sem->state->value) <= 0) {
|
if (xnsynch_wakeup_one_sleeper(&sem->synchbase))
|
xnsched_run();
|
} else if (sem->flags & SEM_PULSE)
|
atomic_set(&sem->state->value, 0);
|
out:
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
static int sem_getvalue(xnhandle_t handle, int *value)
|
{
|
struct cobalt_sem *sem;
|
int ret;
|
spl_t s;
|
|
xnlock_get_irqsave(&nklock, s);
|
|
sem = xnregistry_lookup(handle, NULL);
|
ret = sem_check(sem);
|
if (ret) {
|
xnlock_put_irqrestore(&nklock, s);
|
return ret;
|
}
|
|
*value = atomic_read(&sem->state->value);
|
if ((sem->flags & SEM_REPORT) == 0 && *value < 0)
|
*value = 0;
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
return 0;
|
}
|
|
COBALT_SYSCALL(sem_init, current,
|
(struct cobalt_sem_shadow __user *u_sem,
|
int flags, unsigned int value))
|
{
|
struct cobalt_sem_shadow sm;
|
struct cobalt_sem *sem;
|
|
if (cobalt_copy_from_user(&sm, u_sem, sizeof(sm)))
|
return -EFAULT;
|
|
if (flags & ~(SEM_FIFO|SEM_PULSE|SEM_PSHARED|SEM_REPORT|\
|
SEM_WARNDEL|SEM_RAWCLOCK|SEM_NOBUSYDEL))
|
return -EINVAL;
|
|
sem = __cobalt_sem_init(NULL, &sm, flags, value);
|
if (IS_ERR(sem))
|
return PTR_ERR(sem);
|
|
return cobalt_copy_to_user(u_sem, &sm, sizeof(*u_sem));
|
}
|
|
COBALT_SYSCALL(sem_post, current,
|
(struct cobalt_sem_shadow __user *u_sem))
|
{
|
xnhandle_t handle;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
trace_cobalt_psem_post(handle);
|
|
return sem_post(handle);
|
}
|
|
COBALT_SYSCALL(sem_wait, primary,
|
(struct cobalt_sem_shadow __user *u_sem))
|
{
|
xnhandle_t handle;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
trace_cobalt_psem_wait(handle);
|
|
return sem_wait(handle);
|
}
|
|
COBALT_SYSCALL(sem_timedwait, primary,
|
(struct cobalt_sem_shadow __user *u_sem,
|
const struct __user_old_timespec __user *u_ts))
|
{
|
int ret = 1;
|
struct timespec64 ts64;
|
|
if (u_ts)
|
ret = cobalt_get_u_timespec(&ts64, u_ts);
|
|
return __cobalt_sem_timedwait(u_sem, ret ? NULL : &ts64);
|
}
|
|
COBALT_SYSCALL(sem_timedwait64, primary,
|
(struct cobalt_sem_shadow __user *u_sem,
|
const struct __kernel_timespec __user *u_ts))
|
{
|
return __cobalt_sem_timedwait64(u_sem, u_ts);
|
}
|
|
COBALT_SYSCALL(sem_trywait, primary,
|
(struct cobalt_sem_shadow __user *u_sem))
|
{
|
struct cobalt_sem *sem;
|
xnhandle_t handle;
|
int ret;
|
spl_t s;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
trace_cobalt_psem_trywait(handle);
|
|
xnlock_get_irqsave(&nklock, s);
|
sem = xnregistry_lookup(handle, NULL);
|
ret = do_trywait(sem);
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
COBALT_SYSCALL(sem_getvalue, current,
|
(struct cobalt_sem_shadow __user *u_sem,
|
int __user *u_sval))
|
{
|
int ret, sval = -1;
|
xnhandle_t handle;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
|
ret = sem_getvalue(handle, &sval);
|
trace_cobalt_psem_getvalue(handle, sval);
|
if (ret)
|
return ret;
|
|
return cobalt_copy_to_user(u_sval, &sval, sizeof(sval));
|
}
|
|
COBALT_SYSCALL(sem_destroy, current,
|
(struct cobalt_sem_shadow __user *u_sem))
|
{
|
struct cobalt_sem_shadow sm;
|
int err;
|
|
if (cobalt_copy_from_user(&sm, u_sem, sizeof(sm)))
|
return -EFAULT;
|
|
trace_cobalt_psem_destroy(sm.handle);
|
|
err = sem_destroy(&sm);
|
if (err < 0)
|
return err;
|
|
return cobalt_copy_to_user(u_sem, &sm, sizeof(*u_sem)) ?: err;
|
}
|
|
COBALT_SYSCALL(sem_broadcast_np, current,
|
(struct cobalt_sem_shadow __user *u_sem))
|
{
|
struct cobalt_sem *sem;
|
xnhandle_t handle;
|
spl_t s;
|
int ret;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
trace_cobalt_psem_broadcast(handle);
|
|
xnlock_get_irqsave(&nklock, s);
|
|
sem = xnregistry_lookup(handle, NULL);
|
ret = sem_check(sem);
|
if (ret == 0 && atomic_read(&sem->state->value) < 0) {
|
atomic_set(&sem->state->value, 0);
|
xnsynch_flush(&sem->synchbase, 0);
|
xnsched_run();
|
}
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
return ret;
|
}
|
|
COBALT_SYSCALL(sem_inquire, current,
|
(struct cobalt_sem_shadow __user *u_sem,
|
struct cobalt_sem_info __user *u_info,
|
pid_t __user *u_waitlist,
|
size_t waitsz))
|
{
|
int val = 0, nrwait = 0, nrpids, ret = 0;
|
unsigned long pstamp, nstamp = 0;
|
struct cobalt_sem_info info;
|
pid_t *t = NULL, fbuf[16];
|
struct xnthread *thread;
|
struct cobalt_sem *sem;
|
xnhandle_t handle;
|
spl_t s;
|
|
handle = cobalt_get_handle_from_user(&u_sem->handle);
|
trace_cobalt_psem_inquire(handle);
|
|
nrpids = waitsz / sizeof(pid_t);
|
|
xnlock_get_irqsave(&nklock, s);
|
|
for (;;) {
|
pstamp = nstamp;
|
sem = xnregistry_lookup(handle, &nstamp);
|
if (sem == NULL || sem->magic != COBALT_SEM_MAGIC) {
|
xnlock_put_irqrestore(&nklock, s);
|
return -EINVAL;
|
}
|
/*
|
* Allocate memory to return the wait list without
|
* holding any lock, then revalidate the handle.
|
*/
|
if (t == NULL) {
|
val = atomic_read(&sem->state->value);
|
if (val >= 0 || u_waitlist == NULL)
|
break;
|
xnlock_put_irqrestore(&nklock, s);
|
if (nrpids > -val)
|
nrpids = -val;
|
if (-val <= ARRAY_SIZE(fbuf))
|
t = fbuf; /* Use fast buffer. */
|
else {
|
t = xnmalloc(-val * sizeof(pid_t));
|
if (t == NULL)
|
return -ENOMEM;
|
}
|
xnlock_get_irqsave(&nklock, s);
|
} else if (pstamp == nstamp)
|
break;
|
else if (val != atomic_read(&sem->state->value)) {
|
xnlock_put_irqrestore(&nklock, s);
|
if (t != fbuf)
|
xnfree(t);
|
t = NULL;
|
xnlock_get_irqsave(&nklock, s);
|
}
|
}
|
|
info.flags = sem->flags;
|
info.value = (sem->flags & SEM_REPORT) || val >= 0 ? val : 0;
|
info.nrwait = val < 0 ? -val : 0;
|
|
if (xnsynch_pended_p(&sem->synchbase) && u_waitlist != NULL) {
|
xnsynch_for_each_sleeper(thread, &sem->synchbase) {
|
if (nrwait >= nrpids)
|
break;
|
t[nrwait++] = xnthread_host_pid(thread);
|
}
|
}
|
|
xnlock_put_irqrestore(&nklock, s);
|
|
ret = cobalt_copy_to_user(u_info, &info, sizeof(info));
|
if (ret == 0 && nrwait > 0)
|
ret = cobalt_copy_to_user(u_waitlist, t, nrwait * sizeof(pid_t));
|
|
if (t && t != fbuf)
|
xnfree(t);
|
|
return ret ?: nrwait;
|
}
|
|
void cobalt_sem_reclaim(struct cobalt_resnode *node, spl_t s)
|
{
|
struct cobalt_sem *sem;
|
xnhandle_t handle;
|
int named, ret;
|
|
sem = container_of(node, struct cobalt_sem, resnode);
|
named = (sem->flags & SEM_NAMED) != 0;
|
handle = node->handle;
|
xnlock_put_irqrestore(&nklock, s);
|
ret = __cobalt_sem_destroy(handle);
|
if (named && ret == -EBUSY)
|
xnregistry_unlink(xnregistry_key(handle));
|
}
|
