libultra cleanup (#215)

* cleanup libultra * fixes - use quotes instead of <> for includes - add macros for zelda specific thread priorities - fix Makefile - properly format the remaining pfs structs * fix button macros + add CHECK_BTN_ANY/CHECK_BTN_ALL * remove ULTRA_ABS * fix includes * update z_player.c/z_lib.c + run format.sh * merge upstream/master * fix include in En_Goroiwa * fix includes
2025-08-12 18:01:16 +00:00 · 2020-10-03 17:22:44 +02:00 · 2020-10-03 17:22:44 +02:00 · 174af7384d
commit 174af7384d
parent 6136ee6deb
890 changed files with 2628 additions and 5625 deletions
--- a/src/libultra_code_O2/__osContAddressCrc.c
+++ b/src/libultra_code_O2/__osContAddressCrc.c
@ -0,0 +1,65 @@
+#include "global.h"
+
+// Valid addr up to 0x7FF
+// It's the address of a block of 0x20 bytes in the mempak
+// So that means the whole mempak has a 16-bit address space
+u8 __osContAddressCrc(u16 addr) {
+    u32 addr32 = addr;
+    u32 ret = 0;
+    u32 bit;
+    s32 i;
+
+    for (bit = 0x400; bit; bit >>= 1) {
+        ret <<= 1;
+        if (addr32 & bit) {
+            if (ret & 0x20) {
+                ret ^= 0x14;
+            } else {
+                ++ret;
+            }
+        } else {
+            if (ret & 0x20) {
+                ret ^= 0x15;
+            }
+        }
+    }
+    for (i = 0; i < 5; ++i) {
+        ret <<= 1;
+        if (ret & 0x20) {
+            ret ^= 0x15;
+        }
+    }
+    return ret & 0x1f;
+}
+
+u8 __osContDataCrc(u8* data) {
+    s32 ret;
+    u32 bit;
+    u32 byte;
+
+    ret = 0;
+    for (byte = 0x20; byte; --byte, ++data) {
+        for (bit = 0x80; bit; bit >>= 1) {
+            ret <<= 1;
+            if ((*data & bit) != 0) {
+                if ((ret & 0x100) != 0) {
+                    ret ^= 0x84;
+                } else {
+                    ++ret;
+                }
+            } else {
+                if (ret & 0x100) {
+                    ret ^= 0x85;
+                }
+            }
+        }
+    }
+    do {
+        ret <<= 1;
+        if (ret & 0x100) {
+            ret ^= 0x85;
+        }
+        ++byte;
+    } while (byte < 8U);
+    return ret;
+}
--- a/src/libultra_code_O2/__osContRamRead.c
+++ b/src/libultra_code_O2/__osContRamRead.c
@ -0,0 +1,60 @@
+#include "global.h"
+
+#define BLOCKSIZE 32
+
+s32 __osPfsLastChannel = -1;
+
+s32 __osContRamRead(OSMesgQueue* ctrlrqueue, s32 channel, u16 addr, u8* data) {
+    s32 ret;
+    s32 i;
+    u8* bufptr;
+    s32 retryCount = 2;
+
+    __osSiGetAccess();
+    do {
+        bufptr = &pifMempakBuf;
+        if ((__osContLastPoll != 2) || (__osPfsLastChannel != channel)) {
+
+            __osContLastPoll = 2;
+            __osPfsLastChannel = channel;
+            // clang-format off
+            for (i = 0; i < channel; i++) { *bufptr++ = 0; }
+            // clang-format on
+            pifMempakBuf.status = 1;
+            ((__OSContRamHeader*)bufptr)->unk_00 = 0xFF;
+            ((__OSContRamHeader*)bufptr)->txsize = 3;
+            ((__OSContRamHeader*)bufptr)->rxsize = 0x21;
+            ((__OSContRamHeader*)bufptr)->poll = CONT_CMD_READ_MEMPACK; // read mempak; send byte 0
+            ((__OSContRamHeader*)bufptr)->datacrc = 0xFF;               // read mempak; send byte 0
+            // Received bytes are 6-26 inclusive
+            bufptr[sizeof(__OSContRamHeader)] = CONT_CMD_END; // End of commands
+        } else {
+            bufptr += channel;
+        }
+        ((__OSContRamHeader*)bufptr)->hi = addr >> 3;                                    // send byte 1
+        ((__OSContRamHeader*)bufptr)->lo = (s8)(__osContAddressCrc(addr) | (addr << 5)); // send byte 2
+        __osSiRawStartDma(OS_WRITE, &pifMempakBuf);
+        osRecvMesg(ctrlrqueue, NULL, OS_MESG_BLOCK);
+        __osSiRawStartDma(OS_READ, &pifMempakBuf);
+        osRecvMesg(ctrlrqueue, NULL, OS_MESG_BLOCK);
+        ret = (((__OSContRamHeader*)bufptr)->rxsize & 0xC0) >> 4;
+        if (!ret) {
+            if (((__OSContRamHeader*)bufptr)->datacrc != __osContDataCrc(bufptr + 6)) {
+                ret = __osPfsGetStatus(ctrlrqueue, channel);
+                if (ret) {
+                    break;
+                }
+                ret = 4; // Retry
+            } else {
+                bcopy(bufptr + 6, data, BLOCKSIZE);
+            }
+        } else {
+            ret = 1; // Error
+        }
+        if (ret != 4) {
+            break;
+        }
+    } while (0 <= retryCount--);
+    __osSiRelAccess();
+    return ret;
+}
--- a/src/libultra_code_O2/__osContRamWrite.c
+++ b/src/libultra_code_O2/__osContRamWrite.c
@ -0,0 +1,67 @@
+#include "ultra64.h"
+#include "global.h"
+
+s32 __osContRamWrite(OSMesgQueue* mq, s32 channel, u16 address, u8* buffer, s32 force) {
+    s32 ret = 0;
+    s32 i;
+    u8* ptr;
+    s32 retry = 2;
+    u8 crc;
+
+    if ((force != PFS_FORCE) && (address < PFS_LABEL_AREA) && (address != 0)) {
+        return 0;
+    }
+
+    __osSiGetAccess();
+
+    do {
+        ptr = (u8*)(&pifMempakBuf);
+
+        if (__osContLastPoll != CONT_CMD_WRITE_MEMPACK || __osPfsLastChannel != channel) {
+            __osContLastPoll = CONT_CMD_WRITE_MEMPACK;
+            __osPfsLastChannel = channel;
+
+            // clang-format off
+            for (i = 0; i < channel; i++) { *ptr++ = 0; }
+            // clang-format on
+
+            pifMempakBuf.status = 1;
+
+            ((__OSContRamHeader*)ptr)->unk_00 = 0xff;
+            ((__OSContRamHeader*)ptr)->txsize = 35;
+            ((__OSContRamHeader*)ptr)->rxsize = 1;
+            ((__OSContRamHeader*)ptr)->poll = CONT_CMD_WRITE_MEMPACK;
+            ((__OSContRamHeader*)ptr)->datacrc = 0xff;
+
+            ptr[sizeof(__OSContRamHeader)] = CONT_CMD_END;
+        } else {
+            ptr += channel;
+        }
+        ((__OSContRamHeader*)ptr)->hi = address >> 3;
+        ((__OSContRamHeader*)ptr)->lo = ((address << 5) | __osContAddressCrc(address));
+
+        bcopy(buffer, ((__OSContRamHeader*)ptr)->data, BLOCKSIZE);
+
+        ret = __osSiRawStartDma(OS_WRITE, &pifMempakBuf);
+        crc = __osContDataCrc(buffer);
+        osRecvMesg(mq, (OSMesg*)NULL, OS_MESG_BLOCK);
+
+        ret = __osSiRawStartDma(OS_READ, &pifMempakBuf);
+        osRecvMesg(mq, (OSMesg*)NULL, OS_MESG_BLOCK);
+
+        ret = ((((__OSContRamHeader*)ptr)->rxsize & 0xC0) >> 4);
+        if (!ret) {
+            if (crc != ((__OSContRamHeader*)ptr)->datacrc) {
+                if ((ret = __osPfsGetStatus(mq, channel))) {
+                    break;
+                } else {
+                    ret = PFS_ERR_CONTRFAIL;
+                }
+            }
+        } else {
+            ret = PFS_ERR_NOPACK;
+        }
+    } while ((ret == PFS_ERR_CONTRFAIL) && (retry-- >= 0));
+    __osSiRelAccess();
+    return ret;
+}
--- a/src/libultra_code_O2/__osGetActiveQueue.c
+++ b/src/libultra_code_O2/__osGetActiveQueue.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+OSThread* __osGetActiveQueue() {
+    return __osActiveQueue;
+}
--- a/src/libultra_code_O2/__osGetCurrFaultedThread.c
+++ b/src/libultra_code_O2/__osGetCurrFaultedThread.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+OSThread* __osGetCurrFaultedThread() {
+    return __osFaultedThread;
+}
--- a/src/libultra_code_O2/__osPfsGetStatus.c
+++ b/src/libultra_code_O2/__osPfsGetStatus.c
@ -0,0 +1,77 @@
+#include "ultra64.h"
+#include "global.h"
+
+OSPifRam pifMempakBuf;
+
+s32 __osPfsGetStatus(OSMesgQueue* queue, s32 channel) {
+    s32 ret = 0;
+    OSMesg msg;
+    OSContStatus data;
+
+    __osPfsInodeCacheBank = 250;
+
+    __osPfsRequestOneChannel(channel, CONT_CMD_REQUEST_STATUS);
+    ret = __osSiRawStartDma(OS_WRITE, &pifMempakBuf);
+    osRecvMesg(queue, &msg, OS_MESG_BLOCK);
+
+    ret = __osSiRawStartDma(OS_READ, &pifMempakBuf);
+    osRecvMesg(queue, &msg, OS_MESG_BLOCK);
+
+    __osPfsGetOneChannelData(channel, &data);
+    if (((data.status & CONT_CARD_ON) != 0) && ((data.status & CONT_CARD_PULL) != 0)) {
+        return PFS_ERR_NEW_PACK;
+    } else if (data.errno || ((data.status & CONT_CARD_ON) == 0)) {
+        return PFS_ERR_NOPACK;
+    } else if ((data.status & CONT_ADDR_CRC_ER) != 0) {
+        return PFS_ERR_CONTRFAIL;
+    }
+    return ret;
+}
+
+void __osPfsRequestOneChannel(s32 channel, u8 poll) {
+    u8* bufptr;
+    __OSContRequestHeaderAligned req;
+    s32 idx;
+
+    __osContLastPoll = CONT_CMD_END;
+
+    pifMempakBuf.status = CONT_CMD_READ_BUTTON;
+
+    bufptr = &pifMempakBuf;
+
+    req.txsize = 1;
+    req.rxsize = 3;
+    req.poll = poll;
+    req.typeh = 0xFF;
+    req.typel = 0xFF;
+    req.status = 0xFF;
+
+    for (idx = 0; idx < channel; idx++) {
+        *bufptr++ = 0;
+    }
+
+    *((__OSContRequestHeaderAligned*)bufptr) = req;
+    bufptr += sizeof(req);
+    *((u8*)bufptr) = CONT_CMD_END;
+}
+
+void __osPfsGetOneChannelData(s32 channel, OSContStatus* contData) {
+    u8* bufptr;
+    __OSContRequestHeaderAligned req;
+    s32 idx;
+
+    bufptr = &pifMempakBuf;
+
+    for (idx = 0; idx < channel; idx++) {
+        bufptr++;
+    }
+
+    req = *((__OSContRequestHeaderAligned*)bufptr);
+    contData->errno = (req.rxsize & 0xC0) >> 4;
+    if (contData->errno) {
+        return;
+    }
+
+    contData->type = (req.typel << 8) | req.typeh;
+    contData->status = req.status;
+}
--- a/src/libultra_code_O2/__osPfsSelectBank.c
+++ b/src/libultra_code_O2/__osPfsSelectBank.c
@ -0,0 +1,18 @@
+#include "ultra64/pfs.h"
+#include "global.h"
+
+s32 __osPfsSelectBank(OSPfs* pfs, u8 bank) {
+    u8 temp[BLOCKSIZE];
+    s32 i;
+    s32 ret = 0;
+
+    for (i = 0; i < BLOCKSIZE; i++) {
+        temp[i] = bank;
+    }
+
+    ret = __osContRamWrite(pfs->queue, pfs->channel, 0x8000 / BLOCKSIZE, temp, 0);
+    if (ret == 0) {
+        pfs->activebank = bank;
+    }
+    return ret;
+}
--- a/src/libultra_code_O2/__osSiCreateAccessQueue.c
+++ b/src/libultra_code_O2/__osSiCreateAccessQueue.c
@ -0,0 +1,23 @@
+#include "global.h"
+
+OSMesg osSiMesgBuff[SIAccessQueueSize];
+OSMesgQueue gOSSiMessageQueue;
+u32 gOSSiAccessQueueCreated = 0;
+
+void __osSiCreateAccessQueue() {
+    gOSSiAccessQueueCreated = 1;
+    osCreateMesgQueue(&gOSSiMessageQueue, &osSiMesgBuff[0], SIAccessQueueSize - 1);
+    osSendMesg(&gOSSiMessageQueue, NULL, OS_MESG_NOBLOCK);
+}
+
+void __osSiGetAccess() {
+    OSMesg sp1c;
+    if (!gOSSiAccessQueueCreated) {
+        __osSiCreateAccessQueue();
+    }
+    osRecvMesg(&gOSSiMessageQueue, &sp1c, OS_MESG_BLOCK);
+}
+
+void __osSiRelAccess() {
+    osSendMesg(&gOSSiMessageQueue, NULL, OS_MESG_NOBLOCK);
+}
--- a/src/libultra_code_O2/__osSiRawStartDma.c
+++ b/src/libultra_code_O2/__osSiRawStartDma.c
@ -0,0 +1,20 @@
+#include "global.h"
+
+s32 __osSiRawStartDma(s32 dir, void* addr) {
+    if (HW_REG(SI_STATUS_REG, u32) & (SI_STATUS_DMA_BUSY | SI_STATUS_IO_READ_BUSY)) {
+        return -1;
+    }
+    if (dir == OS_WRITE) {
+        osWritebackDCache(addr, 0x40);
+    }
+    HW_REG(SI_DRAM_ADDR_REG, void*) = (void*)osVirtualToPhysical(addr);
+    if (dir == OS_READ) {
+        HW_REG(SI_PIF_ADDR_RD64B_REG, void*) = (void*)PIF_RAM_START;
+    } else {
+        HW_REG(SI_PIF_ADDR_WR64B_REG, void*) = (void*)PIF_RAM_START;
+    }
+    if (dir == OS_READ) {
+        osInvalDCache(addr, 0x40);
+    }
+    return 0;
+}
--- a/src/libultra_code_O2/__osSpGetStatus.c
+++ b/src/libultra_code_O2/__osSpGetStatus.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+u32 __osSpGetStatus() {
+    return HW_REG(SP_STATUS_REG, u32);
+}
--- a/src/libultra_code_O2/__osSpRawStartDma.c
+++ b/src/libultra_code_O2/__osSpRawStartDma.c
@ -0,0 +1,15 @@
+#include "global.h"
+
+s32 __osSpRawStartDma(s32 direction, void* devAddr, void* dramAddr, u32 size) {
+    if (__osSpDeviceBusy()) {
+        return -1;
+    }
+    HW_REG(SP_MEM_ADDR_REG, u32) = (u32)devAddr;
+    HW_REG(SP_DRAM_ADDR_REG, u32) = osVirtualToPhysical(dramAddr);
+    if (direction == OS_READ) {
+        HW_REG(SP_WR_LEN_REG, u32) = size - 1;
+    } else {
+        HW_REG(SP_RD_LEN_REG, u32) = size - 1;
+    }
+    return 0;
+}
--- a/src/libultra_code_O2/__osSpSetPc.c
+++ b/src/libultra_code_O2/__osSpSetPc.c
@ -0,0 +1,13 @@
+#include "global.h"
+
+s32 __osSpSetPc(void* pc) {
+    register u32 spStatus = HW_REG(SP_STATUS_REG, u32);
+
+    if (!(spStatus & SP_STATUS_HALT)) {
+        return -1;
+    } else {
+        HW_REG(SP_PC_REG, void*) = pc;
+    }
+
+    return 0;
+}
--- a/src/libultra_code_O2/__osSpSetStatus.c
+++ b/src/libultra_code_O2/__osSpSetStatus.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+void __osSpSetStatus(u32 a0) {
+    HW_REG(SP_STATUS_REG, u32) = a0;
+}
--- a/src/libultra_code_O2/code_800E6840.c
+++ b/src/libultra_code_O2/code_800E6840.c
@ -0,0 +1,13 @@
+#include "global.h"
+
+void func_800E6840(void* buf, s32 size) {
+    OSIntMask prevMask = osSetIntMask(1);
+    osInvalDCache(buf, size);
+    osSetIntMask(prevMask);
+}
+
+void func_800E6880(void* buf, s32 size) {
+    OSIntMask prevMask = osSetIntMask(1);
+    osWritebackDCache(buf, size);
+    osSetIntMask(prevMask);
+}
--- a/src/libultra_code_O2/code_80102FA0.c
+++ b/src/libultra_code_O2/code_80102FA0.c
@ -0,0 +1,4 @@
+#include "ultra64.h"
+#include "global.h"
+
+#pragma GLOBAL_ASM("asm/non_matchings/code/code_80102FA0/func_80102FA0.s")
--- a/src/libultra_code_O2/code_80104D60.c
+++ b/src/libultra_code_O2/code_80104D60.c
@ -0,0 +1,4 @@
+#include "ultra64.h"
+#include "global.h"
+
+#pragma GLOBAL_ASM("asm/non_matchings/code/code_80104D60/osPfsFileState.s")
--- a/src/libultra_code_O2/contpfs.c
+++ b/src/libultra_code_O2/contpfs.c
@ -0,0 +1,314 @@
+#include "ultra64.h"
+#include "global.h"
+
+extern __OSInode __osPfsInodeCache; // bss
+s32 __osPfsInodeCacheChannel = -1;
+u8 __osPfsInodeCacheBank = 250;
+
+u16 __osSumcalc(u8* ptr, s32 length) {
+    s32 i;
+    u32 sum = 0;
+    u8* temp;
+
+    temp = ptr;
+    for (i = 0; i < length; i++) {
+        sum += *temp++;
+    }
+    return sum & 0xFFFF;
+}
+
+s32 __osIdCheckSum(u16* ptr, u16* checkSum, u16* idSum) {
+
+    u16 data = 0;
+    u32 i;
+
+    *checkSum = *idSum = 0;
+    for (i = 0; i < ((sizeof(__OSPackId) - sizeof(u32)) / sizeof(u8)); i += 2) {
+        data = *((u16*)((u32)ptr + i));
+        *checkSum += data;
+        *idSum += ~data;
+    }
+    return 0;
+}
+
+s32 __osRepairPackId(OSPfs* pfs, __OSPackId* badid, __OSPackId* newid) {
+    s32 ret = 0;
+    u8 temp[BLOCKSIZE];
+    u8 comp[BLOCKSIZE];
+    u8 mask = 0;
+    s32 i, j = 0;
+    u16 index[4];
+
+    newid->repaired = 0xFFFFFFFF;
+    newid->random = osGetCount();
+    newid->serialMid = badid->serialMid;
+    newid->serialLow = badid->serialLow;
+
+    if ((pfs->activebank != 0) && ((ret = __osPfsSelectBank(pfs, 0)) != 0)) {
+        return ret;
+    }
+
+    do {
+        if ((ret = __osPfsSelectBank(pfs, j)) != 0) {
+            return ret;
+        }
+
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, 0, temp)) != 0) {
+            return ret;
+        }
+        temp[0] = j | 0x80;
+        for (i = 1; i < BLOCKSIZE; i++) {
+            temp[i] = ~temp[i];
+        }
+
+        if ((ret = __osContRamWrite(pfs->queue, pfs->channel, 0, temp, 0)) != 0) {
+            return ret;
+        }
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, 0, comp)) != 0) {
+            return (ret);
+        }
+        for (i = 0; i < BLOCKSIZE; i++) {
+            if (comp[i] != temp[i]) {
+                break;
+            }
+        }
+        if (i != BLOCKSIZE) {
+            break;
+        }
+
+        if (j > 0) {
+            if ((ret = __osPfsSelectBank(pfs, 0)) != 0) {
+                return ret;
+            }
+            if ((ret = __osContRamRead(pfs->queue, pfs->channel, 0, temp)) != 0) {
+                return ret;
+            }
+            if (temp[0] != 0x80) {
+                break;
+            }
+        }
+
+        j++;
+    } while (j < PFS_MAX_BANKS);
+
+    if ((pfs->activebank != 0) && (ret = __osPfsSelectBank(pfs, 0)) != 0) {
+        return ret;
+    }
+
+    mask = (j > 0) ? 1 : 0;
+    newid->deviceid = (badid->deviceid & 0xFFFE) | mask;
+    newid->banks = j;
+    newid->version = badid->version;
+    __osIdCheckSum(newid, &newid->checksum, &newid->invertedChecksum);
+
+    index[0] = PFS_ID_0AREA;
+    index[1] = PFS_ID_1AREA;
+    index[2] = PFS_ID_2AREA;
+    index[3] = PFS_ID_3AREA;
+    for (i = 0; i < 4; i++) {
+        if ((ret = __osContRamWrite(pfs->queue, pfs->channel, index[i], newid, PFS_FORCE)) != 0) {
+            return ret;
+        }
+    }
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_ID_0AREA, temp)) != 0) {
+        return ret;
+    }
+    for (i = 0; i < BLOCKSIZE; i++) {
+        if (temp[i] != *(u8*)((s32)newid + i)) {
+            return PFS_ERR_DEVICE;
+        }
+    }
+    return 0;
+}
+
+s32 __osCheckPackId(OSPfs* pfs, __OSPackId* temp) {
+
+    u16 index[4];
+    s32 ret = 0;
+    u16 sum;
+    u16 idSum;
+    s32 i;
+    s32 j;
+
+    if ((pfs->activebank != 0) && (ret = __osPfsSelectBank(pfs, 0)) != 0) {
+        return ret;
+    }
+
+    index[0] = PFS_ID_0AREA;
+    index[1] = PFS_ID_1AREA;
+    index[2] = PFS_ID_2AREA;
+    index[3] = PFS_ID_3AREA;
+    for (i = 1; i < 4; i++) {
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, index[i], temp)) != 0) {
+            return ret;
+        }
+        __osIdCheckSum(temp, &sum, &idSum);
+        if ((temp->checksum == sum) && (temp->invertedChecksum == idSum)) {
+            break;
+        }
+    }
+    if (i == 4) {
+        return PFS_ERR_ID_FATAL;
+    }
+
+    for (j = 0; j < 4; j++) {
+        if (j != i) {
+            if ((ret = __osContRamWrite(pfs->queue, pfs->channel, index[j], temp, PFS_FORCE)) != 0) {
+                return ret;
+            }
+        }
+    }
+    return 0;
+}
+
+s32 __osGetId(OSPfs* pfs) {
+    u16 sum;
+    u16 isum;
+    u8 temp[BLOCKSIZE];
+    __OSPackId* id;
+    __OSPackId newid;
+    s32 ret;
+
+    if (pfs->activebank != 0) {
+        if ((ret = __osPfsSelectBank(pfs, 0)) != 0) {
+            return ret;
+        }
+    }
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_ID_0AREA, temp)) != 0) {
+        return ret;
+    }
+
+    __osIdCheckSum((u16*)temp, &sum, &isum);
+    id = (__OSPackId*)temp;
+    if ((id->checksum != sum) || (id->invertedChecksum != isum)) {
+        if ((ret = __osCheckPackId(pfs, id)) == PFS_ERR_ID_FATAL) {
+            ret = __osRepairPackId(pfs, id, &newid);
+            if (ret) {
+                return ret;
+            }
+            id = &newid;
+        } else if (ret != 0) {
+            return ret;
+        }
+    }
+
+    if ((id->deviceid & 0x01) == 0) {
+        ret = __osRepairPackId(pfs, id, &newid);
+        if (ret) {
+            return ret;
+        }
+        id = &newid;
+        if ((id->deviceid & 0x01) == 0) {
+            return PFS_ERR_DEVICE;
+        }
+    }
+
+    bcopy(id, pfs->id, BLOCKSIZE);
+
+    if (0) {}
+
+    pfs->version = id->version;
+
+    pfs->banks = id->banks;
+    pfs->inodeStartPage = 1 + DEF_DIR_PAGES + (2 * pfs->banks);
+    pfs->dir_size = DEF_DIR_PAGES * PFS_ONE_PAGE;
+    pfs->inode_table = 1 * PFS_ONE_PAGE;
+    pfs->minode_table = (1 + pfs->banks) * PFS_ONE_PAGE;
+    pfs->dir_table = pfs->minode_table + (pfs->banks * PFS_ONE_PAGE);
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_LABEL_AREA, pfs->label)) != 0) {
+        return ret;
+    }
+
+    return 0;
+}
+
+s32 __osCheckId(OSPfs* pfs) {
+    u8 temp[BLOCKSIZE];
+    s32 ret;
+
+    if (pfs->activebank != 0) {
+        ret = __osPfsSelectBank(pfs, 0);
+        if (ret == PFS_ERR_NEW_PACK) {
+            ret = __osPfsSelectBank(pfs, 0);
+        }
+        if (ret != 0) {
+            return ret;
+        }
+    }
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_ID_0AREA, temp)) != 0) {
+        if (ret != PFS_ERR_NEW_PACK) {
+            return ret;
+        }
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_ID_0AREA, temp)) != 0) {
+            return ret;
+        }
+    }
+
+    if (bcmp(pfs->id, temp, BLOCKSIZE) != 0) {
+        return PFS_ERR_NEW_PACK;
+    }
+
+    return 0;
+}
+
+s32 __osPfsRWInode(OSPfs* pfs, __OSInode* inode, u8 flag, u8 bank) {
+    u8 sum;
+    s32 j;
+    s32 ret;
+    s32 offset;
+    u8* addr;
+
+    if (flag == PFS_READ && bank == __osPfsInodeCacheBank && (pfs->channel == __osPfsInodeCacheChannel)) {
+        bcopy(&__osPfsInodeCache, inode, sizeof(__OSInode));
+        return 0;
+    }
+
+    if ((pfs->activebank != 0) && (ret = __osPfsSelectBank(pfs, 0)) != 0) {
+        return ret;
+    }
+
+    offset = ((bank > 0) ? 1 : pfs->inodeStartPage);
+
+    if (flag == PFS_WRITE) {
+        inode->inodePage[0].inode_t.page =
+            __osSumcalc(inode->inodePage + offset, (PFS_INODE_SIZE_PER_PAGE - offset) * 2);
+    }
+
+    for (j = 0; j < PFS_ONE_PAGE; j++) {
+        addr = (u8*)(((u8*)inode) + (j * BLOCKSIZE));
+        if (flag == PFS_WRITE) {
+            ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->inode_table + (bank * PFS_ONE_PAGE) + j, addr, 0);
+            ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->minode_table + (bank * PFS_ONE_PAGE) + j, addr, 0);
+        } else {
+            ret = __osContRamRead(pfs->queue, pfs->channel, pfs->inode_table + (bank * PFS_ONE_PAGE) + j, addr);
+        }
+        if (ret) {
+            return ret;
+        }
+    }
+
+    if (flag == PFS_READ) {
+        sum = __osSumcalc((u8*)(inode->inodePage + offset), (PFS_INODE_SIZE_PER_PAGE - offset) * 2);
+        if (sum != inode->inodePage[0].inode_t.page) {
+            for (j = 0; j < PFS_ONE_PAGE; j++) {
+                addr = (u8*)(((u8*)inode) + (j * BLOCKSIZE));
+                ret = __osContRamRead(pfs->queue, pfs->channel, pfs->minode_table + (bank * PFS_ONE_PAGE) + j, addr);
+            }
+            sum = __osSumcalc(inode->inodePage + offset, (PFS_INODE_SIZE_PER_PAGE - offset) * 2);
+            if (sum != inode->inodePage[0].inode_t.page) {
+                return PFS_ERR_INCONSISTENT;
+            }
+            for (j = 0; j < PFS_ONE_PAGE; j++) {
+                addr = (u8*)(((u8*)inode) + (j * BLOCKSIZE));
+                ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->inode_table + (bank * PFS_ONE_PAGE) + j, addr, 0);
+            }
+        }
+    }
+    __osPfsInodeCacheBank = bank;
+    bcopy(inode, &__osPfsInodeCache, sizeof(__OSInode));
+    __osPfsInodeCacheChannel = pfs->channel;
+    return 0;
+}
--- a/src/libultra_code_O2/contquery.c
+++ b/src/libultra_code_O2/contquery.c
@ -0,0 +1,32 @@
+#include "global.h"
+
+/**
+ * osContStartQuery:
+ * Starts to read the values for SI device status and type which are connected to the controller port and joyport
+ * connector.
+ **/
+
+s32 osContStartQuery(OSMesgQueue* mq) {
+    s32 ret;
+    ret = 0;
+
+    __osSiGetAccess();
+    if (__osContLastPoll != CONT_CMD_REQUEST_STATUS) {
+        __osPackRequestData(CONT_CMD_REQUEST_STATUS);
+        ret = __osSiRawStartDma(OS_WRITE, &__osPifInternalBuff);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+    }
+    ret = __osSiRawStartDma(OS_READ, &__osPifInternalBuff);
+    __osContLastPoll = CONT_CMD_REQUEST_STATUS;
+    __osSiRelAccess();
+    return ret;
+}
+
+/**
+ * osContGetQuery:
+ * Returns the values from osContStartQuery to status. Both functions must be paired for use.
+ **/
+void osContGetQuery(OSContStatus* data) {
+    u8 pattern;
+    __osContGetInitData(&pattern, data);
+}
--- a/src/libultra_code_O2/cosf.c
+++ b/src/libultra_code_O2/cosf.c
@ -0,0 +1,64 @@
+#include "ultra64.h"
+#include "global.h"
+
+static const du P[] = {
+    { 0x3FF00000, 0x00000000 }, { 0xBFC55554, 0xBC83656D }, { 0x3F8110ED, 0x3804C2A0 },
+    { 0xBF29f6FF, 0xEEA56814 }, { 0x3EC5DBDF, 0x0E314BFE },
+};
+
+static const du rpi = { 0x3FD45F30, 0x6DC9C883 };
+
+static const du pihi = { 0x400921FB, 0x50000000 };
+
+static const du pilo = { 0x3E6110B4, 0x611A6263 };
+
+static const fu zero = { 0x00000000 };
+
+f32 cosf(f32 x) {
+    f32 absx;
+    f64 dx;
+    f64 xSq;
+    f64 polyApprox;
+    f64 dn;
+    s32 n;
+    f64 result;
+    s32 ix, xpt;
+
+    ix = *(s32*)&x;
+    xpt = (ix >> 22);
+    xpt &= 0x1FF;
+
+    if (xpt < 0x136) {
+
+        absx = (x > 0) ? x : -x;
+
+        dx = absx;
+
+        dn = dx * rpi.d + 0.5;
+        n = ROUND(dn);
+        dn = n;
+
+        dn -= 0.5;
+
+        dx -= dn * pihi.d;
+        dx -= dn * pilo.d;
+
+        xSq = SQ(dx);
+
+        polyApprox = ((P[4].d * xSq + P[3].d) * xSq + P[2].d) * xSq + P[1].d;
+
+        result = dx + (dx * xSq) * polyApprox;
+
+        if (!(n & 1)) {
+            return (f32)result;
+        }
+
+        return -(f32)result;
+    }
+
+    if (x != x) {
+        return __libm_qnan_f;
+    }
+
+    return zero.f;
+}
--- a/src/libultra_code_O2/coss.c
+++ b/src/libultra_code_O2/coss.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+s16 coss(u16 angle) {
+    return sins(angle + 0x4000);
+}
--- a/src/libultra_code_O2/guLookAt.c
+++ b/src/libultra_code_O2/guLookAt.c
@ -0,0 +1,61 @@
+#include "global.h"
+
+void guLookAtF(f32 mf[4][4], f32 xEye, f32 yEye, f32 zEye, f32 xAt, f32 yAt, f32 zAt, f32 xUp, f32 yUp, f32 zUp) {
+    f32 length;
+    f32 xLook, yLook, zLook;
+    f32 xRight, yRight, zRight;
+
+    guMtxIdentF(mf);
+
+    xLook = xAt - xEye;
+    yLook = yAt - yEye;
+    zLook = zAt - zEye;
+    length = -1.0 / sqrtf(SQ(xLook) + SQ(yLook) + SQ(zLook));
+    xLook *= length;
+    yLook *= length;
+    zLook *= length;
+
+    xRight = yUp * zLook - zUp * yLook;
+    yRight = zUp * xLook - xUp * zLook;
+    zRight = xUp * yLook - yUp * xLook;
+    length = 1.0 / sqrtf(SQ(xRight) + SQ(yRight) + SQ(zRight));
+    xRight *= length;
+    yRight *= length;
+    zRight *= length;
+
+    xUp = yLook * zRight - zLook * yRight;
+    yUp = zLook * xRight - xLook * zRight;
+    zUp = xLook * yRight - yLook * xRight;
+    length = 1.0 / sqrtf(SQ(xUp) + SQ(yUp) + SQ(zUp));
+    xUp *= length;
+    yUp *= length;
+    zUp *= length;
+
+    mf[0][0] = xRight;
+    mf[1][0] = yRight;
+    mf[2][0] = zRight;
+    mf[3][0] = -(xEye * xRight + yEye * yRight + zEye * zRight);
+
+    mf[0][1] = xUp;
+    mf[1][1] = yUp;
+    mf[2][1] = zUp;
+    mf[3][1] = -(xEye * xUp + yEye * yUp + zEye * zUp);
+
+    mf[0][2] = xLook;
+    mf[1][2] = yLook;
+    mf[2][2] = zLook;
+    mf[3][2] = -(xEye * xLook + yEye * yLook + zEye * zLook);
+
+    mf[0][3] = 0;
+    mf[1][3] = 0;
+    mf[2][3] = 0;
+    mf[3][3] = 1;
+}
+
+void guLookAt(Mtx* m, f32 xEye, f32 yEye, f32 zEye, f32 xAt, f32 yAt, f32 zAt, f32 xUp, f32 yUp, f32 zUp) {
+    f32 mf[4][4];
+
+    guLookAtF(mf, xEye, yEye, zEye, xAt, yAt, zAt, xUp, yUp, zUp);
+
+    guMtxF2L(mf, m);
+}
--- a/src/libultra_code_O2/guLookAtHilite.c
+++ b/src/libultra_code_O2/guLookAtHilite.c
@ -0,0 +1,152 @@
+#include "global.h"
+
+#define FTOFRAC8(x) ((s32)MIN(((x) * (128.0f)), 127.0f) & 0xff)
+
+/**
+ * guLookAtHiliteF
+ * This function creates the viewing matrix (floating point) and sets the LookAt/Hilite structures
+ **/
+void guLookAtHiliteF(f32 mf[4][4], LookAt* l, Hilite* h, f32 xEye, f32 yEye, f32 zEye, f32 xAt, f32 yAt, f32 zAt,
+                     f32 xUp, f32 yUp, f32 zUp, f32 xl1, f32 yl1, f32 zl1, /* light 1 direction */
+                     f32 xl2, f32 yl2, f32 zl2,                            /* light 2 direction */
+                     s32 hiliteWidth, s32 hiliteHeight)                    /* size of hilite texture */
+{
+    f32 length, xLook, yLook, zLook, xRight, yRight, zRight, xHilite, yHilite, zHilite;
+
+    guMtxIdentF(mf);
+
+    xLook = xAt - xEye;
+    yLook = yAt - yEye;
+    zLook = zAt - zEye;
+    length = -1.0 / sqrtf(xLook * xLook + yLook * yLook + zLook * zLook);
+    xLook *= length;
+    yLook *= length;
+    zLook *= length;
+
+    xRight = yUp * zLook - zUp * yLook;
+    yRight = zUp * xLook - xUp * zLook;
+    zRight = xUp * yLook - yUp * xLook;
+    length = 1.0 / sqrtf(xRight * xRight + yRight * yRight + zRight * zRight);
+    xRight *= length;
+    yRight *= length;
+    zRight *= length;
+
+    xUp = yLook * zRight - zLook * yRight;
+    yUp = zLook * xRight - xLook * zRight;
+    zUp = xLook * yRight - yLook * xRight;
+    length = 1.0 / sqrtf(xUp * xUp + yUp * yUp + zUp * zUp);
+    xUp *= length;
+    yUp *= length;
+    zUp *= length;
+
+    /* hilite vectors */
+
+    length = 1.0 / sqrtf(xl1 * xl1 + yl1 * yl1 + zl1 * zl1);
+    xl1 *= length;
+    yl1 *= length;
+    zl1 *= length;
+
+    xHilite = xl1 + xLook;
+    yHilite = yl1 + yLook;
+    zHilite = zl1 + zLook;
+
+    length = sqrtf(xHilite * xHilite + yHilite * yHilite + zHilite * zHilite);
+
+    if (length > 0.1) {
+        length = 1.0 / length;
+        xHilite *= length;
+        yHilite *= length;
+        zHilite *= length;
+
+        h->h.x1 = hiliteWidth * 4 + (xHilite * xRight + yHilite * yRight + zHilite * zRight) * hiliteWidth * 2;
+
+        h->h.y1 = hiliteHeight * 4 + (xHilite * xUp + yHilite * yUp + zHilite * zUp) * hiliteHeight * 2;
+    } else {
+        h->h.x1 = hiliteWidth * 2;
+        h->h.y1 = hiliteHeight * 2;
+    }
+
+    length = 1.0 / sqrtf(xl2 * xl2 + yl2 * yl2 + zl2 * zl2);
+    xl2 *= length;
+    yl2 *= length;
+    zl2 *= length;
+
+    xHilite = xl2 + xLook;
+    yHilite = yl2 + yLook;
+    zHilite = zl2 + zLook;
+    length = sqrtf(xHilite * xHilite + yHilite * yHilite + zHilite * zHilite);
+    if (length > 0.1) {
+        length = 1.0 / length;
+        xHilite *= length;
+        yHilite *= length;
+        zHilite *= length;
+
+        h->h.x2 = hiliteWidth * 4 + (xHilite * xRight + yHilite * yRight + zHilite * zRight) * hiliteWidth * 2;
+
+        h->h.y2 = hiliteHeight * 4 + (xHilite * xUp + yHilite * yUp + zHilite * zUp) * hiliteHeight * 2;
+    } else {
+        h->h.x2 = hiliteWidth * 2;
+        h->h.y2 = hiliteHeight * 2;
+    }
+
+    /* reflectance vectors = Up and Right */
+
+    l->l[0].l.dir[0] = FTOFRAC8(xRight);
+    l->l[0].l.dir[1] = FTOFRAC8(yRight);
+    l->l[0].l.dir[2] = FTOFRAC8(zRight);
+    l->l[1].l.dir[0] = FTOFRAC8(xUp);
+    l->l[1].l.dir[1] = FTOFRAC8(yUp);
+    l->l[1].l.dir[2] = FTOFRAC8(zUp);
+    l->l[0].l.col[0] = 0x00;
+    l->l[0].l.col[1] = 0x00;
+    l->l[0].l.col[2] = 0x00;
+    l->l[0].l.pad1 = 0x00;
+    l->l[0].l.colc[0] = 0x00;
+    l->l[0].l.colc[1] = 0x00;
+    l->l[0].l.colc[2] = 0x00;
+    l->l[0].l.pad2 = 0x00;
+    l->l[1].l.col[0] = 0x00;
+    l->l[1].l.col[1] = 0x80;
+    l->l[1].l.col[2] = 0x00;
+    l->l[1].l.pad1 = 0x00;
+    l->l[1].l.colc[0] = 0x00;
+    l->l[1].l.colc[1] = 0x80;
+    l->l[1].l.colc[2] = 0x00;
+    l->l[1].l.pad2 = 0x00;
+
+    mf[0][0] = xRight;
+    mf[1][0] = yRight;
+    mf[2][0] = zRight;
+    mf[3][0] = -(xEye * xRight + yEye * yRight + zEye * zRight);
+
+    mf[0][1] = xUp;
+    mf[1][1] = yUp;
+    mf[2][1] = zUp;
+    mf[3][1] = -(xEye * xUp + yEye * yUp + zEye * zUp);
+
+    mf[0][2] = xLook;
+    mf[1][2] = yLook;
+    mf[2][2] = zLook;
+    mf[3][2] = -(xEye * xLook + yEye * yLook + zEye * zLook);
+
+    mf[0][3] = 0;
+    mf[1][3] = 0;
+    mf[2][3] = 0;
+    mf[3][3] = 1;
+}
+
+/**
+ * guLookAtHilite
+ * This function creates the viewing matrix (fixed point) and sets the LookAt/Hilite structures
+ * Same args as previous function
+ **/
+void guLookAtHilite(Mtx* m, LookAt* l, Hilite* h, f32 xEye, f32 yEye, f32 zEye, f32 xAt, f32 yAt, f32 zAt, f32 xUp,
+                    f32 yUp, f32 zUp, f32 xl1, f32 yl1, f32 zl1, f32 xl2, f32 yl2, f32 zl2, s32 hiliteWidth,
+                    s32 hiliteHeight) {
+    f32 mf[4][4];
+
+    guLookAtHiliteF(mf, l, h, xEye, yEye, zEye, xAt, yAt, zAt, xUp, yUp, zUp, xl1, yl1, zl1, xl2, yl2, zl2, hiliteWidth,
+                    hiliteHeight);
+
+    guMtxF2L(mf, m);
+}
--- a/src/libultra_code_O2/guPerspectiveF.c
+++ b/src/libultra_code_O2/guPerspectiveF.c
@ -0,0 +1,36 @@
+#include "global.h"
+
+void guPerspectiveF(f32 mf[4][4], u16* perspNorm, f32 fovy, f32 aspect, f32 near, f32 far, f32 scale) {
+    f32 yscale;
+    s32 row;
+    s32 col;
+    guMtxIdentF(mf);
+    fovy *= GU_PI / 180.0;
+    yscale = cosf(fovy / 2) / sinf(fovy / 2);
+    mf[0][0] = yscale / aspect;
+    mf[1][1] = yscale;
+    mf[2][2] = (near + far) / (near - far);
+    mf[2][3] = -1;
+    mf[3][2] = 2 * near * far / (near - far);
+    mf[3][3] = 0.0f;
+    for (row = 0; row < 4; row++) {
+        for (col = 0; col < 4; col++) {
+            mf[row][col] *= scale;
+        }
+    }
+    if (perspNorm != NULL) {
+        if (near + far <= 2.0) {
+            *perspNorm = 65535;
+        } else {
+            *perspNorm = (f64)(1 << 17) / (near + far);
+            if (*perspNorm <= 0) {
+                *perspNorm = 1;
+            }
+        }
+    }
+}
+void guPerspective(Mtx* m, u16* perspNorm, f32 fovy, f32 aspect, f32 near, f32 far, f32 scale) {
+    f32 mat[4][4];
+    guPerspectiveF(mat, perspNorm, fovy, aspect, near, far, scale);
+    guMtxF2L(mat, m);
+}
--- a/src/libultra_code_O2/guPosition.c
+++ b/src/libultra_code_O2/guPosition.c
@ -0,0 +1,55 @@
+#include "global.h"
+
+/**
+ * guPositionF
+ * Creates a rotation/parallel translation modeling matrix (floating point)
+ **/
+
+void guPositionF(f32 mf[4][4], f32 rot, f32 pitch, f32 yaw, f32 scale, f32 x, f32 y, f32 z) {
+    static f32 D_80134D00 = M_PI / 180.0;
+    f32 sinr, sinp, sinh;
+    f32 cosr, cosp, cosh;
+
+    rot *= D_80134D00;
+    pitch *= D_80134D00;
+    yaw *= D_80134D00;
+
+    sinr = sinf(rot);
+    cosr = cosf(rot);
+    sinp = sinf(pitch);
+    cosp = cosf(pitch);
+    sinh = sinf(yaw);
+    cosh = cosf(yaw);
+
+    mf[0][0] = (cosp * cosh) * scale;
+    mf[0][1] = (cosp * sinh) * scale;
+    mf[0][2] = (-sinp) * scale;
+    mf[0][3] = 0.0f;
+
+    mf[1][0] = ((sinr * sinp * cosh) - (cosr * sinh)) * scale;
+    mf[1][1] = ((sinr * sinp * sinh) + (cosr * cosh)) * scale;
+    mf[1][2] = (sinr * cosp) * scale;
+    mf[1][3] = 0.0f;
+
+    mf[2][0] = ((cosr * sinp * cosh) + (sinr * sinh)) * scale;
+    mf[2][1] = ((cosr * sinp * sinh) - (sinr * cosh)) * scale;
+    mf[2][2] = (cosr * cosp) * scale;
+    mf[2][3] = 0.0f;
+
+    mf[3][0] = x;
+    mf[3][1] = y;
+    mf[3][2] = z;
+    mf[3][3] = 1.0f;
+}
+
+/**
+ * guPosition
+ * Creates a rotational/paralell translation moeling matrix (fixed point)
+ */
+void guPosition(Mtx* m, f32 rot, f32 pitch, f32 yaw, f32 scale, f32 x, f32 y, f32 z) {
+    f32 mf[4][4];
+
+    guPositionF(mf, rot, pitch, yaw, scale, x, y, z);
+
+    guMtxF2L(mf, m);
+}
--- a/src/libultra_code_O2/guS2DInitBg.c
+++ b/src/libultra_code_O2/guS2DInitBg.c
@ -0,0 +1,28 @@
+#include "global.h"
+
+void guS2DInitBg(uObjBg* bg) {
+    u16 shift;
+    u32 size;
+    s32 tmem;
+
+    tmem = (bg->b.imageFmt == G_IM_FMT_CI) ? 0x100 : 0x200;
+
+    shift = (6 - bg->b.imageSiz);
+    if (bg->b.imageLoad == G_BGLT_LOADBLOCK) {
+        bg->b.tmemW = bg->b.imageW >> shift;
+        bg->b.tmemH = (tmem / bg->b.tmemW) * 4;
+        bg->b.tmemSizeW = bg->b.tmemW * 2;
+        bg->b.tmemSize = bg->b.tmemH * bg->b.tmemSizeW;
+        bg->b.tmemLoadSH = (bg->b.tmemSize >> 1) - 1;
+        bg->b.tmemLoadTH = (0x7FF / bg->b.tmemW) + 1;
+    } else { // G_BGLT_LOADTILE
+        bg->b.tmemW = (bg->b.frameW >> shift) + 3;
+        bg->b.tmemH = (tmem / bg->b.tmemW) * 4;
+        bg->b.tmemSizeW = (bg->b.imageW >> shift) * 2;
+
+        size = bg->b.tmemH * bg->b.tmemSizeW;
+        bg->b.tmemSize = (size >> 16);
+        bg->b.tmemLoadSH = (size >> 0) & 0xFFFF;
+        bg->b.tmemLoadTH = bg->b.tmemH - 1;
+    }
+}
--- a/src/libultra_code_O2/ortho.c
+++ b/src/libultra_code_O2/ortho.c
@ -0,0 +1,29 @@
+#include "global.h"
+
+void guOrthoF(f32 mf[4][4], f32 left, f32 right, f32 bottom, f32 top, f32 near, f32 far, f32 scale) {
+    s32 i, j;
+
+    guMtxIdentF(mf);
+
+    mf[0][0] = 2 / (right - left);
+    mf[1][1] = 2 / (top - bottom);
+    mf[2][2] = -2 / (far - near);
+    mf[3][0] = -(right + left) / (right - left);
+    mf[3][1] = -(top + bottom) / (top - bottom);
+    mf[3][2] = -(far + near) / (far - near);
+    mf[3][3] = 1;
+
+    for (i = 0; i < 4; i++) {
+        for (j = 0; j < 4; j++) {
+            mf[i][j] *= scale;
+        }
+    }
+}
+
+void guOrtho(Mtx* mtx, f32 left, f32 right, f32 bottom, f32 top, f32 near, f32 far, f32 scale) {
+    MtxF_t mf;
+
+    guOrthoF(mf, left, right, bottom, top, near, far, scale);
+
+    guMtxF2L(mf, mtx);
+}
--- a/src/libultra_code_O2/osAfterPreNMI.c
+++ b/src/libultra_code_O2/osAfterPreNMI.c
@ -0,0 +1,6 @@
+#include "ultra64.h"
+#include "global.h"
+
+s32 osAfterPreNMI(void) {
+    return __osSpSetPc(0);
+}
--- a/src/libultra_code_O2/osAiGetLength.c
+++ b/src/libultra_code_O2/osAiGetLength.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+u32 osAiGetLength() {
+    return HW_REG(AI_LEN_REG, u32);
+}
--- a/src/libultra_code_O2/osAiSetFrequency.c
+++ b/src/libultra_code_O2/osAiSetFrequency.c
@ -0,0 +1,23 @@
+#include "global.h"
+
+s32 osAiSetFrequency(u32 frequency) {
+    u32 dacRate;
+    u8 bitrate;
+    f32 dacRateF;
+
+    dacRateF = ((f32)osViClock / frequency) + 0.5f;
+    dacRate = dacRateF;
+
+    if (dacRate < 132) {
+        return -1;
+    }
+
+    bitrate = (dacRate / 66);
+    if (bitrate > 16) {
+        bitrate = 16;
+    }
+
+    HW_REG(AI_DACRATE_REG, u32) = dacRate - 1;
+    HW_REG(AI_BITRATE_REG, u32) = bitrate - 1;
+    return osViClock / (s32)dacRate;
+}
--- a/src/libultra_code_O2/osAiSetNextBuffer.c
+++ b/src/libultra_code_O2/osAiSetNextBuffer.c
@ -0,0 +1,25 @@
+#include "global.h"
+
+s32 osAiSetNextBuffer(void* buf, u32 size) {
+    static u8 D_80130500 = false;
+    u32 bufAdjusted = (u32)buf;
+    s32 status;
+
+    if (D_80130500) {
+        bufAdjusted = (u32)buf - 0x2000;
+    }
+    if ((((u32)buf + size) & 0x1FFF) == 0) {
+        D_80130500 = true;
+    } else {
+        D_80130500 = false;
+    }
+
+    status = HW_REG(AI_STATUS_REG, s32);
+    if (status & AI_STATUS_AI_FULL) {
+        return -1;
+    }
+
+    HW_REG(AI_DRAM_ADDR_REG, u32) = PHYSICAL_TO_VIRTUAL(bufAdjusted);
+    HW_REG(AI_LEN_REG, u32) = size;
+    return 0;
+}
--- a/src/libultra_code_O2/osContInit.c
+++ b/src/libultra_code_O2/osContInit.c
@ -0,0 +1,88 @@
+#include "global.h"
+
+OSPifRam __osPifInternalBuff;
+u8 __osContLastPoll;
+u8 __osMaxControllers; // always 4
+
+OSTimer __osEepromTimer;
+OSMesgQueue __osEepromTimerMsgQ;
+OSMesg __osEepromTimerMsg;
+
+u32 gOSContInitialized = 0;
+
+#define HALF_SECOND OS_USEC_TO_CYCLES(500000)
+s32 osContInit(OSMesgQueue* mq, u8* ctlBitfield, OSContStatus* status) {
+    OSMesg mesg;
+    s32 ret = 0;
+    OSTime currentTime;
+    OSTimer timer;
+    OSMesgQueue timerqueue;
+
+    if (gOSContInitialized) {
+        return 0;
+    }
+    gOSContInitialized = 1;
+    currentTime = osGetTime();
+    if (HALF_SECOND > currentTime) {
+        osCreateMesgQueue(&timerqueue, &mesg, 1);
+        osSetTimer(&timer, HALF_SECOND - currentTime, 0, &timerqueue, &mesg);
+        osRecvMesg(&timerqueue, &mesg, OS_MESG_BLOCK);
+    }
+    __osMaxControllers = MAXCONTROLLERS;
+    __osPackRequestData(CONT_CMD_REQUEST_STATUS);
+    ret = __osSiRawStartDma(OS_WRITE, &__osPifInternalBuff);
+    osRecvMesg(mq, &mesg, OS_MESG_BLOCK);
+    ret = __osSiRawStartDma(OS_READ, &__osPifInternalBuff);
+    osRecvMesg(mq, &mesg, OS_MESG_BLOCK);
+    __osContGetInitData(ctlBitfield, status);
+    __osContLastPoll = CONT_CMD_REQUEST_STATUS;
+    __osSiCreateAccessQueue();
+    osCreateMesgQueue(&__osEepromTimerMsgQ, &__osEepromTimerMsg, 1);
+    return ret;
+}
+
+void __osContGetInitData(u8* ctlBitfield, OSContStatus* status) {
+    u8* bufptr;
+    __OSContRequestHeader req;
+    s32 i;
+    u8 bitfieldTemp = 0;
+    bufptr = (u8*)(&__osPifInternalBuff);
+    for (i = 0; i < __osMaxControllers; i++, bufptr += sizeof(req), status++) {
+        req = *((__OSContRequestHeader*)bufptr);
+        status->errno = (req.rxsize & 0xC0) >> 4;
+        if (status->errno) {
+            continue;
+        }
+        status->type = req.typel << 8 | req.typeh;
+        status->status = req.status;
+        bitfieldTemp |= 1 << i;
+    }
+    *ctlBitfield = bitfieldTemp;
+}
+
+void __osPackRequestData(u8 poll) {
+    u8* bufptr;
+    __OSContRequestHeader req;
+    s32 i;
+    for (i = 0; i < 0xF; i++) {
+        __osPifInternalBuff.ram[i] = 0;
+    }
+    __osPifInternalBuff.status = 1;
+
+    bufptr = &__osPifInternalBuff;
+
+    req.align = 0xFF;
+    req.txsize = 1;
+    req.rxsize = 3;
+    req.poll = poll;
+    req.typeh = 0xFF;
+    req.typel = 0xFF;
+    req.status = 0xFF;
+    req.align1 = 0xFF;
+
+    for (i = 0; i < __osMaxControllers; i++) {
+        *((__OSContRequestHeader*)bufptr) = req;
+        bufptr += sizeof(req);
+    }
+    *((u8*)bufptr) = 254;
+}
--- a/src/libultra_code_O2/osContQuery.c
+++ b/src/libultra_code_O2/osContQuery.c
@ -0,0 +1,32 @@
+#include "global.h"
+
+/**
+ * osContStartQuery:
+ * Starts to read the values for SI device status and type which are connected to the controller port and joyport
+ * connector.
+ **/
+
+s32 osContStartQuery(OSMesgQueue* mq) {
+    s32 ret;
+    ret = 0;
+
+    __osSiGetAccess();
+    if (__osContLastPoll != CONT_CMD_REQUEST_STATUS) {
+        __osPackRequestData(CONT_CMD_REQUEST_STATUS);
+        ret = __osSiRawStartDma(OS_WRITE, &__osPifInternalBuff);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+    }
+    ret = __osSiRawStartDma(OS_READ, &__osPifInternalBuff);
+    __osContLastPoll = CONT_CMD_REQUEST_STATUS;
+    __osSiRelAccess();
+    return ret;
+}
+
+/**
+ * osContGetQuery:
+ * Returns the values from osContStartQuery to status. Both functions must be paired for use.
+ **/
+void osContGetQuery(OSContStatus* data) {
+    u8 pattern;
+    __osContGetInitData(&pattern, data);
+}
--- a/src/libultra_code_O2/osContSetCh.c
+++ b/src/libultra_code_O2/osContSetCh.c
@ -0,0 +1,21 @@
+#include "ultra64.h"
+#include "global.h"
+
+/*
+ * s32 osContSetCh(u8 ch)
+ * This function specifies the number of devices for the functions to access when those functions access to multiple
+ * direct SI devices.
+ */
+s32 osContSetCh(u8 ch) {
+    __osSiGetAccess();
+
+    if (ch > MAXCONTROLLERS) {
+        __osMaxControllers = MAXCONTROLLERS;
+    } else {
+        __osMaxControllers = ch;
+    }
+
+    __osContLastPoll = -2;
+    __osSiRelAccess();
+    return 0;
+}
--- a/src/libultra_code_O2/osContStartReadData.c
+++ b/src/libultra_code_O2/osContStartReadData.c
@ -0,0 +1,54 @@
+#include "global.h"
+
+s32 osContStartReadData(OSMesgQueue* mq) {
+    s32 ret;
+    __osSiGetAccess();
+    if (__osContLastPoll != 1) {
+        __osPackReadData();
+        __osSiRawStartDma(OS_WRITE, &__osPifInternalBuff);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+    }
+    ret = __osSiRawStartDma(OS_READ, &__osPifInternalBuff);
+    __osContLastPoll = CONT_CMD_READ_BUTTON;
+    __osSiRelAccess();
+    return ret;
+}
+
+void osContGetReadData(OSContPad* contData) {
+    u8* bufptr;
+    __OSContReadHeader read;
+    s32 i;
+    bufptr = (u8*)(&__osPifInternalBuff);
+    for (i = 0; i < __osMaxControllers; i++, bufptr += sizeof(read), contData++) {
+        read = *((__OSContReadHeader*)bufptr);
+        contData->errno = (read.rxsize & 0xC0) >> 4;
+        if (contData->errno == 0) {
+            contData->button = read.button;
+            contData->stick_x = read.joyX;
+            contData->stick_y = read.joyY;
+        }
+    };
+}
+
+void __osPackReadData() {
+    u8* bufptr;
+    __OSContReadHeader read;
+    s32 i;
+    bufptr = (u8*)(&__osPifInternalBuff);
+    for (i = 0; i < 0xF; i++) {
+        __osPifInternalBuff.ram[i] = 0;
+    }
+    __osPifInternalBuff.status = 1;
+    read.align = 0xFF;
+    read.txsize = 1;
+    read.rxsize = 4;
+    read.poll = CONT_CMD_READ_BUTTON;
+    read.button = 0xFFFF;
+    read.joyX = 0xFF;
+    read.joyY = 0xFF;
+    for (i = 0; i < __osMaxControllers; i++) {
+        *((__OSContReadHeader*)bufptr) = read;
+        bufptr += sizeof(read);
+    }
+    *((u8*)bufptr) = CONT_CMD_END;
+}
--- a/src/libultra_code_O2/osDpGetStatus.c
+++ b/src/libultra_code_O2/osDpGetStatus.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+u32 osDpGetStatus(void) {
+    return DPC_STATUS_REG;
+}
--- a/src/libultra_code_O2/osDpSetStatus.c
+++ b/src/libultra_code_O2/osDpSetStatus.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+void osDpSetStatus(u32 status) {
+    DPC_STATUS_REG = status;
+}
--- a/src/libultra_code_O2/osMempakAddrCRC.c
+++ b/src/libultra_code_O2/osMempakAddrCRC.c
@ -0,0 +1,65 @@
+#include "global.h"
+
+// Valid addr up to 0x7FF
+// It's the address of a block of 0x20 bytes in the mempak
+// So that means the whole mempak has a 16-bit address space
+u8 __osContAddressCrc(u16 addr) {
+    u32 addr32 = addr;
+    u32 ret = 0;
+    u32 bit;
+    s32 i;
+
+    for (bit = 0x400; bit; bit /= 2) {
+        ret *= 2;
+        if (addr32 & bit) {
+            if (ret & 0x20) {
+                ret ^= 0x14;
+            } else {
+                ++ret;
+            }
+        } else {
+            if (ret & 0x20) {
+                ret ^= 0x15;
+            }
+        }
+    }
+    for (i = 0; i < 5; ++i) {
+        ret <<= 1;
+        if (ret & 0x20) {
+            ret ^= 0x15;
+        }
+    }
+    return ret & 0x1f;
+}
+
+u8 __osContDataCrc(u8* data) {
+    s32 ret;
+    u32 bit;
+    u32 byte;
+
+    ret = 0;
+    for (byte = 0x20; byte; --byte, ++data) {
+        for (bit = 0x80; bit; bit /= 2) {
+            ret *= 2;
+            if ((*data & bit) != 0) {
+                if ((ret & 0x100) != 0) {
+                    ret ^= 0x84;
+                } else {
+                    ++ret;
+                }
+            } else {
+                if (ret & 0x100) {
+                    ret ^= 0x85;
+                }
+            }
+        }
+    }
+    do {
+        ret *= 2;
+        if (ret & 0x100) {
+            ret ^= 0x85;
+        }
+        ++byte;
+    } while (byte < 8U);
+    return ret;
+}
--- a/src/libultra_code_O2/osPfsDeleteFile.c
+++ b/src/libultra_code_O2/osPfsDeleteFile.c
@ -0,0 +1,70 @@
+#include "ultra64/pfs.h"
+#include "global.h"
+
+s32 osPfsDeleteFile(OSPfs* pfs, u16 companyCode, u32 gameCode, u8* gameName, u8* extName) {
+
+    s32 file_no;
+    s32 ret;
+    __OSInode inode;
+    __OSDir dir;
+    __OSInodeUnit last_page;
+    u8 startpage;
+    u8 bank;
+
+    if ((companyCode == 0) || (gameCode == 0)) {
+        return PFS_ERR_INVALID;
+    }
+    if ((ret = osPfsFindFile(pfs, companyCode, gameCode, gameName, extName, &file_no)) != 0) {
+        return ret;
+    }
+    if ((pfs->activebank != 0) && (ret = __osPfsSelectBank(pfs, 0)) != 0) {
+        return ret;
+    }
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, pfs->dir_table + file_no, (u8*)&dir)) != 0) {
+        return ret;
+    }
+    startpage = dir.start_page.inode_t.page;
+    for (bank = dir.start_page.inode_t.bank; bank < pfs->banks;) {
+        if ((ret = __osPfsRWInode(pfs, &inode, PFS_READ, bank)) != 0) {
+            return ret;
+        }
+        if ((ret = __osPfsReleasePages(pfs, &inode, startpage, bank, &last_page)) != 0) {
+            return ret;
+        }
+        if ((ret = __osPfsRWInode(pfs, &inode, PFS_WRITE, bank)) != 0) {
+            return ret;
+        }
+        if (last_page.ipage == PFS_EOF) {
+            break;
+        }
+        bank = last_page.inode_t.bank;
+        startpage = last_page.inode_t.page;
+    }
+
+    if (bank >= pfs->banks) {
+        return PFS_ERR_INCONSISTENT;
+    }
+    bzero(&dir, sizeof(__OSDir));
+
+    ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->dir_table + file_no, (u8*)&dir, 0);
+    return ret;
+}
+
+s32 __osPfsReleasePages(OSPfs* pfs, __OSInode* inode, u8 initialPage, u8 bank, __OSInodeUnit* finalPage) {
+    __OSInodeUnit next;
+    __OSInodeUnit prev;
+    s32 ret = 0;
+
+    next.ipage = (u16)((bank << 8) + initialPage);
+
+    do {
+        prev = next;
+        next = inode->inodePage[next.inode_t.page];
+        inode->inodePage[prev.inode_t.page].ipage = PFS_PAGE_NOT_USED;
+    } while (next.ipage >= pfs->inodeStartPage && next.inode_t.bank == bank);
+
+    *finalPage = next;
+
+    return ret;
+}
--- a/src/libultra_code_O2/osPfsFindFile.c
+++ b/src/libultra_code_O2/osPfsFindFile.c
@ -0,0 +1,53 @@
+#include "ultra64.h"
+#include "global.h"
+
+s32 osPfsFindFile(OSPfs* pfs, u16 companyCode, u32 gameCode, u8* gameName, u8* extName, s32* fileNo) {
+    s32 j;
+    s32 i;
+    __OSDir dir;
+    s32 ret = 0;
+    s32 err;
+
+    if (!(pfs->status & PFS_INITIALIZED)) {
+        return PFS_ERR_INVALID;
+    }
+
+    if ((ret = __osCheckId(pfs)) != 0) {
+        return ret;
+    }
+
+    for (j = 0; j < pfs->dir_size; j++) {
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, pfs->dir_table + j, &dir)) != 0) {
+            return ret;
+        }
+        if ((ret = __osPfsGetStatus(pfs->queue, pfs->channel)) != 0) {
+            return ret;
+        }
+
+        if ((dir.company_code == companyCode) && (dir.game_code == gameCode)) {
+            err = 0;
+            if (gameName != 0) {
+                for (i = 0; i < PFS_FILE_NAME_LEN; i++) {
+                    if (dir.game_name[i] != gameName[i]) {
+                        err = 1;
+                        break;
+                    }
+                }
+            }
+            if ((extName != 0) && (err == 0)) {
+                for (i = 0; i < PFS_FILE_EXT_LEN; i++) {
+                    if (dir.ext_name[i] != extName[i]) {
+                        err = 1;
+                        break;
+                    }
+                }
+            }
+            if (err == 0) {
+                *fileNo = j;
+                return ret;
+            }
+        }
+    }
+    *fileNo = -1;
+    return PFS_ERR_INVALID;
+}
--- a/src/libultra_code_O2/osPfsFreeBlocks.c
+++ b/src/libultra_code_O2/osPfsFreeBlocks.c
@ -0,0 +1,36 @@
+#include "ultra64.h"
+#include "global.h"
+#include "ultra64/pfs.h"
+
+s32 osPfsFreeBlocks(OSPfs* pfs, s32* leftoverBytes) {
+    s32 j;
+    s32 pages = 0;
+    __OSInode inode;
+    s32 ret = 0;
+    u8 bank;
+    s32 offset;
+
+    if (!(pfs->status & PFS_INITIALIZED)) {
+        return (PFS_ERR_INVALID);
+    }
+
+    if ((ret = __osCheckId(pfs)) != 0) {
+        return ret;
+    }
+
+    for (bank = PFS_ID_BANK_256K; bank < pfs->banks; bank++) {
+        if ((ret = __osPfsRWInode(pfs, &inode, PFS_READ, bank)) != 0) {
+            return ret;
+        }
+
+        offset = ((bank > PFS_ID_BANK_256K) ? 1 : pfs->inodeStartPage);
+        for (j = offset; j < PFS_INODE_SIZE_PER_PAGE; j++) {
+            if (inode.inodePage[j].ipage == PFS_PAGE_NOT_USED) {
+                pages++;
+            }
+        }
+    }
+
+    *leftoverBytes = pages * PFS_ONE_PAGE * BLOCKSIZE;
+    return 0;
+}
--- a/src/libultra_code_O2/osPfsIsPlug.c
+++ b/src/libultra_code_O2/osPfsIsPlug.c
@ -0,0 +1,93 @@
+#include "ultra64.h"
+#include "global.h"
+
+s32 osPfsIsPlug(OSMesgQueue* mq, u8* pattern) {
+    s32 ret = 0;
+    OSMesg msg;
+    u8 bitpattern;
+    OSContStatus contData[MAXCONTROLLERS];
+    s32 channel;
+    u8 bits = 0;
+    s32 crcErrorCount = 3;
+
+    __osSiGetAccess();
+
+    do {
+        __osPfsRequestData(CONT_CMD_REQUEST_STATUS);
+
+        ret = __osSiRawStartDma(OS_WRITE, &pifMempakBuf);
+        osRecvMesg(mq, &msg, OS_MESG_BLOCK);
+
+        ret = __osSiRawStartDma(OS_READ, &pifMempakBuf);
+        osRecvMesg(mq, &msg, OS_MESG_BLOCK);
+
+        __osPfsGetInitData(&bitpattern, &contData[0]);
+
+        for (channel = 0; channel < __osMaxControllers; channel++) {
+            if ((contData[channel].status & CONT_ADDR_CRC_ER) == 0) {
+                crcErrorCount--;
+                break;
+            }
+        }
+        if (channel == __osMaxControllers) {
+            crcErrorCount = 0;
+        }
+    } while (crcErrorCount > 0);
+
+    for (channel = 0; channel < __osMaxControllers; channel++) {
+        if ((contData[channel].errno == 0) && ((contData[channel].status & CONT_CARD_ON) != 0)) {
+            bits |= (1 << channel);
+        }
+    }
+    __osSiRelAccess();
+    *pattern = bits;
+    return ret;
+}
+
+void __osPfsRequestData(u8 poll) {
+    u8* bufPtr = &pifMempakBuf;
+    __OSContRequestHeader req;
+    s32 i;
+
+    __osContLastPoll = poll;
+
+    pifMempakBuf.status = 1;
+
+    req.align = 0xFF;
+    req.txsize = 1;
+    req.rxsize = 3;
+    req.poll = poll;
+    req.typeh = 0xFF;
+    req.typel = 0xFF;
+    req.status = 0xFF;
+    req.align1 = 0xFF;
+
+    for (i = 0; i < __osMaxControllers; i++) {
+        *((__OSContRequestHeader*)bufPtr) = req;
+        bufPtr += sizeof(req);
+    }
+    *((u8*)bufPtr) = CONT_CMD_END;
+}
+
+void __osPfsGetInitData(u8* pattern, OSContStatus* contData) {
+    u8* bufptr;
+    __OSContRequestHeader req;
+    s32 i;
+    u8 bits = 0;
+
+    bufptr = &pifMempakBuf;
+
+    for (i = 0; i < __osMaxControllers; i++, bufptr += sizeof(req), contData++) {
+        req = *((__OSContRequestHeader*)bufptr);
+        contData->errno = ((req.rxsize & 0xC0) >> 4);
+
+        if (contData->errno) {
+            continue;
+        }
+
+        contData->type = ((req.typel << 8) | req.typeh);
+        contData->status = req.status;
+        bits |= (1 << i);
+    }
+    *pattern = bits;
+}
--- a/src/libultra_code_O2/osReadMempak.c
+++ b/src/libultra_code_O2/osReadMempak.c
@ -0,0 +1,60 @@
+#include "global.h"
+
+#define BLOCKSIZE 32
+
+s32 __osPfsLastChannel = -1;
+
+s32 __osContRamRead(OSMesgQueue* ctrlrqueue, s32 channel, u16 addr, u8* data) {
+    s32 ret;
+    s32 i;
+    u8* bufptr;
+    s32 retryCount = 2;
+
+    __osSiGetAccess();
+    do {
+        bufptr = &pifMempakBuf;
+        if ((__osContLastPoll != 2) || (__osPfsLastChannel != channel)) {
+
+            __osContLastPoll = 2;
+            __osPfsLastChannel = channel;
+            // clang-format off
+            for (i = 0; i < channel; i++) { *bufptr++ = 0; }
+            // clang-format on
+            pifMempakBuf.status = 1;
+            ((__OSContRamHeader*)bufptr)->unk_00 = 0xFF;
+            ((__OSContRamHeader*)bufptr)->txsize = 3;
+            ((__OSContRamHeader*)bufptr)->rxsize = 0x21;
+            ((__OSContRamHeader*)bufptr)->poll = CONT_CMD_READ_MEMPACK; // read mempak; send byte 0
+            ((__OSContRamHeader*)bufptr)->datacrc = 0xFF;               // read mempak; send byte 0
+            // Received bytes are 6-26 inclusive
+            bufptr[sizeof(__OSContRamHeader)] = CONT_CMD_END; // End of commands
+        } else {
+            bufptr += channel;
+        }
+        ((__OSContRamHeader*)bufptr)->hi = addr >> 3;                                    // send byte 1
+        ((__OSContRamHeader*)bufptr)->lo = (s8)(__osContAddressCrc(addr) | (addr << 5)); // send byte 2
+        __osSiRawStartDma(OS_WRITE, &pifMempakBuf);
+        osRecvMesg(ctrlrqueue, NULL, OS_MESG_BLOCK);
+        __osSiRawStartDma(OS_READ, &pifMempakBuf);
+        osRecvMesg(ctrlrqueue, NULL, OS_MESG_BLOCK);
+        ret = (((__OSContRamHeader*)bufptr)->rxsize & 0xC0) >> 4;
+        if (!ret) {
+            if (((__OSContRamHeader*)bufptr)->datacrc != __osContDataCrc(bufptr + 6)) {
+                ret = __osPfsGetStatus(ctrlrqueue, channel);
+                if (ret) {
+                    break;
+                }
+                ret = 4; // Retry
+            } else {
+                bcopy(bufptr + 6, data, BLOCKSIZE);
+            }
+        } else {
+            ret = 1; // Error
+        }
+        if (ret != 4) {
+            break;
+        }
+    } while (0 <= retryCount--);
+    __osSiRelAccess();
+    return ret;
+}
--- a/src/libultra_code_O2/osRumblePak.c
+++ b/src/libultra_code_O2/osRumblePak.c
@ -0,0 +1,115 @@
+#include "global.h"
+
+#define BANK_ADDR 0x400
+#define MOTOR_ID 0x80
+
+OSPifRam osPifBuffers[MAXCONTROLLERS];
+
+// func_800CF990 in 1.0
+s32 osSetRumble(OSPfs* pfs, u32 vibrate) {
+    s32 i;
+    s32 ret;
+    u8* buf;
+
+    buf = (u8*)&osPifBuffers[pfs->channel];
+    if (!(pfs->status & 8)) {
+        return 5;
+    }
+    __osSiGetAccess();
+    osPifBuffers[pfs->channel].status = 1;
+    buf += pfs->channel;
+    for (i = 0; i < BLOCKSIZE; i++) {
+        ((__OSContRamHeader*)buf)->data[i] = vibrate;
+    }
+
+    __osContLastPoll = CONT_CMD_END;
+    __osSiRawStartDma(OS_WRITE, &osPifBuffers[pfs->channel]);
+    osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK);
+    __osSiRawStartDma(OS_READ, &osPifBuffers[pfs->channel]);
+    osRecvMesg(pfs->queue, NULL, OS_MESG_BLOCK);
+    ret = ((__OSContRamHeader*)buf)->rxsize & 0xC0;
+    if (!ret) {
+        if (!vibrate) {
+            if (((__OSContRamHeader*)buf)->datacrc != 0) {
+                ret = PFS_ERR_CONTRFAIL;
+            }
+        } else {
+            if (((__OSContRamHeader*)buf)->datacrc != 0xEB) {
+                ret = PFS_ERR_CONTRFAIL;
+            }
+        }
+    }
+    __osSiRelAccess();
+    return ret;
+}
+
+void osSetUpMempakWrite(s32 channel, OSPifRam* buf) {
+    u8* bufptr = (u8*)buf;
+    __OSContRamHeader mempakwr;
+    s32 i;
+    mempakwr.unk_00 = 0xFF;
+    mempakwr.txsize = 0x23;
+    mempakwr.rxsize = 1;
+    mempakwr.poll = 3; // write mempak
+    mempakwr.hi = 0x600 >> 3;
+    mempakwr.lo = (u8)(__osContAddressCrc(0x600) | (0x600 << 5));
+    if (channel != 0) {
+        for (i = 0; i < channel; ++i) {
+            *bufptr++ = 0;
+        }
+    }
+    *(__OSContRamHeader*)bufptr = mempakwr;
+    bufptr += sizeof(mempakwr);
+    *bufptr = 0xFE;
+}
+
+s32 osProbeRumblePak(OSMesgQueue* ctrlrqueue, OSPfs* pfs, u32 channel) {
+    s32 ret;
+    u8 sp24[BLOCKSIZE];
+
+    pfs->queue = ctrlrqueue;
+    pfs->channel = channel;
+    pfs->activebank = 0xFF;
+    pfs->status = 0;
+
+    ret = __osPfsSelectBank(pfs, 0xFE);
+    if (ret == 2) {
+        ret = __osPfsSelectBank(pfs, MOTOR_ID);
+    }
+    if (ret != 0) {
+        return ret;
+    }
+    ret = __osContRamRead(ctrlrqueue, channel, BANK_ADDR, sp24);
+    ret = ret;
+    if (ret == 2) {
+        ret = 4; // "Controller pack communication error"
+    }
+    if (ret != 0) {
+        return ret;
+    }
+    if (sp24[BLOCKSIZE - 1] == 0xFE) {
+        return 0xB;
+    }
+    ret = __osPfsSelectBank(pfs, MOTOR_ID);
+    if (ret == 2) {
+        ret = 4; // "Controller pack communication error"
+    }
+    if (ret != 0) {
+        return ret;
+    }
+    ret = __osContRamRead(ctrlrqueue, channel, BANK_ADDR, sp24);
+    if (ret == 2) {
+        ret = 4; // "Controller pack communication error"
+    }
+    if (ret != 0) {
+        return ret;
+    }
+    if (sp24[BLOCKSIZE - 1] != MOTOR_ID) {
+        return 0xB;
+    }
+    if ((pfs->status & PFS_MOTOR_INITIALIZED) == 0) {
+        osSetUpMempakWrite(channel, &osPifBuffers[channel]);
+    }
+    pfs->status = PFS_MOTOR_INITIALIZED;
+    return 0; // "Recognized rumble pak"
+}
--- a/src/libultra_code_O2/osSpTaskYield.c
+++ b/src/libultra_code_O2/osSpTaskYield.c
@ -0,0 +1,5 @@
+#include "global.h"
+
+void osSpTaskYield() {
+    __osSpSetStatus(SP_STATUS_SIG3);
+}
--- a/src/libultra_code_O2/osSpTaskYielded.c
+++ b/src/libultra_code_O2/osSpTaskYielded.c
@ -0,0 +1,21 @@
+#include "global.h"
+
+u32 osSpTaskYielded(OSTask* task) {
+    u32 ret;
+    u32 status;
+
+    status = __osSpGetStatus();
+
+    if (status & SP_STATUS_YIELDED) {
+        ret = OS_TASK_YIELDED;
+    } else {
+        ret = 0;
+    }
+
+    if (status & SP_STATUS_YIELD) {
+        task->t.flags |= ret;
+        task->t.flags &= ~OS_TASK_DP_WAIT;
+    }
+
+    return ret;
+}
--- a/src/libultra_code_O2/osViGetCurrentFramebuffer.c
+++ b/src/libultra_code_O2/osViGetCurrentFramebuffer.c
@ -0,0 +1,12 @@
+#include "global.h"
+
+u32* osViGetCurrentFramebuffer() {
+    register u32 sMask = __osDisableInt();
+    u32* var1;
+
+    var1 = __osViCurr->buffer;
+
+    __osRestoreInt(sMask);
+
+    return var1;
+}
--- a/src/libultra_code_O2/osViSetEvent.c
+++ b/src/libultra_code_O2/osViSetEvent.c
@ -0,0 +1,12 @@
+#include "global.h"
+
+extern OSViContext* __osViNext;
+
+void osViSetEvent(OSMesgQueue* mq, OSMesg msg, u32 retraceCount) {
+    register u32 saveMask;
+    saveMask = __osDisableInt();
+    __osViNext->mq = mq;
+    __osViNext->msg = msg;
+    __osViNext->retraceCount = retraceCount;
+    __osRestoreInt(saveMask);
+}
--- a/src/libultra_code_O2/pfsallocatefile.c
+++ b/src/libultra_code_O2/pfsallocatefile.c
@ -0,0 +1,137 @@
+#include "ultra64.h"
+#include "global.h"
+#include "ultra64/pfs.h"
+
+s32 osPfsAllocateFile(OSPfs* pfs, u16 companyCode, u32 gameCode, u8* gameName, u8* extName, s32 fileSize, s32* fileNo) {
+    s32 startPage, decleared, prevPage;
+    s32 oldPrevPage = 0;
+    s32 ret = 0;
+    s32 fileSizeInPages;
+    __OSInode inode, backup_inode;
+    __OSDir dir;
+    u8 bank;
+    u8 prevBank = 0;
+    s32 firsttime = 0;
+    s32 bytes;
+    __OSInodeUnit fpage;
+
+    if ((companyCode == 0) || (gameCode == 0)) {
+        return PFS_ERR_INVALID;
+    }
+
+    fileSizeInPages = (fileSize + PFS_PAGE_SIZE - 1) / PFS_PAGE_SIZE;
+
+    if (((ret = osPfsFindFile(pfs, companyCode, gameCode, gameName, extName, fileNo)) != 0) &&
+        (ret != PFS_ERR_INVALID)) {
+        return ret;
+    }
+    if (*fileNo != -1) {
+        return PFS_ERR_EXIST;
+    }
+
+    ret = osPfsFreeBlocks(pfs, &bytes);
+    if (fileSize > bytes) {
+        return PFS_DATA_FULL;
+    }
+
+    if (fileSizeInPages == 0) {
+        return (PFS_ERR_INVALID);
+    }
+
+    if (((ret = osPfsFindFile(pfs, 0, 0, 0, 0, fileNo)) != 0) && (ret != PFS_ERR_INVALID)) {
+        return ret;
+    }
+    if (*fileNo == -1) {
+        return PFS_DIR_FULL;
+    }
+
+    for (bank = PFS_ID_BANK_256K; bank < pfs->banks; bank++) {
+        if ((ret = __osPfsRWInode(pfs, &inode, PFS_READ, bank)) != 0) {
+            return ret;
+        }
+        ret = __osPfsDeclearPage(pfs, &inode, fileSizeInPages, &startPage, bank, &decleared, &prevPage);
+        if (ret) {
+            return ret;
+        }
+        if (startPage != -1) { /* There is free space */
+            if (firsttime == 0) {
+                fpage.inode_t.page = (u8)startPage;
+                fpage.inode_t.bank = bank;
+            } else { /* Writing previous bank inode */
+                backup_inode.inodePage[oldPrevPage].inode_t.bank = bank;
+                backup_inode.inodePage[oldPrevPage].inode_t.page = (u8)startPage;
+                if ((ret = __osPfsRWInode(pfs, &backup_inode, PFS_WRITE, prevBank)) != 0) {
+                    return ret;
+                }
+            }
+            if (fileSizeInPages > decleared) {
+                bcopy(&inode, &backup_inode, sizeof(__OSInode));
+                oldPrevPage = prevPage;
+                prevBank = bank;
+                fileSizeInPages -= decleared;
+                firsttime++;
+            } else {
+                fileSizeInPages = 0;
+                if ((ret = __osPfsRWInode(pfs, &inode, PFS_WRITE, bank)) != 0) {
+                    return ret;
+                }
+                break;
+            }
+        }
+    }
+    if ((fileSizeInPages > 0) || (startPage == -1)) {
+        return PFS_ERR_INCONSISTENT;
+    }
+
+    dir.start_page = fpage;
+    dir.company_code = companyCode;
+    dir.game_code = gameCode;
+    dir.data_sum = 0;
+
+    bcopy(gameName, dir.game_name, PFS_FILE_NAME_LEN);
+    bcopy(extName, dir.ext_name, PFS_FILE_EXT_LEN);
+
+    ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->dir_table + *fileNo, &dir, 0);
+    return ret;
+}
+
+s32 __osPfsDeclearPage(OSPfs* pfs, __OSInode* inode, s32 fileSizeInPages, s32* startPage, u8 bank, s32* decleared,
+                       s32* finalPage) {
+    s32 j;
+    s32 spage, prevPage;
+    s32 ret = 0;
+    s32 offset;
+
+    offset = ((bank > PFS_ID_BANK_256K) ? 1 : pfs->inodeStartPage);
+    for (j = offset; j < PFS_INODE_SIZE_PER_PAGE; j++) {
+        if (inode->inodePage[j].ipage == PFS_PAGE_NOT_USED) {
+            break;
+        }
+    }
+    if (j == PFS_INODE_SIZE_PER_PAGE) {
+        *startPage = -1;
+        return ret;
+    }
+
+    spage = j;
+    *decleared = 1;
+    prevPage = j;
+    j++;
+    while ((fileSizeInPages > *decleared) && (j < PFS_INODE_SIZE_PER_PAGE)) {
+        if (inode->inodePage[j].ipage == PFS_PAGE_NOT_USED) {
+            inode->inodePage[prevPage].inode_t.bank = (u8)bank;
+            inode->inodePage[prevPage].inode_t.page = (u8)j;
+            prevPage = j;
+            (*decleared)++;
+        }
+        j++;
+    }
+    *startPage = spage;
+    if ((j == (PFS_INODE_SIZE_PER_PAGE)) && (fileSizeInPages > *decleared)) {
+        *finalPage = prevPage;
+    } else {
+        inode->inodePage[prevPage].ipage = PFS_EOF;
+        *finalPage = 0;
+    }
+    return ret;
+}
--- a/src/libultra_code_O2/pfschecker.c
+++ b/src/libultra_code_O2/pfschecker.c
@ -0,0 +1,203 @@
+#include "ultra64.h"
+#include "global.h"
+#include "ultra64/pfs.h"
+
+#define CHECK_IPAGE(p)                                                                                      \
+    (((p).ipage >= pfs->inodeStartPage) && ((p).inode_t.bank < pfs->banks) && ((p).inode_t.page >= 0x01) && \
+     ((p).inode_t.page < 0x80))
+
+s32 osPfsChecker(OSPfs* pfs) {
+    s32 j;
+    s32 ret;
+    __OSInodeUnit next;
+    __OSInode checkedInode;
+    __OSInode tempInode;
+    __OSDir tempDir;
+    __OSInodeUnit nextNodeInFile[16];
+
+    __OSInodeCache cache;
+
+    s32 fixed = 0;
+    u8 bank, prevBank = 254;
+    s32 cc, cl;
+    s32 offset;
+
+    ret = __osCheckId(pfs);
+
+    if (ret == PFS_ERR_NEW_PACK) {
+        ret = __osGetId(pfs);
+    }
+
+    if (ret) {
+        return ret;
+    }
+
+    if ((ret = func_80105788(pfs, &cache)) != 0) {
+        return ret;
+    }
+
+    for (j = 0; j < pfs->dir_size; j++) {
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, pfs->dir_table + j, &tempDir)) != 0) {
+            return ret;
+        }
+
+        if ((tempDir.company_code != 0) || (tempDir.game_code != 0)) {
+            if ((tempDir.company_code == 0) || (tempDir.game_code == 0)) {
+                cc = -1;
+            } else {
+                next = tempDir.start_page;
+
+                cl = cc = 0;
+                bank = 255;
+
+                while (CHECK_IPAGE(next)) {
+                    if (bank != next.inode_t.bank) {
+                        bank = next.inode_t.bank;
+                        if (prevBank != bank) {
+                            ret = __osPfsRWInode(pfs, &tempInode, PFS_READ, bank);
+                            prevBank = bank;
+                        }
+
+                        if ((ret != 0) && (ret != PFS_ERR_INCONSISTENT)) {
+                            return ret;
+                        }
+                    }
+
+                    if ((cc = func_80105A60(pfs, next, &cache) - cl) != 0) {
+                        break;
+                    }
+                    cl = 1;
+
+                    next = tempInode.inodePage[next.inode_t.page];
+                }
+            }
+
+            if ((cc != 0) || (next.ipage != PFS_EOF)) {
+                bzero(&tempDir, sizeof(__OSDir));
+                if (pfs->activebank != 0) {
+                    if ((ret = __osPfsSelectBank(pfs, 0)) != 0) {
+                        return ret;
+                    }
+                }
+                if ((ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->dir_table + j, &tempDir, 0)) != 0) {
+                    return ret;
+                }
+
+                fixed++;
+            }
+        }
+    }
+    for (j = 0; j < pfs->dir_size; j++) {
+        if ((ret = __osContRamRead(pfs->queue, pfs->channel, pfs->dir_table + j, &tempDir)) != 0) {
+            return ret;
+        }
+
+        if ((tempDir.company_code != 0) && (tempDir.game_code != 0) &&
+            (tempDir.start_page.ipage >= (u16)pfs->inodeStartPage)) { // cast required
+            nextNodeInFile[j].ipage = tempDir.start_page.ipage;
+        } else {
+            nextNodeInFile[j].ipage = 0;
+        }
+    }
+
+    for (bank = 0; bank < pfs->banks; bank++) {
+        ret = __osPfsRWInode(pfs, &tempInode, PFS_READ, bank);
+        if ((ret != 0) && (ret != PFS_ERR_INCONSISTENT)) {
+            return (ret);
+        }
+        offset = ((bank > PFS_ID_BANK_256K) ? 1 : pfs->inodeStartPage);
+        for (j = 0; j < offset; j++) {
+            checkedInode.inodePage[j].ipage = tempInode.inodePage[j].ipage;
+        }
+        for (; j < PFS_INODE_SIZE_PER_PAGE; j++) {
+            checkedInode.inodePage[j].ipage = PFS_PAGE_NOT_USED;
+        }
+
+        for (j = 0; j < pfs->dir_size; j++) {
+            while (nextNodeInFile[j].inode_t.bank == bank &&
+                   nextNodeInFile[j].ipage >= (u16)pfs->inodeStartPage) { // cast required
+                u8 val;
+                val = nextNodeInFile[j].inode_t.page;
+                nextNodeInFile[j] = checkedInode.inodePage[val] = tempInode.inodePage[val];
+            }
+        }
+        if ((ret = __osPfsRWInode(pfs, &checkedInode, PFS_WRITE, bank)) != 0) {
+            return ret;
+        }
+    }
+
+    if (fixed != 0) {
+        pfs->status |= PFS_CORRUPTED;
+    } else {
+        pfs->status &= ~PFS_CORRUPTED;
+    }
+    return 0;
+}
+
+// Original name: corrupted_init (probably needs better name)
+s32 func_80105788(OSPfs* pfs, __OSInodeCache* cache) {
+    s32 i;
+    s32 n;
+    s32 offset;
+    u8 bank;
+    __OSInodeUnit tpage;
+    __OSInode tempInode;
+    s32 ret;
+
+    for (i = 0; i < PFS_INODE_DIST_MAP; i++) {
+        cache->map[i] = 0;
+    }
+    cache->bank = 255;
+
+    for (bank = PFS_ID_BANK_256K; bank < pfs->banks; bank++) {
+        offset = ((bank > PFS_ID_BANK_256K) ? 1 : pfs->inodeStartPage);
+        ret = __osPfsRWInode(pfs, &tempInode, PFS_READ, bank);
+        if ((ret != 0) && (ret != PFS_ERR_INCONSISTENT)) {
+            return ret;
+        }
+
+        for (i = offset; i < PFS_INODE_SIZE_PER_PAGE; i++) {
+            tpage = tempInode.inodePage[i];
+            if ((tpage.ipage >= pfs->inodeStartPage) && (tpage.inode_t.bank != bank)) {
+                n = ((tpage.inode_t.page & 0x7F) / PFS_SECTOR_SIZE) +
+                    PFS_SECTOR_PER_BANK * (tpage.inode_t.bank % PFS_BANK_LAPPED_BY);
+                cache->map[n] |= (1 << (bank % PFS_BANK_LAPPED_BY));
+            }
+        }
+    }
+    return 0;
+}
+
+// original name: corrupted (probably needs a better name)
+s32 func_80105A60(OSPfs* pfs, __OSInodeUnit fpage, __OSInodeCache* cache) {
+    s32 j;
+    s32 n;
+    s32 hit = 0;
+    u8 bank;
+    s32 offset;
+    s32 ret = 0;
+
+    n = (fpage.inode_t.page / PFS_SECTOR_SIZE) + PFS_SECTOR_PER_BANK * (fpage.inode_t.bank % PFS_BANK_LAPPED_BY);
+
+    for (bank = PFS_ID_BANK_256K; bank < pfs->banks; bank++) {
+        offset = ((bank > PFS_ID_BANK_256K) ? 1 : pfs->inodeStartPage);
+        if ((bank == fpage.inode_t.bank) || (cache->map[n] & (1 << (bank % PFS_BANK_LAPPED_BY))) != 0) {
+            if (bank != cache->bank) {
+                ret = __osPfsRWInode(pfs, &(cache->inode), PFS_READ, bank);
+                if ((ret) && (ret != PFS_ERR_INCONSISTENT)) {
+                    return ret;
+                }
+                cache->bank = bank;
+            }
+            for (j = offset; ((hit < 2) && (j < PFS_INODE_SIZE_PER_PAGE)); j++) {
+                if (cache->inode.inodePage[j].ipage == fpage.ipage) {
+                    hit++;
+                }
+            }
+            if (hit >= 2) {
+                return 2;
+            }
+        }
+    }
+    return hit;
+}
--- a/src/libultra_code_O2/pfsinitpak.c
+++ b/src/libultra_code_O2/pfsinitpak.c
@ -0,0 +1,112 @@
+#include "ultra64.h"
+#include "global.h"
+
+s32 osPfsInitPak(OSMesgQueue* queue, OSPfs* pfs, s32 channel) {
+    s32 ret;
+    u16 sum;
+    u16 isum;
+    u8 temp[BLOCKSIZE];
+    __OSPackId* id;
+    __OSPackId newid;
+
+    __osSiGetAccess();
+
+    ret = __osPfsGetStatus(queue, channel);
+
+    __osSiRelAccess();
+
+    if (ret != 0) {
+        return ret;
+    }
+
+    pfs->queue = queue;
+    pfs->channel = channel;
+    pfs->status = 0;
+
+    if ((ret = __osPfsCheckRamArea(pfs)) != 0) {
+        return ret;
+    }
+
+    if ((ret = __osPfsSelectBank(pfs, 0)) != 0) {
+        return ret;
+    }
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_ID_0AREA, temp)) != 0) {
+        return (ret);
+    }
+
+    __osIdCheckSum((u16*)temp, &sum, &isum);
+    id = (__OSPackId*)temp;
+    if ((id->checksum != sum) || (id->invertedChecksum != isum)) {
+        if ((ret = __osCheckPackId(pfs, id)) != 0) {
+            pfs->status |= PFS_ID_BROKEN;
+            return ret;
+        }
+    }
+
+    if ((id->deviceid & 0x01) == 0) {
+        ret = __osRepairPackId(pfs, id, &newid);
+        if (ret) {
+            if (ret == PFS_ERR_ID_FATAL) {
+                pfs->status |= PFS_ID_BROKEN;
+            }
+            return ret;
+        }
+        id = &newid;
+        if ((id->deviceid & 0x01) == 0) {
+            return PFS_ERR_DEVICE;
+        }
+    }
+
+    bcopy(id, pfs->id, BLOCKSIZE);
+
+    if (0) {}
+
+    pfs->version = id->version;
+    pfs->banks = id->banks;
+    pfs->inodeStartPage = 1 + DEF_DIR_PAGES + (2 * pfs->banks);
+    pfs->dir_size = DEF_DIR_PAGES * PFS_ONE_PAGE;
+    pfs->inode_table = 1 * PFS_ONE_PAGE;
+    pfs->minode_table = (1 + pfs->banks) * PFS_ONE_PAGE;
+    pfs->dir_table = pfs->minode_table + (pfs->banks * PFS_ONE_PAGE);
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, PFS_LABEL_AREA, pfs->label)) != 0) {
+        return ret;
+    }
+
+    ret = osPfsChecker(pfs);
+    pfs->status |= PFS_INITIALIZED;
+
+    return ret;
+}
+
+s32 __osPfsCheckRamArea(OSPfs* pfs) {
+    s32 i = 0;
+    s32 ret = 0;
+    u8 temp1[BLOCKSIZE];
+    u8 temp2[BLOCKSIZE];
+    u8 saveReg[BLOCKSIZE];
+
+    if ((ret = __osPfsSelectBank(pfs, PFS_ID_BANK_256K)) != 0) {
+        return ret;
+    }
+
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, 0, saveReg)) != 0) {
+        return (ret);
+    }
+    for (i = 0; i < BLOCKSIZE; i++) {
+        temp1[i] = i;
+    }
+    if ((ret = __osContRamWrite(pfs->queue, pfs->channel, 0, temp1, 0)) != 0) {
+        return ret;
+    }
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, 0, temp2)) != 0) {
+        return ret;
+    }
+    if (bcmp(temp1, temp2, BLOCKSIZE) != 0) {
+        return PFS_ERR_DEVICE;
+    }
+    ret = __osContRamWrite(pfs->queue, pfs->channel, 0, saveReg, 0);
+
+    return ret;
+}
--- a/src/libultra_code_O2/pfsreadwritefile.c
+++ b/src/libultra_code_O2/pfsreadwritefile.c
@ -0,0 +1,140 @@
+#include "ultra64.h"
+#include "global.h"
+
+#define CHECK_IPAGE(p, pfs)                                                                                   \
+    (((p).ipage >= (pfs).inodeStartPage) && ((p).inode_t.bank < (pfs).banks) && ((p).inode_t.page >= 0x01) && \
+     ((p).inode_t.page < 0x80))
+
+__OSInode __osPfsInodeCache;
+
+s32 __osPfsGetNextPage(OSPfs* pfs, u8* bank, __OSInode* inode, __OSInodeUnit* page) {
+    s32 ret;
+
+    if (page->inode_t.bank != *bank) {
+        *bank = page->inode_t.bank;
+        if ((ret = __osPfsRWInode(pfs, inode, PFS_READ, *bank)) != 0) {
+            return ret;
+        }
+    }
+    *page = inode->inodePage[page->inode_t.page];
+
+    if (!CHECK_IPAGE(*page, *pfs)) {
+        if (page->ipage == PFS_EOF) {
+            return PFS_ERR_INVALID;
+        }
+        return PFS_ERR_INCONSISTENT;
+    }
+    return 0;
+}
+
+s32 osPfsReadWriteFile(OSPfs* pfs, s32 fileNo, u8 flag, s32 offset, s32 size, u8* data) {
+    s32 ret;
+    __OSDir dir;
+    __OSInode inode;
+    __OSInodeUnit curPage;
+    s32 curBlock;
+    s32 blockSize;
+    u8* buffer;
+    u8 bank;
+    u16 blockno;
+
+    if ((fileNo >= pfs->dir_size) || (fileNo < 0)) {
+        return PFS_ERR_INVALID;
+    }
+    if ((size <= 0) || ((size % BLOCKSIZE) != 0)) {
+        return PFS_ERR_INVALID;
+    }
+    if ((offset < 0) || ((offset % BLOCKSIZE) != 0)) {
+        return PFS_ERR_INVALID;
+    }
+    if (!(pfs->status & PFS_INITIALIZED)) {
+        return PFS_ERR_INVALID;
+    }
+
+    if (__osCheckId(pfs) == PFS_ERR_NEW_PACK) {
+        return PFS_ERR_NEW_PACK;
+    }
+
+    if (pfs->activebank != 0) {
+        if ((ret = __osPfsSelectBank(pfs, 0)) != 0) {
+            return ret;
+        }
+    }
+    if ((ret = __osContRamRead(pfs->queue, pfs->channel, pfs->dir_table + fileNo, &dir)) != 0) {
+        return ret;
+    }
+
+    if ((dir.company_code == 0) || (dir.game_code == 0)) {
+        return PFS_ERR_INVALID;
+    }
+
+    if (!CHECK_IPAGE(dir.start_page, *pfs)) {
+        if (dir.start_page.ipage == PFS_EOF) {
+            return PFS_ERR_INVALID;
+        }
+        return PFS_ERR_INCONSISTENT;
+    }
+
+    if ((flag == PFS_READ) && ((dir.status & PFS_WRITTEN) == 0)) {
+        return PFS_ERR_BAD_DATA;
+    }
+
+    bank = 255;
+
+    curBlock = offset / BLOCKSIZE;
+    curPage = dir.start_page;
+
+    while (curBlock >= 8) {
+        if ((ret = __osPfsGetNextPage(pfs, &bank, &inode, &curPage)) != 0) {
+            return ret;
+        }
+        curBlock -= 8;
+    }
+
+    blockSize = size / BLOCKSIZE;
+    buffer = data;
+
+    while (blockSize > 0) {
+        if (curBlock == 8) {
+            if ((ret = __osPfsGetNextPage(pfs, &bank, &inode, &curPage)) != 0) {
+                return ret;
+            }
+            curBlock = 0;
+        }
+
+        if (pfs->activebank != curPage.inode_t.bank) {
+            if ((ret = __osPfsSelectBank(pfs, curPage.inode_t.bank)) != 0) {
+                return ret;
+            }
+        }
+
+        blockno = curPage.inode_t.page * PFS_ONE_PAGE + curBlock;
+        if (flag == PFS_READ) {
+            ret = __osContRamRead(pfs->queue, pfs->channel, blockno, buffer);
+        } else {
+            ret = __osContRamWrite(pfs->queue, pfs->channel, blockno, buffer, 0);
+        }
+        if (ret != 0) {
+            return ret;
+        }
+
+        buffer += BLOCKSIZE;
+        curBlock++;
+        blockSize--;
+    }
+
+    if (flag == PFS_WRITE && !(dir.status & PFS_WRITTEN)) {
+        dir.status |= PFS_WRITTEN;
+        if (pfs->activebank != 0) {
+            if ((ret = __osPfsSelectBank(pfs, 0)) != 0) {
+                return ret;
+            }
+        }
+        if ((ret = __osContRamWrite(pfs->queue, pfs->channel, pfs->dir_table + fileNo, &dir, 0)) != 0) {
+            return ret;
+        }
+    }
+
+    ret = __osPfsGetStatus(pfs->queue, pfs->channel);
+    return ret;
+}
--- a/src/libultra_code_O2/rotate.c
+++ b/src/libultra_code_O2/rotate.c
@ -0,0 +1,50 @@
+#include "global.h"
+
+void guRotateF(f32 m[4][4], f32 a, f32 x, f32 y, f32 z) {
+    static f32 D_80134D10 = M_PI / 180.0f;
+    f32 sine;
+    f32 cosine;
+    f32 ab;
+    f32 bc;
+    f32 ca;
+    f32 t;
+    f32 xs;
+    f32 ys;
+    f32 zs;
+
+    guNormalize(&x, &y, &z);
+
+    a = a * D_80134D10;
+
+    sine = sinf(a);
+    cosine = cosf(a);
+
+    ab = x * y * (1 - cosine);
+    bc = y * z * (1 - cosine);
+    ca = z * x * (1 - cosine);
+
+    guMtxIdentF(m);
+
+    xs = x * sine;
+    ys = y * sine;
+    zs = z * sine;
+
+    t = x * x;
+    m[0][0] = (1 - t) * cosine + t;
+    m[2][1] = bc - xs;
+    m[1][2] = bc + xs;
+    t = y * y;
+    m[1][1] = (1 - t) * cosine + t;
+    m[2][0] = ca + ys;
+    m[0][2] = ca - ys;
+    t = z * z;
+    m[2][2] = (1 - t) * cosine + t;
+    m[1][0] = ab - zs;
+    m[0][1] = ab + zs;
+}
+
+void guRotate(Mtx* m, f32 a, f32 x, f32 y, f32 z) {
+    f32 mf[4][4];
+    guRotateF(mf, a, x, y, z);
+    guMtxF2L(mf, m);
+}
--- a/src/libultra_code_O2/sinf.c
+++ b/src/libultra_code_O2/sinf.c
@ -0,0 +1,66 @@
+#include "global.h"
+#include "ultra64.h"
+
+static const du P[] = {
+    { 0x3FF00000, 0x00000000 }, { 0xBFC55554, 0xBC83656D }, { 0x3F8110ED, 0x3804C2A0 },
+    { 0xBF29F6FF, 0xEEA56814 }, { 0x3EC5DBDF, 0x0E314BFE },
+};
+
+static const du rpi = { 0x3FD45F30, 0x6DC9C883 };
+
+static const du pihi = { 0x400921FB, 0x50000000 };
+
+static const du pilo = { 0x3E6110B4, 0x611A6263 };
+
+static const fu zero = { 0x00000000 };
+
+f32 sinf(f32 x) {
+    f64 dx;
+    f64 xSq;
+    f64 polyApprox;
+    f64 dn;
+    s32 n;
+    f64 result;
+    s32 ix, xpt;
+
+    ix = *(s32*)&x;
+    xpt = (ix >> 22);
+    xpt &= 0x1FF;
+
+    if (xpt < 0xFF) {
+        dx = x;
+
+        if (xpt >= 0xE6) {
+            xSq = SQ(dx);
+            polyApprox = ((P[4].d * xSq + P[3].d) * xSq + P[2].d) * xSq + P[1].d;
+            result = dx + (dx * xSq) * polyApprox;
+            return (f32)result;
+        }
+        return x;
+    }
+
+    if (xpt < 0x136) {
+        dx = x;
+        dn = dx * rpi.d;
+        n = ROUND(dn);
+        dn = n;
+
+        dx -= dn * pihi.d;
+        dx -= dn * pilo.d;
+        xSq = SQ(dx);
+
+        polyApprox = ((P[4].d * xSq + P[3].d) * xSq + P[2].d) * xSq + P[1].d;
+        result = dx + (dx * xSq) * polyApprox;
+
+        if (!(n & 1)) {
+            return (f32)result;
+        }
+
+        return -(f32)result;
+    }
+
+    if (x != x) {
+        return __libm_qnan_f;
+    }
+    return zero.f;
+}
--- a/src/libultra_code_O2/sins.c
+++ b/src/libultra_code_O2/sins.c
@ -0,0 +1,20 @@
+#include "ultra64.h"
+#include "sintable.c"
+
+s16 sins(u16 x) {
+    s16 value;
+
+    x >>= 4;
+
+    if (x & 0x400) {
+        value = sintable[0x3FF - (x & 0x3FF)];
+    } else {
+        value = sintable[x & 0x3FF];
+    }
+
+    if (x & 0x800) {
+        return -value;
+    } else {
+        return value;
+    }
+}
--- a/src/libultra_code_O2/sintable.c
+++ b/src/libultra_code_O2/sintable.c
@ -0,0 +1,78 @@
+#include "ultra64/types.h"
+
+static s16 sintable[0x400] = {
+    0x0000, 0x0032, 0x0064, 0x0096, 0x00C9, 0x00FB, 0x012D, 0x0160, 0x0192, 0x01C4, 0x01F7, 0x0229, 0x025B, 0x028E,
+    0x02C0, 0x02F2, 0x0324, 0x0357, 0x0389, 0x03BB, 0x03EE, 0x0420, 0x0452, 0x0484, 0x04B7, 0x04E9, 0x051B, 0x054E,
+    0x0580, 0x05B2, 0x05E4, 0x0617, 0x0649, 0x067B, 0x06AD, 0x06E0, 0x0712, 0x0744, 0x0776, 0x07A9, 0x07DB, 0x080D,
+    0x083F, 0x0871, 0x08A4, 0x08D6, 0x0908, 0x093A, 0x096C, 0x099F, 0x09D1, 0x0A03, 0x0A35, 0x0A67, 0x0A99, 0x0ACB,
+    0x0AFE, 0x0B30, 0x0B62, 0x0B94, 0x0BC6, 0x0BF8, 0x0C2A, 0x0C5C, 0x0C8E, 0x0CC0, 0x0CF2, 0x0D25, 0x0D57, 0x0D89,
+    0x0DBB, 0x0DED, 0x0E1F, 0x0E51, 0x0E83, 0x0EB5, 0x0EE7, 0x0F19, 0x0F4B, 0x0F7C, 0x0FAE, 0x0FE0, 0x1012, 0x1044,
+    0x1076, 0x10A8, 0x10DA, 0x110C, 0x113E, 0x116F, 0x11A1, 0x11D3, 0x1205, 0x1237, 0x1269, 0x129A, 0x12CC, 0x12FE,
+    0x1330, 0x1361, 0x1393, 0x13C5, 0x13F6, 0x1428, 0x145A, 0x148C, 0x14BD, 0x14EF, 0x1520, 0x1552, 0x1584, 0x15B5,
+    0x15E7, 0x1618, 0x164A, 0x167B, 0x16AD, 0x16DF, 0x1710, 0x1741, 0x1773, 0x17A4, 0x17D6, 0x1807, 0x1839, 0x186A,
+    0x189B, 0x18CD, 0x18FE, 0x1930, 0x1961, 0x1992, 0x19C3, 0x19F5, 0x1A26, 0x1A57, 0x1A88, 0x1ABA, 0x1AEB, 0x1B1C,
+    0x1B4D, 0x1B7E, 0x1BAF, 0x1BE1, 0x1C12, 0x1C43, 0x1C74, 0x1CA5, 0x1CD6, 0x1D07, 0x1D38, 0x1D69, 0x1D9A, 0x1DCB,
+    0x1DFC, 0x1E2D, 0x1E5D, 0x1E8E, 0x1EBF, 0x1EF0, 0x1F21, 0x1F52, 0x1F82, 0x1FB3, 0x1FE4, 0x2015, 0x2045, 0x2076,
+    0x20A7, 0x20D7, 0x2108, 0x2139, 0x2169, 0x219A, 0x21CA, 0x21FB, 0x222B, 0x225C, 0x228C, 0x22BD, 0x22ED, 0x231D,
+    0x234E, 0x237E, 0x23AE, 0x23DF, 0x240F, 0x243F, 0x2470, 0x24A0, 0x24D0, 0x2500, 0x2530, 0x2560, 0x2591, 0x25C1,
+    0x25F1, 0x2621, 0x2651, 0x2681, 0x26B1, 0x26E1, 0x2711, 0x2740, 0x2770, 0x27A0, 0x27D0, 0x2800, 0x2830, 0x285F,
+    0x288F, 0x28BF, 0x28EE, 0x291E, 0x294E, 0x297D, 0x29AD, 0x29DD, 0x2A0C, 0x2A3C, 0x2A6B, 0x2A9B, 0x2ACA, 0x2AF9,
+    0x2B29, 0x2B58, 0x2B87, 0x2BB7, 0x2BE6, 0x2C15, 0x2C44, 0x2C74, 0x2CA3, 0x2CD2, 0x2D01, 0x2D30, 0x2D5F, 0x2D8E,
+    0x2DBD, 0x2DEC, 0x2E1B, 0x2E4A, 0x2E79, 0x2EA8, 0x2ED7, 0x2F06, 0x2F34, 0x2F63, 0x2F92, 0x2FC0, 0x2FEF, 0x301E,
+    0x304C, 0x307B, 0x30A9, 0x30D8, 0x3107, 0x3135, 0x3163, 0x3192, 0x31C0, 0x31EF, 0x321D, 0x324B, 0x3279, 0x32A8,
+    0x32D6, 0x3304, 0x3332, 0x3360, 0x338E, 0x33BC, 0x33EA, 0x3418, 0x3446, 0x3474, 0x34A2, 0x34D0, 0x34FE, 0x352B,
+    0x3559, 0x3587, 0x35B5, 0x35E2, 0x3610, 0x363D, 0x366B, 0x3698, 0x36C6, 0x36F3, 0x3721, 0x374E, 0x377C, 0x37A9,
+    0x37D6, 0x3803, 0x3831, 0x385E, 0x388B, 0x38B8, 0x38E5, 0x3912, 0x393F, 0x396C, 0x3999, 0x39C6, 0x39F3, 0x3A20,
+    0x3A4D, 0x3A79, 0x3AA6, 0x3AD3, 0x3B00, 0x3B2C, 0x3B59, 0x3B85, 0x3BB2, 0x3BDE, 0x3C0B, 0x3C37, 0x3C64, 0x3C90,
+    0x3CBC, 0x3CE9, 0x3D15, 0x3D41, 0x3D6D, 0x3D99, 0x3DC5, 0x3DF1, 0x3E1D, 0x3E49, 0x3E75, 0x3EA1, 0x3ECD, 0x3EF9,
+    0x3F25, 0x3F50, 0x3F7C, 0x3FA8, 0x3FD3, 0x3FFF, 0x402B, 0x4056, 0x4082, 0x40AD, 0x40D8, 0x4104, 0x412F, 0x415A,
+    0x4186, 0x41B1, 0x41DC, 0x4207, 0x4232, 0x425D, 0x4288, 0x42B3, 0x42DE, 0x4309, 0x4334, 0x435F, 0x4389, 0x43B4,
+    0x43DF, 0x4409, 0x4434, 0x445F, 0x4489, 0x44B4, 0x44DE, 0x4508, 0x4533, 0x455D, 0x4587, 0x45B1, 0x45DC, 0x4606,
+    0x4630, 0x465A, 0x4684, 0x46AE, 0x46D8, 0x4702, 0x472C, 0x4755, 0x477F, 0x47A9, 0x47D2, 0x47FC, 0x4826, 0x484F,
+    0x4879, 0x48A2, 0x48CC, 0x48F5, 0x491E, 0x4948, 0x4971, 0x499A, 0x49C3, 0x49EC, 0x4A15, 0x4A3E, 0x4A67, 0x4A90,
+    0x4AB9, 0x4AE2, 0x4B0B, 0x4B33, 0x4B5C, 0x4B85, 0x4BAD, 0x4BD6, 0x4BFE, 0x4C27, 0x4C4F, 0x4C78, 0x4CA0, 0x4CC8,
+    0x4CF0, 0x4D19, 0x4D41, 0x4D69, 0x4D91, 0x4DB9, 0x4DE1, 0x4E09, 0x4E31, 0x4E58, 0x4E80, 0x4EA8, 0x4ED0, 0x4EF7,
+    0x4F1F, 0x4F46, 0x4F6E, 0x4F95, 0x4FBD, 0x4FE4, 0x500B, 0x5032, 0x505A, 0x5081, 0x50A8, 0x50CF, 0x50F6, 0x511D,
+    0x5144, 0x516B, 0x5191, 0x51B8, 0x51DF, 0x5205, 0x522C, 0x5253, 0x5279, 0x52A0, 0x52C6, 0x52EC, 0x5313, 0x5339,
+    0x535F, 0x5385, 0x53AB, 0x53D1, 0x53F7, 0x541D, 0x5443, 0x5469, 0x548F, 0x54B5, 0x54DA, 0x5500, 0x5525, 0x554B,
+    0x5571, 0x5596, 0x55BB, 0x55E1, 0x5606, 0x562B, 0x5650, 0x5675, 0x569B, 0x56C0, 0x56E5, 0x5709, 0x572E, 0x5753,
+    0x5778, 0x579D, 0x57C1, 0x57E6, 0x580A, 0x582F, 0x5853, 0x5878, 0x589C, 0x58C0, 0x58E5, 0x5909, 0x592D, 0x5951,
+    0x5975, 0x5999, 0x59BD, 0x59E1, 0x5A04, 0x5A28, 0x5A4C, 0x5A6F, 0x5A93, 0x5AB7, 0x5ADA, 0x5AFD, 0x5B21, 0x5B44,
+    0x5B67, 0x5B8B, 0x5BAE, 0x5BD1, 0x5BF4, 0x5C17, 0x5C3A, 0x5C5D, 0x5C7F, 0x5CA2, 0x5CC5, 0x5CE7, 0x5D0A, 0x5D2D,
+    0x5D4F, 0x5D71, 0x5D94, 0x5DB6, 0x5DD8, 0x5DFA, 0x5E1D, 0x5E3F, 0x5E61, 0x5E83, 0x5EA5, 0x5EC6, 0x5EE8, 0x5F0A,
+    0x5F2C, 0x5F4D, 0x5F6F, 0x5F90, 0x5FB2, 0x5FD3, 0x5FF4, 0x6016, 0x6037, 0x6058, 0x6079, 0x609A, 0x60BB, 0x60DC,
+    0x60FD, 0x611E, 0x613E, 0x615F, 0x6180, 0x61A0, 0x61C1, 0x61E1, 0x6202, 0x6222, 0x6242, 0x6263, 0x6283, 0x62A3,
+    0x62C3, 0x62E3, 0x6303, 0x6323, 0x6342, 0x6362, 0x6382, 0x63A1, 0x63C1, 0x63E0, 0x6400, 0x641F, 0x643F, 0x645E,
+    0x647D, 0x649C, 0x64BB, 0x64DA, 0x64F9, 0x6518, 0x6537, 0x6556, 0x6574, 0x6593, 0x65B2, 0x65D0, 0x65EF, 0x660D,
+    0x662B, 0x664A, 0x6668, 0x6686, 0x66A4, 0x66C2, 0x66E0, 0x66FE, 0x671C, 0x673A, 0x6757, 0x6775, 0x6792, 0x67B0,
+    0x67CD, 0x67EB, 0x6808, 0x6825, 0x6843, 0x6860, 0x687D, 0x689A, 0x68B7, 0x68D4, 0x68F1, 0x690D, 0x692A, 0x6947,
+    0x6963, 0x6980, 0x699C, 0x69B9, 0x69D5, 0x69F1, 0x6A0E, 0x6A2A, 0x6A46, 0x6A62, 0x6A7E, 0x6A9A, 0x6AB5, 0x6AD1,
+    0x6AED, 0x6B08, 0x6B24, 0x6B40, 0x6B5B, 0x6B76, 0x6B92, 0x6BAD, 0x6BC8, 0x6BE3, 0x6BFE, 0x6C19, 0x6C34, 0x6C4F,
+    0x6C6A, 0x6C84, 0x6C9F, 0x6CBA, 0x6CD4, 0x6CEF, 0x6D09, 0x6D23, 0x6D3E, 0x6D58, 0x6D72, 0x6D8C, 0x6DA6, 0x6DC0,
+    0x6DDA, 0x6DF3, 0x6E0D, 0x6E27, 0x6E40, 0x6E5A, 0x6E73, 0x6E8D, 0x6EA6, 0x6EBF, 0x6ED9, 0x6EF2, 0x6F0B, 0x6F24,
+    0x6F3D, 0x6F55, 0x6F6E, 0x6F87, 0x6FA0, 0x6FB8, 0x6FD1, 0x6FE9, 0x7002, 0x701A, 0x7032, 0x704A, 0x7062, 0x707A,
+    0x7092, 0x70AA, 0x70C2, 0x70DA, 0x70F2, 0x7109, 0x7121, 0x7138, 0x7150, 0x7167, 0x717E, 0x7196, 0x71AD, 0x71C4,
+    0x71DB, 0x71F2, 0x7209, 0x7220, 0x7236, 0x724D, 0x7264, 0x727A, 0x7291, 0x72A7, 0x72BD, 0x72D4, 0x72EA, 0x7300,
+    0x7316, 0x732C, 0x7342, 0x7358, 0x736E, 0x7383, 0x7399, 0x73AE, 0x73C4, 0x73D9, 0x73EF, 0x7404, 0x7419, 0x742E,
+    0x7443, 0x7458, 0x746D, 0x7482, 0x7497, 0x74AC, 0x74C0, 0x74D5, 0x74EA, 0x74FE, 0x7512, 0x7527, 0x753B, 0x754F,
+    0x7563, 0x7577, 0x758B, 0x759F, 0x75B3, 0x75C7, 0x75DA, 0x75EE, 0x7601, 0x7615, 0x7628, 0x763B, 0x764F, 0x7662,
+    0x7675, 0x7688, 0x769B, 0x76AE, 0x76C1, 0x76D3, 0x76E6, 0x76F9, 0x770B, 0x771E, 0x7730, 0x7742, 0x7754, 0x7767,
+    0x7779, 0x778B, 0x779D, 0x77AF, 0x77C0, 0x77D2, 0x77E4, 0x77F5, 0x7807, 0x7818, 0x782A, 0x783B, 0x784C, 0x785D,
+    0x786E, 0x787F, 0x7890, 0x78A1, 0x78B2, 0x78C3, 0x78D3, 0x78E4, 0x78F4, 0x7905, 0x7915, 0x7925, 0x7936, 0x7946,
+    0x7956, 0x7966, 0x7976, 0x7985, 0x7995, 0x79A5, 0x79B5, 0x79C4, 0x79D4, 0x79E3, 0x79F2, 0x7A02, 0x7A11, 0x7A20,
+    0x7A2F, 0x7A3E, 0x7A4D, 0x7A5B, 0x7A6A, 0x7A79, 0x7A87, 0x7A96, 0x7AA4, 0x7AB3, 0x7AC1, 0x7ACF, 0x7ADD, 0x7AEB,
+    0x7AF9, 0x7B07, 0x7B15, 0x7B23, 0x7B31, 0x7B3E, 0x7B4C, 0x7B59, 0x7B67, 0x7B74, 0x7B81, 0x7B8E, 0x7B9B, 0x7BA8,
+    0x7BB5, 0x7BC2, 0x7BCF, 0x7BDC, 0x7BE8, 0x7BF5, 0x7C02, 0x7C0E, 0x7C1A, 0x7C27, 0x7C33, 0x7C3F, 0x7C4B, 0x7C57,
+    0x7C63, 0x7C6F, 0x7C7A, 0x7C86, 0x7C92, 0x7C9D, 0x7CA9, 0x7CB4, 0x7CBF, 0x7CCB, 0x7CD6, 0x7CE1, 0x7CEC, 0x7CF7,
+    0x7D02, 0x7D0C, 0x7D17, 0x7D22, 0x7D2C, 0x7D37, 0x7D41, 0x7D4B, 0x7D56, 0x7D60, 0x7D6A, 0x7D74, 0x7D7E, 0x7D88,
+    0x7D91, 0x7D9B, 0x7DA5, 0x7DAE, 0x7DB8, 0x7DC1, 0x7DCB, 0x7DD4, 0x7DDD, 0x7DE6, 0x7DEF, 0x7DF8, 0x7E01, 0x7E0A,
+    0x7E13, 0x7E1B, 0x7E24, 0x7E2C, 0x7E35, 0x7E3D, 0x7E45, 0x7E4D, 0x7E56, 0x7E5E, 0x7E66, 0x7E6D, 0x7E75, 0x7E7D,
+    0x7E85, 0x7E8C, 0x7E94, 0x7E9B, 0x7EA3, 0x7EAA, 0x7EB1, 0x7EB8, 0x7EBF, 0x7EC6, 0x7ECD, 0x7ED4, 0x7EDB, 0x7EE1,
+    0x7EE8, 0x7EEE, 0x7EF5, 0x7EFB, 0x7F01, 0x7F08, 0x7F0E, 0x7F14, 0x7F1A, 0x7F20, 0x7F25, 0x7F2B, 0x7F31, 0x7F36,
+    0x7F3C, 0x7F41, 0x7F47, 0x7F4C, 0x7F51, 0x7F56, 0x7F5B, 0x7F60, 0x7F65, 0x7F6A, 0x7F6F, 0x7F74, 0x7F78, 0x7F7D,
+    0x7F81, 0x7F85, 0x7F8A, 0x7F8E, 0x7F92, 0x7F96, 0x7F9A, 0x7F9E, 0x7FA2, 0x7FA6, 0x7FA9, 0x7FAD, 0x7FB0, 0x7FB4,
+    0x7FB7, 0x7FBA, 0x7FBE, 0x7FC1, 0x7FC4, 0x7FC7, 0x7FCA, 0x7FCC, 0x7FCF, 0x7FD2, 0x7FD4, 0x7FD7, 0x7FD9, 0x7FDC,
+    0x7FDE, 0x7FE0, 0x7FE2, 0x7FE4, 0x7FE6, 0x7FE8, 0x7FEA, 0x7FEC, 0x7FED, 0x7FEF, 0x7FF1, 0x7FF2, 0x7FF3, 0x7FF5,
+    0x7FF6, 0x7FF7, 0x7FF8, 0x7FF9, 0x7FFA, 0x7FFB, 0x7FFB, 0x7FFC, 0x7FFD, 0x7FFD, 0x7FFE, 0x7FFE, 0x7FFE, 0x7FFE,
+    0x7FFE, 0x7FFF,
+};
--- a/src/libultra_code_O2/sp.c
+++ b/src/libultra_code_O2/sp.c
@ -0,0 +1,11 @@
+#include "global.h"
+
+u32 __osSpDeviceBusy() {
+    register u32 status = HW_REG(SP_STATUS_REG, u32);
+
+    if (status & (SP_STATUS_DMA_BUSY | SP_STATUS_DMA_FULL | SP_STATUS_IO_FULL)) {
+        return 1;
+    }
+
+    return 0;
+}
--- a/src/libultra_code_O2/sptask.c
+++ b/src/libultra_code_O2/sptask.c
@ -0,0 +1,62 @@
+#include "global.h"
+
+#define _osVirtualToPhysical(ptr)              \
+    if (ptr != NULL) {                         \
+        ptr = (void*)osVirtualToPhysical(ptr); \
+    }
+
+static OSTask sTmpTask;
+
+OSTask* _VirtualToPhysicalTask(OSTask* intp) {
+    OSTask* tp = &sTmpTask;
+
+    bcopy(intp, tp, sizeof(OSTask));
+
+    _osVirtualToPhysical(tp->t.ucode);
+    _osVirtualToPhysical(tp->t.ucode_data);
+    _osVirtualToPhysical(tp->t.dram_stack);
+    _osVirtualToPhysical(tp->t.output_buff);
+    _osVirtualToPhysical(tp->t.output_buff_size);
+    _osVirtualToPhysical(tp->t.data_ptr);
+    _osVirtualToPhysical(tp->t.yield_data_ptr);
+    return tp;
+}
+
+void osSpTaskLoad(OSTask* intp) {
+    OSTask* tp = _VirtualToPhysicalTask(intp);
+
+    if (tp->t.flags & OS_TASK_YIELDED) {
+        tp->t.ucode_data = tp->t.yield_data_ptr;
+        tp->t.ucode_data_size = tp->t.yield_data_size;
+        intp->t.flags &= ~OS_TASK_YIELDED;
+        if (tp->t.flags & OS_TASK_LOADABLE) {
+            tp->t.ucode = HW_REG((u32)intp->t.yield_data_ptr + OS_YIELD_DATA_SIZE - 4, u32);
+        }
+    }
+    osWritebackDCache(tp, sizeof(OSTask));
+    __osSpSetStatus(SP_CLR_SIG0 | SP_CLR_SIG1 | SP_CLR_SIG2 | SP_SET_INTR_BREAK);
+    while (__osSpSetPc((void*)SP_IMEM_START) == -1) {
+        ;
+    }
+
+    while (__osSpRawStartDma(1, (void*)(SP_IMEM_START - sizeof(*tp)), tp, sizeof(OSTask)) == -1) {
+        ;
+    }
+
+    while (__osSpDeviceBusy()) {
+        ;
+    }
+
+    while (__osSpRawStartDma(1, (void*)SP_IMEM_START, tp->t.ucode_boot, tp->t.ucode_boot_size) == -1) {
+        ;
+    }
+}
+
+void osSpTaskStartGo(OSTask* tp) {
+
+    while (__osSpDeviceBusy()) {
+        ;
+    }
+
+    __osSpSetStatus(SP_SET_INTR_BREAK | SP_CLR_SSTEP | SP_CLR_BROKE | SP_CLR_HALT);
+}
--- a/src/libultra_code_O2/sqrtf.c
+++ b/src/libultra_code_O2/sqrtf.c
@ -0,0 +1,10 @@
+#include "global.h"
+
+#ifndef __GNUC__
+#pragma intrinsic(sqrtf)
+#define __builtin_sqrtf sqrtf
+#endif
+
+f32 sqrtf(f32 f) {
+    return __builtin_sqrtf(f);
+}