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;
}
