void xntbase_start(xntbase_t *base) { xnticks_t start_date; spl_t s; if (base == &nktbase || xntbase_enabled_p(base)) return; trace_mark(xn_nucleus, tbase_start, "base %s", base->name); xnlock_get_irqsave(&nklock, s); start_date = xnarch_get_cpu_time(); /* Only synchronise non-isolated time bases on the master base. */ if (!xntbase_isolated_p(base)) { base->wallclock_offset = xntbase_ns2ticks(base, start_date + nktbase.wallclock_offset); __setbits(base->status, XNTBSET); } start_date += base->tickvalue; __setbits(base->status, XNTBRUN); xnlock_put_irqrestore(&nklock, s); xntslave_start(base2slave(base), start_date, base->tickvalue); }
ssize_t xnpipe_send(int minor, struct xnpipe_mh *mh, size_t size, int flags) { struct xnpipe_state *state; int need_sched = 0; spl_t s; if (minor < 0 || minor >= XNPIPE_NDEVS) return -ENODEV; if (size <= sizeof(*mh)) return -EINVAL; state = &xnpipe_states[minor]; xnlock_get_irqsave(&nklock, s); if (!testbits(state->status, XNPIPE_KERN_CONN)) { xnlock_put_irqrestore(&nklock, s); return -EBADF; } inith(xnpipe_m_link(mh)); xnpipe_m_size(mh) = size - sizeof(*mh); xnpipe_m_rdoff(mh) = 0; state->ionrd += xnpipe_m_size(mh); if (flags & XNPIPE_URGENT) prependq(&state->outq, xnpipe_m_link(mh)); else appendq(&state->outq, xnpipe_m_link(mh)); if (!testbits(state->status, XNPIPE_USER_CONN)) { xnlock_put_irqrestore(&nklock, s); return (ssize_t) size; } if (testbits(state->status, XNPIPE_USER_WREAD)) { /* * Wake up the regular Linux task waiting for input * from the Xenomai side. */ __setbits(state->status, XNPIPE_USER_WREAD_READY); need_sched = 1; } if (state->asyncq) { /* Schedule asynch sig. */ __setbits(state->status, XNPIPE_USER_SIGIO); need_sched = 1; } if (need_sched) xnpipe_schedule_request(); xnlock_put_irqrestore(&nklock, s); return (ssize_t) size; }
int xntimer_start_aperiodic(xntimer_t *timer, xnticks_t value, xnticks_t interval, xntmode_t mode) { xnticks_t date, now; trace_mark(xn_nucleus, timer_start, "timer %p base %s value %Lu interval %Lu mode %u", timer, xntimer_base(timer)->name, value, interval, mode); if (!testbits(timer->status, XNTIMER_DEQUEUED)) xntimer_dequeue_aperiodic(timer); now = xnarch_get_cpu_tsc(); __clrbits(timer->status, XNTIMER_REALTIME | XNTIMER_FIRED | XNTIMER_PERIODIC); switch (mode) { case XN_RELATIVE: if ((xnsticks_t)value < 0) return -ETIMEDOUT; date = xnarch_ns_to_tsc(value) + now; break; case XN_REALTIME: __setbits(timer->status, XNTIMER_REALTIME); value -= nktbase.wallclock_offset; /* fall through */ default: /* XN_ABSOLUTE || XN_REALTIME */ date = xnarch_ns_to_tsc(value); if ((xnsticks_t)(date - now) <= 0) return -ETIMEDOUT; break; } xntimerh_date(&timer->aplink) = date; timer->interval = XN_INFINITE; if (interval != XN_INFINITE) { timer->interval = xnarch_ns_to_tsc(interval); timer->pexpect = date; __setbits(timer->status, XNTIMER_PERIODIC); } xntimer_enqueue_aperiodic(timer); if (xntimer_heading_p(timer)) { if (xntimer_sched(timer) != xnpod_current_sched()) xntimer_next_remote_shot(xntimer_sched(timer)); else xntimer_next_local_shot(xntimer_sched(timer)); } return 0; }
int xnpipe_connect(int minor, struct xnpipe_operations *ops, void *xstate) { struct xnpipe_state *state; int need_sched = 0, ret; spl_t s; minor = xnpipe_minor_alloc(minor); if (minor < 0) return minor; state = &xnpipe_states[minor]; xnlock_get_irqsave(&nklock, s); ret = xnpipe_set_ops(state, ops); if (ret) { xnlock_put_irqrestore(&nklock, s); return ret; } __setbits(state->status, XNPIPE_KERN_CONN); xnsynch_init(&state->synchbase, XNSYNCH_FIFO, NULL); state->xstate = xstate; state->ionrd = 0; if (testbits(state->status, XNPIPE_USER_CONN)) { if (testbits(state->status, XNPIPE_USER_WREAD)) { /* * Wake up the regular Linux task waiting for * the kernel side to connect (xnpipe_open). */ __setbits(state->status, XNPIPE_USER_WREAD_READY); need_sched = 1; } if (state->asyncq) { /* Schedule asynch sig. */ __setbits(state->status, XNPIPE_USER_SIGIO); need_sched = 1; } } if (need_sched) xnpipe_schedule_request(); xnlock_put_irqrestore(&nklock, s); return minor; }
int xnintr_attach(xnintr_t *intr, void *cookie) { int ret; trace_mark(xn_nucleus, irq_attach, "irq %u name %s", intr->irq, intr->name); intr->cookie = cookie; memset(&intr->stat, 0, sizeof(intr->stat)); #ifdef CONFIG_SMP xnarch_set_irq_affinity(intr->irq, nkaffinity); #endif /* CONFIG_SMP */ down(&intrlock); if (__testbits(intr->flags, XN_ISR_ATTACHED)) { ret = -EBUSY; goto out; } ret = xnintr_irq_attach(intr); if (ret) goto out; __setbits(intr->flags, XN_ISR_ATTACHED); xnintr_stat_counter_inc(); out: up(&intrlock); return ret; }
_WCRTLINK CHAR_TYPE *__F_NAME(strpbrk,wcspbrk) ( const CHAR_TYPE *str, const CHAR_TYPE *charset ) { #if defined(__WIDECHAR__) const CHAR_TYPE *p1; const CHAR_TYPE *p2; CHAR_TYPE tc1; CHAR_TYPE tc2; size_t len; len = 0; for( p1 = str; tc1 = *p1; p1++, len++ ) { for( p2 = charset; tc2 = *p2; p2++ ) { if( tc1 == tc2 ) break; } if( tc2 != NULLCHAR ) return( (CHAR_TYPE *)p1 ); } return( NULL ); #else char tc; unsigned char vector[ CHARVECTOR_SIZE ]; __setbits( vector, charset ); for( ; tc = *str; ++str ) { /* quit when we find any char in charset */ if( GETCHARBIT( vector, tc ) != 0 ) return( (char *)str ); } return( NULL ); #endif }
static inline void xnpipe_enqueue_wait(struct xnpipe_state *state, int mask) { if (state->wcount != 0x7fffffff && state->wcount++ == 0) appendq(&xnpipe_sleepq, &state->slink); __setbits(state->status, mask); }
_WCRTLINK size_t __F_NAME(strcspn,wcscspn) ( const CHAR_TYPE *str, const CHAR_TYPE *charset ) { #if defined(__WIDECHAR__) const CHAR_TYPE *p1; const CHAR_TYPE *p2; CHAR_TYPE tc1; CHAR_TYPE tc2; size_t len; len = 0; for( p1 = str; tc1 = *p1; p1++, len++ ) { for( p2 = charset; tc2 = *p2; p2++ ) { if( tc1 == tc2 ) break; } if( tc2 != NULLCHAR ) break; } return( len ); #else unsigned /*char*/ tc; unsigned char vector[32]; size_t len; __setbits( vector, charset ); len = 0; for( ; (tc = (unsigned char) *str); ++len, ++str ) { /* quit if we find any char in charset */ if( ( vector[ tc >> 3 ] & _Bits[ tc & 0x07 ] ) != 0 ) break; } return( len ); #endif }
void xnsynch_renice_sleeper(xnthread_t *thread) { xnsynch_t *synch = thread->wchan; xnthread_t *owner; if (!testbits(synch->status, XNSYNCH_PRIO)) return; removepq(&synch->pendq, &thread->plink); insertpqf(&synch->pendq, &thread->plink, thread->cprio); owner = synch->owner; if (owner != NULL && thread->cprio > owner->cprio) { /* The new priority of the sleeping thread is higher * than the priority of the current owner of the * resource: we need to update the PI state. */ if (testbits(synch->status, XNSYNCH_CLAIMED)) { /* The resource is already claimed, just reorder the claim queue. */ removepq(&owner->claimq, &synch->link); insertpqf(&owner->claimq, &synch->link, thread->cprio); } else { /* The resource was NOT claimed, claim it now * and boost the owner. */ __setbits(synch->status, XNSYNCH_CLAIMED); insertpqf(&owner->claimq, &synch->link, thread->cprio); owner->bprio = owner->cprio; xnthread_set_state(owner, XNBOOST); } /* Renice the owner thread, progressing in the PI chain as needed. */ xnsynch_renice_thread(owner, thread->cprio); } }
void xntslave_destroy(xntslave_t *slave) { int nr_cpus, cpu, n; spl_t s; for (cpu = 0, nr_cpus = xnarch_num_online_cpus(); cpu < nr_cpus; cpu++) { struct percpu_cascade *pc = &slave->cascade[cpu]; xnlock_get_irqsave(&nklock, s); xntimer_destroy(&pc->timer); xnlock_put_irqrestore(&nklock, s); for (n = 0; n < XNTIMER_WHEELSIZE; n++) { xnqueue_t *timerq = &pc->wheel[n]; xntlholder_t *holder; while ((holder = xntlist_head(timerq)) != NULL) { __setbits(plink2timer(holder)->status, XNTIMER_DEQUEUED); xntlist_remove(timerq, holder); } } } }
void xntimer_freeze(void) { int nr_cpus, cpu; spl_t s; trace_mark(xn_nucleus, timer_freeze, MARK_NOARGS); xnlock_get_irqsave(&nklock, s); nr_cpus = xnarch_num_online_cpus(); for (cpu = 0; cpu < nr_cpus; cpu++) { xntimerq_t *timerq = &xnpod_sched_slot(cpu)->timerqueue; xntimerh_t *holder; while ((holder = xntimerq_head(timerq)) != NULL) { __setbits(aplink2timer(holder)->status, XNTIMER_DEQUEUED); xntimerq_remove(timerq, holder); } /* Dequeuing all timers from the master time base * freezes all slave time bases the same way, so there * is no need to handle anything more here. */ } xnlock_put_irqrestore(&nklock, s); }
static inline void xntimer_dequeue_periodic(xntimer_t *timer) { unsigned slot = (xntlholder_date(&timer->plink) & XNTIMER_WHEELMASK); unsigned cpu = xnsched_cpu(timer->sched); struct percpu_cascade *pc = &base2slave(timer->base)->cascade[cpu]; xntlist_remove(&pc->wheel[slot], &timer->plink); __setbits(timer->status, XNTIMER_DEQUEUED); }
void addmlq(struct xnsched_mlq *q, struct xnpholder *h, int idx, int lifo) { struct xnqueue *queue = &q->queue[idx]; int hi = idx / BITS_PER_LONG; int lo = idx % BITS_PER_LONG; if (lifo) prependq(queue, &h->plink); else appendq(queue, &h->plink); h->prio = idx; q->elems++; __setbits(q->himap, 1UL << hi); __setbits(q->lomap[hi], 1UL << lo); }
static int xnpipe_open(struct inode *inode, struct file *file) { int minor, err = 0, sigpending; struct xnpipe_state *state; spl_t s; minor = MINOR(inode->i_rdev); if (minor >= XNPIPE_NDEVS) return -ENXIO; /* TssTss... stop playing with mknod() ;o) */ state = &xnpipe_states[minor]; xnlock_get_irqsave(&nklock, s); /* Enforce exclusive open for the message queues. */ if (testbits(state->status, XNPIPE_USER_CONN)) { xnlock_put_irqrestore(&nklock, s); return -EBUSY; } __setbits(state->status, XNPIPE_USER_CONN); file->private_data = state; init_waitqueue_head(&state->readq); init_waitqueue_head(&state->syncq); state->wcount = 0; __clrbits(state->status, XNPIPE_USER_ALL_WAIT | XNPIPE_USER_ALL_READY | XNPIPE_USER_SIGIO); if (!testbits(state->status, XNPIPE_KERN_CONN)) { if (testbits(file->f_flags, O_NONBLOCK)) { xnpipe_cleanup_user_conn(state, s); xnlock_put_irqrestore(&nklock, s); return -EWOULDBLOCK; } sigpending = xnpipe_wait(state, XNPIPE_USER_WREAD, s, testbits(state->status, XNPIPE_KERN_CONN)); if (sigpending) { xnpipe_cleanup_user_conn(state, s); xnlock_put_irqrestore(&nklock, s); return -ERESTARTSYS; } } if (err) xnpipe_cleanup_user_conn(state, s); xnlock_put_irqrestore(&nklock, s); return err; }
int xnmap_remove(xnmap_t *map, int key) { int ofkey = key - map->offset, hi, lo; spl_t s; if (ofkey < 0 || ofkey >= map->nkeys) return -ESRCH; hi = ofkey / BITS_PER_LONG; lo = ofkey % BITS_PER_LONG; xnlock_get_irqsave(&nklock, s); map->objarray[ofkey] = NULL; __setbits(map->himap, 1UL << hi); __setbits(map->lomap[hi], 1UL << lo); --map->ukeys; xnlock_put_irqrestore(&nklock, s); return 0; }
static inline int xnintr_irq_attach(xnintr_t *intr) { xnintr_irq_t *shirq = &xnirqs[intr->irq]; xnintr_t *prev, **p = &shirq->handlers; int err; if (intr->irq >= RTHAL_NR_IRQS) return -EINVAL; if (__testbits(intr->flags, XN_ISR_ATTACHED)) return -EPERM; if ((prev = *p) != NULL) { /* Check on whether the shared mode is allowed. */ if (!(prev->flags & intr->flags & XN_ISR_SHARED) || (prev->iack != intr->iack) || ((prev->flags & XN_ISR_EDGE) != (intr->flags & XN_ISR_EDGE))) return -EBUSY; /* Get a position at the end of the list to insert the new element. */ while (prev) { p = &prev->next; prev = *p; } } else { /* Initialize the corresponding interrupt channel */ void (*handler) (unsigned, void *) = &xnintr_irq_handler; if (intr->flags & XN_ISR_SHARED) { if (intr->flags & XN_ISR_EDGE) handler = &xnintr_edge_shirq_handler; else handler = &xnintr_shirq_handler; } shirq->unhandled = 0; err = xnarch_hook_irq(intr->irq, handler, intr->iack, intr); if (err) return err; } __setbits(intr->flags, XN_ISR_ATTACHED); intr->next = NULL; /* Add the given interrupt object. No need to synchronise with the IRQ handler, we are only extending the chain. */ *p = intr; return 0; }
static int xntimer_start_periodic(xntimer_t *timer, xnticks_t value, xnticks_t interval, xntmode_t mode) { trace_mark(xn_nucleus, timer_start, "timer %p base %s value %Lu interval %Lu mode %u", timer, xntimer_base(timer)->name, value, interval, mode); if (!testbits(timer->status, XNTIMER_DEQUEUED)) xntimer_dequeue_periodic(timer); __clrbits(timer->status, XNTIMER_REALTIME | XNTIMER_FIRED | XNTIMER_PERIODIC); switch (mode) { case XN_RELATIVE: if ((xnsticks_t)value < 0) return -ETIMEDOUT; value += timer->base->jiffies; break; case XN_REALTIME: __setbits(timer->status, XNTIMER_REALTIME); value -= timer->base->wallclock_offset; /* fall through */ default: /* XN_ABSOLUTE || XN_REALTIME */ if ((xnsticks_t)(value - timer->base->jiffies) <= 0) return -ETIMEDOUT; break; } xntlholder_date(&timer->plink) = value; timer->interval = interval; if (interval != XN_INFINITE) { __setbits(timer->status, XNTIMER_PERIODIC); timer->pexpect = value; } xntimer_enqueue_periodic(timer); return 0; }
void xntimer_destroy(xntimer_t *timer) { spl_t s; xnlock_get_irqsave(&nklock, s); xntimer_stop(timer); __setbits(timer->status, XNTIMER_KILLED); timer->sched = NULL; #ifdef CONFIG_XENO_OPT_STATS removeq(&xntimer_base(timer)->timerq, &timer->tblink); xntimer_base(timer)->timerq_rev++; #endif /* CONFIG_XENO_OPT_TIMING_PERIODIC */ xnlock_put_irqrestore(&nklock, s); }
void xnintr_clock_handler(void) { xnstat_exectime_t *prev; struct xnsched *sched; unsigned cpu; cpu = xnarch_current_cpu(); if (!cpumask_test_cpu(cpu, &xnarch_supported_cpus)) { xnarch_relay_tick(); return; } sched = xnpod_sched_slot(cpu); prev = xnstat_exectime_switch(sched, &nkclock.stat[xnsched_cpu(sched)].account); xnstat_counter_inc(&nkclock.stat[xnsched_cpu(sched)].hits); trace_mark(xn_nucleus, irq_enter, "irq %u", XNARCH_TIMER_IRQ); trace_mark(xn_nucleus, tbase_tick, "base %s", nktbase.name); ++sched->inesting; __setbits(sched->lflags, XNINIRQ); xnlock_get(&nklock); xntimer_tick_aperiodic(); xnlock_put(&nklock); xnstat_exectime_switch(sched, prev); if (--sched->inesting == 0) { __clrbits(sched->lflags, XNINIRQ); xnpod_schedule(); sched = xnpod_current_sched(); } /* * If the clock interrupt preempted a real-time thread, any * transition to the root thread has already triggered a host * tick propagation from xnpod_schedule(), so at this point, * we only need to propagate the host tick in case the * interrupt preempted the root thread. */ if (testbits(sched->lflags, XNHTICK) && xnthread_test_state(sched->curr, XNROOT)) xnintr_host_tick(sched); trace_mark(xn_nucleus, irq_exit, "irq %u", XNARCH_TIMER_IRQ); }
void xnsynch_requeue_sleeper(struct xnthread *thread) { struct xnsynch *synch = thread->wchan; struct xnthread *owner; if (!testbits(synch->status, XNSYNCH_PRIO)) return; removepq(&synch->pendq, &thread->plink); insertpqf(&synch->pendq, &thread->plink, w_cprio(thread)); owner = synch->owner; if (owner != NULL && w_cprio(thread) > w_cprio(owner)) { /* * The new (weighted) priority of the sleeping thread * is higher than the priority of the current owner of * the resource: we need to update the PI state. */ if (testbits(synch->status, XNSYNCH_CLAIMED)) { /* * The resource is already claimed, just * reorder the claim queue. */ removepq(&owner->claimq, &synch->link); insertpqf(&owner->claimq, &synch->link, w_cprio(thread)); } else { /* * The resource was NOT claimed, claim it now * and boost the owner. */ __setbits(synch->status, XNSYNCH_CLAIMED); insertpqf(&owner->claimq, &synch->link, w_cprio(thread)); if (!xnthread_test_state(owner, XNBOOST)) { owner->bprio = owner->cprio; xnthread_set_state(owner, XNBOOST); } } /* * Renice the owner thread, progressing in the PI * chain as needed. */ xnsynch_renice_thread(owner, thread); } }
void xntimer_tick_periodic_inner(xntslave_t *slave) { xnsched_t *sched = xnpod_current_sched(); xntbase_t *base = &slave->base; xntlholder_t *holder; xnqueue_t *timerq; xntimer_t *timer; /* * Update the periodic clocks keeping the things strictly * monotonous (this routine is run on every cpu, but only CPU * XNTIMER_KEEPER_ID should do this). */ if (sched == xnpod_sched_slot(XNTIMER_KEEPER_ID)) ++base->jiffies; timerq = &slave->cascade[xnsched_cpu(sched)].wheel[base->jiffies & XNTIMER_WHEELMASK]; while ((holder = xntlist_head(timerq)) != NULL) { timer = plink2timer(holder); if ((xnsticks_t) (xntlholder_date(&timer->plink) - base->jiffies) > 0) break; trace_mark(xn_nucleus, timer_expire, "timer %p", timer); xntimer_dequeue_periodic(timer); xnstat_counter_inc(&timer->fired); timer->handler(timer); if (!xntimer_reload_p(timer)) continue; __setbits(timer->status, XNTIMER_FIRED); xntlholder_date(&timer->plink) = base->jiffies + timer->interval; xntimer_enqueue_periodic(timer); } xnsched_tick(sched->curr, base); /* Do time-slicing if required. */ }
int xnintr_attach(xnintr_t *intr, void *cookie) { int ret; spl_t s; trace_mark(xn_nucleus, irq_attach, "irq %u name %s", intr->irq, intr->name); intr->cookie = cookie; memset(&intr->stat, 0, sizeof(intr->stat)); #ifdef CONFIG_SMP xnarch_set_irq_affinity(intr->irq, nkaffinity); #endif /* CONFIG_SMP */ xnlock_get_irqsave(&intrlock, s); if (intr->irq >= XNARCH_NR_IRQS) { ret = -EINVAL; goto out; } if (__testbits(intr->flags, XN_ISR_ATTACHED)) { ret = -EBUSY; goto out; } ret = xnintr_irq_attach(intr); if (ret) goto out; __setbits(intr->flags, XN_ISR_ATTACHED); xnintr_stat_counter_inc(); out: xnlock_put_irqrestore(&intrlock, s); return ret; }
void xntbase_adjust_time(xntbase_t *base, xnsticks_t delta) { xnticks_t now; #ifdef CONFIG_XENO_OPT_TIMING_PERIODIC if (xntbase_isolated_p(base)) { /* Only update the specified isolated base. */ base->wallclock_offset += delta; __setbits(base->status, XNTBSET); xntslave_adjust(base2slave(base), delta); } else { xnholder_t *holder; #endif /* CONFIG_XENO_OPT_TIMING_PERIODIC */ /* Update all non-isolated bases in the system. */ nktbase.wallclock_offset += xntbase_ticks2ns(base, delta); now = xnarch_get_cpu_time() + nktbase.wallclock_offset; xntimer_adjust_all_aperiodic(xntbase_ticks2ns(base, delta)); #ifdef CONFIG_XENO_OPT_TIMING_PERIODIC for (holder = getheadq(&nktimebaseq); holder != NULL; holder = nextq(&nktimebaseq, holder)) { xntbase_t *tbase = link2tbase(holder); if (tbase == &nktbase || xntbase_isolated_p(tbase)) continue; tbase->wallclock_offset = xntbase_ns2ticks(tbase, now) - xntbase_get_jiffies(tbase); xntslave_adjust(base2slave(tbase), delta); } } #endif /* CONFIG_XENO_OPT_TIMING_PERIODIC */ trace_mark(xn_nucleus, tbase_adjust, "base %s delta %Lu", base->name, delta); }
static inline int xnpipe_minor_alloc(int minor) { spl_t s; if ((minor < 0 && minor != XNPIPE_MINOR_AUTO) || minor >= XNPIPE_NDEVS) return -ENODEV; xnlock_get_irqsave(&nklock, s); if (minor == XNPIPE_MINOR_AUTO) minor = find_first_zero_bit(xnpipe_bitmap, XNPIPE_NDEVS); if (minor == XNPIPE_NDEVS || testbits(xnpipe_bitmap[minor / BITS_PER_LONG], 1UL << (minor % BITS_PER_LONG))) minor = -EBUSY; else __setbits(xnpipe_bitmap[minor / BITS_PER_LONG], 1UL << (minor % BITS_PER_LONG)); xnlock_put_irqrestore(&nklock, s); return minor; }
int xnpipe_flush(int minor, int mode) { struct xnpipe_state *state; int msgcount; spl_t s; if (minor < 0 || minor >= XNPIPE_NDEVS) return -ENODEV; state = &xnpipe_states[minor]; xnlock_get_irqsave(&nklock, s); if (!testbits(state->status, XNPIPE_KERN_CONN)) { xnlock_put_irqrestore(&nklock, s); return -EBADF; } msgcount = countq(&state->outq) + countq(&state->inq); if (mode & XNPIPE_OFLUSH) state->ionrd -= xnpipe_flushq(state, outq, free_obuf, s); if (mode & XNPIPE_IFLUSH) xnpipe_flushq(state, inq, free_ibuf, s); if (testbits(state->status, XNPIPE_USER_WSYNC) && msgcount > countq(&state->outq) + countq(&state->inq)) { __setbits(state->status, XNPIPE_USER_WSYNC_READY); xnpipe_schedule_request(); } xnlock_put_irqrestore(&nklock, s); return 0; }
xnflags_t xnsynch_acquire(struct xnsynch *synch, xnticks_t timeout, xntmode_t timeout_mode) { struct xnthread *thread = xnpod_current_thread(), *owner; xnhandle_t threadh = xnthread_handle(thread), fastlock, old; const int use_fastlock = xnsynch_fastlock_p(synch); spl_t s; XENO_BUGON(NUCLEUS, !testbits(synch->status, XNSYNCH_OWNER)); trace_mark(xn_nucleus, synch_acquire, "synch %p", synch); redo: if (use_fastlock) { xnarch_atomic_t *lockp = xnsynch_fastlock(synch); fastlock = xnarch_atomic_cmpxchg(lockp, XN_NO_HANDLE, threadh); if (likely(fastlock == XN_NO_HANDLE)) { if (xnthread_test_state(thread, XNOTHER)) xnthread_inc_rescnt(thread); xnthread_clear_info(thread, XNRMID | XNTIMEO | XNBREAK); return 0; } xnlock_get_irqsave(&nklock, s); /* Set claimed bit. In case it appears to be set already, re-read its state under nklock so that we don't miss any change between the lock-less read and here. But also try to avoid cmpxchg where possible. Only if it appears not to be set, start with cmpxchg directly. */ if (xnsynch_fast_is_claimed(fastlock)) { old = xnarch_atomic_get(lockp); goto test_no_owner; } do { old = xnarch_atomic_cmpxchg(lockp, fastlock, xnsynch_fast_set_claimed(fastlock, 1)); if (likely(old == fastlock)) break; test_no_owner: if (old == XN_NO_HANDLE) { /* Owner called xnsynch_release (on another cpu) */ xnlock_put_irqrestore(&nklock, s); goto redo; } fastlock = old; } while (!xnsynch_fast_is_claimed(fastlock)); owner = xnthread_lookup(xnsynch_fast_mask_claimed(fastlock)); if (!owner) { /* The handle is broken, therefore pretend that the synch object was deleted to signal an error. */ xnthread_set_info(thread, XNRMID); goto unlock_and_exit; } xnsynch_set_owner(synch, owner); } else { xnlock_get_irqsave(&nklock, s); owner = synch->owner; if (!owner) { synch->owner = thread; if (xnthread_test_state(thread, XNOTHER)) xnthread_inc_rescnt(thread); xnthread_clear_info(thread, XNRMID | XNTIMEO | XNBREAK); goto unlock_and_exit; } } xnsynch_detect_relaxed_owner(synch, thread); if (!testbits(synch->status, XNSYNCH_PRIO)) /* i.e. FIFO */ appendpq(&synch->pendq, &thread->plink); else if (w_cprio(thread) > w_cprio(owner)) { if (xnthread_test_info(owner, XNWAKEN) && owner->wwake == synch) { /* Ownership is still pending, steal the resource. */ synch->owner = thread; xnthread_clear_info(thread, XNRMID | XNTIMEO | XNBREAK); xnthread_set_info(owner, XNROBBED); goto grab_and_exit; } insertpqf(&synch->pendq, &thread->plink, w_cprio(thread)); if (testbits(synch->status, XNSYNCH_PIP)) { if (!xnthread_test_state(owner, XNBOOST)) { owner->bprio = owner->cprio; xnthread_set_state(owner, XNBOOST); } if (testbits(synch->status, XNSYNCH_CLAIMED)) removepq(&owner->claimq, &synch->link); else __setbits(synch->status, XNSYNCH_CLAIMED); insertpqf(&owner->claimq, &synch->link, w_cprio(thread)); xnsynch_renice_thread(owner, thread); } } else insertpqf(&synch->pendq, &thread->plink, w_cprio(thread)); xnpod_suspend_thread(thread, XNPEND, timeout, timeout_mode, synch); thread->wwake = NULL; xnthread_clear_info(thread, XNWAKEN); if (xnthread_test_info(thread, XNRMID | XNTIMEO | XNBREAK)) goto unlock_and_exit; if (xnthread_test_info(thread, XNROBBED)) { /* Somebody stole us the ownership while we were ready to run, waiting for the CPU: we need to wait again for the resource. */ if (timeout_mode != XN_RELATIVE || timeout == XN_INFINITE) { xnlock_put_irqrestore(&nklock, s); goto redo; } timeout = xntimer_get_timeout_stopped(&thread->rtimer); if (timeout > 1) { /* Otherwise, it's too late. */ xnlock_put_irqrestore(&nklock, s); goto redo; } xnthread_set_info(thread, XNTIMEO); } else { grab_and_exit: if (xnthread_test_state(thread, XNOTHER)) xnthread_inc_rescnt(thread); if (use_fastlock) { xnarch_atomic_t *lockp = xnsynch_fastlock(synch); /* We are the new owner, update the fastlock accordingly. */ if (xnsynch_pended_p(synch)) threadh = xnsynch_fast_set_claimed(threadh, 1); xnarch_atomic_set(lockp, threadh); } } unlock_and_exit: xnlock_put_irqrestore(&nklock, s); return xnthread_test_info(thread, XNRMID|XNTIMEO|XNBREAK); }
void xntimer_next_local_shot(xnsched_t *sched) { struct xntimer *timer; xnsticks_t delay; xntimerq_it_t it; xntimerh_t *h; /* * Do not reprogram locally when inside the tick handler - * will be done on exit anyway. Also exit if there is no * pending timer. */ if (testbits(sched->status, XNINTCK)) return; h = xntimerq_it_begin(&sched->timerqueue, &it); if (h == NULL) return; /* * Here we try to defer the host tick heading the timer queue, * so that it does not preempt a real-time activity uselessly, * in two cases: * * 1) a rescheduling is pending for the current CPU. We may * assume that a real-time thread is about to resume, so we * want to move the host tick out of the way until the host * kernel resumes, unless there is no other outstanding * timers. * * 2) the current thread is running in primary mode, in which * case we may also defer the host tick until the host kernel * resumes. * * The host tick deferral is cleared whenever Xenomai is about * to yield control to the host kernel (see * __xnpod_schedule()), or a timer with an earlier timeout * date is scheduled, whichever comes first. */ __clrbits(sched->status, XNHDEFER); timer = aplink2timer(h); if (unlikely(timer == &sched->htimer)) { if (xnsched_self_resched_p(sched) || !xnthread_test_state(sched->curr, XNROOT)) { h = xntimerq_it_next(&sched->timerqueue, &it, h); if (h) { __setbits(sched->status, XNHDEFER); timer = aplink2timer(h); } } } delay = xntimerh_date(&timer->aplink) - (xnarch_get_cpu_tsc() + nklatency); if (delay < 0) delay = 0; else if (delay > ULONG_MAX) delay = ULONG_MAX; xnarch_trace_tick((unsigned)delay); xnarch_program_timer_shot(delay); }
static inline void xntimer_dequeue_aperiodic(xntimer_t *timer) { xntimerq_remove(&timer->sched->timerqueue, &timer->aplink); __setbits(timer->status, XNTIMER_DEQUEUED); }
void xntimer_tick_aperiodic(void) { xnsched_t *sched = xnpod_current_sched(); xntimerq_t *timerq = &sched->timerqueue; xntimerh_t *holder; xntimer_t *timer; xnsticks_t delta; xnticks_t now; /* * Optimisation: any local timer reprogramming triggered by * invoked timer handlers can wait until we leave the tick * handler. Use this status flag as hint to * xntimer_start_aperiodic. */ __setbits(sched->status, XNINTCK); now = xnarch_get_cpu_tsc(); while ((holder = xntimerq_head(timerq)) != NULL) { timer = aplink2timer(holder); /* * If the delay to the next shot is greater than the * intrinsic latency value, we may stop scanning the * timer queue there, since timeout dates are ordered * by increasing values. */ delta = (xnsticks_t)(xntimerh_date(&timer->aplink) - now); if (delta > (xnsticks_t)nklatency) break; trace_mark(xn_nucleus, timer_expire, "timer %p", timer); xntimer_dequeue_aperiodic(timer); xnstat_counter_inc(&timer->fired); if (likely(timer != &sched->htimer)) { if (likely(!testbits(nktbase.status, XNTBLCK) || testbits(timer->status, XNTIMER_NOBLCK))) { timer->handler(timer); now = xnarch_get_cpu_tsc(); /* * If the elapsed timer has no reload * value, or was re-enqueued or killed * by the timeout handler: don't not * re-enqueue it for the next shot. */ if (!xntimer_reload_p(timer)) continue; __setbits(timer->status, XNTIMER_FIRED); } else if (likely(!testbits(timer->status, XNTIMER_PERIODIC))) { /* * Postpone the next tick to a * reasonable date in the future, * waiting for the timebase to be * unlocked at some point. */ xntimerh_date(&timer->aplink) = xntimerh_date(&sched->htimer.aplink); continue; } } else { /* * By postponing the propagation of the * low-priority host tick to the interrupt * epilogue (see xnintr_irq_handler()), we * save some I-cache, which translates into * precious microsecs on low-end hw. */ __setbits(sched->status, XNHTICK); __clrbits(sched->status, XNHDEFER); if (!testbits(timer->status, XNTIMER_PERIODIC)) continue; } do { xntimerh_date(&timer->aplink) += timer->interval; } while (xntimerh_date(&timer->aplink) < now + nklatency); xntimer_enqueue_aperiodic(timer); } __clrbits(sched->status, XNINTCK); xntimer_next_local_shot(sched); }
/* * Low-level interrupt handler dispatching non-shared ISRs -- Called with * interrupts off. */ static void xnintr_irq_handler(unsigned irq, void *cookie) { struct xnsched *sched = xnpod_current_sched(); xnstat_exectime_t *prev; struct xnintr *intr; xnticks_t start; int s; prev = xnstat_exectime_get_current(sched); start = xnstat_exectime_now(); trace_mark(xn_nucleus, irq_enter, "irq %u", irq); ++sched->inesting; __setbits(sched->lflags, XNINIRQ); xnlock_get(&xnirqs[irq].lock); #ifdef CONFIG_SMP /* * In SMP case, we have to reload the cookie under the per-IRQ * lock to avoid racing with xnintr_detach. However, we * assume that no CPU migration will occur while running the * interrupt service routine, so the scheduler pointer will * remain valid throughout this function. */ intr = xnarch_get_irq_cookie(irq); if (unlikely(!intr)) { s = 0; goto unlock_and_exit; } #else /* cookie always valid, attach/detach happens with IRQs disabled */ intr = cookie; #endif s = intr->isr(intr); if (unlikely(s == XN_ISR_NONE)) { if (++intr->unhandled == XNINTR_MAX_UNHANDLED) { xnlogerr("%s: IRQ%d not handled. Disabling IRQ " "line.\n", __FUNCTION__, irq); s |= XN_ISR_NOENABLE; } } else { xnstat_counter_inc(&intr->stat[xnsched_cpu(sched)].hits); xnstat_exectime_lazy_switch(sched, &intr->stat[xnsched_cpu(sched)].account, start); intr->unhandled = 0; } #ifdef CONFIG_SMP unlock_and_exit: #endif xnlock_put(&xnirqs[irq].lock); if (s & XN_ISR_PROPAGATE) xnarch_chain_irq(irq); else if (!(s & XN_ISR_NOENABLE)) xnarch_end_irq(irq); xnstat_exectime_switch(sched, prev); if (--sched->inesting == 0) { __clrbits(sched->lflags, XNINIRQ); xnpod_schedule(); } trace_mark(xn_nucleus, irq_exit, "irq %u", irq); }