static void test_cib_avail(void)
{
TEST_ASSERT_EQUAL_INT(0, cib_avail(&cib));
TEST_ASSERT_EQUAL_INT(0, cib_put(&cib));
TEST_ASSERT_EQUAL_INT(1, cib_avail(&cib));
TEST_ASSERT_EQUAL_INT(1, cib_put(&cib));
TEST_ASSERT_EQUAL_INT(2, cib_avail(&cib));
}
u32_t sys_arch_mbox_fetch(sys_mbox_t *mbox, void **msg, u32_t timeout)
{
u32_t time_needed = 0;
unsigned int idx;
LWIP_ASSERT("invalid mbox", sys_mbox_valid(mbox));
mutex_lock(&mbox->mutex);
while (cib_avail(&mbox->cib) == 0) {
sys_sem_t *not_empty = (sys_sem_t *)(&mbox->not_empty);
mutex_unlock(&mbox->mutex);
if (timeout != 0) {
time_needed = sys_arch_sem_wait(not_empty, timeout);
if (time_needed == SYS_ARCH_TIMEOUT) {
return SYS_ARCH_TIMEOUT;
}
}
else {
sys_arch_sem_wait(not_empty, 0);
}
mutex_lock(&mbox->mutex);
}
idx = cib_get(&mbox->cib);
if (msg != NULL) {
*msg = mbox->msgs[idx];
}
if (mbox->waiting) {
sema_post(&mbox->not_full);
}
mutex_unlock(&mbox->mutex);
return time_needed;
}
int msg_avail(void)
{
DEBUG("msg_available: %" PRIkernel_pid ": msg_available.\n",
sched_active_thread->pid);
thread_t *me = (thread_t*) sched_active_thread;
int queue_index = -1;
if (me->msg_array) {
queue_index = cib_avail(&(me->msg_queue));
}
return queue_index;
}
u32_t sys_arch_mbox_tryfetch(sys_mbox_t *mbox, void **msg)
{
int idx;
LWIP_ASSERT("invalid mbox", sys_mbox_valid(mbox));
mutex_lock(&mbox->mutex);
if (cib_avail(&mbox->cib) == 0) {
mutex_unlock(&mbox->mutex);
return SYS_MBOX_EMPTY;
}
idx = cib_get(&mbox->cib);
if (msg != NULL) {
*msg = mbox->msgs[idx];
}
mutex_unlock(&mbox->mutex);
return 0;
}
err_t sys_mbox_trypost(sys_mbox_t *mbox, void *msg)
{
int idx;
LWIP_ASSERT("invalid mbox", sys_mbox_valid(mbox));
mutex_lock(&mbox->mutex);
if ((idx = cib_put(&mbox->cib)) < 0) {
mutex_unlock(&mbox->mutex);
return ERR_MEM;
}
mbox->msgs[idx] = msg;
if (cib_avail(&mbox->cib) == 1) {
sema_post(&mbox->not_empty);
}
mutex_unlock(&mbox->mutex);
return ERR_OK;
}
void thread_print_msg_queue(void)
{
volatile tcb_t *tcb = sched_active_thread;
volatile cib_t *msg_queue = &tcb->msg_queue;
msg_t *msg_array = tcb->msg_array;
unsigned int i = msg_queue->read_count & msg_queue->mask;
printf("Message queue of thread %" PRIkernel_pid "\n", tcb->pid);
printf(" size: %u (avail: %d)\n", msg_queue->mask + 1,
cib_avail((cib_t *)msg_queue));
for (; i != (msg_queue->write_count & msg_queue->mask);
i = (i + 1) & msg_queue->mask) {
msg_t *m = &msg_array[i];
printf(" * %u: sender: %" PRIkernel_pid ", type: 0x%04" PRIu16
", content: %" PRIu32 " (%p)\n", i, m->sender_pid, m->type,
m->content.value, (void *)m->content.ptr);
}
}
void sys_mbox_post(sys_mbox_t *mbox, void *msg)
{
int idx;
LWIP_ASSERT("invalid mbox", sys_mbox_valid(mbox));
mutex_lock(&mbox->mutex);
while ((idx = cib_put(&mbox->cib)) < 0) {
mbox->waiting++;
mutex_unlock(&mbox->mutex);
sema_wait_timed(&mbox->not_full, 0);
mutex_lock(&mbox->mutex);
mbox->waiting--;
}
mbox->msgs[idx] = msg;
if (cib_avail(&mbox->cib) == 1) {
sema_post(&mbox->not_empty);
}
mutex_unlock(&mbox->mutex);
}
void msg_queue_print(void)
{
unsigned state = irq_disable();
thread_t *thread =(thread_t *)sched_active_thread;
cib_t *msg_queue = &thread->msg_queue;
msg_t *msg_array = thread->msg_array;
unsigned int i = msg_queue->read_count & msg_queue->mask;
printf("Message queue of thread %" PRIkernel_pid "\n", thread->pid);
printf(" size: %u (avail: %d)\n", msg_queue->mask + 1,
cib_avail((cib_t *)msg_queue));
for (; i != (msg_queue->write_count & msg_queue->mask);
i = (i + 1) & msg_queue->mask) {
msg_t *m = &msg_array[i];
printf(" * %u: sender: %" PRIkernel_pid ", type: 0x%04" PRIu16
", content: %" PRIu32 " (%p)\n", i, m->sender_pid, m->type,
m->content.value, m->content.ptr);
}
irq_restore(state);
}
