some OSThread functions

libs/RVL_SDK/csv/os.csv

@@ -196,13 +196,13 @@ __OSPromoteThread,OSThread.o,os.a,true
 SelectThread,OSThread.o,os.a,true
 __OSReschedule,OSThread.o,os.a,true
 OSYieldThread,OSThread.o,os.a,false
-OSCreateThread,OSThread.o,os.a,false
-OSExitThread,OSThread.o,os.a,false
-OSCancelThread,OSThread.o,os.a,false
-OSJoinThread,OSThread.o,os.a,false
-OSDetachThread,OSThread.o,os.a,false
-OSResumeThread,OSThread.o,os.a,false
-OSSuspendThread,OSThread.o,os.a,false
+OSCreateThread,OSThread.o,os.a,true
+OSExitThread,OSThread.o,os.a,true
+OSCancelThread,OSThread.o,os.a,true
+OSJoinThread,OSThread.o,os.a,true
+OSDetachThread,OSThread.o,os.a,true
+OSResumeThread,OSThread.o,os.a,true
+OSSuspendThread,OSThread.o,os.a,true
 OSSleepThread,OSThread.o,os.a,true
 OSWakeupThread,OSThread.o,os.a,true
 OSSetThreadPriority,OSThread.o,os.a,false

libs/RVL_SDK/data/SDK.json

@@ -1,6 +1,6 @@
 {
 	"schemaVersion": 1,
 	"label": "SDK",
-	"message": "12.442%",
+	"message": "12.862%",
 	"color": "blue"
 }

libs/RVL_SDK/docs/PROGRESS.md

@@ -22,7 +22,7 @@
 | [nand](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/nand.md) | 100.0% |
 | [net](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/net.md) | 0.0% |
 | [nwc24](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/nwc24.md) | 5.5843520782396086% |
-| [os](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/os.md) | 51.62414610344347% |
+| [os](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/os.md) | 56.62205492820298% |
 | [pad](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/pad.md) | 0.0% |
 | [rso](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/rso.md) | 0.0% |
 | [sc](https://github.com/shibbo/Petari/tree/master/libs/RVL_SDK/docs/lib/sc.md) | 59.34331240946402% |

libs/RVL_SDK/docs/lib/os.md

@@ -28,7 +28,7 @@
 | OSReset.o | 7.744107744107744% | 2 / 14 | 14.285714285714285% | :eight_pointed_black_star: 
 | OSRtc.o | 100.0% | 9 / 9 | 100.0% | :white_check_mark: 
 | OSSync.o | 100.0% | 2 / 2 | 100.0% | :white_check_mark: 
-| OSThread.o | 43.38235294117647% | 16 / 28 | 57.14285714285714% | :eight_pointed_black_star: 
+| OSThread.o | 91.31016042780749% | 23 / 28 | 82.14285714285714% | :eight_pointed_black_star: 
 | OSTime.o | 100.0% | 6 / 6 | 100.0% | :white_check_mark: 
 | OSUtf.o | 0.0% | 0 / 4 | 0.0% | :x: 
 | OSIpc.o | 100.0% | 3 / 3 | 100.0% | :white_check_mark: 
@@ -342,13 +342,13 @@
 | SelectThread | :white_check_mark: |
 | __OSReschedule | :white_check_mark: |
 | OSYieldThread | :x: |
-| OSCreateThread | :x: |
-| OSExitThread | :x: |
-| OSCancelThread | :x: |
-| OSJoinThread | :x: |
-| OSDetachThread | :x: |
-| OSResumeThread | :x: |
-| OSSuspendThread | :x: |
+| OSCreateThread | :white_check_mark: |
+| OSExitThread | :white_check_mark: |
+| OSCancelThread | :white_check_mark: |
+| OSJoinThread | :white_check_mark: |
+| OSDetachThread | :white_check_mark: |
+| OSResumeThread | :white_check_mark: |
+| OSSuspendThread | :white_check_mark: |
 | OSSleepThread | :white_check_mark: |
 | OSWakeupThread | :white_check_mark: |
 | OSSetThreadPriority | :x: |

libs/RVL_SDK/include/revolution/os/OSError.h

@@ -12,6 +12,8 @@ typedef void (*OSErrorHandler)(OSError, OSContext *, ...);
 
 OSErrorHandler OSSetErrorHandler(OSError, OSErrorHandler);
 
+extern u32 __OSFpscrEnableBits;
+
 #ifdef __cplusplus
 }
 #endif

