static void test_cib_put(void)
{
TEST_ASSERT_EQUAL_INT(0, cib_put(&cib));
TEST_ASSERT_EQUAL_INT(1, cib_put(&cib));
TEST_ASSERT_EQUAL_INT(-1, cib_put(&cib));
}
static int queue_msg(tcb_t *target, msg_t *m)
{
int n = cib_put(&(target->msg_queue));
if (n != -1) {
target->msg_array[n] = *m;
return 1;
}
return 0;
}
static int queue_msg(thread_t *target, const msg_t *m)
{
int n = cib_put(&(target->msg_queue));
if (n < 0) {
DEBUG("queue_msg(): message queue is full (or there is none)\n");
return 0;
}
DEBUG("queue_msg(): queuing message\n");
msg_t *dest = &target->msg_array[n];
*dest = *m;
return 1;
}
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 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);
}
static int _msg_receive(msg_t *m, int block)
{
dINT();
DEBUG("_msg_receive: %s: _msg_receive.\n", sched_active_thread->name);
tcb_t *me = (tcb_t*) sched_threads[sched_active_pid];
int queue_index = -1;
if (me->msg_array) {
queue_index = cib_get(&(me->msg_queue));
}
/* no message, fail */
if ((!block) && (queue_index == -1)) {
eINT();
return -1;
}
if (queue_index >= 0) {
DEBUG("_msg_receive: %s: _msg_receive(): We've got a queued message.\n", sched_active_thread->name);
*m = me->msg_array[queue_index];
}
else {
me->wait_data = (void *) m;
}
queue_node_t *node = queue_remove_head(&(me->msg_waiters));
if (node == NULL) {
DEBUG("_msg_receive: %s: _msg_receive(): No thread in waiting list.\n", sched_active_thread->name);
if (queue_index < 0) {
DEBUG("_msg_receive(): %s: No msg in queue. Going blocked.\n", sched_active_thread->name);
sched_set_status(me, STATUS_RECEIVE_BLOCKED);
eINT();
thread_yield();
/* sender copied message */
}
else {
eINT();
}
return 1;
}
else {
DEBUG("_msg_receive: %s: _msg_receive(): Waking up waiting thread.\n", sched_active_thread->name);
tcb_t *sender = (tcb_t*) node->data;
if (queue_index >= 0) {
/* We've already got a message from the queue. As there is a
* waiter, take it's message into the just freed queue space.
*/
m = &(me->msg_array[cib_put(&(me->msg_queue))]);
}
/* copy msg */
msg_t *sender_msg = (msg_t*) sender->wait_data;
*m = *sender_msg;
/* remove sender from queue */
if (sender->status != STATUS_REPLY_BLOCKED) {
sender->wait_data = NULL;
sched_set_status(sender, STATUS_PENDING);
}
eINT();
return 1;
}
DEBUG("This should have never been reached!\n");
}
static int _msg_receive(msg_t *m, int block)
{
unsigned state = irq_disable();
DEBUG("_msg_receive: %" PRIkernel_pid ": _msg_receive.\n",
sched_active_thread->pid);
thread_t *me = (thread_t*) sched_threads[sched_active_pid];
int queue_index = -1;
if (me->msg_array) {
queue_index = cib_get(&(me->msg_queue));
}
/* no message, fail */
if ((!block) && ((!me->msg_waiters.next) && (queue_index == -1))) {
irq_restore(state);
return -1;
}
if (queue_index >= 0) {
DEBUG("_msg_receive: %" PRIkernel_pid ": _msg_receive(): We've got a queued message.\n",
sched_active_thread->pid);
*m = me->msg_array[queue_index];
}
else {
me->wait_data = (void *) m;
}
list_node_t *next = list_remove_head(&me->msg_waiters);
if (next == NULL) {
DEBUG("_msg_receive: %" PRIkernel_pid ": _msg_receive(): No thread in waiting list.\n",
sched_active_thread->pid);
if (queue_index < 0) {
DEBUG("_msg_receive(): %" PRIkernel_pid ": No msg in queue. Going blocked.\n",
sched_active_thread->pid);
sched_set_status(me, STATUS_RECEIVE_BLOCKED);
irq_restore(state);
thread_yield_higher();
/* sender copied message */
}
else {
irq_restore(state);
}
return 1;
}
else {
DEBUG("_msg_receive: %" PRIkernel_pid ": _msg_receive(): Waking up waiting thread.\n",
sched_active_thread->pid);
thread_t *sender = container_of((clist_node_t*)next, thread_t, rq_entry);
if (queue_index >= 0) {
/* We've already got a message from the queue. As there is a
* waiter, take it's message into the just freed queue space.
*/
m = &(me->msg_array[cib_put(&(me->msg_queue))]);
}
/* copy msg */
msg_t *sender_msg = (msg_t*) sender->wait_data;
*m = *sender_msg;
/* remove sender from queue */
uint16_t sender_prio = THREAD_PRIORITY_IDLE;
if (sender->status != STATUS_REPLY_BLOCKED) {
sender->wait_data = NULL;
sched_set_status(sender, STATUS_PENDING);
sender_prio = sender->priority;
}
irq_restore(state);
if (sender_prio < THREAD_PRIORITY_IDLE) {
sched_switch(sender_prio);
}
return 1;
}
DEBUG("This should have never been reached!\n");
}
int msg_receive(msg_t *m)
{
dINT();
DEBUG("%s: msg_receive.\n", active_thread->name);
tcb_t *me = (tcb_t*) sched_threads[thread_pid];
int n = -1;
if(me->msg_array) {
n = cib_get(&(me->msg_queue));
}
if(n >= 0) {
DEBUG("%s: msg_receive(): We've got a queued message.\n", active_thread->name);
*m = me->msg_array[n];
}
else {
me->wait_data = (void *) m;
}
queue_node_t *node = queue_remove_head(&(me->msg_waiters));
if(node == NULL) {
DEBUG("%s: msg_receive(): No thread in waiting list.\n", active_thread->name);
if(n < 0) {
DEBUG("%s: msg_receive(): No msg in queue. Going blocked.\n", active_thread->name);
sched_set_status(me, STATUS_RECEIVE_BLOCKED);
eINT();
thread_yield();
/* sender copied message */
}
return 1;
}
else {
DEBUG("%s: msg_receive(): Wakeing up waiting thread.\n", active_thread->name);
tcb_t *sender = (tcb_t*) node->data;
if(n >= 0) {
/* we've already got a messgage from the queue. as there is a
* waiter, take it's message into the just freed queue space.
*/
m = &(me->msg_array[cib_put(&(me->msg_queue))]);
}
/* copy msg */
msg_t *sender_msg = (msg_t*) sender->wait_data;
*m = *sender_msg;
/* remove sender from queue */
sender->wait_data = NULL;
sched_set_status(sender, STATUS_PENDING);
eINT();
return 1;
}
}
