/* WARNING: this function NEVER returns! */
NORETURN void core_panic(int crash_code, const char *message)
{
/* copy panic datas to "public" global variables */
panic_code = crash_code;
strncpy(panic_str, message, 80);
/* print panic message to console (if possible) */
if (crashed == 0) {
crashed = 1;
puts("******** SYSTEM FAILURE ********\n");
puts(message);
#if DEVELHELP
puts("******** RIOT HALTS HERE ********\n");
#else
puts("******** RIOT WILL REBOOT ********\n");
#endif
puts("\n\n");
}
/* disable watchdog and all possible sources of interrupts */
//TODO
dINT();
#if DEVELHELP
/* enter infinite loop, into deepest possible sleep mode */
while (1) {
lpm_set(LPM_OFF);
}
#else
/* DEVELHELP not set => reboot system */
/* use dummy mode until used */
reboot(1);
#endif
}
/* WARNING: this function NEVER returns! */
NORETURN void core_panic(int crash_code, const char *message)
{
/* copy panic datas to "public" global variables */
panic_code = crash_code;
strncpy(panic_str, message, 80);
/* (try to) print panic message to console */
if (crashed == 0) {
crashed = 1;
puts("******** SYSTEM FAILURE ********\n");
puts(message);
#if DEVELHELP
puts("******** RIOT HALTS HERE ********\n");
#else
puts("******** RIOT WILL REBOOT ********\n");
#endif
puts("\n\n");
}
/* disable watchdog and all possible sources of interrupts */
WDTCTL = WDTPW | WDTHOLD;
dINT();
#if DEVELHELP
/* enter infinite loop, into deepest possible sleep mode */
while (1) {
lpm_set(LPM_OFF);
}
#else
/* DEVELHELP not set => reboot system */
(void) reboot(RB_AUTOBOOT);
#endif
/* tell the compiler that we won't return from this function
(even if we actually won't even get here...) */
UNREACHABLE();
}
void cpu_switch_context_exit(void)
{
#ifdef NATIVE_AUTO_EXIT
if (sched_num_threads <= 1) {
DEBUG("cpu_switch_context_exit(): last task has ended. exiting.\n");
exit(EXIT_SUCCESS);
}
#endif
if (_native_in_isr == 0) {
dINT();
_native_in_isr = 1;
native_isr_context.uc_stack.ss_sp = __isr_stack;
native_isr_context.uc_stack.ss_size = SIGSTKSZ;
native_isr_context.uc_stack.ss_flags = 0;
makecontext(&native_isr_context, isr_cpu_switch_context_exit, 0);
if (setcontext(&native_isr_context) == -1) {
err(EXIT_FAILURE, "cpu_switch_context_exit: swapcontext");
}
errx(EXIT_FAILURE, "1 this should have never been reached!!");
}
else {
isr_cpu_switch_context_exit();
}
errx(EXIT_FAILURE, "3 this should have never been reached!!");
}
void _native_syscall_leave(void)
{
#if LOCAL_DEBUG
real_write(STDERR_FILENO, "< _native_in_syscall\n", 21);
#endif
_native_in_syscall--;
if (
(_native_sigpend > 0)
&& (_native_in_isr == 0)
&& (_native_in_syscall == 0)
&& (native_interrupts_enabled == 1)
&& (sched_active_thread != NULL)
)
{
_native_in_isr = 1;
dINT();
_native_cur_ctx = (ucontext_t *)sched_active_thread->sp;
native_isr_context.uc_stack.ss_sp = __isr_stack;
native_isr_context.uc_stack.ss_size = SIGSTKSZ;
native_isr_context.uc_stack.ss_flags = 0;
makecontext(&native_isr_context, native_irq_handler, 0);
if (swapcontext(_native_cur_ctx, &native_isr_context) == -1) {
err(EXIT_FAILURE, "_native_syscall_leave: swapcontext");
}
eINT();
}
}
unsigned int atomic_set_return(unsigned int* p, unsigned int uiVal) {
//unsigned int cspr = disableIRQ(); //crashes
dINT();
unsigned int uiOldVal = *p;
*p = uiVal;
//restoreIRQ(cspr); //crashes
eINT();
return uiOldVal;
}
void thread_sleep(void)
{
if (inISR()) {
return;
}
dINT();
sched_set_status((tcb_t *)sched_active_thread, STATUS_SLEEPING);
eINT();
thread_yield();
}
void thread_yield()
{
__save_context();
dINT();
/* have active_thread point to the next thread */
sched_run();
eINT();
__restore_context();
}
int msg_send_receive(msg_t *m, msg_t *reply, unsigned int target_pid)
{
dINT();
tcb_t *me = (tcb_t*) sched_threads[sched_active_pid];
sched_set_status(me, STATUS_REPLY_BLOCKED);
me->wait_data = (void*) reply;
/* msg_send blocks until reply received */
return msg_send(m, target_pid, true);
}
void sched_task_exit(void) {
DEBUG("sched_task_exit(): ending task %s...\n", active_thread->name);
dINT();
sched_threads[active_thread->pid] = NULL;
num_tasks--;
sched_set_status((tcb_t*)active_thread, STATUS_STOPPED);
active_thread = NULL;
cpu_switch_context_exit();
}
void receive_mc1322x_packet(ieee802154_packet_t *trans_p)
{
maca_packet_t *maca_pkt;
dINT();
maca_pkt = maca_get_rx_packet();
trans_p->lqi = maca_pkt->lqi;
trans_p->length = maca_pkt->length;
memcpy((void *) &(data_buffer[transceiver_buffer_pos * PAYLOAD_SIZE]), maca_pkt->data, MACA_MAX_PAYLOAD_SIZE);
maca_free_packet(maca_pkt);
eINT();
trans_p->frame.payload = (uint8_t *) &(data_buffer[transceiver_buffer_pos * MACA_MAX_PAYLOAD_SIZE]);
}
void receive_cc2420_packet(ieee802154_packet_t *trans_p)
{
DEBUG("transceiver: Handling CC2420 packet\n");
dINT();
cc2420_packet_t *p = &cc2420_rx_buffer[rx_buffer_pos];
trans_p->length = p->length;
memcpy(&trans_p->frame, &p->frame, sizeof(trans_p->frame));
trans_p->rssi = p->rssi;
trans_p->crc = p->crc;
trans_p->lqi = p->lqi;
memcpy(&data_buffer[transceiver_buffer_pos * CC2420_MAX_DATA_LENGTH],
p->frame.payload, p->frame.payload_len);
trans_p->frame.payload = (uint8_t *) & (data_buffer[transceiver_buffer_pos * CC2420_MAX_DATA_LENGTH]);
trans_p->frame.payload_len = p->frame.payload_len;
eINT();
#if ENABLE_DEBUG
if (trans_p->frame.fcf.dest_addr_m == IEEE_802154_SHORT_ADDR_M) {
if (trans_p->frame.fcf.src_addr_m == IEEE_802154_SHORT_ADDR_M) {
DEBUG("Packet %p was from %" PRIu16 " to %" PRIu16 ", size: %u\n", trans_p, *((uint16_t *) &trans_p->frame.src_addr[0]), *((uint16_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
else if (trans_p->frame.fcf.src_addr_m == IEEE_802154_LONG_ADDR_M) {
DEBUG("Packet %p was from %016" PRIx64 " to %" PRIu16 ", size: %u\n", trans_p, *((uint64_t *) &trans_p->frame.src_addr[0]), *((uint16_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
else {
DEBUG("Illegal source address mode: %d\n", trans_p->frame.fcf.src_addr_m);
return;
}
}
else if (trans_p->frame.fcf.dest_addr_m == IEEE_802154_LONG_ADDR_M) {
if (trans_p->frame.fcf.src_addr_m == IEEE_802154_SHORT_ADDR_M) {
DEBUG("Packet %p was from %" PRIu16 " to %016" PRIx64 ", size: %u\n", trans_p, *((uint16_t *) &trans_p->frame.src_addr[0]), *((uint64_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
else if (trans_p->frame.fcf.src_addr_m == IEEE_802154_LONG_ADDR_M) {
DEBUG("Packet %p was from %016" PRIx64 " to %016" PRIx64 ", size: %u\n", trans_p, *((uint64_t *) &trans_p->frame.src_addr[0]), *((uint16_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
else {
DEBUG("Illegal source address mode: %d\n", trans_p->frame.fcf.src_addr_m);
return;
}
}
else {
DEBUG("Illegal destination address mode: %d\n", trans_p->frame.fcf.src_addr_m);
return;
}
#endif
DEBUG("transceiver: Content: %s\n", trans_p->frame.payload);
}
void receive_cc1100_packet(radio_packet_t *trans_p)
{
dINT();
trans_p->src = cc1100_packet_info->source;
trans_p->dst = cc1100_packet_info->destination;
trans_p->rssi = cc1100_packet_info->rssi;
trans_p->lqi = cc1100_packet_info->lqi;
trans_p->length = cc1100_payload_size;
memcpy((void *) &(data_buffer[transceiver_buffer_pos * PAYLOAD_SIZE]), cc1100_payload, CC1100_MAX_DATA_LENGTH);
eINT();
trans_p->data = (uint8_t *) &(data_buffer[transceiver_buffer_pos * CC1100_MAX_DATA_LENGTH]);
DEBUG("transceiver: Packet %p (%p) was from %hu to %hu, size: %u\n", trans_p, trans_p->data, trans_p->src, trans_p->dst, trans_p->length);
}
void pthread_exit(void *retval)
{
pthread_t self_id = pthread_self();
if (self_id == 0) {
DEBUG("ERROR called pthread_self() returned 0 in \"%s\"!\n", __func__);
}
else {
pthread_thread_t *self = pthread_sched_threads[self_id - 1];
while (self->cleanup_top) {
__pthread_cleanup_datum_t *ct = self->cleanup_top;
self->cleanup_top = ct->__next;
ct->__routine(ct->__arg);
}
/* Prevent linking in pthread_tls.o if no TSS functions were used. */
extern void __pthread_keys_exit(int self_id) __attribute__((weak));
if (__pthread_keys_exit) {
__pthread_keys_exit(self_id);
}
self->thread_pid = KERNEL_PID_UNDEF;
DEBUG("pthread_exit(%p), self == %p\n", retval, (void *) self);
if (self->status != PTS_DETACHED) {
self->returnval = retval;
self->status = PTS_ZOMBIE;
if (self->joining_thread) {
/* our thread got an other thread waiting for us */
thread_wakeup(self->joining_thread);
}
}
dINT();
if (self->stack) {
msg_t m;
m.content.ptr = self->stack;
msg_send_int(&m, pthread_reaper_pid);
}
}
sched_task_exit();
}
void cpu_switch_context_exit()
{
if (_native_in_isr == 0) {
dINT();
_native_in_isr = 1;
native_isr_context.uc_stack.ss_sp = __isr_stack;
native_isr_context.uc_stack.ss_size = SIGSTKSZ;
native_isr_context.uc_stack.ss_flags = 0;
makecontext(&native_isr_context, isr_cpu_switch_context_exit, 0);
if (setcontext(&native_isr_context) == -1) {
err(EXIT_FAILURE, "cpu_switch_context_exit: swapcontext");
}
}
else {
isr_cpu_switch_context_exit();
}
errx(EXIT_FAILURE, "this should have never been reached!!");
}
/*
* @brief process packets from CC1100
*
* @param trans_p The current entry in the transceiver buffer
*/
static void receive_cc110x_packet(radio_packet_t *trans_p)
{
DEBUG("transceiver: Handling CC1100 packet\n");
/* disable interrupts while copying packet */
dINT();
cc110x_packet_t p = cc110x_rx_buffer[rx_buffer_pos].packet;
trans_p->src = p.phy_src;
trans_p->dst = p.address;
trans_p->rssi = cc110x_rx_buffer[rx_buffer_pos].rssi;
trans_p->lqi = cc110x_rx_buffer[rx_buffer_pos].lqi;
trans_p->length = p.length - CC1100_HEADER_LENGTH;
memcpy((void *) &(data_buffer[transceiver_buffer_pos * PAYLOAD_SIZE]), p.data, CC1100_MAX_DATA_LENGTH);
eINT();
trans_p->data = (uint8_t *) &(data_buffer[transceiver_buffer_pos * CC1100_MAX_DATA_LENGTH]);
DEBUG("transceiver: Packet %p (%p) was from %hu to %hu, size: %u\n", trans_p, trans_p->data, trans_p->src, trans_p->dst, trans_p->length);
}
void kernel_init(void)
{
dINT();
printf("kernel_init(): This is RIOT! (Version: %s)\n", VERSION);
sched_init();
if (thread_create(idle_stack, sizeof(idle_stack), PRIORITY_IDLE, CREATE_WOUT_YIELD | CREATE_STACKTEST, idle_thread, idle_name) < 0) {
printf("kernel_init(): error creating idle task.\n");
}
if (thread_create(main_stack, sizeof(main_stack), PRIORITY_MAIN, CREATE_WOUT_YIELD | CREATE_STACKTEST, MAIN_FUNC, main_name) < 0) {
printf("kernel_init(): error creating main task.\n");
}
printf("kernel_init(): jumping into first task...\n");
cpu_switch_context_exit();
}
void thread_yield(void)
{
ucontext_t *ctx = (ucontext_t *)(sched_active_thread->sp);
if (_native_in_isr == 0) {
_native_in_isr = 1;
dINT();
native_isr_context.uc_stack.ss_sp = __isr_stack;
native_isr_context.uc_stack.ss_size = SIGSTKSZ;
native_isr_context.uc_stack.ss_flags = 0;
makecontext(&native_isr_context, isr_thread_yield, 0);
if (swapcontext(ctx, &native_isr_context) == -1) {
err(EXIT_FAILURE, "thread_yield: swapcontext");
}
eINT();
}
else {
isr_thread_yield();
}
}
void receive_at86rf231_packet(ieee802154_packet_t *trans_p)
{
DEBUG("Handling AT86RF231 packet\n");
dINT();
at86rf231_packet_t *p = &at86rf231_rx_buffer[rx_buffer_pos];
trans_p->length = p->length;
trans_p->rssi = p->rssi;
trans_p->crc = p->crc;
trans_p->lqi = p->lqi;
memcpy(&trans_p->frame, &p->frame, sizeof(trans_p->frame));
memcpy(&data_buffer[transceiver_buffer_pos * AT86RF231_MAX_DATA_LENGTH], p->frame.payload,
p->frame.payload_len);
trans_p->frame.payload = (uint8_t *) & (data_buffer[transceiver_buffer_pos * AT86RF231_MAX_DATA_LENGTH]);
trans_p->frame.payload_len = p->frame.payload_len;
eINT();
#if ENABLE_DEBUG
if (trans_p->frame.fcf.dest_addr_m == IEEE_802154_SHORT_ADDR_M) {
if (trans_p->frame.fcf.src_addr_m == IEEE_802154_SHORT_ADDR_M) {
DEBUG("Packet %p was from %" PRIu16 " to %" PRIu16 ", size: %u\n", trans_p, *((uint16_t *) &trans_p->frame.src_addr[0]), *((uint16_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
else {
DEBUG("Packet %p was from %016" PRIx64 " to %" PRIu16 ", size: %u\n", trans_p, *((uint64_t *) &trans_p->frame.src_addr[0]), *((uint16_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
}
else {
if (trans_p->frame.fcf.src_addr_m == IEEE_802154_SHORT_ADDR_M) {
DEBUG("Packet %p was from %" PRIu16 " to %016" PRIx64 ", size: %u\n", trans_p, *((uint16_t *) &trans_p->frame.src_addr[0]), *((uint64_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
else {
DEBUG("Packet %p was from %016" PRIx64 " to %016" PRIx64 ", size: %u\n", trans_p, *((uint64_t *) &trans_p->frame.src_addr[0]), *((uint16_t *) &trans_p->frame.dest_addr), trans_p->frame.payload_len);
}
}
#endif
DEBUG("Content: %s\n", trans_p->frame.payload);
}
static int _hwtimer_set(unsigned long offset, void (*callback)(void*), void *ptr, bool absolute)
{
DEBUG("_hwtimer_set: offset=%lu callback=%p ptr=%p absolute=%d\n", offset, callback, ptr, absolute);
if (!inISR()) {
dINT();
}
int n = lifo_get(lifo);
if (n == -1) {
if (!inISR()) {
eINT();
}
puts("No hwtimer left.");
return -1;
}
timer[n].callback = callback;
timer[n].data = ptr;
if (absolute) {
DEBUG("hwtimer_arch_set_absolute n=%d\n", n);
hwtimer_arch_set_absolute(offset, n);
}
else {
DEBUG("hwtimer_arch_set n=%d\n", n);
hwtimer_arch_set(offset, n);
}
lpm_prevent_sleep++;
if (!inISR()) {
eINT();
}
return n;
}
void pthread_exit(void *retval)
{
pthread_t self_id = pthread_self();
if (self_id == 0) {
DEBUG("ERROR called pthread_self() returned 0 in \"%s\"!\n", __func__);
}
else {
pthread_thread_t *self = pthread_sched_threads[self_id-1];
while (self->cleanup_top) {
__pthread_cleanup_datum_t *ct = self->cleanup_top;
self->cleanup_top = ct->__next;
ct->__routine(ct->__arg);
}
self->thread_pid = KERNEL_PID_NULL;
DEBUG("pthread_exit(%p), self == %p\n", retval, (void *) self);
if (self->status != PTS_DETACHED) {
self->returnval = retval;
self->status = PTS_ZOMBIE;
if (self->joining_thread) {
/* our thread got an other thread waiting for us */
thread_wakeup(self->joining_thread);
}
}
dINT();
if (self->stack) {
msg_t m;
m.content.ptr = self->stack;
msg_send_int(&m, pthread_reaper_pid);
}
}
sched_task_exit();
}
void kernel_init(void)
{
dINT();
printf("kernel_init(): This is RIOT! (Version: %s)\n", VERSION);
hwtimer_init();
if (thread_create(idle_stack, sizeof(idle_stack), PRIORITY_IDLE, CREATE_WOUT_YIELD | CREATE_STACKTEST, idle_thread, idle_name) < 0) {
printf("kernel_init(): error creating idle task.\n");
}
if (thread_create(main_stack, sizeof(main_stack), PRIORITY_MAIN, CREATE_WOUT_YIELD | CREATE_STACKTEST, MAIN_FUNC, main_name) < 0) {
printf("kernel_init(): error creating main task.\n");
}
#ifdef MODULE_CONFIG
DEBUG("kernel_init(): loading config\n");
config_load();
#endif
printf("kernel_init(): jumping into first task...\n");
cpu_switch_context_exit();
}
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;
}
}
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;
}
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;
}
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");
}
kernel_pid_t thread_create(char *stack, int stacksize, char priority, int flags, void *(*function)(void *arg), void *arg, const char *name)
{
/* allocate our thread control block at the top of our stackspace */
#ifdef DEVELHELP
int total_stacksize = stacksize;
#endif
stacksize -= sizeof(tcb_t);
/* align tcb address on 32bit boundary */
unsigned int tcb_address = (unsigned int) stack + stacksize;
if (tcb_address & 1) {
tcb_address--;
stacksize--;
}
if (tcb_address & 2) {
tcb_address -= 2;
stacksize -= 2;
}
tcb_t *cb = (tcb_t *) tcb_address;
if (priority >= SCHED_PRIO_LEVELS) {
return -EINVAL;
}
#ifdef DEVELHELP
if (flags & CREATE_STACKTEST) {
/* assign each int of the stack the value of it's address */
unsigned int *stackmax = (unsigned int *)((char *)stack + stacksize);
unsigned int *stackp = (unsigned int *)stack;
while (stackp < stackmax) {
*stackp = (unsigned int)stackp;
stackp++;
}
}
else {
/* create stack guard */
*stack = (unsigned int)stack;
}
#endif
if (!inISR()) {
dINT();
}
kernel_pid_t pid = KERNEL_PID_UNDEF;
for (kernel_pid_t i = KERNEL_PID_FIRST; i <= KERNEL_PID_LAST; ++i) {
if (sched_threads[i] == NULL) {
pid = i;
break;
}
}
if (pid == KERNEL_PID_UNDEF) {
DEBUG("thread_create(): too many threads!\n");
if (!inISR()) {
eINT();
}
return -EOVERFLOW;
}
sched_threads[pid] = cb;
cb->pid = pid;
cb->sp = thread_stack_init(function, arg, stack, stacksize);
cb->stack_start = stack;
#ifdef DEVELHELP
cb->stack_size = total_stacksize;
#endif
cb->priority = priority;
cb->status = 0;
cb->rq_entry.next = NULL;
cb->rq_entry.prev = NULL;
cb->name = name;
cb->wait_data = NULL;
cb->msg_waiters.first = NULL;
cib_init(&(cb->msg_queue), 0);
cb->msg_array = NULL;
sched_num_threads++;
DEBUG("Created thread %s. PID: %" PRIkernel_pid ". Priority: %u.\n", name, cb->pid, priority);
if (flags & CREATE_SLEEPING) {
sched_set_status(cb, STATUS_SLEEPING);
}
else {
sched_set_status(cb, STATUS_PENDING);
if (!(flags & CREATE_WOUT_YIELD)) {
if (!inISR()) {
eINT();
thread_yield();
}
else {
sched_context_switch_request = 1;
}
}
}
if (!inISR() && sched_active_thread != NULL) {
eINT();
}
return pid;
}
