1 files changed, 397 insertions, 88 deletions

diff --git a/firmware/thread.c b/firmware/thread.c
index eb39c7ad32..e4dcbbcf9a 100644
--- a/firmware/thread.c
+++ b/firmware/thread.c
@@ -23,12 +23,18 @@
 #include "system.h"
 #include "kernel.h"
 #include "cpu.h"
-
+#include "string.h"
 
 #define DEADBEEF ((unsigned int)0xdeadbeef)
 /* Cast to the the machine int type, whose size could be < 4. */
 
 struct core_entry cores[NUM_CORES] IBSS_ATTR;
+#ifdef HAVE_PRIORITY_SCHEDULING
+static unsigned short highest_priority IBSS_ATTR;
+#endif
+
+/* Define to enable additional checks for blocking violations etc. */
+// #define THREAD_EXTRA_CHECKS
 
 static const char main_thread_name[] = "main";
 
@@ -48,7 +54,16 @@ int *cop_stackend = stackend;
 #endif
 #endif
 
-void switch_thread(void) ICODE_ATTR;
+/* Conserve IRAM
+static void add_to_list(struct thread_entry **list,
+                        struct thread_entry *thread) ICODE_ATTR;
+static void remove_from_list(struct thread_entry **list,
+                             struct thread_entry *thread) ICODE_ATTR;
+*/
+
+void switch_thread(bool save_context, struct thread_entry **blocked_list)
+    ICODE_ATTR;
+
 static inline void store_context(void* addr) __attribute__ ((always_inline));
 static inline void load_context(const void* addr) __attribute__ ((always_inline));
 
@@ -219,24 +234,109 @@ static inline void load_context(const void* addr)
 
 #endif
 
-/*---------------------------------------------------------------------------
- * Switch thread in round robin fashion.
- *---------------------------------------------------------------------------
- */
-void switch_thread(void)
+static void add_to_list(struct thread_entry **list,
+                        struct thread_entry *thread)
 {
-#ifdef RB_PROFILE
-    profile_thread_stopped(cores[CURRENT_CORE].current_thread);
-#endif
-    int current;
-    unsigned int *stackptr;
+    if (*list == NULL)
+    {
+        thread->next = thread;
+        thread->prev = thread;
+        *list = thread;
+    }
+    else
+    {
+        /* Insert last */
+        thread->next = *list;
+        thread->prev = (*list)->prev;
+        thread->prev->next = thread;
+        (*list)->prev = thread;
+        
+        /* Insert next
+         thread->next = (*list)->next;
+         thread->prev = *list;
+         thread->next->prev = thread;
+         (*list)->next = thread;
+         */
+    }
+}
 
