Fault and Fault_Drawer (3 MATCHING and 1 NON_MATCHING) (#434)

* Matched last functions except Fault_FindNextStackCall

* Bss in drawer and cleanup

* Update define and format

* PR comments

* Formating PR suggestions

* Small format

* Fault_FindNextStackCall

* Change grays to dark gray and light gray

2 files changed, 149 insertions, 77 deletions

diff --git a/src/boot_O2_g3/fault.c b/src/boot_O2_g3/fault.c
index 4c9a1a28c..c5af62f82 100644
--- a/src/boot_O2_g3/fault.c
+++ b/src/boot_O2_g3/fault.c
@@ -575,66 +575,59 @@ void Fault_DrawMemDump(u32 pc, u32 sp, u32 unk0, u32 unk1) {
     sFaultContext->faultActive = 1;
 }
 
-#ifdef NON_MATCHING
-// This function still needs a bit of work
-void Fault_FindNextStackCall(u32** sp, u32** pc, u32** ra) {
-    u32* currentSp;
-    u32* currentPc;
-    u32* currentRa;
-    u32 lastInst;
-    u32 currInst;
-
-    currentSp = *sp;
-    currentPc = *pc;
-    currentRa = *ra;
-
-    if ((((u32)currentSp & 3) != 0) || (currentSp < (u32*)0x80000000) || (currentSp >= (u32*)0xC0000000) ||
-        (((u32)currentRa & 3) != 0) || (currentRa < (u32*)0x80000000) || (currentRa >= (u32*)0xC0000000)) {
-        *sp = NULL;
-        *pc = NULL;
-        *ra = NULL;
+void Fault_FindNextStackCall(uintptr_t* spPtr, uintptr_t* pcPtr, uintptr_t* raPtr) {
+    uintptr_t sp = *spPtr;
+    uintptr_t pc = *pcPtr;
+    uintptr_t ra = *raPtr;
+    u32 lastOpc;
+    u16 opcHi;
+    s16 opcLo;
+    u32 imm;
+
+    if (sp & 3 || sp < 0x80000000 || sp >= 0xC0000000 || ra & 3 || ra < 0x80000000 || ra >= 0xC0000000) {
+        *spPtr = 0;
+        *pcPtr = 0;
+        *raPtr = 0;
         return;
     }
 
-    if ((((u32)currentPc & 3) != 0) || (currentPc < (u32*)0x80000000) || (currentPc >= (u32*)0xC0000000)) {
-        *pc = currentRa;
+    if (pc & 3 || pc < 0x80000000 || pc >= 0xC0000000) {
+        *pcPtr = ra;
         return;
     }
 
-    lastInst = 0;
-    while (1) {
-        currInst = *currentPc;
-        if (((currInst >> 0x10) & 0xFFFF) == 0x8FBF) {
-            currentRa = *(u32**)((u32)currentSp + (s16)currInst);
-        } else if (((currInst >> 0x10) & 0xFFFF) == 0x27BD) {
-            currentSp = (u32*)((u32)currentSp + (s16)currInst);
-        } else if (currInst == 0x42000018) {
-            currentSp = NULL;
-            currentPc = NULL;
-            currentRa = NULL;
-            break;
-        }
+    lastOpc = 0;
+    while (true) {
+        opcHi = *(uintptr_t*)pc >> 16;
+        opcLo = *(uintptr_t*)pc & 0xFFFF;
+        imm = opcLo;
 
-        if (lastInst == 0x03E00008) {
-            break;
+        if (opcHi == 0x8FBF) {
+            ra = *(uintptr_t*)(sp + imm);
+        } else if (opcHi == 0x27BD) {
+            sp += imm;
+        } else if (*(uintptr_t*)pc == 0x42000018) {
+            sp = 0;
+            pc = 0;
+            ra = 0;
+            goto end;
         }
-
-        if ((lastInst >> 0x1A) == 2) {
-            currentPc = (u32*)((((u32)currentPc >> 0x1C) << 0x1C) | ((lastInst << 6) >> 4));
-            break;
+        if (lastOpc == 0x3E00008) {
+            pc = ra;
+            goto end;
+        } else if ((lastOpc >> 26) == 2) {
+            pc = pc >> 28 << 28 | lastOpc << 6 >> 4;
+            goto end;
         }
-
-        lastInst = currInst;
-        currentPc++;
+        lastOpc = *(uintptr_t*)pc;
+        pc += 4;
     }
 
-    *sp = currentSp;
-    *pc = currentPc;
-    *ra = currentRa;
+end:
+    *spPtr = sp;
+    *pcPtr = pc;
+    *raPtr = ra;
 }
-#else
-#pragma GLOBAL_ASM("asm/non_matchings/boot/fault/Fault_FindNextStackCall.s")
-#endif
 
 void Fault_DrawStackTrace(OSThread* t, u32 flags) {
     s32 y;
@@ -664,7 +657,7 @@ void Fault_DrawStackTrace(OSThread* t, u32 flags) {
             FaultDrawer_Printf(" -> ????????");
         }
 
-        Fault_FindNextStackCall((u32**)&sp, (u32**)&pc, (u32**)&ra);
+        Fault_FindNextStackCall(&sp, &pc, &ra);
     }
 }
 
@@ -695,7 +688,7 @@ void osSyncPrintfStackTrace(OSThread* t, u32 flags) {
         }
         osSyncPrintf("\n");
 
-        Fault_FindNextStackCall((u32**)&sp, (u32**)&pc, (u32**)&ra);
+        Fault_FindNextStackCall(&sp, &pc, &ra);
     }
 }
 
@@ -751,32 +744,33 @@ void Fault_ProcessClients(void) {
 }
 
 #ifdef NON_MATCHING
-// regalloc and ordering differences around the two bool variables (faultCustomOptions and faultCopyToLog)
+// needs in-function static bss
 void Fault_SetOptionsFromController3(void) {
-    Input* input3;
+    static u32 faultCustomOptions;
+    Input* input3 = &sFaultContext->padInput[3];
     u32 pad;
     u32 graphPC;
     u32 graphRA;
     u32 graphSP;
 
-    input3 = &sFaultContext->padInput[3];
-
     if (CHECK_BTN_ALL(input3->press.button, 0x80)) {
-        faultCustomOptions = faultCustomOptions == 0;
+        faultCustomOptions = !faultCustomOptions;
     }
 
     if (faultCustomOptions) {
         graphPC = sGraphThread.context.pc;
         graphRA = sGraphThread.context.ra;
         graphSP = sGraphThread.context.sp;
-        if (CHECK_BTN_ALL(input3->press.button, BTN_R)) {
+        if (CHECK_BTN_ALL(input3->cur.button, BTN_R)) {
+            static u32 faultCopyToLog;
+
             faultCopyToLog = !faultCopyToLog;
             FaultDrawer_SetOsSyncPrintfEnabled(faultCopyToLog);
         }
-        if (CHECK_BTN_ALL(input3->press.button, BTN_A)) {
+        if (CHECK_BTN_ALL(input3->cur.button, BTN_A)) {
             osSyncPrintf("GRAPH PC=%08x RA=%08x STACK=%08x\n", graphPC, graphRA, graphSP);
         }
-        if (CHECK_BTN_ALL(input3->press.button, BTN_B)) {
+        if (CHECK_BTN_ALL(input3->cur.button, BTN_B)) {
             FaultDrawer_SetDrawerFB(osViGetNextFramebuffer(), 0x140, 0xF0);
             Fault_DrawRec(0, 0xD7, 0x140, 9, 1);
             FaultDrawer_SetCharPad(-2, 0);
diff --git a/src/boot_O2_g3/fault_drawer.c b/src/boot_O2_g3/fault_drawer.c
index 41f6c1762..eacfc56d1 100644
--- a/src/boot_O2_g3/fault_drawer.c
+++ b/src/boot_O2_g3/fault_drawer.c
@@ -3,9 +3,11 @@
 
 extern const u32 sFaultDrawerFont[];
 
+FaultDrawer sFaultDrawerStruct;
+
 FaultDrawer* sFaultDrawContext = &sFaultDrawerStruct;
 FaultDrawer sFaultDrawerDefault = {
-    (u16*)FAULT_FB_ADDRESS,             // fb
+    FAULT_FB_ADDRESS,                   // fb
     SCREEN_WIDTH,                       // w
     SCREEN_HEIGHT,                      // h
     16,                                 // yStart
@@ -16,23 +18,23 @@ FaultDrawer sFaultDrawerDefault = {
     GPACK_RGBA5551(0, 0, 0, 0),         // backColor
     22,                                 // cursorX
     16,                                 // cursorY
-    (u32*)sFaultDrawerFont,             // font
+    sFaultDrawerFont,                   // font
     8,                                  // charW
     8,                                  // charH
     0,                                  // charWPad
-    0,                                  //  charHPad
+    0,                                  // charHPad
     {
         // printColors
-        GPACK_RGBA5551(0, 0, 0, 1),
-        GPACK_RGBA5551(255, 0, 0, 1),
-        GPACK_RGBA5551(0, 255, 0, 1),
-        GPACK_RGBA5551(255, 255, 0, 1),
-        GPACK_RGBA5551(0, 0, 255, 1),
-        GPACK_RGBA5551(255, 0, 255, 1),
-        GPACK_RGBA5551(0, 255, 255, 1),
-        GPACK_RGBA5551(255, 255, 255, 1),
-        GPACK_RGBA5551(120, 120, 120, 1),
-        GPACK_RGBA5551(176, 176, 176, 1),
+        GPACK_RGBA5551(0, 0, 0, 1),       // BLACK
+        GPACK_RGBA5551(255, 0, 0, 1),     // RED
+        GPACK_RGBA5551(0, 255, 0, 1),     // GREEN
+        GPACK_RGBA5551(255, 255, 0, 1),   // YELLOW
+        GPACK_RGBA5551(0, 0, 255, 1),     // BLUE
+        GPACK_RGBA5551(255, 0, 255, 1),   // MAGENTA
+        GPACK_RGBA5551(0, 255, 255, 1),   // CYAN
+        GPACK_RGBA5551(255, 255, 255, 1), // WHITE
+        GPACK_RGBA5551(120, 120, 120, 1), // DARK GRAY
+        GPACK_RGBA5551(176, 176, 176, 1), // LIGHT GRAY
     },
     0,    // escCode
     0,    // osSyncPrintfEnabled
@@ -47,7 +49,8 @@ void FaultDrawer_SetOsSyncPrintfEnabled(u32 enabled) {
 
 void FaultDrawer_DrawRecImpl(s32 xStart, s32 yStart, s32 xEnd, s32 yEnd, u16 color) {
     u16* fb;
-    s32 x, y;
+    s32 x;
+    s32 y;
     s32 xDiff = sFaultDrawContext->w - xStart;
     s32 yDiff = sFaultDrawContext->h - yStart;
     s32 xSize = xEnd - xStart + 1;
@@ -74,7 +77,37 @@ void FaultDrawer_DrawRecImpl(s32 xStart, s32 yStart, s32 xEnd, s32 yEnd, u16 col
     }
 }
 
-#pragma GLOBAL_ASM("asm/non_matchings/boot/fault_drawer/FaultDrawer_DrawChar.s")
+void FaultDrawer_DrawChar(char c) {
+    s32 x;
+    s32 y;
+    u32 data;
+    s32 cursorX = sFaultDrawContext->cursorX;
+    s32 cursorY = sFaultDrawContext->cursorY;
+    s32 shift = c % 4;
+    const u32* dataPtr = &sFaultDrawContext->font[(((c / 8) * 16) + ((c & 4) >> 2))];
+    u16* fb = sFaultDrawContext->fb + (sFaultDrawContext->w * cursorY) + cursorX;
+
+    if ((sFaultDrawContext->xStart <= cursorX) &&
+        ((sFaultDrawContext->charW + cursorX - 1) <= sFaultDrawContext->xEnd) &&
+        (sFaultDrawContext->yStart <= cursorY) &&
+        ((sFaultDrawContext->charH + cursorY - 1) <= sFaultDrawContext->yEnd)) {
+        for (y = 0; y < sFaultDrawContext->charH; y++) {
+            u32 mask = 0x10000000 << shift;
+
+            data = *dataPtr;
+            for (x = 0; x < sFaultDrawContext->charW; x++) {
+                if (mask & data) {
+                    fb[x] = sFaultDrawContext->foreColor;
+                } else if (sFaultDrawContext->backColor & 1) {
+                    fb[x] = sFaultDrawContext->backColor;
+                }
+                mask >>= 4;
+            }
+            fb += sFaultDrawContext->w;
+            dataPtr += 2;
+        }
+    }
+}
 
 s32 FaultDrawer_ColorToPrintColor(u16 color) {
     s32 i;
@@ -142,12 +175,57 @@ void FaultDrawer_FillScreen() {
     FaultDrawer_SetCursor(sFaultDrawContext->xStart, sFaultDrawContext->yStart);
 }
 
-#pragma GLOBAL_ASM("asm/non_matchings/boot/fault_drawer/FaultDrawer_FormatStringFunc.s")
+void* FaultDrawer_FormatStringFunc(void* arg, const char* str, size_t count) {
+    for (; count != 0; count--, str++) {
+        if (sFaultDrawContext->escCode) {
+            sFaultDrawContext->escCode = false;
+            if (*str >= '1' && *str <= '9') {
+                FaultDrawer_SetForeColor(sFaultDrawContext->printColors[*str - '0']);
+            }
+        } else {
+            switch (*str) {
+                case '\n':
+                    if (sFaultDrawContext->osSyncPrintfEnabled) {
+                        osSyncPrintf("\n");
+                    }
+
+                    sFaultDrawContext->cursorX = sFaultDrawContext->w;
+                    break;
+                case '\x1A':
+                    sFaultDrawContext->escCode = true;
+                    break;
+                default:
+                    if (sFaultDrawContext->osSyncPrintfEnabled) {
+                        osSyncPrintf("%c", *str);
+                    }
+
+                    FaultDrawer_DrawChar(*str);
+                    sFaultDrawContext->cursorX += sFaultDrawContext->charW + sFaultDrawContext->charWPad;
+            }
+        }
+
+        if (sFaultDrawContext->cursorX >= (sFaultDrawContext->xEnd - sFaultDrawContext->charW)) {
+            sFaultDrawContext->cursorX = sFaultDrawContext->xStart;
+            sFaultDrawContext->cursorY += sFaultDrawContext->charH + sFaultDrawContext->charHPad;
+            if (sFaultDrawContext->yEnd - sFaultDrawContext->charH <= sFaultDrawContext->cursorY) {
+                if (sFaultDrawContext->inputCallback != NULL) {
+                    sFaultDrawContext->inputCallback();
+                    FaultDrawer_FillScreen();
+                }
+                sFaultDrawContext->cursorY = sFaultDrawContext->yStart;
+            }
+        }
+    }
+
+    osWritebackDCacheAll();
+
+    return arg;
+}
 
 const char D_80099080[] = "(null)";
 
-void FaultDrawer_VPrintf(const char* str, char* args) { // va_list
-    _Printf((PrintCallback)FaultDrawer_FormatStringFunc, sFaultDrawContext, str, args);
+void FaultDrawer_VPrintf(const char* fmt, va_list ap) {
+    _Printf(FaultDrawer_FormatStringFunc, sFaultDrawContext, fmt, ap);
 }
 
 void FaultDrawer_Printf(const char* fmt, ...) {
@@ -175,7 +253,7 @@ void FaultDrawer_SetDrawerFB(void* fb, u16 w, u16 h) {
     sFaultDrawContext->h = h;
 }
 
-void FaultDrawer_SetInputCallback(void* callback) {
+void FaultDrawer_SetInputCallback(FaultDrawerCallback callback) {
     sFaultDrawContext->inputCallback = callback;
 }