static void cleanup_buffer(struct print_buffer *buffer)
{
struct print_buffer *prev, *next;
assert_nrt();
pthread_setspecific(buffer_key, NULL);
pthread_mutex_lock(&buffer_lock);
print_buffers();
pthread_mutex_unlock(&buffer_lock);
#ifdef CONFIG_XENO_FASTSYNCH
/* Return the buffer to the pool */
{
unsigned long old_bitmap, bitmap;
unsigned i, j;
if ((unsigned long)buffer - pool_start >= pool_len)
goto dofree;
j = ((unsigned long)buffer - pool_start) / pool_buf_size;
i = j / BITS_PER_LONG;
j = j % BITS_PER_LONG;
old_bitmap = xnarch_atomic_get(&pool_bitmap[i]);
do {
bitmap = old_bitmap;
old_bitmap = xnarch_atomic_cmpxchg(&pool_bitmap[i],
bitmap,
bitmap | (1UL << j));
} while (old_bitmap != bitmap);
return;
}
dofree:
#endif /* CONFIG_XENO_FASTSYNCH */
pthread_mutex_lock(&buffer_lock);
prev = buffer->prev;
next = buffer->next;
if (prev)
prev->next = next;
else
first_buffer = next;
if (next)
next->prev = prev;
buffers--;
pthread_mutex_unlock(&buffer_lock);
free(buffer->ring);
free(buffer);
}
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);
}
int rt_print_init(size_t buffer_size, const char *buffer_name)
{
struct print_buffer *buffer = pthread_getspecific(buffer_key);
size_t size = buffer_size;
unsigned long old_bitmap;
unsigned j;
if (!size)
size = default_buffer_size;
else if (size < RT_PRINT_LINE_BREAK)
return EINVAL;
if (buffer) {
/* Only set name if buffer size is unchanged or default */
if (size == buffer->size || !buffer_size) {
set_buffer_name(buffer, buffer_name);
return 0;
}
cleanup_buffer(buffer);
buffer = NULL;
}
#ifdef CONFIG_XENO_FASTSYNCH
/* Find a free buffer in the pool */
do {
unsigned long bitmap;
unsigned i;
for (i = 0; i < pool_bitmap_len; i++) {
old_bitmap = xnarch_atomic_get(&pool_bitmap[i]);
if (old_bitmap)
goto acquire;
}
goto not_found;
acquire:
do {
bitmap = old_bitmap;
j = __builtin_ffsl(bitmap) - 1;
old_bitmap = xnarch_atomic_cmpxchg(&pool_bitmap[i],
bitmap,
bitmap & ~(1UL << j));
} while (old_bitmap != bitmap && old_bitmap);
j += i * BITS_PER_LONG;
} while (!old_bitmap);
buffer = (struct print_buffer *)(pool_start + j * pool_buf_size);
not_found:
#endif /* CONFIG_XENO_FASTSYNCH */
if (!buffer) {
assert_nrt();
buffer = malloc(sizeof(*buffer));
if (!buffer)
return ENOMEM;
buffer->ring = malloc(size);
if (!buffer->ring) {
free(buffer);
return ENOMEM;
}
rt_print_init_inner(buffer, size);
}
set_buffer_name(buffer, buffer_name);
pthread_setspecific(buffer_key, buffer);
return 0;
}