-#ifdef SIMULATOR
-    /* Do nothing */
-#else
-    while (cores[CURRENT_CORE].num_sleepers == cores[CURRENT_CORE].num_threads)
+static void remove_from_list(struct thread_entry **list,
+                      struct thread_entry *thread)
+{
+    if (list != NULL)
+    {
+        if (thread == thread->next)
+        {
+            *list = NULL;
+            return;
+        }
+        
+        if (thread == *list)
+            *list = thread->next;
+    }
+    
+    /* Fix links to jump over the removed entry. */
+    thread->prev->next = thread->next;
+    thread->next->prev = thread->prev;
+}
+
+/* Compiler trick: Don't declare as static to prevent putting 
+ * function in IRAM. */
+void check_sleepers(void)
+{
+    struct thread_entry *current, *next;
+    
+    /* Check sleeping threads. */
+    current = cores[CURRENT_CORE].sleeping;
+    if (current == NULL)
+        return ;
+    
+    for (;;)
+    {
+        next = current->next;
+        
+        if ((unsigned)current_tick >= GET_STATE_ARG(current->statearg))
+        {
+            /* Sleep timeout has been reached so bring the thread
+             * back to life again. */
+            remove_from_list(&cores[CURRENT_CORE].sleeping, current);
+            add_to_list(&cores[CURRENT_CORE].running, current);
+            
+            /* If there is no more processes in the list, break the loop. */
+            if (cores[CURRENT_CORE].sleeping == NULL)
+                break;
+            
+            current = next;
+            continue;
+        }
+        
+        current = next;
+        
+        /* Break the loop once we have walked through the list of all
+         * sleeping processes. */
+        if (current == cores[CURRENT_CORE].sleeping)
+            break;
+    }
+}
+
+static inline void sleep_core(void)
+{
+    static long last_tick = 0;
+    
+    for (;;)
     {
-        /* Enter sleep mode, woken up on interrupt */
+        if (last_tick != current_tick)
+        {
+            check_sleepers();
+            last_tick = current_tick;
+        }
+        
+        /* We must sleep until there is at least one process in the list
+         * of running processes. */
+        if (cores[CURRENT_CORE].running != NULL)
+            break;
+
+        /* Enter sleep mode to reduce power usage, woken up on interrupt */
 #ifdef CPU_COLDFIRE
         asm volatile ("stop #0x2000");
 #elif CONFIG_CPU == SH7034
@@ -257,49 +357,232 @@ void switch_thread(void)
         CLKCON |= 2;
 #endif
     }
-#endif
-    current = cores[CURRENT_CORE].current_thread;
-    store_context(&cores[CURRENT_CORE].threads[current].context);
-
-#if CONFIG_CPU != TCC730
-    /* Check if the current thread stack is overflown */
-    stackptr = cores[CURRENT_CORE].threads[current].stack;
-    if(stackptr[0] != DEADBEEF)
-       panicf("Stkov %s", cores[CURRENT_CORE].threads[current].name);
+}
+
+#ifdef RB_PROFILE
+static int get_threadnum(struct thread_entry *thread)
+{
+    int i;
+    
+    for (i = 0; i < MAXTHREADS; i++)
+    {
+        if (&cores[CURRENT_CORE].threads[i] == thread)
+            return i;
+    }
+    
+    return -1;
+}
 #endif
 
-    if (++current >= cores[CURRENT_CORE].num_threads)
-        current = 0;
+/* Compiler trick: Don't declare as static to prevent putting 
+ * function in IRAM. */
+void change_thread_state(struct thread_entry **blocked_list)
+{
+    struct thread_entry *old;
+    
+    /* Remove the thread from the list of running threads. */
+    old = cores[CURRENT_CORE].running;
+    remove_from_list(&cores[CURRENT_CORE].running, old);
+    
+    /* And put the thread into a new list of inactive threads. */
+    if (GET_STATE(old->statearg) == STATE_BLOCKED)
+        add_to_list(blocked_list, old);
+    else
+        add_to_list(&cores[CURRENT_CORE].sleeping, old);
+    
+#ifdef HAVE_PRIORITY_SCHEDULING
+    /* Reset priorities */
+    if (old->priority == highest_priority)
+        highest_priority = 100;
+#endif
+}
 