libs/RVL_SDK/include/revolution/os/OSThread.h

@@ -72,7 +72,7 @@ BOOL OSIsThreadSuspended(OSThread *);
 BOOL OSIsThreadTerminated(OSThread *);
 
 BOOL OSCreateThread(OSThread *, void* (*func)(void*), void *, void *, u32, OSPriority, u16);
-
+void OSExitThread(void *);
 void OSCancelThread(OSThread *);
 BOOL OSJoinThread(OSThread *, void **);
 void OSDetachThread(OSThread *);

libs/RVL_SDK/source/revolution/os/OSThread.c

@@ -1,7 +1,13 @@
 #include <revolution/os.h>
 
+extern OSErrorHandler __OSErrorTable[];
+
 static void DefaultSwitchThreadCallback(OSThread *, OSThread *);
 
+#define OFFSET(n, a) (((u32) (n)) & ((a) - 1))
+#define TRUNC(n, a)  (((u32) (n)) & ~((a) - 1))
+#define ROUND(n, a)  (((u32) (n) + (a) - 1) & ~((a) - 1))
+
 static volatile u32 RunQueueBits;
 static OSThreadQueue RunQueue[32];
 static volatile BOOL RunQueueHint;
@@ -160,6 +166,22 @@ BOOL OSIsThreadTerminated(OSThread *thread) {
     return ((thread->state == 8) || (thread->state == 0)) ? TRUE : FALSE;
 }
 
