static void xnsynch_clear_boost(struct xnsynch *synch,
struct xnthread *owner)
{
struct xnthread *target;
struct xnsynch *hsynch;
struct xnpholder *h;
int wprio;
removepq(&owner->claimq, &synch->link);
__clrbits(synch->status, XNSYNCH_CLAIMED);
wprio = w_bprio(owner);
if (emptypq_p(&owner->claimq)) {
xnthread_clear_state(owner, XNBOOST);
target = owner;
} else {
/* Find the highest priority needed to enforce the PIP. */
hsynch = link2synch(getheadpq(&owner->claimq));
h = getheadpq(&hsynch->pendq);
XENO_BUGON(NUCLEUS, h == NULL);
target = link2thread(h, plink);
if (w_cprio(target) > wprio)
wprio = w_cprio(target);
else
target = owner;
}
if (w_cprio(owner) != wprio &&
!xnthread_test_state(owner, XNZOMBIE))
xnsynch_renice_thread(owner, target);
}
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 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 xnsynch_forget_sleeper(struct xnthread *thread)
{
struct xnsynch *synch = thread->wchan;
struct xnthread *owner, *target;
struct xnpholder *h;
trace_mark(xn_nucleus, synch_forget,
"thread %p thread_name %s synch %p",
thread, xnthread_name(thread), synch);
xnthread_clear_state(thread, XNPEND);
thread->wchan = NULL;
removepq(&synch->pendq, &thread->plink);
if (testbits(synch->status, XNSYNCH_CLAIMED)) {
/* Find the highest priority needed to enforce the PIP. */
owner = synch->owner;
if (emptypq_p(&synch->pendq))
/* No more sleepers: clear the boost. */
xnsynch_clear_boost(synch, owner);
else {
target = link2thread(getheadpq(&synch->pendq), plink);
h = getheadpq(&owner->claimq);
if (w_cprio(target) != h->prio) {
/*
* Reorder the claim queue, and lower
* the priority to the required
* minimum needed to prevent priority
* inversion.
*/
removepq(&owner->claimq, &synch->link);
insertpqf(&owner->claimq, &synch->link,
w_cprio(target));
h = getheadpq(&owner->claimq);
if (h->prio < w_cprio(owner))
xnsynch_renice_thread(owner, target);
}
}
}
xnarch_post_graph_if(synch, 0, emptypq_p(&synch->pendq));
}
static void xnsynch_clear_boost(xnsynch_t *synch, xnthread_t *lastowner)
{
int downprio;
removepq(&lastowner->claimq, &synch->link);
downprio = lastowner->bprio;
__clrbits(synch->status, XNSYNCH_CLAIMED);
if (emptypq_p(&lastowner->claimq))
xnthread_clear_state(lastowner, XNBOOST);
else {
/* Find the highest priority needed to enforce the PIP. */
int rprio = getheadpq(&lastowner->claimq)->prio;
if (rprio > downprio)
downprio = rprio;
}
if (lastowner->cprio != downprio)
xnsynch_renice_thread(lastowner, downprio);
}
void xnsynch_forget_sleeper(xnthread_t *thread)
{
xnsynch_t *synch = thread->wchan;
trace_mark(xn_nucleus_synch_forget,
"thread %p thread_name %s synch %p",
thread, xnthread_name(thread), synch);
xnthread_clear_state(thread, XNPEND);
thread->wchan = NULL;
removepq(&synch->pendq, &thread->plink);
if (testbits(synch->status, XNSYNCH_CLAIMED)) {
/* Find the highest priority needed to enforce the PIP. */
xnthread_t *owner = synch->owner;
int rprio;
if (emptypq_p(&synch->pendq))
/* No more sleepers: clear the boost. */
xnsynch_clear_boost(synch, owner);
else if (getheadpq(&synch->pendq)->prio !=
getheadpq(&owner->claimq)->prio) {
/* Reorder the claim queue, and lower the priority to the
required minimum needed to prevent priority
inversion. */
removepq(&owner->claimq, &synch->link);
insertpqf(&owner->claimq,
&synch->link, getheadpq(&synch->pendq)->prio);
rprio = getheadpq(&owner->claimq)->prio;
if (rprio < owner->cprio)
xnsynch_renice_thread(owner, rprio);
}
}
xnarch_post_graph_if(synch, 0, emptypq_p(&synch->pendq));
}
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 xnsynch_sleep_on(xnsynch_t *synch, xnticks_t timeout,
xntmode_t timeout_mode)
{
xnthread_t *thread = xnpod_current_thread(), *owner;
spl_t s;
xnlock_get_irqsave(&nklock, s);
trace_mark(xn_nucleus_synch_sleepon,
"thread %p thread_name %s synch %p",
thread, xnthread_name(thread), synch);
if (!testbits(synch->status, XNSYNCH_PRIO)) { /* i.e. FIFO */
appendpq(&synch->pendq, &thread->plink);
xnpod_suspend_thread(thread, XNPEND, timeout, timeout_mode, synch);
goto unlock_and_exit;
}
if (!testbits(synch->status, XNSYNCH_PIP)) { /* i.e. no ownership */
insertpqf(&synch->pendq, &thread->plink, thread->cprio);
xnpod_suspend_thread(thread, XNPEND, timeout, timeout_mode, synch);
goto unlock_and_exit;
}
redo:
owner = synch->owner;
if (!owner) {
synch->owner = thread;
xnthread_clear_info(thread, XNRMID | XNTIMEO | XNBREAK);
goto unlock_and_exit;
}
if (thread->cprio > owner->cprio) {
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 unlock_and_exit;
}
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, thread->cprio);
insertpqf(&synch->pendq, &thread->plink, thread->cprio);
xnsynch_renice_thread(owner, thread->cprio);
} else
insertpqf(&synch->pendq, &thread->plink, thread->cprio);
xnpod_suspend_thread(thread, XNPEND, timeout, timeout_mode, synch);
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)
goto redo;
timeout = xntimer_get_timeout_stopped(&thread->rtimer);
if (timeout > 1) /* Otherwise, it's too late. */
goto redo;
xnthread_set_info(thread, XNTIMEO);
}
unlock_and_exit:
thread->wwake = NULL;
xnthread_clear_info(thread, XNWAKEN);
xnlock_put_irqrestore(&nklock, s);
}