-    cores[CURRENT_CORE].current_thread = current;
-    load_context(&cores[CURRENT_CORE].threads[current].context);
+/*---------------------------------------------------------------------------
+ * Switch thread in round robin fashion.
+ *---------------------------------------------------------------------------
+ */
+void switch_thread(bool save_context, struct thread_entry **blocked_list)
+{
 #ifdef RB_PROFILE
-    profile_thread_started(cores[CURRENT_CORE].current_thread);
+    profile_thread_stopped(get_threadnum(cores[CURRENT_CORE].running));
+#endif
+    unsigned int *stackptr;
+    
+#ifdef SIMULATOR
+    /* Do nothing */
+#else
+    
+    /* Begin task switching by saving our current context so that we can
+     * restore the state of the current thread later to the point prior
+     * to this call. */
+    if (save_context)
+    {
+        store_context(&cores[CURRENT_CORE].running->context);
+
+# if CONFIG_CPU != TCC730
+        /* Check if the current thread stack is overflown */
+        stackptr = cores[CURRENT_CORE].running->stack;
+        if(stackptr[0] != DEADBEEF)
+        panicf("Stkov %s", cores[CURRENT_CORE].running->name);
+# endif
+        
+        /* Check if a thread state change has been requested. */
+        if (cores[CURRENT_CORE].running->statearg)
+        {
+            /* Change running thread state and switch to next thread. */
+            change_thread_state(blocked_list);
+        }
+        else
+        {
+            /* Switch to the next running thread. */
+            cores[CURRENT_CORE].running = cores[CURRENT_CORE].running->next;
+        }
+    }
+    
+    /* Go through the list of sleeping task to check if we need to wake up
+     * any of them due to timeout. Also puts core into sleep state until
+     * there is at least one running process again. */
+    sleep_core();
+    
+#ifdef HAVE_PRIORITY_SCHEDULING
+    /* Select the new task based on priorities and the last time a process
+     * got CPU time. */
+    for (;;)
+    {
+        int priority = cores[CURRENT_CORE].running->priority;
+        
+        if (priority < highest_priority)
+            highest_priority = priority;
+        
+        if (priority == highest_priority || (current_tick 
+            - cores[CURRENT_CORE].running->last_run > priority * 8))
+        {
+            break;
+        }
+        cores[CURRENT_CORE].running = cores[CURRENT_CORE].running->next;
+    }
+    
+    /* Reset the value of thread's last running time to the current time. */
+    cores[CURRENT_CORE].running->last_run = current_tick;
+#endif
+    
+#endif
+    /* And finally give control to the next thread. */
+    load_context(&cores[CURRENT_CORE].running->context);
+    
+#ifdef RB_PROFILE
+    profile_thread_started(get_threadnum(cores[CURRENT_CORE].running));
 #endif
 }
 
-void sleep_thread(void)
+void sleep_thread(int ticks)
 {
-    ++cores[CURRENT_CORE].num_sleepers;
-    switch_thread();
+    /* Set the thread's new state and timeout and finally force a task switch
+     * so that scheduler removes thread from the list of running processes
+     * and puts it in list of sleeping tasks. */
+    cores[CURRENT_CORE].running->statearg = 
+        SET_STATE(STATE_SLEEPING, current_tick + ticks + 1);
+    switch_thread(true, NULL);
+    
+    /* Clear all flags to indicate we are up and running again. */
+    cores[CURRENT_CORE].running->statearg = 0;
 }
 
-void wake_up_thread(void)
+void block_thread(struct thread_entry **list, int timeout)
 {
-    cores[CURRENT_CORE].num_sleepers = 0;
+    struct thread_entry *current;
+    
+    /* Get the entry for the current running thread. */
+    current = cores[CURRENT_CORE].running;
+    
+    /* At next task switch scheduler will immediately change the thread
+     * state (and we also force the task switch to happen). */
+    if (timeout)
+    {
+#ifdef THREAD_EXTRA_CHECKS
+        /* We can store only one thread to the "list" if thread is used
+         * in other list (such as core's list for sleeping tasks). */
+        if (*list)
+            panicf("Blocking violation T->*B");
+#endif
+        
+        current->statearg = 
+            SET_STATE(STATE_BLOCKED_W_TMO, current_tick + timeout);
+        *list = current;
+
+        /* Now force a task switch and block until we have been woken up
+         * by another thread or timeout is reached. */
+        switch_thread(true, NULL);
+        
+        /* If timeout is reached, we must set list back to NULL here. */
+        *list = NULL;
+    }
+    else
+    {
+#ifdef THREAD_EXTRA_CHECKS
+        /* We are not allowed to mix blocking types in one queue. */
+        if (*list && GET_STATE((*list)->statearg) == STATE_BLOCKED_W_TMO)
+            panicf("Blocking violation B->*T");
+#endif
+        
+        current->statearg = SET_STATE(STATE_BLOCKED, 0);
+        
+        /* Now force a task switch and block until we have been woken up
+         * by another thread or timeout is reached. */
+        switch_thread(true, list);
+    }
+    
+    /* Clear all flags to indicate we are up and running again. */
+    current->statearg = 0;
 }
 