+static BOOL __OSIsThreadActive(OSThread* thread) {
+    OSThread* active;
+
+    if (thread->state == 0) {
+        return FALSE;
+    }
+
+    for (active = __OSActiveThreadQueue.head; active; active = active->linkActive.next) {
+        if (thread == active) {
+            return TRUE;
+        }
+    }
+
+    return FALSE;
+}
+
 s32 OSDisableScheduler(void) {
     BOOL enabled;
     s32 count;
@@ -242,6 +264,22 @@ OSThread* SetEffectivePriority(OSThread *thread, OSPriority priority) {
     return NULL;
 }
 
+static void UpdatePriority(OSThread* thread) {
+    OSPriority priority;
+
+    do {
+        if (IsSuspended(thread->suspend)) {
+            break;
+        }
+        priority = __OSGetEffectivePriority(thread);
+
+        if (thread->priority == priority) {
+            break;
+        }
+        thread = SetEffectivePriority(thread, priority);
+    } while (thread);
+}
+
 void __OSPromoteThread(OSThread *thread, OSPriority priority) {
     do {
         if ((thread->suspend < 0) || thread->priority <= priority) {
@@ -330,19 +368,238 @@ void OSYieldThread(void) {
     OSRestoreInterrupts(enabled);
 }
 
-// OSCreateThread
-
-void OSClearStack(u8 val) {
+BOOL OSCreateThread(OSThread* thread, void* (*func)(void *), void* param, void* stack, u32 stackSize, OSPriority priority, u16 attr) {
+    BOOL enabled;
     u32 sp;
-    u32* p;
-    u32 pattern;
+    int i;
 
-    pattern = ((u32)val << 24) | ((u32)val << 16) | ((u32)val << 8) | (u32)val;
-    sp = OSGetStackPointer();
-
-    for (p = __OSCurrentThread->stackEnd + 1; p < (u32*)sp; ++p) {
-        *p = pattern;
+    if (priority < 0 || 31 < priority) {
+        return FALSE;
     }
+
+    thread->state = 1;
+    thread->attr = (u16)(attr & 1);
+    thread->priority = thread->base = priority;
+    thread->suspend = 1;
+    thread->value = (void*)-1;
+    thread->mutex = NULL;
+    OSInitThreadQueue(&thread->queueJoin);
+    OSInitMutexQueue(&thread->queueMutex);
+
+    sp = (u32)stack;
+    sp = TRUNC(sp, 8);
+    sp -= 8;
+
+    ((u32*)sp)[0] = 0;
+    ((u32*)sp)[1] = 0;
+
+    OSInitContext(&thread->context, (u32)func, sp);
+    thread->context.lr = (u32)OSExitThread;
+    thread->context.gpr[3] = (u32)param;
+    thread->stackBase = stack;
+    thread->stackEnd = (u32*)((u32)stack - stackSize);
+    *(thread->stackEnd) = 0xDEADBABE;
+    thread->error = 0;
+    for (i = 0; i < 2; ++i) {
+        thread->specific[i] = 0;
+    }
+
+    enabled = OSDisableInterrupts();
+    if (__OSErrorTable[16] != NULL) {
+        thread->context.srr1 |= 0x900;
+        thread->context.state |= 1;
+        thread->context.fpscr = (__OSFpscrEnableBits & 248) | 4;
+
+        for (i = 0; i < 32; ++i) {
+            *(u64*) &thread->context.fpr[i] = (u64)0xffffffffffffffffLL;
+            *(u64*) &thread->context.psf[i] = (u64) 0xffffffffffffffffLL;
+        }
+    }
+
+    EnqueueTail(&__OSActiveThreadQueue, thread, linkActive);
+    OSRestoreInterrupts(enabled);
+    return TRUE;
+}
+
+void OSExitThread(void* val) {
+    BOOL enabled;
+    OSThread* currentThread;
+
+    enabled = OSDisableInterrupts();
+    currentThread = OSGetCurrentThread();
+    OSClearContext(&currentThread->context);
+
+    if (currentThread->attr & 1) {
+        DequeueItem(&__OSActiveThreadQueue, currentThread, linkActive);
+        currentThread->state = 0;
+    }
+    else {
+        currentThread->state = 8;
+        currentThread->value = val;
+    }
+
+    __OSUnlockAllMutex(currentThread);
+    OSWakeupThread(&currentThread->queueJoin);
+    RunQueueHint = TRUE;
+    __OSReschedule();
+    OSRestoreInterrupts(enabled);
+}
+
+void OSCancelThread(OSThread* thread) {
+    BOOL enabled = OSDisableInterrupts();
+
+    switch (thread->state) {
+      case OS_THREAD_STATE_READY:
+        if (!IsSuspended(thread->suspend)) {
+            UnsetRun(thread);
+        }
+        break;
+
+      case OS_THREAD_STATE_RUNNING:
+        RunQueueHint = TRUE;
+        break;
+
+      case OS_THREAD_STATE_WAITING:
+        DequeueItem(thread->queue, thread, link);
+        thread->queue = NULL;
+        if (!IsSuspended(thread->suspend) && thread->mutex) {
+            ASSERT(thread->mutex->thread);
+            UpdatePriority(thread->mutex->thread);
+        }
+        break;
+
+      default:
+        OSRestoreInterrupts(enabled);
+        return;
+    }
+
+    OSClearContext(&thread->context);
+    if (thread->attr & 1) {
+        DequeueItem(&__OSActiveThreadQueue, thread, linkActive);
+        thread->state = 0;
+    } 
+    else {
+        thread->state = 8;
+    }
+
+    __OSUnlockAllMutex(thread);
+    OSWakeupThread(&thread->queueJoin);
+    __OSReschedule();
+    OSRestoreInterrupts(enabled);
+}
+
+BOOL OSJoinThread(OSThread* thread, void** val) {
+    BOOL enabled = OSDisableInterrupts();
+
+    if (!(thread->attr & 1) && thread->state != OS_THREAD_STATE_MORIBUND && thread->queueJoin.head == NULL) {
+        OSSleepThread(&thread->queueJoin);
+        if (!__OSIsThreadActive(thread)) {
+            OSRestoreInterrupts(enabled);
+            return FALSE;
+        }
+    }
+
+    if (((volatile OSThread*) thread)->state == OS_THREAD_STATE_MORIBUND) {
+        if (val) {
+            *val = thread->value;
+        }
+
+        DequeueItem(&__OSActiveThreadQueue, thread, linkActive);
+        thread->state = 0;
+        OSRestoreInterrupts(enabled);
+        return TRUE;
+    }
+
+    OSRestoreInterrupts(enabled);
+    return FALSE;
+}
+
+void OSDetachThread(OSThread* thread)
+{
+    BOOL enabled = OSDisableInterrupts();
+
+    thread->attr |= 1;
+    if (thread->state == OS_THREAD_STATE_MORIBUND) {
+        DequeueItem(&__OSActiveThreadQueue, thread, linkActive);
+        thread->state = 0;
+    }
+
+    OSWakeupThread(&thread->queueJoin);
+    OSRestoreInterrupts(enabled);
+}
+
+s32 OSResumeThread(OSThread* thread) {
+    BOOL enabled;
+    s32  suspendCount;
+
+    enabled = OSDisableInterrupts();
+
+    suspendCount = thread->suspend--;
+
+    if (thread->suspend < 0) {
+        thread->suspend = 0;
+    }
+    else if (thread->suspend == 0) {
+        switch (thread->state) {
+          case OS_THREAD_STATE_READY:
+            thread->priority = __OSGetEffectivePriority(thread);
+            SetRun(thread);
+            break;
+
+          case OS_THREAD_STATE_WAITING:
+            ASSERT(thread->queue);
+            DequeueItem(thread->queue, thread, link);
+            thread->priority = __OSGetEffectivePriority(thread);
+            EnqueuePrio(thread->queue, thread, link);
+            if (thread->mutex) {
+                UpdatePriority(thread->mutex->thread);
+            }
+            break;
+        }
+
+        __OSReschedule();
+    }
+
+    OSRestoreInterrupts(enabled);
+    return suspendCount;
+}
+
+s32 OSSuspendThread(OSThread* thread)
+{
+    BOOL enabled;
+    s32  suspendCount;
+
+    enabled = OSDisableInterrupts();
+    suspendCount = thread->suspend++;
+
+    if (suspendCount == 0) {
+        switch (thread->state) {
+          case OS_THREAD_STATE_RUNNING:
+            RunQueueHint = TRUE;
+            thread->state = OS_THREAD_STATE_READY;
+            break;
+
+          case OS_THREAD_STATE_READY:
+            UnsetRun(thread);
+            break;
+
+          case OS_THREAD_STATE_WAITING:
+            DequeueItem(thread->queue, thread, link);
+            thread->priority = 32;
+            EnqueueTail(thread->queue, thread, link);
+
+            if (thread->mutex) {
+                ASSERT(thread->mutex->thread);
+                UpdatePriority(thread->mutex->thread);
+            }
+            break;
+        }
+
+        __OSReschedule();
+    }
+
+    OSRestoreInterrupts(enabled);
+    return suspendCount;
 }
 
 void OSSleepThread(OSThreadQueue* queue) {
@@ -376,4 +633,17 @@ void OSWakeupThread(OSThreadQueue* queue) {
 
     __OSReschedule();
     OSRestoreInterrupts(enabled);
+}
+
+void OSClearStack(u8 val) {
+    u32 sp;
+    u32* p;
+    u32 pattern;
+
+    pattern = ((u32)val << 24) | ((u32)val << 16) | ((u32)val << 8) | (u32)val;
+    sp = OSGetStackPointer();
+
+    for (p = __OSCurrentThread->stackEnd + 1; p < (u32*)sp; ++p) {
+        *p = pattern;
+    }
 }