static struct xnthread *
xnsynch_release_thread(struct xnsynch *synch, struct xnthread *lastowner)
{
const int use_fastlock = xnsynch_fastlock_p(synch);
xnhandle_t lastownerh, newownerh;
struct xnthread *newowner;
struct xnpholder *holder;
spl_t s;
XENO_BUGON(NUCLEUS, !testbits(synch->status, XNSYNCH_OWNER));
#ifdef CONFIG_XENO_OPT_PERVASIVE
if (xnthread_test_state(lastowner, XNOTHER)) {
if (xnthread_get_rescnt(lastowner) == 0)
xnshadow_send_sig(lastowner, SIGDEBUG,
SIGDEBUG_MIGRATE_PRIOINV, 1);
else
xnthread_dec_rescnt(lastowner);
}
#endif
lastownerh = xnthread_handle(lastowner);
if (use_fastlock &&
likely(xnsynch_fast_release(xnsynch_fastlock(synch), lastownerh)))
return NULL;
xnlock_get_irqsave(&nklock, s);
trace_mark(xn_nucleus, synch_release, "synch %p", synch);
holder = getpq(&synch->pendq);
if (holder) {
newowner = link2thread(holder, plink);
newowner->wchan = NULL;
newowner->wwake = synch;
synch->owner = newowner;
xnthread_set_info(newowner, XNWAKEN);
xnpod_resume_thread(newowner, XNPEND);
if (testbits(synch->status, XNSYNCH_CLAIMED))
xnsynch_clear_boost(synch, lastowner);
newownerh = xnsynch_fast_set_claimed(xnthread_handle(newowner),
xnsynch_pended_p(synch));
} else {
newowner = NULL;
synch->owner = NULL;
newownerh = XN_NO_HANDLE;
}
if (use_fastlock) {
xnarch_atomic_t *lockp = xnsynch_fastlock(synch);
xnarch_atomic_set(lockp, newownerh);
}
xnlock_put_irqrestore(&nklock, s);
xnarch_post_graph_if(synch, 0, emptypq_p(&synch->pendq));
return newowner;
}
struct xnthread *xnsynch_release(struct xnsynch *synch)
{
const int use_fastlock = xnsynch_fastlock_p(synch);
struct xnthread *newowner, *lastowner;
xnhandle_t lastownerh, newownerh;
struct xnpholder *holder;
spl_t s;
XENO_BUGON(NUCLEUS, !testbits(synch->status, XNSYNCH_OWNER));
lastownerh = xnthread_handle(xnpod_current_thread());
if (use_fastlock &&
likely(xnsynch_fast_release(xnsynch_fastlock(synch), lastownerh)))
return NULL;
xnlock_get_irqsave(&nklock, s);
trace_mark(xn_nucleus, synch_release, "synch %p", synch);
holder = getpq(&synch->pendq);
if (holder) {
newowner = link2thread(holder, plink);
newowner->wchan = NULL;
newowner->wwake = synch;
lastowner = synch->owner;
synch->owner = newowner;
xnthread_set_info(newowner, XNWAKEN);
xnpod_resume_thread(newowner, XNPEND);
if (testbits(synch->status, XNSYNCH_CLAIMED))
xnsynch_clear_boost(synch, lastowner);
newownerh = xnsynch_fast_set_claimed(xnthread_handle(newowner),
xnsynch_pended_p(synch));
} else {
newowner = NULL;
synch->owner = NULL;
newownerh = XN_NO_HANDLE;
}
if (use_fastlock) {
xnarch_atomic_t *lockp = xnsynch_fastlock(synch);
xnarch_atomic_set(lockp, newownerh);
}
xnlock_put_irqrestore(&nklock, s);
xnarch_post_graph_if(synch, 0, emptypq_p(&synch->pendq));
return newowner;
}
void xnsynch_init(struct xnsynch *synch, xnflags_t flags, xnarch_atomic_t *fastlock)
{
initph(&synch->link);
if (flags & XNSYNCH_PIP)
flags |= XNSYNCH_PRIO | XNSYNCH_OWNER; /* Obviously... */
synch->status = flags & ~XNSYNCH_CLAIMED;
synch->owner = NULL;
synch->cleanup = NULL; /* Only works for PIP-enabled objects. */
#ifdef CONFIG_XENO_FASTSYNCH
if ((flags & XNSYNCH_OWNER) && fastlock) {
synch->fastlock = fastlock;
xnarch_atomic_set(fastlock, XN_NO_HANDLE);
} else
synch->fastlock = NULL;
#endif /* CONFIG_XENO_FASTSYNCH */
initpq(&synch->pendq);
xnarch_init_display_context(synch);
}
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);
}
static __attribute__ ((constructor)) void __rt_print_init(void)
{
const char *value_str;
unsigned long long period;
first_buffer = NULL;
seq_no = 0;
auto_init = 0;
default_buffer_size = RT_PRINT_DEFAULT_BUFFER;
value_str = getenv(RT_PRINT_BUFFER_ENV);
if (value_str) {
errno = 0;
default_buffer_size = strtol(value_str, NULL, 10);
if (errno || default_buffer_size < RT_PRINT_LINE_BREAK) {
fprintf(stderr, "Invalid %s\n", RT_PRINT_BUFFER_ENV);
exit(1);
}
}
period = RT_PRINT_DEFAULT_PERIOD;
value_str = getenv(RT_PRINT_PERIOD_ENV);
if (value_str) {
errno = 0;
period = strtoll(value_str, NULL, 10);
if (errno) {
fprintf(stderr, "Invalid %s\n", RT_PRINT_PERIOD_ENV);
exit(1);
}
}
print_period.tv_sec = period / 1000;
print_period.tv_nsec = (period % 1000) * 1000000;
#ifdef CONFIG_XENO_FASTSYNCH
/* Fill the buffer pool */
{
unsigned buffers_count, i;
buffers_count = RT_PRINT_DEFAULT_BUFFERS_COUNT;
value_str = getenv(RT_PRINT_BUFFERS_COUNT_ENV);
if (value_str) {
errno = 0;
buffers_count = strtoul(value_str, NULL, 0);
if (errno) {
fprintf(stderr, "Invalid %s\n",
RT_PRINT_BUFFERS_COUNT_ENV);
exit(1);
}
}
pool_bitmap_len = (buffers_count + BITS_PER_LONG - 1)
/ BITS_PER_LONG;
if (!pool_bitmap_len)
goto done;
pool_bitmap = malloc(pool_bitmap_len * sizeof(*pool_bitmap));
if (!pool_bitmap) {
fprintf(stderr, "Error allocating rt_printf "
"buffers\n");
exit(1);
}
pool_buf_size = sizeof(struct print_buffer) + default_buffer_size;
pool_len = buffers_count * pool_buf_size;
pool_start = (unsigned long)malloc(pool_len);
if (!pool_start) {
fprintf(stderr, "Error allocating rt_printf "
"buffers\n");
exit(1);
}
for (i = 0; i < buffers_count / BITS_PER_LONG; i++)
xnarch_atomic_set(&pool_bitmap[i], ~0UL);
if (buffers_count % BITS_PER_LONG)
xnarch_atomic_set(&pool_bitmap[i],
(1UL << (buffers_count % BITS_PER_LONG)) - 1);
for (i = 0; i < buffers_count; i++) {
struct print_buffer *buffer =
(struct print_buffer *)
(pool_start + i * pool_buf_size);
buffer->ring = (char *)(buffer + 1);
rt_print_init_inner(buffer, default_buffer_size);
}
}
done:
#endif /* CONFIG_XENO_FASTSYNCH */
pthread_mutex_init(&buffer_lock, NULL);
pthread_key_create(&buffer_key, (void (*)(void*))cleanup_buffer);
pthread_cond_init(&printer_wakeup, NULL);
spawn_printer_thread();
pthread_atfork(NULL, NULL, forked_child_init);
}