+void wakeup_thread(struct thread_entry **list)
+{
+    struct thread_entry *thread;
+    
+    /* Check if there is a blocked thread at all. */
+    if (*list == NULL)
+        return ;
+    
+    /* Wake up the last thread first. */
+    thread = *list;
+    
+    /* Determine thread's current state. */
+    switch (GET_STATE(thread->statearg)) 
+    {
+        case STATE_BLOCKED:
+            /* Remove thread from the list of blocked threads and add it
+             * to the scheduler's list of running processes. */
+            remove_from_list(list, thread);
+            add_to_list(&cores[CURRENT_CORE].running, thread);
+            thread->statearg = 0;
+            break;
+        
+        case STATE_BLOCKED_W_TMO:
+            /* Just remove the timeout to cause scheduler to immediately
+             * wake up the thread. */
+            thread->statearg &= 0xC0000000;
+            *list = NULL;
+            break;
+        
+        default:
+            /* Nothing to do. Thread has already been woken up
+             * or it's state is not blocked or blocked with timeout. */
+            return ;
+    }
+}
 
 /*---------------------------------------------------------------------------
  * Create thread on the current core.
  * Return ID if context area could be allocated, else -1.
  *---------------------------------------------------------------------------
  */
-int create_thread(void (*function)(void), void* stack, int stack_size,
-                  const char *name)
+struct thread_entry* 
+    create_thread(void (*function)(void), void* stack, int stack_size,
+                  const char *name IF_PRIO(, int priority))
 {
     return create_thread_on_core(CURRENT_CORE, function, stack, stack_size,
-                  name);
+                  name IF_PRIO(, priority));
 }
 
 /*---------------------------------------------------------------------------
@@ -307,18 +590,28 @@ int create_thread(void (*function)(void), void* stack, int stack_size,
  * Return ID if context area could be allocated, else -1.
  *---------------------------------------------------------------------------
  */
-int create_thread_on_core(unsigned int core, void (*function)(void), void* stack, int stack_size,
-                  const char *name)
+struct thread_entry* 
+    create_thread_on_core(unsigned int core, void (*function)(void), 
+                          void* stack, int stack_size,
+                          const char *name IF_PRIO(, int priority))
 {
     unsigned int i;
     unsigned int stacklen;
     unsigned int *stackptr;
+    int n;
     struct regs *regs;
     struct thread_entry *thread;
 
-    if (cores[core].num_threads >= MAXTHREADS)
-        return -1;
-
+    for (n = 0; n < MAXTHREADS; n++)
+    {
+        if (cores[core].threads[n].name == NULL)
+            break;
+    }
+    
+    if (n == MAXTHREADS)
+        return NULL;
+    
+    
     /* Munge the stack to make it easy to spot stack overflows */
     stacklen = stack_size / sizeof(int);
     stackptr = stack;
@@ -328,10 +621,17 @@ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack
     }
 
     /* Store interesting information */
-    thread = &cores[core].threads[cores[core].num_threads];
+    thread = &cores[core].threads[n];
     thread->name = name;
     thread->stack = stack;
     thread->stack_size = stack_size;
+    thread->statearg = 0;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    thread->priority = priority;
+    highest_priority = 100;
+#endif
+    add_to_list(&cores[core].running, thread);
+    
     regs = &thread->context;
 #if defined(CPU_COLDFIRE) || (CONFIG_CPU == SH7034) || defined(CPU_ARM)
     /* Align stack to an even 32 bit boundary */
@@ -343,8 +643,7 @@ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack
 #endif
     regs->start = (void*)function;
 
-    wake_up_thread();
-    return cores[core].num_threads++; /* return the current ID, e.g for remove_thread() */
+    return thread;
 }
 
 /*---------------------------------------------------------------------------
@@ -352,44 +651,58 @@ int create_thread_on_core(unsigned int core, void (*function)(void), void* stack
  * Parameter is the ID as returned from create_thread().
  *---------------------------------------------------------------------------
  */
