MSG_Q_ID msgQCreate(int nb_msgs, int length, int flags)
{
static unsigned long msgq_ids;
wind_msgq_t *queue;
xnflags_t bflags = 0;
int i, msg_size;
char *msgs_mem;
spl_t s;
check_NOT_ISR_CALLABLE(return 0);
error_check(nb_msgs <= 0, S_msgQLib_INVALID_QUEUE_TYPE, return 0);
error_check(flags & ~WIND_MSG_Q_OPTION_MASK,
S_msgQLib_INVALID_QUEUE_TYPE, return 0);
error_check(length < 0, S_msgQLib_INVALID_MSG_LENGTH, return 0);
msgs_mem = xnmalloc(sizeof(wind_msgq_t) +
nb_msgs * (sizeof(wind_msg_t) + length));
error_check(msgs_mem == NULL, S_memLib_NOT_ENOUGH_MEMORY, return 0);
queue = (wind_msgq_t *)msgs_mem;
msgs_mem += sizeof(wind_msgq_t);
queue->magic = WIND_MSGQ_MAGIC;
queue->msg_length = length;
queue->free_list = NULL;
initq(&queue->msgq);
inith(&queue->rlink);
queue->rqueue = &wind_get_rholder()->msgQq;
/* init of the synch object : */
if (flags & MSG_Q_PRIORITY)
bflags |= XNSYNCH_PRIO;
xnsynch_init(&queue->synchbase, bflags, NULL);
msg_size = sizeof(wind_msg_t) + length;
for (i = 0; i < nb_msgs; ++i, msgs_mem += msg_size)
free_msg(queue, (wind_msg_t *)msgs_mem);
xnlock_get_irqsave(&nklock, s);
appendq(queue->rqueue, &queue->rlink);
xnlock_put_irqrestore(&nklock, s);
sprintf(queue->name, "mq%lu", msgq_ids++);
if (xnregistry_enter(queue->name, queue,
&queue->handle, &msgq_pnode)) {
wind_errnoset(S_objLib_OBJ_ID_ERROR);
msgQDelete((MSG_Q_ID)queue);
return 0;
}
return (MSG_Q_ID)queue;
}
static int __wind_wd_wait(struct task_struct *curr, struct pt_regs *regs)
{
xnholder_t *holder;
wind_rholder_t *rh;
WIND_TCB *pTcb;
wind_wd_t *wd;
int err = 0;
spl_t s;
if (!__xn_access_ok
(curr, VERIFY_WRITE, __xn_reg_arg1(regs), sizeof(wd->wdt)))
return -EFAULT;
rh = wind_get_rholder();
xnlock_get_irqsave(&nklock, s);
pTcb = __wind_task_current(curr);
if (xnthread_base_priority(&pTcb->threadbase) != XNCORE_IRQ_PRIO)
/* Renice the waiter above all regular tasks if needed. */
xnpod_renice_thread(&pTcb->threadbase, XNCORE_IRQ_PRIO);
if (!emptyq_p(&rh->wdpending))
goto pull_event;
xnsynch_sleep_on(&rh->wdsynch, XN_INFINITE, XN_RELATIVE);
if (xnthread_test_info(&pTcb->threadbase, XNBREAK)) {
err = -EINTR; /* Unblocked. */
goto unlock_and_exit;
}
if (xnthread_test_info(&pTcb->threadbase, XNRMID)) {
err = -EIDRM; /* Watchdog deleted while pending. */
goto unlock_and_exit;
}
pull_event:
holder = getq(&rh->wdpending);
if (holder) {
wd = link2wind_wd(holder);
/* We need the following to mark the watchdog as unqueued. */
inith(holder);
xnlock_put_irqrestore(&nklock, s);
__xn_copy_to_user(curr, (void __user *)__xn_reg_arg1(regs),
&wd->wdt, sizeof(wd->wdt));
return 0;
}
unlock_and_exit:
xnlock_put_irqrestore(&nklock, s);
return err;
}
static int __wind_wd_wait(struct pt_regs *regs)
{
union xnsched_policy_param param;
xnholder_t *holder;
wind_rholder_t *rh;
WIND_TCB *pTcb;
wind_wd_t *wd;
int err = 0;
spl_t s;
rh = wind_get_rholder();
xnlock_get_irqsave(&nklock, s);
pTcb = __wind_task_current(current);
if (xnthread_base_priority(&pTcb->threadbase) != XNSCHED_IRQ_PRIO) {
/* Boost the waiter above all regular tasks if needed. */
param.rt.prio = XNSCHED_IRQ_PRIO;
xnpod_set_thread_schedparam(&pTcb->threadbase,
&xnsched_class_rt, ¶m);
}
if (!emptyq_p(&rh->wdpending))
goto pull_event;
xnsynch_sleep_on(&rh->wdsynch, XN_INFINITE, XN_RELATIVE);
if (xnthread_test_info(&pTcb->threadbase, XNBREAK)) {
err = -EINTR; /* Unblocked. */
goto unlock_and_exit;
}
if (xnthread_test_info(&pTcb->threadbase, XNRMID)) {
err = -EIDRM; /* Watchdog deleted while pending. */
goto unlock_and_exit;
}
pull_event:
holder = getq(&rh->wdpending);
if (holder) {
wd = link2wind_wd(holder);
/* We need the following to mark the watchdog as unqueued. */
inith(holder);
xnlock_put_irqrestore(&nklock, s);
return __xn_safe_copy_to_user((void __user *)__xn_reg_arg1(regs),
&wd->wdt, sizeof(wd->wdt));
}
unlock_and_exit:
xnlock_put_irqrestore(&nklock, s);
return err;
}
static int __wind_wd_start(struct task_struct *curr, struct pt_regs *regs)
{
wind_rholder_t *rh;
long start_server;
xnhandle_t handle;
wind_wd_t *wd;
int timeout;
spl_t s;
if (!__xn_access_ok(curr, VERIFY_WRITE, __xn_reg_arg5(regs), sizeof(start_server)))
return -EFAULT;
handle = __xn_reg_arg1(regs);
wd = (wind_wd_t *)xnregistry_fetch(handle);
if (!wd)
return S_objLib_OBJ_ID_ERROR;
rh = wind_get_rholder();
if (wd->rh != rh)
/*
* User may not fiddle with watchdogs created from
* other processes.
*/
return S_objLib_OBJ_UNAVAILABLE;
timeout = __xn_reg_arg2(regs);
xnlock_get_irqsave(&nklock, s);
if (wdStart
((WDOG_ID)wd, timeout, (wind_timer_t) & __wind_wd_handler,
(long)wd) == ERROR) {
xnlock_put_irqrestore(&nklock, s);
return wind_errnoget();
}
wd->wdt.handler = (wind_timer_t) __xn_reg_arg3(regs);
wd->wdt.arg = (long)__xn_reg_arg4(regs);
start_server = rh->wdcount++ == 0;
xnlock_put_irqrestore(&nklock, s);
__xn_copy_to_user(curr, (void __user *)__xn_reg_arg5(regs), &start_server,
sizeof(start_server));
return 0;
}
static int __wind_wd_create(struct task_struct *curr, struct pt_regs *regs)
{
WDOG_ID wdog_id;
wind_wd_t *wd;
if (!__xn_access_ok
(curr, VERIFY_WRITE, __xn_reg_arg1(regs), sizeof(wdog_id)))
return -EFAULT;
wd = (wind_wd_t *)wdCreate();
if (!wd)
return wind_errnoget();
wd->rh = wind_get_rholder();
wdog_id = wd->handle;
__xn_copy_to_user(curr, (void __user *)__xn_reg_arg1(regs), &wdog_id,
sizeof(wdog_id));
return 0;
}