int pthread_cond_broadcast(struct pthread_cond_t *cond)
{
unsigned old_state = disableIRQ();
int other_prio = -1;
while (1) {
priority_queue_node_t *head = priority_queue_remove_head(&(cond->queue));
if (head == NULL) {
break;
}
tcb_t *other_thread = (tcb_t *) sched_threads[head->data];
if (other_thread) {
other_prio = max_prio(other_prio, other_thread->priority);
sched_set_status(other_thread, STATUS_PENDING);
}
head->data = -1u;
}
restoreIRQ(old_state);
if (other_prio >= 0) {
sched_switch(other_prio);
}
return 0;
}
void mutex_unlock(struct mutex_t *mutex)
{
unsigned irqstate = disableIRQ();
DEBUG("mutex_unlock(): val: %u pid: %" PRIkernel_pid "\n", ATOMIC_VALUE(mutex->val), sched_active_pid);
if (ATOMIC_VALUE(mutex->val) == 0) {
/* the mutex was not locked */
restoreIRQ(irqstate);
return;
}
priority_queue_node_t *next = priority_queue_remove_head(&(mutex->queue));
if (!next) {
/* the mutex was locked and no thread was waiting for it */
ATOMIC_VALUE(mutex->val) = 0;
restoreIRQ(irqstate);
return;
}
thread_t *process = (thread_t *) next->data;
DEBUG("mutex_unlock: waking up waiting thread %" PRIkernel_pid "\n", process->pid);
sched_set_status(process, STATUS_PENDING);
uint16_t process_priority = process->priority;
restoreIRQ(irqstate);
sched_switch(process_priority);
}
void vtimer_callback(void *ptr)
{
DEBUG("vtimer_callback ptr=%p\n", ptr);
(void) ptr;
in_callback = true;
hwtimer_id = -1;
/* get the vtimer that fired */
vtimer_t *timer = node_get_timer(priority_queue_remove_head(&shortterm_priority_queue_root));
if (timer) {
#if ENABLE_DEBUG
vtimer_print(timer);
#endif
DEBUG("vtimer_callback(): Shooting %" PRIu32 ".\n", timer->absolute.microseconds);
/* shoot timer */
timer->action(timer);
}
else {
DEBUG("vtimer_callback(): spurious call.\n");
}
in_callback = false;
update_shortterm();
}
gnrc_pktsnip_t* gnrc_priority_pktqueue_pop(gnrc_priority_pktqueue_t* queue)
{
if(!queue || (gnrc_priority_pktqueue_length(queue) == 0)){
return NULL;
}
priority_queue_node_t *head = priority_queue_remove_head(queue);
gnrc_pktsnip_t* pkt = (gnrc_pktsnip_t*) head->data;
_free_node((gnrc_priority_pktqueue_node_t *)head);
return pkt;
}
void gnrc_priority_pktqueue_flush(gnrc_priority_pktqueue_t* queue)
{
assert(queue != NULL);
if(gnrc_priority_pktqueue_length(queue) == 0){
return;
}
gnrc_priority_pktqueue_node_t* node;
while( (node = (gnrc_priority_pktqueue_node_t *)priority_queue_remove_head(queue)) )
{
gnrc_pktbuf_release(node->pkt);
_free_node(node);
}
}
void condition_variable::notify_one() noexcept {
unsigned old_state = disableIRQ();
priority_queue_node_t* head = priority_queue_remove_head(&m_queue);
int other_prio = -1;
if (head != NULL) {
tcb_t* other_thread = (tcb_t*)sched_threads[head->data];
if (other_thread) {
other_prio = other_thread->priority;
sched_set_status(other_thread, STATUS_PENDING);
}
head->data = -1u;
}
restoreIRQ(old_state);
if (other_prio >= 0) {
sched_switch(other_prio);
}
}
void mutex_unlock_and_sleep(struct mutex_t *mutex)
{
DEBUG("%s: unlocking mutex. val: %u pid: %" PRIkernel_pid ", and taking a nap\n", sched_active_thread->name, ATOMIC_VALUE(mutex->val), sched_active_pid);
unsigned irqstate = disableIRQ();
if (ATOMIC_VALUE(mutex->val) != 0) {
priority_queue_node_t *next = priority_queue_remove_head(&(mutex->queue));
if (next) {
thread_t *process = (thread_t *) next->data;
DEBUG("%s: waking up waiter.\n", process->name);
sched_set_status(process, STATUS_PENDING);
}
else {
ATOMIC_VALUE(mutex->val) = 0; /* This is safe, interrupts are disabled */
}
}
DEBUG("%s: going to sleep.\n", sched_active_thread->name);
sched_set_status((thread_t*) sched_active_thread, STATUS_SLEEPING);
restoreIRQ(irqstate);
thread_yield_higher();
}
void mutex_unlock(struct mutex_t *mutex)
{
DEBUG("%s: unlocking mutex. val: %u pid: %" PRIkernel_pid "\n", sched_active_thread->name, ATOMIC_VALUE(mutex->val), sched_active_pid);
unsigned irqstate = disableIRQ();
if (ATOMIC_VALUE(mutex->val) != 0) {
priority_queue_node_t *next = priority_queue_remove_head(&(mutex->queue));
if (next) {
tcb_t *process = (tcb_t *) next->data;
DEBUG("%s: waking up waiter.\n", process->name);
sched_set_status(process, STATUS_PENDING);
sched_switch(process->priority);
}
else {
ATOMIC_VALUE(mutex->val) = 0; /* This is safe, interrupts are disabled */
}
}
restoreIRQ(irqstate);
}
void condition_variable::notify_all() noexcept {
unsigned old_state = disableIRQ();
int other_prio = -1;
while (true) {
priority_queue_node_t* head = priority_queue_remove_head(&m_queue);
if (head == NULL) {
break;
}
tcb_t* other_thread = (tcb_t*)sched_threads[head->data];
if (other_thread) {
auto max_prio
= [](int a, int b) { return (a < 0) ? b : ((a < b) ? a : b); };
other_prio = max_prio(other_prio, other_thread->priority);
sched_set_status(other_thread, STATUS_PENDING);
}
head->data = -1u;
}
restoreIRQ(old_state);
if (other_prio >= 0) {
sched_switch(other_prio);
}
}
void vtimer_callback_tick(vtimer_t *timer)
{
(void) timer;
DEBUG("vtimer_callback_tick().\n");
longterm_tick_start += MICROSECONDS_PER_TICK;
longterm_tick_timer.absolute.seconds += SECONDS_PER_TICK;
longterm_tick_timer.absolute.microseconds = MICROSECONDS_PER_TICK; // Should never change, just for clarity.
set_shortterm(&longterm_tick_timer);
while (longterm_priority_queue_root.first) {
vtimer_t *timer = node_get_timer(longterm_priority_queue_root.first);
if (timer->absolute.seconds == longterm_tick_timer.absolute.seconds) {
priority_queue_remove_head(&longterm_priority_queue_root);
set_shortterm(timer);
}
else {
break;
}
}
}
int pthread_cond_signal(struct pthread_cond_t *cond)
{
unsigned old_state = disableIRQ();
priority_queue_node_t *head = priority_queue_remove_head(&(cond->queue));
int other_prio = -1;
if (head != NULL) {
tcb_t *other_thread = (tcb_t *) sched_threads[head->data];
if (other_thread) {
other_prio = other_thread->priority;
sched_set_status(other_thread, STATUS_PENDING);
}
head->data = -1u;
}
restoreIRQ(old_state);
if (other_prio >= 0) {
sched_switch(other_prio);
}
return 0;
}
int pthread_rwlock_unlock(pthread_rwlock_t *rwlock)
{
if (rwlock == NULL) {
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): rwlock=NULL supplied\n", thread_getpid(), "unlock");
return EINVAL;
}
mutex_lock(&rwlock->mutex);
if (rwlock->readers == 0) {
/* the lock is open */
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): lock is open\n", thread_getpid(), "unlock");
mutex_unlock(&rwlock->mutex);
return EPERM;
}
if (rwlock->readers > 0) {
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): release %s lock\n", thread_getpid(), "unlock", "read");
--rwlock->readers;
}
else {
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): release %s lock\n", thread_getpid(), "unlock", "write");
rwlock->readers = 0;
}
if (rwlock->readers != 0 || rwlock->queue.first == NULL) {
/* this thread was not the last reader, or no one is waiting to aquire the lock */
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): no one is waiting\n", thread_getpid(), "unlock");
mutex_unlock(&rwlock->mutex);
return 0;
}
/* wake up the next thread */
priority_queue_node_t *qnode = priority_queue_remove_head(&rwlock->queue);
__pthread_rwlock_waiter_node_t *waiting_node = (__pthread_rwlock_waiter_node_t *) qnode->data;
waiting_node->continue_ = true;
uint16_t prio = qnode->priority;
sched_set_status(waiting_node->thread, STATUS_PENDING);
if (waiting_node->is_writer) {
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continue %s %" PRIkernel_pid "\n",
thread_getpid(), "unlock", "writer", waiting_node->thread->pid);
--rwlock->readers;
}
else {
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continue %s %" PRIkernel_pid "\n",
thread_getpid(), "unlock", "reader", waiting_node->thread->pid);
++rwlock->readers;
/* wake up further readers */
while (rwlock->queue.first) {
waiting_node = (__pthread_rwlock_waiter_node_t *) rwlock->queue.first->data;
if (waiting_node->is_writer) {
/* Not to be unfair to writers, we don't try to wake up readers that came after the first writer. */
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continuing readers blocked by writer %" PRIkernel_pid "\n",
thread_getpid(), "unlock", waiting_node->thread->pid);
break;
}
waiting_node->continue_ = true;
DEBUG("Thread %" PRIkernel_pid ": pthread_rwlock_%s(): continue %s %" PRIkernel_pid "\n",
thread_getpid(), "unlock", "reader", waiting_node->thread->pid);
/* wake up this reader */
qnode = priority_queue_remove_head(&rwlock->queue);
if (qnode->priority < prio) {
prio = qnode->priority;
}
sched_set_status(waiting_node->thread, STATUS_PENDING);
++rwlock->readers;
}
}
mutex_unlock(&rwlock->mutex);
/* yield if a woken up thread had a higher priority */
sched_switch(prio);
return 0;
}
static int _msg_receive(msg_t *m, int block)
{
unsigned state = disableIRQ();
DEBUG("_msg_receive: %" PRIkernel_pid ": _msg_receive.\n",
sched_active_thread->pid);
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) && ((!me->msg_waiters.first) && (queue_index == -1))) {
restoreIRQ(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;
}
priority_queue_node_t *node = priority_queue_remove_head(&(me->msg_waiters));
if (node == 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);
restoreIRQ(state);
thread_yield_higher();
/* sender copied message */
}
else {
restoreIRQ(state);
}
return 1;
}
else {
DEBUG("_msg_receive: %" PRIkernel_pid ": _msg_receive(): Waking up waiting thread.\n",
sched_active_thread->pid);
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 */
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;
}
restoreIRQ(state);
if (sender_prio < THREAD_PRIORITY_IDLE) {
sched_switch(sender_prio);
}
return 1;
}
DEBUG("This should have never been reached!\n");
}