-void remove_thread(int threadnum)
+void remove_thread(struct thread_entry *thread)
 {
-    remove_thread_on_core(CURRENT_CORE, threadnum);
+    if (thread == NULL)
+        thread = cores[CURRENT_CORE].running;
+    
+    /* Free the entry by removing thread name. */
+    thread->name = NULL;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    highest_priority = 100;
+#endif
+    
+    if (thread == cores[CURRENT_CORE].running)
+    {
+        remove_from_list(&cores[CURRENT_CORE].running, thread);
+        switch_thread(false, NULL);
+        return ;
+    }
+    
+    if (thread == cores[CURRENT_CORE].sleeping)
+        remove_from_list(&cores[CURRENT_CORE].sleeping, thread);
+    
+    remove_from_list(NULL, thread);
 }
 
-/*---------------------------------------------------------------------------
- * Remove a thread on the specified core from the scheduler.
- * Parameters are the core and the ID as returned from create_thread().
- *---------------------------------------------------------------------------
- */
-void remove_thread_on_core(unsigned int core, int threadnum)
+#ifdef HAVE_PRIORITY_SCHEDULING
+void thread_set_priority(struct thread_entry *thread, int priority)
 {
-    int i;
-
-    if (threadnum >= cores[core].num_threads)
-       return;
-
-    cores[core].num_threads--;
-    for (i=threadnum; i<cores[core].num_threads-1; i++)
-    {   /* move all entries which are behind */
-        cores[core].threads[i] = cores[core].threads[i+1];
-    }
-
-    if (cores[core].current_thread == threadnum) /* deleting the current one? */
-        cores[core].current_thread = cores[core].num_threads; /* set beyond last, avoid store harm */
-    else if (cores[core].current_thread > threadnum) /* within the moved positions? */
-        cores[core].current_thread--; /* adjust it, point to same context again */
+    if (thread == NULL)
+        thread = cores[CURRENT_CORE].running;
+    
+    thread->priority = priority;
+    highest_priority = 100;
 }
+#endif
 
 void init_threads(void)
 {
     unsigned int core = CURRENT_CORE;
 
-    cores[core].num_threads = 1; /* We have 1 thread to begin with */
-    cores[core].current_thread = 0; /* The current thread is number 0 */
+    memset(cores, 0, sizeof cores);
+    cores[core].sleeping = NULL;
+    cores[core].running = NULL;
     cores[core].threads[0].name = main_thread_name;
-/* In multiple core setups, each core has a different stack.  There is probably
-   a much better way to do this. */
+    cores[core].threads[0].statearg = 0;
+#ifdef HAVE_PRIORITY_SCHEDULING
+    cores[core].threads[0].priority = PRIORITY_USER_INTERFACE;
+    highest_priority = 100;
+#endif
+    add_to_list(&cores[core].running, &cores[core].threads[0]);
+    
+    /* In multiple core setups, each core has a different stack.  There is probably
+     a much better way to do this. */
     if (core == CPU)
     {
         cores[CPU].threads[0].stack = stackbegin;
@@ -405,28 +718,24 @@ void init_threads(void)
 #else
     cores[core].threads[0].context.start = 0; /* thread 0 already running */
 #endif
-    cores[core].num_sleepers = 0;
-}
-
-int thread_stack_usage(int threadnum)
-{
-    return thread_stack_usage_on_core(CURRENT_CORE, threadnum);
 }
 
-int thread_stack_usage_on_core(unsigned int core, int threadnum)
+int thread_stack_usage(const struct thread_entry *thread)
 {
     unsigned int i;
-    unsigned int *stackptr = cores[core].threads[threadnum].stack;
-
-    if (threadnum >= cores[core].num_threads)
-        return -1;
+    unsigned int *stackptr = thread->stack;
 
-    for (i = 0;i < cores[core].threads[threadnum].stack_size/sizeof(int);i++)
+    for (i = 0;i < thread->stack_size/sizeof(int);i++)
     {
         if (stackptr[i] != DEADBEEF)
             break;
     }
 
-    return ((cores[core].threads[threadnum].stack_size - i * sizeof(int)) * 100) /
-        cores[core].threads[threadnum].stack_size;
+    return ((thread->stack_size - i * sizeof(int)) * 100) /
+        thread->stack_size;
+}
+
+int thread_get_status(const struct thread_entry *thread)
+{
+    return GET_STATE(thread->statearg);
 }