int thread_create(char *stack, int stacksize, char priority, int flags, void (*function)(void), const char *name) { /* allocate our thread control block at the top of our stackspace */ int total_stacksize = stacksize; 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; } 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; } if (!inISR()) { dINT(); } int pid = 0; while (pid < MAXTHREADS) { if (sched_threads[pid] == NULL) { sched_threads[pid] = cb; cb->pid = pid; break; } pid++; } if (pid == MAXTHREADS) { DEBUG("thread_create(): too many threads!\n"); if (!inISR()) { eINT(); } return -EOVERFLOW; } cb->sp = thread_stack_init(function, stack, stacksize); cb->stack_start = stack; cb->stack_size = total_stacksize; cb->priority = priority; cb->status = 0; cb->rq_entry.data = (unsigned int) cb; cb->rq_entry.next = NULL; cb->rq_entry.prev = NULL; cb->name = name; cb->wait_data = NULL; cb->msg_waiters.data = 0; cb->msg_waiters.priority = 0; cb->msg_waiters.next = NULL; cib_init(&(cb->msg_queue), 0); cb->msg_array = NULL; sched_num_threads++; DEBUG("Created thread %s. PID: %u. 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; }
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; n.next = NULL; 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; }