mostly stable except the exit condition

diff --git a/tasks.c b/tasks.c
index 2cbcfd8db0b82952ca1665b5dc01e20c17a42bb6..b54d65dd99fcfca196a4c3f7c39fad33d83648c1 100644 (file)
--- a/tasks.c
+++ b/tasks.c
@@ -36,13 +36,14 @@ typedef struct {
 static pthread_mutex_t ScheduleLock;
 static pthread_cond_t ScheduleCond;
 static __thread int CpuID = 0;
+static long int TaskCount = 0;
 static TaskQueue_T ReadyQueue = {0};
 static TaskQueue_T DeadQueue = {0};
 static CpuState_T* Running = NULL;
 static int CpuCount;
 
 /***************************************
-    Lopck Operations
+    Lock Operations
 ***************************************/
 static void AcquireLock(void)
 {
@@ -54,24 +55,29 @@ static void ReleaseLock(void)
     pthread_mutex_unlock(&ScheduleLock);
 }
 
+static void WaitForCondition(pthread_cond_t* cond, pthread_mutex_t* mutex, int ms)
+{
+    struct timespec expire_time;
+    clock_gettime(CLOCK_REALTIME, &expire_time);
+    expire_time.tv_nsec += (ms * 1000);
+    pthread_cond_timedwait(cond, mutex, &expire_time);
+}
+
 /***************************************
     Queue Operations
 ***************************************/
 
 static void Enqueue(TaskQueue_T* queue, Task_T* task)
 {
-    if (task)
+    if (queue->tail)
     {
-        if (queue->tail)
-        {
-            queue->tail->next = task;
-            task->next = NULL;
-        }
-        queue->tail = task;
-        if (!queue->head)
-        {
-            queue->head = queue->tail;
-        }
+        queue->tail->next = task;
+        task->next = NULL;
+    }
+    queue->tail = task;
+    if (!queue->head)
+    {
+        queue->head = queue->tail;
     }
 }
 
@@ -80,6 +86,7 @@ static Task_T* Dequeue(TaskQueue_T* queue)
     Task_T* task = queue->head;
     if (task)
     {
+        task->next = NULL;
         queue->head = queue->head->next;
         if (!queue->head)
         {
@@ -96,7 +103,7 @@ static Task_T* Dequeue(TaskQueue_T* queue)
 
 static void Enter(Task_T* task)
 {
-    if (task)
+    if (task && task != Running[CpuID].idle)
     {
         Enqueue(&ReadyQueue, task);
         pthread_cond_signal(&ScheduleCond);
@@ -105,18 +112,25 @@ static void Enter(Task_T* task)
 
 static Task_T* Select(void)
 {
-    pthread_cond_signal(&ScheduleCond);
-    return Dequeue(&ReadyQueue);
+    Task_T* task = Dequeue(&ReadyQueue);
+    if (!task)
+    {
+        task = Running[CpuID].idle;
+    }
+    else
+    {
+        pthread_cond_signal(&ScheduleCond);
+    }
+    return task;
 }
 
 /***************************************
     Task Switching Operations
 ***************************************/
 
-extern void SwitchTo(Task_T* prev, Task_T* next);
-
+extern void SwapTask(Task_T* prev, Task_T* next);
 __asm__(
-"SwitchTo:\n"
+"SwapTask:\n"
 "    push %rdi\n"
 "    push %rbp\n"
 "    push %rbx\n"
@@ -136,46 +150,47 @@ __asm__(
 "    ret\n"
 );
 
+extern void StartTask(Task_T* task);
+__asm__(
+"StartTask:\n"
+"    mov (%rdi),%rsp\n"
+"    pop %r15\n"
+"    pop %r14\n"
+"    pop %r13\n"
+"    pop %r12\n"
+"    pop %rbx\n"
+"    pop %rbp\n"
+"    pop %rdi\n"
+"    ret\n"
+);
+
 static void Yield(void)
 {
     AcquireLock();
-    Task_T* next = Select();
-    if (next)
-    {
-        puts("found task");
-        Task_T* prev = Running[CpuID].task;
-        Enter(prev);
-        Running[CpuID].task = next;
-        prev = (prev ? prev : &(Task_T){0});
-        ReleaseLock();
-        SwitchTo(prev, next);
-    }
-    else
-    {
-        ReleaseLock();
-    }
+    printf("CPU %d is yielding\n", CpuID);
+    Task_T* prev = Running[CpuID].task;
+    Running[CpuID].task = NULL;
+    Enter(prev);
+    printf("CPU %d is picking task\n", CpuID);
+    Running[CpuID].task = Select();
+    printf("CPU %d switching to task %p\n", CpuID, Running[CpuID].task);
+    ReleaseLock();
+    SwapTask(prev, Running[CpuID].task);
 }
 
 void FinalizeTask(void)
 {
-    /* mark the current task as dead */
+    puts("exiting");
     AcquireLock();
-    Running[CpuID].task = NULL;
-    pthread_cond_signal(&ScheduleCond);
-//    Recycle the task somehow here...
+    TaskCount--;
+    Running[CpuID].task = NULL; // TODO: Enqueue to dead queue
+    Running[CpuID].task = Select();
+    printf("starting task %p\n", Running[CpuID].task);
     ReleaseLock();
-
-    /* now we loop until there is a task to jump to */
-    while(1)
-    {
-        Yield();
-        AcquireLock();
-        pthread_cond_wait(&ScheduleCond, &ScheduleLock);
-        ReleaseLock();
-    }
+    StartTask(Running[CpuID].task);
 }
 
-void CreateTask(void (*task_fn)(void*), void* arg, int stacksize)
+static Task_T* CreateTask(void (*task_fn)(void*), void* arg, int stacksize)
 {
     if (stacksize == 0) { stacksize = 32768; }
     Task_T* task = calloc(1, sizeof(Task_T));
@@ -189,20 +204,40 @@ void CreateTask(void (*task_fn)(void*), void* arg, int stacksize)
         *(--task->stack_top) = 0xdeadbeef; // initial values for saved registers
     }
     printf("created task %p\n", task);
+    return task;
+}
 
+void SpawnTask(void (*task_fn)(void*), void* arg, int stacksize)
+{
+    Task_T* task = CreateTask(task_fn, arg, stacksize);
     /* put task in the ready queue */
     AcquireLock();
+    TaskCount++;
     Enter(task);
     ReleaseLock();
 }
 
-
+static void CpuIdle(void* arg)
+{
+    printf("CPU %d is started\n", CpuID);
+    while(1)
+    {
+        Yield();
+        printf("CPU %d is idle\n", CpuID);
+        AcquireLock();
+        WaitForCondition(&ScheduleCond, &ScheduleLock, 1);
+        ReleaseLock();
+    }
+}
 
 static void* CpuMain(void* arg)
 {
     CpuID = (long int)arg;
-    FinalizeTask();
-    return NULL;
+    Running[CpuID].idle = CreateTask(CpuIdle, 0, 0);
+    Running[CpuID].task = Running[CpuID].idle;
+    printf("CPU %d initializing\n", CpuID);
+    StartTask(Running[CpuID].idle);
+    return NULL; /* unreachable */
 }
 
 /***************************************
@@ -235,31 +270,43 @@ int main(int argc, char** argv)
     pthread_mutex_init(&ScheduleLock, 0);
     pthread_cond_init(&ScheduleCond, NULL);
     CpuCount = sysconf(_SC_NPROCESSORS_ONLN);
+//    CpuCount = 1;
     Running = calloc(CpuCount, sizeof(CpuState_T));
     for (long int i = 0; i < CpuCount; i++)
     {
         pthread_create(&Running[i].thread, NULL, CpuMain, (void*)i);
     }
 
-    CreateTask(task1, 0, 0);
-    CreateTask(task2, 0, 0);
+    SpawnTask(task1, 0, 0);
+    SpawnTask(task2, 0, 0);
 
     /* wait for all jobs to be done */
     while(1)
     {
         AcquireLock();
-        pthread_cond_wait(&ScheduleCond, &ScheduleLock);
-        if (!ReadyQueue.head)
+        WaitForCondition(&ScheduleCond, &ScheduleLock, 10);
+//        printf("%ld %d %p\n", TaskCount, CpuCount, ReadyQueue.head);
+        if (TaskCount == 0)
         {
-            long int val = 0;
-            for (int i = 0; i < CpuCount; i++)
-            {
-                val |= (long int)Running[i].task;
-            }
-            if (!val)
+            break;
+        }
+//        else
+        {
+            printf("count: %ld\n", TaskCount);
+            printf("head:  %p\n", ReadyQueue.head);
+            printf("tasks:\n");
+            for (long int i = 0; i < CpuCount; i++)
             {
-                break;
+                if (Running[i].task == Running[i].idle)
+                {
+                    printf("    %02ld idle\n", i);
+                }
+                else
+                {
+                    printf("    %02ld %p\n", i, Running[i].task);
+                }
             }
+
         }
         ReleaseLock();
     }