RTAI_SYSCALL_MODE int _rt_bits_wait_if(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, unsigned long *resulting_mask, int space)
{
unsigned long flags, mask;
int retval;
CHECK_BITS_MAGIC(bits);
flags = rt_global_save_flags_and_cli();
mask = bits->mask;
if (test_fun[testfun](bits, testmasks)) {
exec_fun[exitfun](bits, exitmasks);
retval = 1;
} else {
retval = 0;
}
rt_global_restore_flags(flags);
if (resulting_mask) {
if (space) {
*resulting_mask = mask;
} else {
rt_copy_to_user(resulting_mask, &mask, sizeof(mask));
}
}
return retval;
}
RTAI_SYSCALL_MODE void rt_set_tasklet_data(struct rt_tasklet_struct *tasklet, unsigned long data)
{
tasklet->data = data;
if (tasklet->task) {
rt_copy_to_user(tasklet->usptasklet, tasklet, sizeof(struct rt_usp_tasklet_struct));
}
}
RTAI_SYSCALL_MODE int rt_insert_tasklet(struct rt_tasklet_struct *tasklet, int priority, void (*handler)(unsigned long), unsigned long data, unsigned long id, int pid)
{
unsigned long flags;
// tasklet initialization
if (!handler || !id) {
return -EINVAL;
}
tasklet->uses_fpu = 0;
tasklet->priority = priority;
tasklet->handler = handler;
tasklet->data = data;
tasklet->id = id;
if (!pid) {
tasklet->task = 0;
} else {
(tasklet->task)->priority = priority;
rt_copy_to_user(tasklet->usptasklet, tasklet, sizeof(struct rt_usp_tasklet_struct));
}
// tasklet insertion tasklets_list
flags = rt_spin_lock_irqsave(&tasklets_lock);
tasklet->next = &tasklets_list;
tasklet->prev = tasklets_list.prev;
(tasklets_list.prev)->next = tasklet;
tasklets_list.prev = tasklet;
rt_spin_unlock_irqrestore(flags, &tasklets_lock);
return 0;
}
static int mbxevdrp(MBX *mbx, char **msg, int msg_size, int space)
{
int tocpy, fbyte, avbs;
fbyte = mbx->fbyte;
avbs = mbx->avbs;
while (msg_size > 0 && avbs) {
if ((tocpy = mbx->size - fbyte) > msg_size) {
tocpy = msg_size;
}
if (tocpy > avbs) {
tocpy = avbs;
}
if (space) {
memcpy(*msg, mbx->bufadr + fbyte, tocpy);
} else {
rt_copy_to_user(*msg, mbx->bufadr + mbx->fbyte, tocpy);
}
avbs -= tocpy;
msg_size -= tocpy;
*msg += tocpy;
fbyte = MOD_SIZE(fbyte + tocpy);
}
return msg_size;
}
static int mbxget(MBX *mbx, char **msg, int msg_size, int space)
{
unsigned long flags;
int tocpy;
while (msg_size > 0 && mbx->avbs) {
if ((tocpy = mbx->size - mbx->fbyte) > msg_size) {
tocpy = msg_size;
}
if (tocpy > mbx->avbs) {
tocpy = mbx->avbs;
}
if (space) {
memcpy(*msg, mbx->bufadr + mbx->fbyte, tocpy);
} else {
rt_copy_to_user(*msg, mbx->bufadr + mbx->fbyte, tocpy);
}
flags = rt_spin_lock_irqsave(&(mbx->lock));
mbx->frbs += tocpy;
mbx->avbs -= tocpy;
rt_spin_unlock_irqrestore(flags, &(mbx->lock));
msg_size -= tocpy;
*msg += tocpy;
mbx->fbyte = MOD_SIZE(mbx->fbyte + tocpy);
}
return msg_size;
}
RTAI_SYSCALL_MODE int _rt_msg_evdrp(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
{
int size;
RT_MSG *msg_ptr;
void *p;
size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
if (space)
{
memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
*msgpri = msg_ptr->hdr.priority;
}
}
else
{
rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
rt_put_user(msg_ptr->hdr.priority, msgpri);
}
}
return 0;
}
RTAI_SYSCALL_MODE int rt_insert_timer(struct rt_tasklet_struct *timer, int priority, RTIME firing_time, RTIME period, void (*handler)(unsigned long), unsigned long data, int pid)
{
spinlock_t *lock;
unsigned long flags, cpuid;
RT_TASK *timer_manager;
// timer initialization
timer->uses_fpu = 0;
if (pid >= 0) {
if (!handler) {
return -EINVAL;
}
timer->handler = handler;
timer->data = data;
} else {
if (timer->handler != NULL || timer->handler == (void *)1) {
timer->handler = (void *)1;
timer->data = data;
}
}
timer->priority = priority;
REALTIME2COUNT(firing_time)
timer->firing_time = firing_time;
timer->period = period;
if (!pid) {
timer->task = 0;
timer->cpuid = cpuid = NUM_CPUS > 1 ? rtai_cpuid() : 0;
} else {
timer->cpuid = cpuid = NUM_CPUS > 1 ? (timer->task)->runnable_on_cpus : 0;
(timer->task)->priority = priority;
rt_copy_to_user(timer->usptasklet, timer, sizeof(struct rt_usp_tasklet_struct));
}
// timer insertion in timers_list
flags = rt_spin_lock_irqsave(lock = &timers_lock[LIST_CPUID]);
enq_timer(timer);
rt_spin_unlock_irqrestore(flags, lock);
// timers_manager priority inheritance
if (timer->priority < (timer_manager = &timers_manager[LIST_CPUID])->priority) {
timer_manager->priority = timer->priority;
}
// timers_task deadline inheritance
flags = rt_global_save_flags_and_cli();
if (timers_list[LIST_CPUID].next == timer && (timer_manager->state & RT_SCHED_DELAYED) && firing_time < timer_manager->resume_time) {
timer_manager->resume_time = firing_time;
rem_timed_task(timer_manager);
enq_timed_task(timer_manager);
rt_schedule();
}
rt_global_restore_flags(flags);
return 0;
}
RTAI_SYSCALL_MODE int rt_set_tasklet_handler(struct rt_tasklet_struct *tasklet, void (*handler)(unsigned long))
{
if (!handler) {
return -EINVAL;
}
tasklet->handler = handler;
if (tasklet->task) {
rt_copy_to_user(tasklet->usptasklet, tasklet, sizeof(struct rt_usp_tasklet_struct));
}
return 0;
}
RTAI_SYSCALL_MODE int _rt_bits_wait(BITS *bits, int testfun, unsigned long testmasks, int exitfun, unsigned long exitmasks, unsigned long *resulting_mask, int space)
{
RT_TASK *rt_current;
unsigned long flags, mask = 0;
int retval;
CHECK_BITS_MAGIC(bits);
flags = rt_global_save_flags_and_cli();
if (!test_fun[testfun](bits, testmasks))
{
void *retpnt;
long bits_test[2];
rt_current = RT_CURRENT;
TEST_BUF(rt_current, bits_test);
TEST_FUN(rt_current) = testfun;
TEST_MASK(rt_current) = testmasks;
rt_current->state |= RT_SCHED_SEMAPHORE;
rem_ready_current(rt_current);
enqueue_blocked(rt_current, &bits->queue, 1);
rt_schedule();
if (unlikely((retpnt = rt_current->blocked_on) != NULL))
{
if (likely(retpnt != RTP_OBJREM))
{
dequeue_blocked(rt_current);
retval = RTE_UNBLKD;
}
else
{
rt_current->prio_passed_to = NULL;
retval = RTE_OBJREM;
}
goto retmask;
}
}
retval = 0;
mask = bits->mask;
exec_fun[exitfun](bits, exitmasks);
retmask:
rt_global_restore_flags(flags);
if (resulting_mask)
{
if (space)
{
*resulting_mask = mask;
}
else
{
rt_copy_to_user(resulting_mask, &mask, sizeof(mask));
}
}
return retval;
}
static int _receive(RT_MSGQ *mq, void *msg, int msg_size, int *msgpri, int space)
{
int size;
RT_MSG *msg_ptr;
void *p;
size = min((msg_ptr = mq->firstmsg)->hdr.size, msg_size);
if (space)
{
memcpy(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
*msgpri = msg_ptr->hdr.priority;
}
}
else
{
rt_copy_to_user(msg, (p = msg_ptr->hdr.malloc) ? p : msg_ptr->msg, size);
if (msgpri)
{
rt_put_user(msg_ptr->hdr.priority, msgpri);
}
}
if (msg_ptr->hdr.broadcast)
{
if (!--msg_ptr->hdr.broadcast)
{
rt_sem_wait_barrier(&mq->broadcast);
goto relslot;
}
else
{
rt_sem_signal(&mq->received);
rt_sem_signal(&mq->receivers);
rt_sem_wait_barrier(&mq->broadcast);
}
}
else
{
unsigned long flags;
relslot: flags = rt_spin_lock_irqsave(&mq->lock);
mq->firstmsg = msg_ptr->hdr.next;
mq->slots[--mq->slot] = msg_ptr;
rt_spin_unlock_irqrestore(flags, &mq->lock);
rt_sem_signal(&mq->freslots);
rt_sem_signal(&mq->receivers);
if (p)
{
rt_free(p);
}
}
return msg_size - size;
}
RTAI_SYSCALL_MODE int rt_delete_tasklet(struct rt_tasklet_struct *tasklet)
{
int thread;
rt_remove_tasklet(tasklet);
tasklet->handler = 0;
rt_copy_to_user(tasklet->usptasklet, tasklet, sizeof(struct rt_usp_tasklet_struct));
rt_task_resume(tasklet->task);
thread = tasklet->thread;
rt_free(tasklet);
return thread;
}
RTAI_SYSCALL_MODE int rt_ptimer_delete(timer_t timer, long space)
{
struct rt_tasklet_struct *tasklet;
int rtn = 0;
tasklet = posix_timer[timer].timer;
gvb_ptimer_indx(timer);
rt_remove_tasklet(tasklet);
if (space) {
tasklet->handler = 0;
rt_copy_to_user(tasklet->usptasklet, tasklet, sizeof(struct rt_usp_tasklet_struct));
rt_task_resume(tasklet->task);
rtn = tasklet->thread;
}
rt_free(tasklet);
return rtn;
}
RTAI_SYSCALL_MODE void rt_register_task(struct rt_tasklet_struct *tasklet, struct rt_tasklet_struct *usptasklet, RT_TASK *task)
{
tasklet->task = task;
tasklet->usptasklet = usptasklet;
rt_copy_to_user(usptasklet, tasklet, sizeof(struct rt_usp_tasklet_struct));
}
static inline long long handle_lxrt_request (unsigned int lxsrq, long *arg, RT_TASK *task)
{
#define larg ((struct arg *)arg)
union {unsigned long name; RT_TASK *rt_task; SEM *sem; MBX *mbx; RWL *rwl; SPL *spl; int i; void *p; long long ll; } arg0;
int srq;
if (likely((srq = SRQ(lxsrq)) < MAX_LXRT_FUN)) {
unsigned long type;
struct rt_fun_entry *funcm;
/*
* The next two lines of code do a lot. It makes possible to extend the use of
* USP to any other real time module service in user space, both for soft and
* hard real time. Concept contributed and copyrighted by: Giuseppe Renoldi
* ([email protected]).
*/
if (unlikely(!(funcm = rt_fun_ext[INDX(lxsrq)]))) {
rt_printk("BAD: null rt_fun_ext, no module for extension %d?\n", INDX(lxsrq));
return -ENOSYS;
}
if (!(type = funcm[srq].type)) {
return ((RTAI_SYSCALL_MODE long long (*)(unsigned long, ...))funcm[srq].fun)(RTAI_FUN_ARGS);
}
if (unlikely(NEED_TO_RW(type))) {
lxrt_fun_call_wbuf(task, funcm[srq].fun, NARG(lxsrq), arg, type);
} else {
lxrt_fun_call(task, funcm[srq].fun, NARG(lxsrq), arg);
}
return task->retval;
}
arg0.name = arg[0];
switch (srq) {
case LXRT_GET_ADR: {
arg0.p = rt_get_adr(arg0.name);
return arg0.ll;
}
case LXRT_GET_NAME: {
arg0.name = rt_get_name(arg0.p);
return arg0.ll;
}
case LXRT_TASK_INIT: {
struct arg { unsigned long name; long prio, stack_size, max_msg_size, cpus_allowed; };
arg0.rt_task = __task_init(arg0.name, larg->prio, larg->stack_size, larg->max_msg_size, larg->cpus_allowed);
return arg0.ll;
}
case LXRT_TASK_DELETE: {
arg0.i = __task_delete(arg0.rt_task ? arg0.rt_task : task);
return arg0.ll;
}
case LXRT_SEM_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.sem = rt_malloc(sizeof(SEM)))) {
struct arg { unsigned long name; long cnt; long typ; };
lxrt_typed_sem_init(arg0.sem, larg->cnt, larg->typ);
if (rt_register(larg->name, arg0.sem, IS_SEM, current)) {
return arg0.ll;
} else {
rt_free(arg0.sem);
}
}
return 0;
}
case LXRT_SEM_DELETE: {
if (lxrt_sem_delete(arg0.sem)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.sem);
arg0.i = rt_drg_on_adr(arg0.sem);
return arg0.ll;
}
case LXRT_MBX_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.mbx = rt_malloc(sizeof(MBX)))) {
struct arg { unsigned long name; long size; int qtype; };
if (lxrt_typed_mbx_init(arg0.mbx, larg->size, larg->qtype) < 0) {
rt_free(arg0.mbx);
return 0;
}
if (rt_register(larg->name, arg0.mbx, IS_MBX, current)) {
return arg0.ll;
} else {
rt_free(arg0.mbx);
}
}
return 0;
}
case LXRT_MBX_DELETE: {
if (lxrt_mbx_delete(arg0.mbx)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.mbx);
arg0.i = rt_drg_on_adr(arg0.mbx);
return arg0.ll;
}
case LXRT_RWL_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.rwl = rt_malloc(sizeof(RWL)))) {
struct arg { unsigned long name; long type; };
lxrt_typed_rwl_init(arg0.rwl, larg->type);
if (rt_register(larg->name, arg0.rwl, IS_SEM, current)) {
return arg0.ll;
} else {
rt_free(arg0.rwl);
}
}
return 0;
}
case LXRT_RWL_DELETE: {
if (lxrt_rwl_delete(arg0.rwl)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.rwl);
arg0.i = rt_drg_on_adr(arg0.rwl);
return arg0.ll;
}
case LXRT_SPL_INIT: {
if (rt_get_adr(arg0.name)) {
return 0;
}
if ((arg0.spl = rt_malloc(sizeof(SPL)))) {
struct arg { unsigned long name; };
lxrt_spl_init(arg0.spl);
if (rt_register(larg->name, arg0.spl, IS_SEM, current)) {
return arg0.ll;
} else {
rt_free(arg0.spl);
}
}
return 0;
}
case LXRT_SPL_DELETE: {
if (lxrt_spl_delete(arg0.spl)) {
arg0.i = -EFAULT;
return arg0.ll;
}
rt_free(arg0.spl);
arg0.i = rt_drg_on_adr(arg0.spl);
return arg0.ll;
}
case MAKE_HARD_RT: {
rt_make_hard_real_time(task);
return 0;
if (!task || task->is_hard) {
return 0;
}
steal_from_linux(task);
return 0;
}
case MAKE_SOFT_RT: {
rt_make_soft_real_time(task);
return 0;
if (!task || !task->is_hard) {
return 0;
}
if (task->is_hard < 0) {
task->is_hard = 0;
} else {
give_back_to_linux(task, 0);
}
return 0;
}
case PRINT_TO_SCREEN: {
struct arg { char *display; long nch; };
arg0.i = rtai_print_to_screen("%s", larg->display);
return arg0.ll;
}
case PRINTK: {
struct arg { char *display; long nch; };
arg0.i = rt_printk("%s", larg->display);
return arg0.ll;
}
case NONROOT_HRT: {
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,24)
current->cap_effective |= ((1 << CAP_IPC_LOCK) |
(1 << CAP_SYS_RAWIO) |
(1 << CAP_SYS_NICE));
#else
set_lxrt_perm(CAP_IPC_LOCK);
set_lxrt_perm(CAP_SYS_RAWIO);
set_lxrt_perm(CAP_SYS_NICE);
#endif
return 0;
}
case RT_BUDDY: {
arg0.rt_task = task && current->rtai_tskext(TSKEXT1) == current ? task : NULL;
return arg0.ll;
}
case HRT_USE_FPU: {
struct arg { RT_TASK *task; long use_fpu; };
if(!larg->use_fpu) {
clear_lnxtsk_uses_fpu((larg->task)->lnxtsk);
} else {
init_fpu((larg->task)->lnxtsk);
}
return 0;
}
case GET_USP_FLAGS: {
arg0.name = arg0.rt_task->usp_flags;
return arg0.ll;
}
case SET_USP_FLAGS: {
struct arg { RT_TASK *task; unsigned long flags; };
arg0.rt_task->usp_flags = larg->flags;
arg0.rt_task->force_soft = (arg0.rt_task->is_hard > 0) && (larg->flags & arg0.rt_task->usp_flags_mask & FORCE_SOFT);
return 0;
}
case GET_USP_FLG_MSK: {
arg0.name = arg0.rt_task->usp_flags_mask;
return arg0.ll;
}
case SET_USP_FLG_MSK: {
task->usp_flags_mask = arg0.name;
task->force_soft = (task->is_hard > 0) && (task->usp_flags & arg0.name & FORCE_SOFT);
return 0;
}
case FORCE_TASK_SOFT: {
extern void rt_do_force_soft(RT_TASK *rt_task);
struct task_struct *ltsk;
if ((ltsk = find_task_by_pid(arg0.name))) {
if ((arg0.rt_task = ltsk->rtai_tskext(TSKEXT0))) {
if ((arg0.rt_task->force_soft = (arg0.rt_task->is_hard != 0) && FORCE_SOFT)) {
rt_do_force_soft(arg0.rt_task);
}
return arg0.ll;
}
}
return 0;
}
case IS_HARD: {
arg0.i = arg0.rt_task || (arg0.rt_task = current->rtai_tskext(TSKEXT0)) ? arg0.rt_task->is_hard : 0;
return arg0.ll;
}
case GET_EXECTIME: {
struct arg { RT_TASK *task; RTIME *exectime; };
if ((larg->task)->exectime[0] && (larg->task)->exectime[1]) {
larg->exectime[0] = (larg->task)->exectime[0];
larg->exectime[1] = (larg->task)->exectime[1];
larg->exectime[2] = rtai_rdtsc();
}
return 0;
}
case GET_TIMEORIG: {
struct arg { RTIME *time_orig; };
if (larg->time_orig) {
RTIME time_orig[2];
rt_gettimeorig(time_orig);
rt_copy_to_user(larg->time_orig, time_orig, sizeof(time_orig));
} else {
rt_gettimeorig(NULL);
}
return 0;
}
case LINUX_SERVER: {
struct arg { struct linux_syscalls_list syscalls; };
if (larg->syscalls.nr) {
if (larg->syscalls.task->linux_syscall_server) {
RT_TASK *serv;
rt_get_user(serv, &larg->syscalls.serv);
rt_task_masked_unblock(serv, ~RT_SCHED_READY);
}
larg->syscalls.task->linux_syscall_server = larg->syscalls.serv;
rtai_set_linux_task_priority(current, (larg->syscalls.task)->lnxtsk->policy, (larg->syscalls.task)->lnxtsk->rt_priority);
arg0.rt_task = __task_init((unsigned long)larg->syscalls.task, larg->syscalls.task->base_priority >= BASE_SOFT_PRIORITY ? larg->syscalls.task->base_priority - BASE_SOFT_PRIORITY : larg->syscalls.task->base_priority, 0, 0, 1 << larg->syscalls.task->runnable_on_cpus);
larg->syscalls.task->linux_syscall_server = arg0.rt_task;
arg0.rt_task->linux_syscall_server = larg->syscalls.serv;
return arg0.ll;
} else {
if (!larg->syscalls.task) {
larg->syscalls.task = RT_CURRENT;
}
if ((arg0.rt_task = larg->syscalls.task->linux_syscall_server)) {
larg->syscalls.task->linux_syscall_server = NULL;
arg0.rt_task->suspdepth = -RTE_HIGERR;
rt_task_masked_unblock(arg0.rt_task, ~RT_SCHED_READY);
}
}
return 0;
}
default: {
rt_printk("RTAI/LXRT: Unknown srq #%d\n", srq);
arg0.i = -ENOSYS;
return arg0.ll;
}
}
return 0;
}
static inline void lxrt_fun_call_wbuf(RT_TASK *rt_task, void *fun, int narg, long *arg, unsigned long type)
{
int rsize, r2size, wsize, w2size, msg_size;
long *wmsg_adr, *w2msg_adr, *fun_args;
rsize = r2size = wsize = w2size = 0 ;
wmsg_adr = w2msg_adr = NULL;
fun_args = arg - 1;
if (NEED_TO_R(type)) {
rsize = USP_RSZ1(type);
rsize = rsize ? fun_args[rsize] : sizeof(long);
if (NEED_TO_R2ND(type)) {
r2size = USP_RSZ2(type);
r2size = r2size ? fun_args[r2size] : sizeof(long);
}
}
if (NEED_TO_W(type)) {
wsize = USP_WSZ1(type);
wsize = wsize ? fun_args[wsize] : sizeof(long);
if (NEED_TO_W2ND(type)) {
w2size = USP_WSZ2(type);
w2size = w2size ? fun_args[w2size] : sizeof(long);
}
}
if ((msg_size = rsize > wsize ? rsize : wsize) > 0) {
if (msg_size > rt_task->max_msg_size[0]) {
rt_free(rt_task->msg_buf[0]);
rt_task->max_msg_size[0] = (msg_size << 7)/100;
rt_task->msg_buf[0] = rt_malloc(rt_task->max_msg_size[0]);
}
if (rsize) {
long *buf_arg = fun_args + USP_RBF1(type);
rt_copy_from_user(rt_task->msg_buf[0], (long *)buf_arg[0], rsize);
buf_arg[0] = (long)rt_task->msg_buf[0];
}
if (wsize) {
long *buf_arg = fun_args + USP_WBF1(type);
wmsg_adr = (long *)buf_arg[0];
buf_arg[0] = (long)rt_task->msg_buf[0];
}
}
if ((msg_size = r2size > w2size ? r2size : w2size) > 0) {
if (msg_size > rt_task->max_msg_size[1]) {
rt_free(rt_task->msg_buf[1]);
rt_task->max_msg_size[1] = (msg_size << 7)/100;
rt_task->msg_buf[1] = rt_malloc(rt_task->max_msg_size[1]);
}
if (r2size) {
long *buf_arg = fun_args + USP_RBF2(type);
rt_copy_from_user(rt_task->msg_buf[1], (long *)buf_arg[0], r2size);
buf_arg[0] = (long)rt_task->msg_buf[1];
}
if (w2size) {
long *buf_arg = fun_args + USP_WBF2(type);
w2msg_adr = (long *)buf_arg[0];
buf_arg[0] = (long)rt_task->msg_buf[1];
}
}
lxrt_fun_call(rt_task, fun, narg, arg);
if (wsize) {
rt_copy_to_user(wmsg_adr, rt_task->msg_buf[0], wsize);
if (w2size) {
rt_copy_to_user(w2msg_adr, rt_task->msg_buf[1], w2size);
}
}
}
