static void test_queue_remove_one(void)
{
queue_t *root = &q;
queue_node_t *elem1 = &(qe[1]), *elem2 = &(qe[2]), *elem3 = &(qe[3]);
queue_priority_add(root, elem1);
queue_priority_add(root, elem2);
queue_priority_add(root, elem3);
queue_remove(root, elem2);
TEST_ASSERT(root->first == elem1);
TEST_ASSERT(root->first->next == elem3);
TEST_ASSERT_NULL(root->first->next->next);
}
int pthread_cond_wait(struct pthread_cond_t *cond, struct mutex_t *mutex)
{
queue_node_t n;
n.priority = sched_active_thread->priority;
n.data = sched_active_thread->pid;
n.next = NULL;
/* the signaling thread may not hold the mutex, the queue is not thread safe */
unsigned old_state = disableIRQ();
queue_priority_add(&(cond->queue), &n);
restoreIRQ(old_state);
mutex_unlock_and_sleep(mutex);
if (n.data != -1u) {
/* on signaling n.data is set to -1u */
/* if it isn't set, then the wakeup is either spurious or a timer wakeup */
old_state = disableIRQ();
queue_remove(&(cond->queue), &n);
restoreIRQ(old_state);
}
mutex_lock(mutex);
return 0;
}
void mutex_wait(struct mutex_t *mutex)
{
int irqstate = disableIRQ();
DEBUG("%s: Mutex in use. %u\n", active_thread->name, mutex->val);
if (mutex->val == 0) {
/* somebody released the mutex. return. */
mutex->val = thread_pid;
DEBUG("%s: mutex_wait early out. %u\n", active_thread->name, mutex->val);
restoreIRQ(irqstate);
return;
}
sched_set_status((tcb_t*) active_thread, STATUS_MUTEX_BLOCKED);
queue_node_t n;
n.priority = (unsigned int) active_thread->priority;
n.data = (unsigned int) active_thread;
n.next = NULL;
DEBUG("%s: Adding node to mutex queue: prio: %" PRIu32 "\n", active_thread->name, n.priority);
queue_priority_add(&(mutex->queue), &n);
restoreIRQ(irqstate);
thread_yield();
/* we were woken up by scheduler. waker removed us from queue. we have the mutex now. */
}
static int set_shortterm(vtimer_t *timer)
{
DEBUG("set_shortterm(): Absolute: %" PRIu32 " %" PRIu32 "\n", timer->absolute.seconds, timer->absolute.microseconds);
timer->queue_entry.priority = timer->absolute.microseconds;
queue_priority_add(&shortterm_queue_root, (queue_node_t *)timer);
return 1;
}
static void test_queue_priority_add_two_distinct(void)
{
queue_node_t *root = &(q[0]), *elem1 = &(q[1]), *elem2 = &(q[2]);
elem1->data = 46421;
elem1->priority = 4567;
elem2->data = 43088;
elem2->priority = 1234;
queue_priority_add(root, elem1);
queue_priority_add(root, elem2);
TEST_ASSERT(root->next == elem2);
TEST_ASSERT_EQUAL_INT(43088, root->next->data);
TEST_ASSERT_EQUAL_INT(1234, root->next->priority);
TEST_ASSERT(root->next->next == elem1);
TEST_ASSERT_EQUAL_INT(46421, root->next->next->data);
TEST_ASSERT_EQUAL_INT(4567, root->next->next->priority);
TEST_ASSERT_NULL(root->next->next->next);
}
static void test_queue_priority_add_two_equal(void)
{
queue_node_t *root = &(q[0]), *elem1 = &(q[1]), *elem2 = &(q[2]);
elem1->data = 27088;
elem1->priority = 14202;
elem2->data = 4356;
elem2->priority = 14202;
queue_priority_add(root, elem1);
queue_priority_add(root, elem2);
TEST_ASSERT(root->next == elem1);
TEST_ASSERT_EQUAL_INT(27088, root->next->data);
TEST_ASSERT_EQUAL_INT(14202, root->next->priority);
TEST_ASSERT(root->next->next == elem2);
TEST_ASSERT_EQUAL_INT(4356, root->next->next->data);
TEST_ASSERT_EQUAL_INT(14202, root->next->next->priority);
TEST_ASSERT_NULL(root->next->next->next);
}
static void test_queue_priority_add_one(void)
{
queue_node_t *root = &(q[0]), *elem = &(q[1]);
elem->data = 7317;
elem->priority = 713643658;
queue_priority_add(root, elem);
TEST_ASSERT(root->next == elem);
TEST_ASSERT_EQUAL_INT(7317, root->next->data);
TEST_ASSERT_EQUAL_INT(713643658, root->next->priority);
TEST_ASSERT_NULL(root->next->next);
}
static void test_queue_remove_head_one(void)
{
queue_t *root = &q;
queue_node_t *elem = &(qe[1]), *res;
elem->data = 62801;
queue_priority_add(root, elem);
res = queue_remove_head(root);
TEST_ASSERT(res == elem);
TEST_ASSERT_EQUAL_INT(62801, res->data);
res = queue_remove_head(root);
TEST_ASSERT_NULL(res);
}
static void sem_thread_blocked(sem_t *sem)
{
/* I'm going blocked */
sched_set_status((tcb_t*) active_thread, STATUS_MUTEX_BLOCKED);
queue_node_t n;
n.priority = (uint32_t) active_thread->priority;
n.data = (size_t) active_thread;
n.next = NULL;
DEBUG("%s: Adding node to mutex queue: prio: %" PRIu32 "\n",
active_thread->name, n.priority);
/* add myself to the waiters queue */
queue_priority_add(&sem->queue, &n);
/* scheduler should schedule an other thread, that unlocks the
* mutex in the future, when this happens I get scheduled again
*/
thread_yield();
}
int msg_send(msg_t *m, unsigned int target_pid, bool block)
{
if (inISR()) {
return msg_send_int(m, target_pid);
}
if ((unsigned int)sched_active_pid == target_pid) {
return msg_send_to_self(m);
}
dINT();
tcb_t *target = (tcb_t*) sched_threads[target_pid];
m->sender_pid = sched_active_pid;
if (target == NULL) {
DEBUG("msg_send(): target thread does not exist\n");
eINT();
return -1;
}
DEBUG("msg_send() %s:%i: Sending from %i to %i. block=%i src->state=%i target->state=%i\n", __FILE__, __LINE__, sched_active_pid, target_pid, block, sched_active_thread->status, target->status);
if (target->status != STATUS_RECEIVE_BLOCKED) {
DEBUG("msg_send() %s:%i: Target %i is not RECEIVE_BLOCKED.\n", __FILE__, __LINE__, target_pid);
if (target->msg_array && queue_msg(target, m)) {
DEBUG("msg_send() %s:%i: Target %i has a msg_queue. Queueing message.\n", __FILE__, __LINE__, target_pid);
eINT();
if (sched_active_thread->status == STATUS_REPLY_BLOCKED) {
thread_yield();
}
return 1;
}
if (!block) {
DEBUG("msg_send: %s: Receiver not waiting, block=%u\n", sched_active_thread->name, block);
eINT();
return 0;
}
DEBUG("msg_send: %s: send_blocked.\n", sched_active_thread->name);
queue_node_t n;
n.priority = sched_active_thread->priority;
n.data = (unsigned int) sched_active_thread;
n.next = NULL;
DEBUG("msg_send: %s: Adding node to msg_waiters:\n", sched_active_thread->name);
queue_priority_add(&(target->msg_waiters), &n);
sched_active_thread->wait_data = (void*) m;
int newstatus;
if (sched_active_thread->status == STATUS_REPLY_BLOCKED) {
newstatus = STATUS_REPLY_BLOCKED;
}
else {
newstatus = STATUS_SEND_BLOCKED;
}
sched_set_status((tcb_t*) sched_active_thread, newstatus);
DEBUG("msg_send: %s: Back from send block.\n", sched_active_thread->name);
}
else {
DEBUG("msg_send: %s: Direct msg copy from %i to %i.\n", sched_active_thread->name, thread_getpid(), target_pid);
/* copy msg to target */
msg_t *target_message = (msg_t*) target->wait_data;
*target_message = *m;
sched_set_status(target, STATUS_PENDING);
}
eINT();
thread_yield();
return 1;
}
int msg_send(msg_t *m, unsigned int target_pid, bool block)
{
if(inISR()) {
return msg_send_int(m, target_pid);
}
tcb_t *target = (tcb_t*) sched_threads[target_pid];
m->sender_pid = thread_pid;
if(m->sender_pid == target_pid) {
return -1;
}
if(target == NULL) {
return -1;
}
dINT();
if(target->status != STATUS_RECEIVE_BLOCKED) {
if(target->msg_array && queue_msg(target, m)) {
eINT();
return 1;
}
if(!block) {
DEBUG("%s: receiver not waiting. block=%u\n", active_thread->name, block);
eINT();
return 0;
}
DEBUG("%s: send_blocked.\n", active_thread->name);
queue_node_t n;
n.priority = active_thread->priority;
n.data = (unsigned int) active_thread;
DEBUG("%s: Adding node to msg_waiters:\n", active_thread->name);
queue_priority_add(&(target->msg_waiters), &n);
active_thread->wait_data = (void*) m;
int newstatus;
if(active_thread->status == STATUS_REPLY_BLOCKED) {
newstatus = STATUS_REPLY_BLOCKED;
}
else {
newstatus = STATUS_SEND_BLOCKED;
}
sched_set_status((tcb_t*) active_thread, newstatus);
DEBUG("%s: back from send block.\n", active_thread->name);
}
else {
DEBUG("%s: direct msg copy.\n", active_thread->name);
/* copy msg to target */
msg_t *target_message = (msg_t*) target->wait_data;
*target_message = *m;
sched_set_status(target, STATUS_PENDING);
}
eINT();
thread_yield();
return 1;
}
static int set_longterm(vtimer_t *timer)
{
timer->queue_entry.priority = timer->absolute.seconds;
queue_priority_add(&longterm_queue_root, (queue_node_t *)timer);
return 0;
}
