/*
|
* Copyright (C) 2008 Philippe Gerum <rpm@xenomai.org>.
|
*
|
* 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.
|
*/
|
|
#include <stdio.h>
|
#include <memory.h>
|
#include <errno.h>
|
#include <string.h>
|
#include <stdlib.h>
|
#include <unistd.h>
|
#include <pthread.h>
|
#include <sched.h>
|
#include "boilerplate/namegen.h"
|
#include "copperplate/heapobj.h"
|
#include "copperplate/threadobj.h"
|
#include "copperplate/syncobj.h"
|
#include "copperplate/clockobj.h"
|
#include "copperplate/cluster.h"
|
#include "copperplate/internal.h"
|
#include "psos/psos.h"
|
#include "internal.h"
|
#include "task.h"
|
#include "tm.h"
|
#include "queue.h"
|
#include "rn.h"
|
|
union psos_wait_union {
|
struct psos_queue_wait queue_wait;
|
struct psos_rn_wait rn_wait;
|
};
|
|
struct cluster psos_task_table;
|
|
static DEFINE_NAME_GENERATOR(task_namegen, "task",
|
struct psos_task, name);
|
|
static struct psos_task *find_psos_task(u_long tid, int *err_r)
|
{
|
struct psos_task *task = mainheap_deref(tid, struct psos_task);
|
unsigned int magic;
|
|
/*
|
* Best-effort to validate a TCB pointer the cheap way,
|
* without relying on any syscall.
|
*/
|
if (task == NULL || ((uintptr_t)task & (sizeof(uintptr_t)-1)) != 0)
|
goto objid_error;
|
|
magic = threadobj_get_magic(&task->thobj);
|
|
if (magic == task_magic)
|
return task;
|
|
if (magic == ~task_magic) {
|
*err_r = ERR_OBJDEL;
|
return NULL;
|
}
|
|
if ((magic >> 16) == 0x8181) {
|
*err_r = ERR_OBJTYPE;
|
return NULL;
|
}
|
|
objid_error:
|
*err_r = ERR_OBJID;
|
|
return NULL;
|
}
|
|
static struct psos_task *find_psos_task_or_self(u_long tid, int *err_r)
|
{
|
struct psos_task *current;
|
|
if (tid)
|
return find_psos_task(tid, err_r);
|
|
current = psos_task_current();
|
if (current == NULL) {
|
*err_r = ERR_SSFN;
|
return NULL;
|
}
|
|
return current;
|
}
|
|
struct psos_task *get_psos_task(u_long tid, int *err_r)
|
{
|
struct psos_task *task = find_psos_task(tid, err_r);
|
|
/*
|
* Grab the task lock, assuming that the task might have been
|
* deleted, and/or maybe we have been lucky, and some random
|
* opaque pointer might lead us to something which is laid in
|
* valid memory but certainly not to a task object. Last
|
* chance is pthread_mutex_lock() detecting a wrong mutex kind
|
* and bailing out.
|
*
|
* XXX: threadobj_lock() disables cancellability for the
|
* caller upon success, until the lock is dropped in
|
* threadobj_unlock(), so there is no way it may vanish while
|
* holding the lock. Therefore we need no cleanup handler
|
* here.
|
*/
|
if (task == NULL || threadobj_lock(&task->thobj) == -EINVAL)
|
return NULL;
|
|
/* Check the magic word again, while we hold the lock. */
|
if (threadobj_get_magic(&task->thobj) != task_magic) {
|
threadobj_unlock(&task->thobj);
|
*err_r = ERR_OBJDEL;
|
return NULL;
|
}
|
|
return task;
|
}
|
|
struct psos_task *get_psos_task_or_self(u_long tid, int *err_r)
|
{
|
struct psos_task *current;
|
|
if (tid)
|
return get_psos_task(tid, err_r);
|
|
current = psos_task_current();
|
if (current == NULL) {
|
*err_r = ERR_SSFN;
|
return NULL;
|
}
|
|
/* This one might block but can't fail, it is ours. */
|
threadobj_lock(¤t->thobj);
|
|
return current;
|
}
|
|
void put_psos_task(struct psos_task *task)
|
{
|
threadobj_unlock(&task->thobj);
|
}
|
|
static void task_finalizer(struct threadobj *thobj)
|
{
|
struct psos_task *task = container_of(thobj, struct psos_task, thobj);
|
struct psos_tm *tm, *tmp;
|
struct syncstate syns;
|
int ret;
|
|
cluster_delobj(&psos_task_table, &task->cobj);
|
|
if (!pvlist_empty(&task->timer_list)) {
|
pvlist_for_each_entry_safe(tm, tmp, &task->timer_list, link)
|
tm_cancel((u_long)tm);
|
}
|
|
/* We have to hold a lock on a syncobj to destroy it. */
|
ret = __bt(syncobj_lock(&task->sobj, &syns));
|
if (ret == 0)
|
syncobj_destroy(&task->sobj, &syns);
|
}
|
|
static int task_prologue(void *arg)
|
{
|
struct psos_task *task = arg;
|
|
return __bt(threadobj_prologue(&task->thobj, task->name));
|
}
|
|
static void *task_trampoline(void *arg)
|
{
|
struct psos_task *task = arg;
|
struct psos_task_args *args = &task->args;
|
struct sched_param_ex param_ex;
|
struct service svc;
|
|
CANCEL_DEFER(svc);
|
threadobj_wait_start();
|
threadobj_lock(&task->thobj);
|
|
if (task->mode & T_TSLICE) {
|
param_ex.sched_priority = threadobj_get_priority(&task->thobj);
|
param_ex.sched_rr_quantum.tv_sec = psos_rrperiod.tv_sec;
|
param_ex.sched_rr_quantum.tv_nsec = psos_rrperiod.tv_nsec;
|
threadobj_set_schedparam(&task->thobj, SCHED_RR, ¶m_ex);
|
}
|
|
if (task->mode & T_NOPREEMPT)
|
__threadobj_lock_sched(&task->thobj);
|
|
threadobj_unlock(&task->thobj);
|
threadobj_notify_entry();
|
CANCEL_RESTORE(svc);
|
|
args->entry(args->arg0, args->arg1, args->arg2, args->arg3);
|
|
return NULL;
|
}
|
|
/*
|
* By default, pSOS priorities are mapped 1:1 to SCHED_FIFO
|
* levels. The available priority range is [1..256] over Cobalt when
|
* running in primary mode, and [1..99] over the regular kernel with
|
* the POSIX interface.
|
*
|
* NOTE: over Cobalt, a thread transitioning to secondary mode has its
|
* priority ceiled to 99 in the regular POSIX SCHED_FIFO class.
|
*
|
* The application code may override the routine doing the priority
|
* mapping from pSOS to SCHED_FIFO (normalize). Normalized priorities
|
* returned by this routine must be in the range [ 1
|
* .. threadobj_high_prio] inclusive.
|
*/
|
__weak int psos_task_normalize_priority(unsigned long psos_prio)
|
{
|
if (psos_prio > threadobj_high_prio)
|
panic("current implementation restricts pSOS "
|
"priority levels to range [1..%d]",
|
threadobj_high_prio);
|
|
/* Map a pSOS priority level to a SCHED_FIFO one. */
|
return psos_prio;
|
}
|
|
/*
|
* Although default pSOS priorities are mapped 1:1 to SCHED_FIFO, we
|
* do still have to use a denormalize function because these calls are
|
* weak and application code may be override the call and implement
|
* the mapping differently.
|
*/
|
__weak unsigned long psos_task_denormalize_priority(int core_prio)
|
{
|
/* Map a SCHED_FIFO priority level to a pSOS one. */
|
return core_prio;
|
}
|
|
static int check_task_priority(u_long psos_prio, int *core_prio)
|
{
|
if (psos_prio < 1 || psos_prio > 255) /* In theory. */
|
return ERR_PRIOR;
|
|
*core_prio = psos_task_normalize_priority(psos_prio);
|
|
return SUCCESS;
|
}
|
|
static int psos_task_get_priority(struct psos_task *task)
|
{
|
int prio = threadobj_get_priority(&task->thobj);
|
return psos_task_denormalize_priority(prio);
|
}
|
|
u_long t_create(const char *name, u_long prio,
|
u_long sstack, u_long ustack, u_long flags, u_long *tid_r)
|
{
|
struct corethread_attributes cta;
|
struct threadobj_init_data idata;
|
struct psos_task *task;
|
struct service svc;
|
int ret, cprio = 1;
|
char short_name[5];
|
|
ret = check_task_priority(prio, &cprio);
|
if (ret)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
|
task = threadobj_alloc(struct psos_task,
|
thobj, union psos_wait_union);
|
if (task == NULL) {
|
ret = ERR_NOTCB;
|
goto out;
|
}
|
|
ustack += sstack;
|
|
/*
|
* Make sure we are granted a minimal amount of stack space
|
* for common usage of the Glibc. If zero, we will pick a
|
* value based on the implementation default for such minimum.
|
*/
|
if (ustack > 0 && ustack < 8192) {
|
threadobj_free(&task->thobj);
|
ret = ERR_TINYSTK;
|
goto out;
|
}
|
|
if (name == NULL || *name == '\0')
|
generate_name(task->name, name, &task_namegen);
|
else {
|
name = psos_trunc_name(short_name, name);
|
namecpy(task->name, name);
|
}
|
|
task->flags = flags; /* We don't do much with those. */
|
task->mode = 0; /* Not yet known. */
|
task->events = 0;
|
ret = syncobj_init(&task->sobj, CLOCK_COPPERPLATE, 0, fnref_null);
|
if (ret)
|
goto fail_syncinit;
|
|
memset(task->notepad, 0, sizeof(task->notepad));
|
pvlist_init(&task->timer_list);
|
*tid_r = mainheap_ref(task, u_long);
|
|
idata.magic = task_magic;
|
idata.finalizer = task_finalizer;
|
idata.policy = cprio ? SCHED_FIFO : SCHED_OTHER;
|
idata.param_ex.sched_priority = cprio;
|
ret = threadobj_init(&task->thobj, &idata);
|
if (ret)
|
goto fail_threadinit;
|
|
ret = __bt(cluster_addobj_dup(&psos_task_table, task->name, &task->cobj));
|
if (ret) {
|
warning("cannot register task: %s", task->name);
|
goto fail_register;
|
}
|
|
cta.policy = idata.policy;
|
cta.param_ex.sched_priority = cprio;
|
cta.prologue = task_prologue;
|
cta.run = task_trampoline;
|
cta.arg = task;
|
cta.stacksize = ustack;
|
cta.detachstate = PTHREAD_CREATE_DETACHED;
|
|
ret = __bt(copperplate_create_thread(&cta, &task->thobj.ptid));
|
if (ret) {
|
cluster_delobj(&psos_task_table, &task->cobj);
|
fail_register:
|
threadobj_uninit(&task->thobj);
|
fail_threadinit:
|
syncobj_uninit(&task->sobj);
|
fail_syncinit:
|
ret = ERR_NOTCB;
|
threadobj_free(&task->thobj);
|
}
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_start(u_long tid,
|
u_long mode,
|
void (*entry)(u_long, u_long, u_long, u_long),
|
u_long args[])
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
task->args.entry = entry;
|
if (args) {
|
task->args.arg0 = args[0];
|
task->args.arg1 = args[1];
|
task->args.arg2 = args[2];
|
task->args.arg3 = args[3];
|
} else {
|
task->args.arg0 = 0;
|
task->args.arg1 = 0;
|
task->args.arg2 = 0;
|
task->args.arg3 = 0;
|
}
|
task->mode = mode;
|
ret = threadobj_start(&task->thobj);
|
switch (ret) {
|
case -EIDRM:
|
ret = SUCCESS;
|
break;
|
default:
|
ret = ERR_OBJDEL;
|
case 0: /* == SUCCESS */
|
put_psos_task(task);
|
}
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_suspend(u_long tid)
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task_or_self(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
ret = threadobj_suspend(&task->thobj);
|
if (ret)
|
ret = ERR_OBJDEL;
|
|
put_psos_task(task);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_resume(u_long tid)
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
ret = threadobj_resume(&task->thobj);
|
if (ret)
|
ret = ERR_OBJDEL;
|
|
put_psos_task(task);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_setpri(u_long tid, u_long newprio, u_long *oldprio_r)
|
{
|
int policy, ret = SUCCESS, cprio = 1;
|
struct sched_param_ex param_ex;
|
struct psos_task *task;
|
struct service svc;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task_or_self(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
*oldprio_r = psos_task_get_priority(task);
|
|
if (newprio == 0) /* Only inquires for the task priority. */
|
goto done;
|
|
ret = check_task_priority(newprio, &cprio);
|
if (ret) {
|
ret = ERR_SETPRI;
|
goto done;
|
}
|
|
policy = cprio ? SCHED_FIFO : SCHED_OTHER;
|
param_ex.sched_priority = cprio;
|
ret = threadobj_set_schedparam(&task->thobj, policy, ¶m_ex);
|
switch (ret) {
|
case -EIDRM:
|
ret = SUCCESS;
|
goto out;
|
default:
|
ret = ERR_OBJDEL;
|
case 0: /* == SUCCESS */
|
break;
|
}
|
done:
|
put_psos_task(task);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_delete(u_long tid)
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task_or_self(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
ret = threadobj_cancel(&task->thobj);
|
if (ret)
|
ret = ERR_OBJDEL;
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_ident(const char *name, u_long node, u_long *tid_r)
|
{
|
struct clusterobj *cobj;
|
struct psos_task *task;
|
struct service svc;
|
char short_name[5];
|
int ret = SUCCESS;
|
|
if (node)
|
return ERR_NODENO;
|
|
CANCEL_DEFER(svc);
|
|
if (name == NULL) {
|
task = find_psos_task_or_self(0, &ret);
|
if (task == NULL)
|
goto out;
|
} else {
|
name = psos_trunc_name(short_name, name);
|
cobj = cluster_findobj(&psos_task_table, name);
|
if (cobj == NULL) {
|
ret = ERR_OBJNF;
|
goto out;
|
}
|
task = container_of(cobj, struct psos_task, cobj);
|
/*
|
* Last attempt to check whether the task is valid, in
|
* case it is pending deletion.
|
*/
|
if (threadobj_get_magic(&task->thobj) != task_magic) {
|
ret = ERR_OBJNF;
|
goto out;
|
}
|
}
|
|
*tid_r = mainheap_ref(task, u_long);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_getreg(u_long tid, u_long regnum, u_long *regvalue_r)
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret = SUCCESS;
|
|
if (regnum >= PSOSTASK_NR_REGS)
|
return ERR_REGNUM;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task_or_self(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
*regvalue_r = task->notepad[regnum];
|
put_psos_task(task);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_setreg(u_long tid, u_long regnum, u_long regvalue)
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret = SUCCESS;
|
|
if (regnum >= PSOSTASK_NR_REGS)
|
return ERR_REGNUM;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task_or_self(tid, &ret);
|
if (task == NULL)
|
goto out;
|
|
task->notepad[regnum] = regvalue;
|
put_psos_task(task);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
u_long t_mode(u_long mask, u_long newmask, u_long *oldmode_r)
|
{
|
struct sched_param_ex param_ex;
|
int policy, ret = SUCCESS;
|
struct psos_task *task;
|
struct service svc;
|
|
CANCEL_DEFER(svc);
|
|
task = get_psos_task_or_self(0, &ret);
|
if (task == NULL)
|
goto out;
|
|
*oldmode_r = task->mode;
|
|
if (mask == 0)
|
goto done;
|
|
task->mode &= ~mask;
|
task->mode |= (newmask & mask);
|
|
if (task->mode & T_NOPREEMPT)
|
__threadobj_lock_sched_once(&task->thobj);
|
else if (*oldmode_r & T_NOPREEMPT)
|
__threadobj_unlock_sched(&task->thobj);
|
|
param_ex.sched_priority = threadobj_get_priority(&task->thobj);
|
|
if (((task->mode ^ *oldmode_r) & T_TSLICE) == 0)
|
goto done; /* rr status not changed. */
|
|
if (task->mode & T_TSLICE) {
|
policy = SCHED_RR;
|
param_ex.sched_rr_quantum.tv_sec = psos_rrperiod.tv_sec;
|
param_ex.sched_rr_quantum.tv_nsec = psos_rrperiod.tv_nsec;
|
} else {
|
policy = param_ex.sched_priority ? SCHED_FIFO : SCHED_OTHER;
|
}
|
|
/* Working on self, so -EIDRM can't happen. */
|
threadobj_set_schedparam(&task->thobj, policy, ¶m_ex);
|
done:
|
put_psos_task(task);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
static int collect_events(struct psos_task *task,
|
u_long flags, u_long events, u_long *events_r)
|
{
|
if (((flags & EV_ANY) && (events & task->events) != 0) ||
|
(!(flags & EV_ANY) && ((events & task->events) == events))) {
|
/*
|
* The condition is satisfied; update the return value
|
* with the set of matched events, and clear the
|
* collected events from the task's mask.
|
*/
|
*events_r = (task->events & events);
|
task->events &= ~events;
|
return 1;
|
}
|
|
return 0;
|
}
|
|
u_long ev_receive(u_long events, u_long flags,
|
u_long timeout, u_long *events_r)
|
{
|
struct timespec ts, *timespec;
|
struct psos_task *current;
|
struct syncstate syns;
|
struct service svc;
|
int ret;
|
|
current = find_psos_task_or_self(0, &ret);
|
if (current == NULL)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
|
ret = syncobj_lock(¤t->sobj, &syns);
|
if (ret) {
|
ret = ERR_OBJDEL;
|
goto out;
|
}
|
|
if (events == 0) {
|
*events_r = current->events; /* Only polling events. */
|
goto done;
|
}
|
|
if (collect_events(current, flags, events, events_r))
|
goto done;
|
|
if (flags & EV_NOWAIT) {
|
ret = ERR_NOEVS;
|
goto done;
|
}
|
|
if (timeout != 0) {
|
timespec = &ts;
|
clockobj_ticks_to_timeout(&psos_clock, timeout, timespec);
|
} else
|
timespec = NULL;
|
|
for (;;) {
|
ret = syncobj_wait_grant(¤t->sobj, timespec, &syns);
|
if (ret == -ETIMEDOUT) {
|
ret = ERR_TIMEOUT;
|
break;
|
}
|
if (collect_events(current, flags, events, events_r))
|
break;
|
}
|
done:
|
syncobj_unlock(¤t->sobj, &syns);
|
out:
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
|
int __ev_send(struct psos_task *task, u_long events)
|
{
|
struct syncstate syns;
|
int ret;
|
|
ret = syncobj_lock(&task->sobj, &syns);
|
if (ret)
|
return ERR_OBJDEL;
|
|
task->events |= events;
|
/*
|
* If the task is pending in ev_receive(), it's likely that we
|
* are posting events the task is waiting for, so we can wake
|
* it up immediately and let it confirm whether the condition
|
* is now satisfied.
|
*/
|
syncobj_grant_one(&task->sobj);
|
|
syncobj_unlock(&task->sobj, &syns);
|
|
return 0;
|
}
|
|
u_long ev_send(u_long tid, u_long events)
|
{
|
struct psos_task *task;
|
struct service svc;
|
int ret = SUCCESS;
|
|
task = find_psos_task_or_self(tid, &ret);
|
if (task == NULL)
|
return ret;
|
|
CANCEL_DEFER(svc);
|
ret = __ev_send(task, events);
|
CANCEL_RESTORE(svc);
|
|
return ret;
|
}
|
