/*
|
* Copyright (C) 2011 Philippe Gerum <rpm@xenomai.org>.
|
* Copyright (C) 2008, 2009 Jan Kiszka <jan.kiszka@siemens.com>.
|
*
|
* This library is free software; you can redistribute it and/or
|
* modify it under the terms of the GNU Lesser General Public
|
* License as published by the Free Software Foundation; either
|
* version 2 of the License, or (at your option) any later version.
|
*
|
* This library 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
|
* Lesser General Public License for more details.
|
|
* You should have received a copy of the GNU Lesser General Public
|
* License along with this library; if not, write to the Free Software
|
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
|
*
|
* --
|
* Internal Cobalt services. No sanity check will be done with
|
* respect to object validity, callers have to take care of this.
|
*/
|
#include <sys/types.h>
|
#include <stddef.h>
|
#include <stdlib.h>
|
#include <limits.h>
|
#include <stdio.h>
|
#include <unistd.h>
|
#include <signal.h>
|
#include <errno.h>
|
#include <stdarg.h>
|
#include <pthread.h>
|
#include <asm/xenomai/syscall.h>
|
#include <asm/xenomai/tsc.h>
|
#include <cobalt/ticks.h>
|
#include <cobalt/sys/cobalt.h>
|
#include "internal.h"
|
|
int cobalt_extend(unsigned int magic)
|
{
|
return XENOMAI_SYSCALL1(sc_cobalt_extend, magic);
|
}
|
|
int cobalt_corectl(int request, void *buf, size_t bufsz)
|
{
|
return XENOMAI_SYSCALL3(sc_cobalt_corectl, request, buf, bufsz);
|
}
|
|
void cobalt_thread_harden(void)
|
{
|
int status = cobalt_get_current_mode();
|
|
/* non-RT shadows are NOT allowed to force primary mode. */
|
if ((status & (XNRELAX|XNWEAK)) == XNRELAX)
|
XENOMAI_SYSCALL1(sc_cobalt_migrate, COBALT_PRIMARY);
|
}
|
|
void cobalt_thread_relax(void)
|
{
|
if (!cobalt_is_relaxed())
|
XENOMAI_SYSCALL1(sc_cobalt_migrate, COBALT_SECONDARY);
|
}
|
|
int cobalt_thread_stat(pid_t pid, struct cobalt_threadstat *stat)
|
{
|
return XENOMAI_SYSCALL2(sc_cobalt_thread_getstat, pid, stat);
|
}
|
|
pid_t cobalt_thread_pid(pthread_t thread)
|
{
|
return XENOMAI_SYSCALL1(sc_cobalt_thread_getpid, thread);
|
}
|
|
int cobalt_thread_mode(void)
|
{
|
return cobalt_get_current_mode();
|
}
|
|
int cobalt_thread_join(pthread_t thread)
|
{
|
int ret, oldtype;
|
|
/*
|
* Serialize with the regular task exit path, so that no call
|
* for the joined pthread may succeed after this routine
|
* returns. A successful call to sc_cobalt_thread_join
|
* receives -EIDRM, meaning that we eventually joined the
|
* exiting thread as seen by the Cobalt core.
|
*
|
* -ESRCH means that the joined thread has already exited
|
* linux-wise, while we were about to wait for it from the
|
* Cobalt side, in which case we are fine.
|
*
|
* -EBUSY denotes a multiple join for several threads in
|
* parallel to the same target.
|
*
|
* -EPERM may be received because the caller is not a
|
* Cobalt thread.
|
*
|
* -EINVAL is received in case the target is not a joinable
|
* thread (i.e. detached).
|
*
|
* Zero is unexpected.
|
*
|
* CAUTION: this service joins a thread Cobat-wise only, not
|
* glibc-wise. For a complete join comprising the libc
|
* cleanups, __STD(pthread_join()) should be paired with this
|
* call.
|
*/
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
do
|
ret = XENOMAI_SYSCALL1(sc_cobalt_thread_join, thread);
|
while (ret == -EINTR);
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
return ret;
|
}
|
|
int cobalt_thread_probe(pid_t pid)
|
{
|
return XENOMAI_SYSCALL2(sc_cobalt_kill, pid, 0);
|
}
|
|
void __cobalt_commit_memory(void *p, size_t len)
|
{
|
volatile char *_p = (volatile char *)p, *end;
|
long pagesz = sysconf(_SC_PAGESIZE);
|
|
end = _p + len;
|
do {
|
*_p = *_p;
|
_p += pagesz;
|
} while (_p < end);
|
}
|
|
int cobalt_serial_debug(const char *fmt, ...)
|
{
|
char msg[128];
|
va_list ap;
|
int n, ret;
|
|
/*
|
* The serial debug output handler disables hw IRQs while
|
* writing to the UART console port, so the message ought to
|
* be reasonably short.
|
*/
|
va_start(ap, fmt);
|
n = vsnprintf(msg, sizeof(msg), fmt, ap);
|
ret = XENOMAI_SYSCALL2(sc_cobalt_serialdbg, msg, n);
|
va_end(ap);
|
|
return ret;
|
}
|
|
static inline
|
struct cobalt_monitor_state *get_monitor_state(cobalt_monitor_t *mon)
|
{
|
return mon->flags & COBALT_MONITOR_SHARED ?
|
cobalt_umm_shared + mon->state_offset :
|
cobalt_umm_private + mon->state_offset;
|
}
|
|
int cobalt_monitor_init(cobalt_monitor_t *mon, clockid_t clk_id, int flags)
|
{
|
struct cobalt_monitor_state *state;
|
int ret;
|
|
ret = XENOMAI_SYSCALL3(sc_cobalt_monitor_init,
|
mon, clk_id, flags);
|
if (ret)
|
return ret;
|
|
state = get_monitor_state(mon);
|
cobalt_commit_memory(state);
|
|
return 0;
|
}
|
|
int cobalt_monitor_destroy(cobalt_monitor_t *mon)
|
{
|
return XENOMAI_SYSCALL1(sc_cobalt_monitor_destroy, mon);
|
}
|
|
int cobalt_monitor_enter(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state;
|
int status, ret, oldtype;
|
xnhandle_t cur;
|
|
/*
|
* Assumptions on entry:
|
*
|
* - this is a Cobalt thread (caller checked this).
|
* - no recursive entry/locking.
|
*/
|
|
status = cobalt_get_current_mode();
|
if (status & (XNRELAX|XNWEAK|XNDEBUG))
|
goto syscall;
|
|
state = get_monitor_state(mon);
|
cur = cobalt_get_current();
|
ret = xnsynch_fast_acquire(&state->owner, cur);
|
if (ret == 0) {
|
state->flags &= ~(COBALT_MONITOR_SIGNALED|COBALT_MONITOR_BROADCAST);
|
return 0;
|
}
|
syscall:
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
/*
|
* Jump to kernel to wait for entry. We redo in case of
|
* interrupt.
|
*/
|
do
|
ret = XENOMAI_SYSCALL1(sc_cobalt_monitor_enter, mon);
|
while (ret == -EINTR);
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
return ret;
|
}
|
|
int cobalt_monitor_exit(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state;
|
int status, ret;
|
xnhandle_t cur;
|
|
__sync_synchronize();
|
|
state = get_monitor_state(mon);
|
if ((state->flags & COBALT_MONITOR_PENDED) &&
|
(state->flags & COBALT_MONITOR_SIGNALED))
|
goto syscall;
|
|
status = cobalt_get_current_mode();
|
if (status & (XNWEAK|XNDEBUG))
|
goto syscall;
|
|
cur = cobalt_get_current();
|
if (xnsynch_fast_release(&state->owner, cur))
|
return 0;
|
syscall:
|
do
|
ret = XENOMAI_SYSCALL1(sc_cobalt_monitor_exit, mon);
|
while (ret == -EINTR);
|
|
return ret;
|
}
|
|
int cobalt_monitor_wait(cobalt_monitor_t *mon, int event,
|
const struct timespec *ts)
|
{
|
int ret, opret, oldtype;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
#ifdef __USE_TIME_BITS64
|
ret = XENOMAI_SYSCALL4(sc_cobalt_monitor_wait64, mon, event, ts,
|
&opret);
|
#else
|
ret = XENOMAI_SYSCALL4(sc_cobalt_monitor_wait, mon, event, ts, &opret);
|
#endif
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
/*
|
* If we got interrupted while trying to re-enter the monitor,
|
* we need to redo. In the meantime, any pending linux signal
|
* has been processed.
|
*/
|
if (ret == -EINTR)
|
ret = cobalt_monitor_enter(mon);
|
|
return ret ?: opret;
|
}
|
|
void cobalt_monitor_grant(cobalt_monitor_t *mon,
|
struct xnthread_user_window *u_window)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
|
state->flags |= COBALT_MONITOR_GRANTED;
|
u_window->grant_value = 1;
|
}
|
|
int cobalt_monitor_grant_sync(cobalt_monitor_t *mon,
|
struct xnthread_user_window *u_window)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
int ret, oldtype;
|
|
cobalt_monitor_grant(mon, u_window);
|
|
if ((state->flags & COBALT_MONITOR_PENDED) == 0)
|
return 0;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
ret = XENOMAI_SYSCALL1(sc_cobalt_monitor_sync, mon);
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
if (ret == -EINTR)
|
return cobalt_monitor_enter(mon);
|
|
return ret;
|
}
|
|
void cobalt_monitor_grant_all(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
|
state->flags |= COBALT_MONITOR_GRANTED|COBALT_MONITOR_BROADCAST;
|
}
|
|
int cobalt_monitor_grant_all_sync(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
int ret, oldtype;
|
|
cobalt_monitor_grant_all(mon);
|
|
if ((state->flags & COBALT_MONITOR_PENDED) == 0)
|
return 0;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
ret = XENOMAI_SYSCALL1(sc_cobalt_monitor_sync, mon);
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
if (ret == -EINTR)
|
return cobalt_monitor_enter(mon);
|
|
return ret;
|
}
|
|
void cobalt_monitor_drain(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
|
state->flags |= COBALT_MONITOR_DRAINED;
|
}
|
|
int cobalt_monitor_drain_sync(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
int ret, oldtype;
|
|
cobalt_monitor_drain(mon);
|
|
if ((state->flags & COBALT_MONITOR_PENDED) == 0)
|
return 0;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
ret = XENOMAI_SYSCALL1(sc_cobalt_monitor_sync, mon);
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
if (ret == -EINTR)
|
return cobalt_monitor_enter(mon);
|
|
return ret;
|
}
|
|
void cobalt_monitor_drain_all(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
|
state->flags |= COBALT_MONITOR_DRAINED|COBALT_MONITOR_BROADCAST;
|
}
|
|
int cobalt_monitor_drain_all_sync(cobalt_monitor_t *mon)
|
{
|
struct cobalt_monitor_state *state = get_monitor_state(mon);
|
int ret, oldtype;
|
|
cobalt_monitor_drain_all(mon);
|
|
if ((state->flags & COBALT_MONITOR_PENDED) == 0)
|
return 0;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
ret = XENOMAI_SYSCALL1(sc_cobalt_monitor_sync, mon);
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
if (ret == -EINTR)
|
return cobalt_monitor_enter(mon);
|
|
return ret;
|
}
|
|
#define __raw_write_out(__msg) \
|
do { \
|
int __ret; \
|
__ret = write(1, __msg , sizeof(__msg) - 1); \
|
(void)__ret; \
|
} while (0)
|
|
#define raw_write_out(__msg) __raw_write_out("Xenomai/cobalt: " __msg "\n")
|
|
void cobalt_sigdebug_handler(int sig, siginfo_t *si, void *context)
|
{
|
if (!sigdebug_marked(si))
|
goto forward;
|
|
switch (sigdebug_reason(si)) {
|
case SIGDEBUG_NOMLOCK:
|
raw_write_out("process memory not locked");
|
_exit(4);
|
case SIGDEBUG_RESCNT_IMBALANCE:
|
raw_write_out("resource locking imbalance");
|
break;
|
case SIGDEBUG_MUTEX_SLEEP:
|
raw_write_out("sleeping while holding mutex");
|
break;
|
case SIGDEBUG_WATCHDOG:
|
raw_write_out("watchdog triggered");
|
break;
|
}
|
|
forward:
|
sigaction(SIGDEBUG, &__cobalt_orig_sigdebug, NULL);
|
pthread_kill(pthread_self(), SIGDEBUG);
|
}
|
|
static inline
|
struct cobalt_event_state *get_event_state(cobalt_event_t *event)
|
{
|
return event->flags & COBALT_EVENT_SHARED ?
|
cobalt_umm_shared + event->state_offset :
|
cobalt_umm_private + event->state_offset;
|
}
|
|
int cobalt_event_init(cobalt_event_t *event, unsigned int value,
|
int flags)
|
{
|
struct cobalt_event_state *state;
|
int ret;
|
|
ret = XENOMAI_SYSCALL3(sc_cobalt_event_init, event, value, flags);
|
if (ret)
|
return ret;
|
|
state = get_event_state(event);
|
cobalt_commit_memory(state);
|
|
return 0;
|
}
|
|
int cobalt_event_destroy(cobalt_event_t *event)
|
{
|
return XENOMAI_SYSCALL1(sc_cobalt_event_destroy, event);
|
}
|
|
int cobalt_event_post(cobalt_event_t *event, unsigned int bits)
|
{
|
struct cobalt_event_state *state = get_event_state(event);
|
|
if (bits == 0)
|
return 0;
|
|
__sync_or_and_fetch(&state->value, bits); /* full barrier. */
|
|
if ((state->flags & COBALT_EVENT_PENDED) == 0)
|
return 0;
|
|
return XENOMAI_SYSCALL1(sc_cobalt_event_sync, event);
|
}
|
|
int cobalt_event_wait(cobalt_event_t *event,
|
unsigned int bits, unsigned int *bits_r,
|
int mode, const struct timespec *timeout)
|
{
|
int ret, oldtype;
|
|
pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, &oldtype);
|
|
#ifdef __USE_TIME_BITS64
|
ret = XENOMAI_SYSCALL5(sc_cobalt_event_wait64,
|
event, bits, bits_r, mode, timeout);
|
#else
|
ret = XENOMAI_SYSCALL5(sc_cobalt_event_wait,
|
event, bits, bits_r, mode, timeout);
|
#endif
|
|
pthread_setcanceltype(oldtype, NULL);
|
|
return ret;
|
}
|
|
unsigned long cobalt_event_clear(cobalt_event_t *event,
|
unsigned int bits)
|
{
|
struct cobalt_event_state *state = get_event_state(event);
|
|
return __sync_fetch_and_and(&state->value, ~bits);
|
}
|
|
int cobalt_event_inquire(cobalt_event_t *event,
|
struct cobalt_event_info *info,
|
pid_t *waitlist, size_t waitsz)
|
{
|
return XENOMAI_SYSCALL4(sc_cobalt_event_inquire, event,
|
info, waitlist, waitsz);
|
}
|
|
int cobalt_sem_inquire(sem_t *sem, struct cobalt_sem_info *info,
|
pid_t *waitlist, size_t waitsz)
|
{
|
struct cobalt_sem_shadow *_sem = &((union cobalt_sem_union *)sem)->shadow_sem;
|
|
return XENOMAI_SYSCALL4(sc_cobalt_sem_inquire, _sem,
|
info, waitlist, waitsz);
|
}
|
|
int cobalt_sched_weighted_prio(int policy,
|
const struct sched_param_ex *param_ex)
|
{
|
return XENOMAI_SYSCALL2(sc_cobalt_sched_weightprio, policy, param_ex);
|
}
|
|
int cobalt_xlate_schedparam(int policy,
|
const struct sched_param_ex *param_ex,
|
struct sched_param *param)
|
{
|
int std_policy, priority;
|
|
/*
|
* Translates Cobalt scheduling parameters to native ones,
|
* based on a best approximation for Cobalt policies which are
|
* not available from the host kernel.
|
*/
|
std_policy = policy;
|
priority = param_ex->sched_priority;
|
|
switch (policy) {
|
case SCHED_WEAK:
|
std_policy = priority ? SCHED_FIFO : SCHED_OTHER;
|
break;
|
default:
|
std_policy = SCHED_FIFO;
|
/* falldown wanted. */
|
case SCHED_OTHER:
|
case SCHED_FIFO:
|
case SCHED_RR:
|
/*
|
* The Cobalt priority range is larger than those of
|
* the native SCHED_FIFO/RR classes, so we have to cap
|
* the priority value accordingly. We also remap
|
* "weak" (negative) priorities - which are only
|
* meaningful for the Cobalt core - to regular values.
|
*/
|
if (priority > __cobalt_std_fifo_maxpri)
|
priority = __cobalt_std_fifo_maxpri;
|
}
|
|
if (priority < 0)
|
priority = -priority;
|
|
memset(param, 0, sizeof(*param));
|
param->sched_priority = priority;
|
|
return std_policy;
|
}
|
|
void cobalt_assert_nrt(void)
|
{
|
if (cobalt_should_warn())
|
pthread_kill(pthread_self(), SIGDEBUG);
|
}
|
|
unsigned long long cobalt_read_tsc(void)
|
{
|
struct timespec ts;
|
|
if (cobalt_use_legacy_tsc())
|
return cobalt_read_legacy_tsc();
|
|
__cobalt_vdso_gettime(CLOCK_MONOTONIC, &ts);
|
|
return ts.tv_sec * 1000000000ULL + ts.tv_nsec;
|
}
|
|
unsigned int cobalt_features;
|
|
void cobalt_features_init(struct cobalt_featinfo *f)
|
{
|
cobalt_features = f->feat_all;
|
|
/* Trigger arch specific feature initialization */
|
cobalt_arch_check_features(f);
|
}
|
