8 files changed, 188 insertions, 0 deletions
diff --git a/lib/ultralib/src/debug/osint_debug.h b/lib/ultralib/src/debug/osint_debug.h
new file mode 100644
index 0000000..b493035
--- /dev/null
+++ b/lib/ultralib/src/debug/osint_debug.h
@@ -0,0 +1,11 @@
+#include "PR/os_internal.h"
+
+extern s32 __osThprofFlag;
+extern void (*__osThprofFunc)(OSThread*);
+extern u32 __osThprofLastTimer;
+extern u32 __osThprofCount;
+extern __OSThreadprofile_s thprof[THPROF_IDMAX];
+extern u64 __osThprofHeap[THPROF_STACKSIZE];
+extern void* __osThprofStack;
+
+void osThreadProfileCallback(OSThread*);
diff --git a/lib/ultralib/src/debug/threadprofile.c b/lib/ultralib/src/debug/threadprofile.c
new file mode 100644
index 0000000..e383773
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofile.c
@@ -0,0 +1,22 @@
+#include "osint_debug.h"
+
+#include "macros.h"
+
+s32 __osThprofFlag = 0;
+void (*__osThprofFunc)(OSThread*) = NULL;
+
+u32 __osThprofLastTimer;
+u32 __osThprofCount;
+__OSThreadprofile_s thprof[THPROF_IDMAX] ALIGNED(0x10);
+u64 __osThprofHeap[THPROF_STACKSIZE] ALIGNED(0x10);
+void* __osThprofStack;
+
+void osThreadProfileCallback(OSThread* osthread) {
+    register u32 now_time = osGetCount();
+    __OSThreadprofile_s* thprof = osthread->thprof;
+
+    thprof->time += now_time - __osThprofLastTimer;
+    thprof->count++;
+    __osThprofCount++;
+    __osThprofLastTimer = now_time;
+}
diff --git a/lib/ultralib/src/debug/threadprofileclear.c b/lib/ultralib/src/debug/threadprofileclear.c
new file mode 100644
index 0000000..42788f8
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofileclear.c
@@ -0,0 +1,11 @@
+#include "osint_debug.h"
+
+void osThreadProfileClear(OSId id) {
+    register OSIntMask saveMask = __osDisableInt();
+
+    thprof[id].flag = 0;
+    thprof[id].count = 0;
+    thprof[id].time = 0;
+
+    __osRestoreInt(saveMask);
+}
diff --git a/lib/ultralib/src/debug/threadprofileinit.c b/lib/ultralib/src/debug/threadprofileinit.c
new file mode 100644
index 0000000..7fef229
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofileinit.c
@@ -0,0 +1,16 @@
+#include "osint_debug.h"
+
+void osThreadProfileInit(void) {
+    register u32 saveMask = __osDisableInt();
+    OSId i;
+
+    __osThprofFunc = NULL;
+    __osThprofFlag = 1;
+    __osThprofCount = 0;
+    __osThprofStack = &__osThprofHeap[(THPROF_STACKSIZE - 8) / sizeof(*__osThprofHeap)];
+    __osRestoreInt(saveMask);
+
+    for (i = 0; i < THPROF_IDMAX; i++) {
+        osThreadProfileClear(i);
+    }
+}
diff --git a/lib/ultralib/src/debug/threadprofilereadcount.c b/lib/ultralib/src/debug/threadprofilereadcount.c
new file mode 100644
index 0000000..c12d313
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofilereadcount.c
@@ -0,0 +1,31 @@
+#include "osint_debug.h"
+
+u32 osThreadProfileReadCount(OSId id) {
+    if (!__osThprofFlag) {
+        __osError(139, 0);
+        return 0;
+    }
+
+    if (id >= THPROF_IDMAX) {
+        __osError(143, 1, id);
+        return 0;
+    }
+    return thprof[id].count;
+}
+
+u32 osThreadProfileReadCountTh(OSThread* thread) {
+    OSId id;
+
+    if (!__osThprofFlag) {
+        __osError(141, 0);
+        return 0;
+    }
+
+    id = osGetThreadId(thread);
+
+    if (id >= THPROF_IDMAX) {
+        __osError(145, 1, id);
+        return 0;
+    }
+    return thprof[id].count;
+}
diff --git a/lib/ultralib/src/debug/threadprofilereadtime.c b/lib/ultralib/src/debug/threadprofilereadtime.c
new file mode 100644
index 0000000..c5c4cca
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofilereadtime.c
@@ -0,0 +1,43 @@
+#include "osint_debug.h"
+
+OSTime osThreadProfileReadTime(OSId id) {
+    OSTime adjust = 0;
+    u32 now_time = osGetCount();
+
+    if (!__osThprofFlag) {
+        __osError(140, 0);
+        return 0;
+    }
+    if (id >= THPROF_IDMAX) {
+        __osError(144, 1, id);
+        return 0;
+    }
+
+    if (id == osGetThreadId(NULL) && __osThprofFunc != NULL) {
+        adjust = now_time - __osThprofLastTimer;
+    }
+    return thprof[id].time + adjust;
+}
+
+OSTime osThreadProfileReadTimeTh(OSThread* thread) {
+    OSId id;
+    OSTime adjust = 0;
+    u32 now_time = osGetCount();
+
+    if (!__osThprofFlag) {
+        __osError(142, 0);
+        return 0;
+    }
+
+    id = osGetThreadId(thread);
+
+    if (id >= THPROF_IDMAX) {
+        __osError(146, 1, id);
+        return 0;
+    }
+
+    if (id == osGetThreadId(NULL) && __osThprofFunc != NULL) {
+        adjust = now_time - __osThprofLastTimer;
+    }
+    return thprof[id].time + adjust;
+}
diff --git a/lib/ultralib/src/debug/threadprofilestart.c b/lib/ultralib/src/debug/threadprofilestart.c
new file mode 100644
index 0000000..0891ad7
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofilestart.c
@@ -0,0 +1,20 @@
+#include "osint_debug.h"
+
+void osThreadProfileStart(void) {
+    register u32 saveMask;
+    
+    if (!__osThprofFlag) {
+        __osError(136, 0);
+        return;
+    }
+    if (__osThprofFunc != NULL) {
+        __osError(137, 0);
+        return;
+    }
+
+    saveMask = __osDisableInt();
+
+    __osThprofLastTimer = osGetCount();
+    __osThprofFunc = osThreadProfileCallback;
+    __osRestoreInt(saveMask);
+}
diff --git a/lib/ultralib/src/debug/threadprofilestop.c b/lib/ultralib/src/debug/threadprofilestop.c
new file mode 100644
index 0000000..666d7cd
--- /dev/null
+++ b/lib/ultralib/src/debug/threadprofilestop.c
@@ -0,0 +1,34 @@
+#include "osint_debug.h"
+
+void osThreadProfileStop(void) {
+    register u32 saveMask;
+    int i;
+    OSId id;
+    u32 now_time = osGetCount();
+
+    saveMask = __osDisableInt();
+
+    if (__osThprofFlag == 0) {
+        __osRestoreInt(saveMask);
+        __osError(138, 0);
+        return;
+    }
+
+    if (__osThprofFunc != NULL) {
+        id = osGetThreadId(0);
+
+        if (id < THPROF_IDMAX) {
+            thprof[id].time += now_time - __osThprofLastTimer;
+        } else {
+            __osRestoreInt(saveMask);
+            __osError(147, 1, id);
+            saveMask = __osDisableInt();
+        }
+    }
+    __osThprofFunc = NULL;
+
+    for (i = 0; i < THPROF_IDMAX; i++) {
+        thprof[i].flag = 0;
+    }
+    __osRestoreInt(saveMask);
+}