int xnmap_enter(xnmap_t *map, int key, void *objaddr)
{
int hi, lo, ofkey = key - map->offset;
spl_t s;
xnlock_get_irqsave(&nklock, s);
if (ofkey >= 0 && ofkey < map->nkeys) {
if (map->objarray[ofkey] != NULL) {
key = -EEXIST;
goto unlock_and_exit;
}
} else if (map->ukeys >= map->nkeys) {
key = -ENOSPC;
goto unlock_and_exit;
}
else {
/* The himask implements a namespace reservation of
half of the bitmap space which cannot be used to
draw keys. */
hi = ffnz(map->himap & ~map->himask);
lo = ffnz(map->lomap[hi]);
ofkey = hi * BITS_PER_LONG + lo;
++map->ukeys;
__clrbits(map->lomap[hi], 1UL << lo);
if (map->lomap[hi] == 0)
__clrbits(map->himap, 1UL << hi);
}
map->objarray[ofkey] = objaddr;
unlock_and_exit:
xnlock_put_irqrestore(&nklock, s);
return ofkey + map->offset;
}
static inline RT_TASK* __task_init(unsigned long name, int prio, int stack_size, int max_msg_size, int cpus_allowed)
{
void *msg_buf0, *msg_buf1;
RT_TASK *rt_task;
if ((rt_task = current->rtai_tskext(TSKEXT0))) {
if (num_online_cpus() > 1 && cpus_allowed) {
cpus_allowed = hweight32(cpus_allowed) > 1 ? get_min_tasks_cpuid() : ffnz(cpus_allowed);
} else {
cpus_allowed = rtai_cpuid();
}
put_current_on_cpu(cpus_allowed);
return rt_task;
}
if (rt_get_adr(name)) {
return 0;
}
if (prio > RT_SCHED_LOWEST_PRIORITY) {
prio = RT_SCHED_LOWEST_PRIORITY;
}
if (!max_msg_size) {
max_msg_size = USRLAND_MAX_MSG_SIZE;
}
if (!(msg_buf0 = rt_malloc(max_msg_size))) {
return 0;
}
if (!(msg_buf1 = rt_malloc(max_msg_size))) {
rt_free(msg_buf0);
return 0;
}
rt_task = rt_malloc(sizeof(RT_TASK) + 3*sizeof(struct fun_args));
if (rt_task) {
rt_task->magic = 0;
if (num_online_cpus() > 1 && cpus_allowed) {
cpus_allowed = hweight32(cpus_allowed) > 1 ? get_min_tasks_cpuid() : ffnz(cpus_allowed);
} else {
cpus_allowed = rtai_cpuid();
}
if (!set_rtext(rt_task, prio, 0, 0, cpus_allowed, 0)) {
rt_task->fun_args = (long *)((struct fun_args *)(rt_task + 1));
rt_task->msg_buf[0] = msg_buf0;
rt_task->msg_buf[1] = msg_buf1;
rt_task->max_msg_size[0] =
rt_task->max_msg_size[1] = max_msg_size;
if (rt_register(name, rt_task, IS_TASK, 0)) {
rt_task->state = 0;
#ifdef __IPIPE_FEATURE_ENABLE_NOTIFIER
ipipe_enable_notifier(current);
#else
current->flags |= PF_EVNOTIFY;
#endif
#if (defined VM_PINNED) && (defined CONFIG_MMU)
ipipe_disable_ondemand_mappings(current);
#endif
RTAI_OOM_DISABLE();
return rt_task;
} else {
clr_rtext(rt_task);
}
}
rt_free(rt_task);
}
rt_free(msg_buf0);
rt_free(msg_buf1);
return 0;
}
