First proper commit.

src/libultra_code/__osSiCreateAccessQueue.c

@@ -0,0 +1,26 @@
+#include <ultra64.h>
+#include <global.h>
+
+#include <ultra64/controller.h>
+
+OSMesg osSiMesgBuff[SIAccessQueueSize];
+OSMesgQueue gOsSiMessageQueue;
+u32 gOsSiAccessQueueCreated = 0;
+
+void __osSiCreateAccessQueue() { //func_80100B50
+    gOsSiAccessQueueCreated = 1;
+    osCreateMesgQueue(&gOsSiMessageQueue, &osSiMesgBuff[0], SIAccessQueueSize - 1);
+    osSendMesg(&gOsSiMessageQueue, NULL, OS_MESG_NOBLOCK);
+}
+
+void __osSiGetAccess() { //func_80100BA0
+    OSMesg sp1c;
+    if (!gOsSiAccessQueueCreated) {
+        __osSiCreateAccessQueue();
+    }
+    osRecvMesg(&gOsSiMessageQueue, &sp1c, OS_MESG_BLOCK);
+}
+
+void __osSiRelAccess() { //func_80100BE4
+    osSendMesg(&gOsSiMessageQueue, NULL, OS_MESG_NOBLOCK);
+}

src/libultra_code/__osSiRawStartDma.c

@@ -0,0 +1,22 @@
+#include "libultra_internal.h"
+#include <ultra64/hardware.h>
+
+s32 __osSiRawStartDma(s32 dir, void *addr) //func_801013F0
+{
+    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;
+}

src/libultra_code/__osSpGetStatus.c

@@ -0,0 +1,8 @@
+#include <ultra64.h>
+#include <global.h>
+#include <ultra64/hardware.h>
+
+u32 __osSpGetStatus()
+{
+    return HW_REG(SP_STATUS_REG, u32);
+}

src/libultra_code/__osSpSetStatus.c

@@ -0,0 +1,8 @@
+#include <ultra64.h>
+#include <global.h>
+#include <ultra64/hardware.h>
+
+void __osSpSetStatus(u32 a0)
+{
+    HW_REG(SP_STATUS_REG, u32) = a0;
+}

src/libultra_code/code_80101B40.c.ignore

@@ -0,0 +1,22 @@
+#include <ultra64.h>
+#include <global.h>
+
+void func_80101B40(u32* a0)
+{
+    a0[0] = 0x3F800000; // Float?
+    a0[1] = 0;
+    a0[2] = 0;
+    a0[3] = 0;
+    a0[4] = 0;
+    a0[5] = 0;
+    a0[6] = 0x3F800000;
+    a0[7] = 0;
+    a0[8] = 0;
+    a0[9] = 0;
+    a0[10] = 0x3F800000;
+    a0[11] = 0;
+    a0[12] = 0;
+    a0[13] = 0;
+    a0[14] = 0;
+    a0[15] = 0x3F800000;
+}

src/libultra_code/code_80103B30.c

@@ -0,0 +1,15 @@
+#include <ultra64.h>
+#include <global.h>
+#include <ultra64/hardware.h>
+
+u32 func_80103B30()
+{
+    register u32 status;
+
+    status = HW_REG(SP_STATUS_REG, u32);
+
+    if (status & 0x1C)
+        return 1;
+
+    return 0;
+}

src/libultra_code/code_80104140.c

@@ -0,0 +1,6 @@
+#include <global.h>
+
+OSThread* func_80104140()
+{
+    return __osActiveQueue;
+}

src/libultra_code/code_801064C0.c

@@ -0,0 +1,7 @@
+#include <ultra64.h>
+#include <global.h>
+
+OSThread* __osGetCurrFaultedThread()
+{
+    return __osFaultedThread;
+}

src/libultra_code/code_80106760.c

@@ -0,0 +1,14 @@
+#include <ultra64.h>
+#include <global.h>
+
+u32* func_80106760()
+{
+    register u32 sMask = __osDisableInt();
+    u32* var1;
+
+    var1 = __osViCurr->buffer;
+
+    __osRestoreInt(sMask);
+
+    return var1;
+}

src/libultra_code/code_801067A0.c

@@ -0,0 +1,17 @@
+#include <ultra64.h>
+#include <global.h>
+#include <ultra64/hardware.h>
+
+s32 func_801067A0(u32 a0)
+{
+    register u32 spStatus;
+
+    spStatus = HW_REG(SP_STATUS_REG, u32);
+
+    if (!(spStatus & SP_STATUS_HALT))
+        return -1;
+
+    HW_REG(SP_PC_REG, u32) = a0;
+
+    return 0;
+}

src/libultra_code/guPerspectiveF.c

@@ -0,0 +1,37 @@
+#include "libultra_internal.h"
+
+void guPerspectiveF(float mf[4][4], u16 *perspNorm, float fovy, float aspect, float near, float far,
+                    float scale) {
+    float yscale;
+    int row;
+    int 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 = (double) (1 << 17) / (near + far);
+            if (*perspNorm <= 0)
+                *perspNorm = 1;
+        }
+    }
+}
+void guPerspective(Mtx *m, u16 *perspNorm, float fovy, float aspect, float near, float far,
+                   float scale) {
+    float mat[4][4];
+    guPerspectiveF(mat, perspNorm, fovy, aspect, near, far, scale);
+    guMtxF2L(mat, m);
+}

src/libultra_code/libultra_internal.h

@@ -0,0 +1,46 @@
+#ifndef _LIBULTRA_INTERNAL_H_
+#define _LIBULTRA_INTERNAL_H_
+#include <ultra64.h>
+
+// TODO: rename these
+//      SM64                OOT
+#define guMtxIdentF         func_80101B40
+#define guMtxF2L            func_801064E0
+
+s32 __osDisableInt();
+void __osRestoreInt(s32);
+void __osEnqueueAndYield(OSThread**);
+void __osDequeueThread(OSThread**, OSThread*);
+void __osEnqueueThread(OSThread**, OSThread*);
+OSThread* __osPopThread(OSThread**);
+s32 __osSiRawStartDma(s32, void*);
+void __osSiCreateAccessQueue();
+void __osSiGetAccess();
+void __osSiRelAccess();
+u32 __osProbeTLB(void*);
+void __osPiCreateAccessQueue();
+void __osPiGetAccess();
+void __osSetSR(u32);
+u32 __osGetSR();
+void __osSetFpcCsr(u32);
+u32 __osGetFpcCsr();
+s32 __osSiRawReadIo(void*, u32*);
+s32 __osSiRawWriteIo(void*, u32);
+s32 osPiRawReadIo(u32 a0, u32 *a1);
+void __osSpSetStatus(u32);
+u32 __osSpGetStatus();
+s32 __osSpSetPc(void*);
+s32 __osSpDeviceBusy();
+s32 __osSiDeviceBusy();
+s32 __osSpRawStartDma(u32 dir, void *sp_ptr, void *dram_ptr, size_t size);
+void __osViInit();
+//OSViContext * __osViGetCurrentContext();
+void __osViSwapContext();
+void __osSetTimerIntr(OSTime);
+OSTime __osInsertTimer(OSTimer *);
+void __osSetCompare(u32);
+s32 __osAiDeviceBusy();
+void __osDispatchThread();
+u32 __osGetCause();
+s32 __osAtomicDec(u32*);
+#endif

src/libultra_code/osAi.h

@@ -0,0 +1,7 @@
+#ifndef _OSAI_H_
+#define _OSAI_H_
+
+s32 osAiSetFrequency(u32);
+s32 osAiSetNextBuffer(void *, u32);
+u32 osAiGetLength(void);
+#endif

src/libultra_code/osAiGetLength.c

@@ -0,0 +1,7 @@
+#include "libultra_internal.h"
+#include "osAi.h"
+#include <ultra64/hardware.h>
+
+u32 osAiGetLength() {
+    return HW_REG(AI_LEN_REG, u32);
+}

src/libultra_code/osContInit.c

@@ -0,0 +1,90 @@
+#include <ultra64.h>
+#include <global.h>
+
+#include <ultra64/controller.h>
+
+pif_data_buffer_t _osPifInternalBuff;
+u8 _osCont_lastPollType;
+u8 _osCont_numControllers; //always 4
+
+//Not sure if the following is a struct together with the last two variables
+u16 unk_80175812;
+u32 unk_80175814;
+u8  unk_80175818[0x20];
+
+u32 gOsContInitialized = 0;
+OSMesgQueue _osContMesgQueue;
+OSMesg _osContMesgBuff[4];
+
+#define HALF_SECOND OS_USEC_TO_CYCLES(500000)
+s32 osContInit(OSMesgQueue *mq, u8 *ctl_present_bitfield, OSContStatus *status) { //func_80100C10
+    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);
+    }
+    _osCont_numControllers = 4;
+    __osPackRequestData(0);
+    ret = __osSiRawStartDma(OS_WRITE, &_osPifInternalBuff);
+    osRecvMesg(mq, &mesg, OS_MESG_BLOCK);
+    ret = __osSiRawStartDma(OS_READ, &_osPifInternalBuff);
+    osRecvMesg(mq, &mesg, OS_MESG_BLOCK);
+    __osContGetInitData(ctl_present_bitfield, status);
+    _osCont_lastPollType = 0;
+    __osSiCreateAccessQueue();
+    osCreateMesgQueue(&_osContMesgQueue, _osContMesgBuff, 1);
+    return ret;
+}
+
+void __osContGetInitData(u8 *ctl_present_bitfield, OSContStatus *status) { //func_80100D6C 
+    PIF_IO_slot_t *slot_ptr;
+    PIF_IO_slot_t slot;
+    s32 i;
+    u8 bitfield_temp;
+    bitfield_temp = 0;
+    slot_ptr = _osPifInternalBuff.slots;
+    for (i = 0; i < _osCont_numControllers; i++, slot_ptr++, status++) {
+        slot = *slot_ptr;
+        status->errno = (slot.hdr.status_hi_bytes_rec_lo & 0xc0) >> 4;
+        if (status->errno == 0) {
+            status->type = slot.ctl_type_hi << 8 | slot.ctl_type_lo;
+            status->status = slot.ctl_status;
+            bitfield_temp |= 1 << i;
+        }
+    }
+    *ctl_present_bitfield = bitfield_temp;
+}
+
+void __osPackRequestData(u8 command) { //func_80100E18
+    PIF_IO_slot_t *slot_ptr;
+    PIF_IO_slot_t slot;
+    s32 i;
+    for(i = 0; i < 0xF; i++) {
+        _osPifInternalBuff.words[i] = 0;
+    }
+    _osPifInternalBuff.status_control = 1;
+    slot.hdr.slot_type = 0xFF;
+    slot.hdr.bytes_send = 1;
+    slot.hdr.status_hi_bytes_rec_lo = 3;
+    slot.hdr.command = command;
+    slot.ctl_type_lo = 0xFF;
+    slot.ctl_type_hi = 0xFF;
+    slot.ctl_status = 0xFF;
+    slot.dummy = 0xFF;
+    slot_ptr = _osPifInternalBuff.slots;
+    for (i = 0; i < _osCont_numControllers; i++) {
+        *slot_ptr++ = slot;
+    }
+    slot_ptr->hdr.slot_type = 254;
+}

src/libultra_code/osContStartReadData.c

@@ -0,0 +1,58 @@
+#include <ultra64.h>
+#include <global.h>
+
+#include <ultra64/controller.h>
+
+s32 osContStartReadData(OSMesgQueue *mq) //func_80100EF0
+{
+    s32 ret;
+    __osSiGetAccess(); //__osSiGetAccess
+    if (_osCont_lastPollType != 1)
+    {
+        __osPackReadData();
+        __osSiRawStartDma(OS_WRITE, &_osPifInternalBuff);
+        osRecvMesg(mq, NULL, OS_MESG_BLOCK);
+    }
+    ret = __osSiRawStartDma(OS_READ, &_osPifInternalBuff);
+    _osCont_lastPollType = 1;
+    __osSiRelAccess();
+    return ret;
+}
+
+void osContGetReadData(OSContPad *pad) { //func_80100F74
+    PIF_IO_slot_t *slot_ptr;
+    PIF_IO_slot_t slot;
+    s32 i;
+    slot_ptr = _osPifInternalBuff.slots;
+    for (i = 0; i < _osCont_numControllers; i++, slot_ptr++, pad++) {
+        slot = *slot_ptr;
+        pad->errno = (slot.hdr.status_hi_bytes_rec_lo & 0xc0) >> 4;
+        if (pad->errno == 0) {
+            pad->button = slot.button;
+            pad->stick_x = slot.rawStickX;
+            pad->stick_y = slot.rawStickY;
+        }
+    };
+}
+
+void __osPackReadData() { //func_80101000 
+    PIF_IO_slot_t *slot_ptr;
+    PIF_IO_slot_t slot;
+    s32 i;
+    slot_ptr = _osPifInternalBuff.slots;
+    for(i = 0; i < 0xF; i++) {
+        _osPifInternalBuff.words[i] = 0;
+    }
+    _osPifInternalBuff.status_control = 1;
+    slot.hdr.slot_type = 0xFF;
+    slot.hdr.bytes_send = 1;
+    slot.hdr.status_hi_bytes_rec_lo = 4;
+    slot.hdr.command = 1;
+    slot.button = 0xFFFF;
+    slot.rawStickX = 0xFF;
+    slot.rawStickY = 0xFF;
+    for(i = 0; i < _osCont_numControllers; i++) {
+        *slot_ptr++ = slot;
+    }
+    slot_ptr->hdr.slot_type = 0xFE;
+}

src/libultra_code/osDpGetStatus.c

@@ -0,0 +1,6 @@
+#include <global.h>
+
+u32 osDpGetStatus(void)
+{
+    return DPC_STATUS_REG;
+}

src/libultra_code/osDpSetStatus.c

@@ -0,0 +1,6 @@
+#include <global.h>
+
+void osDpSetStatus(u32 status)
+{
+    DPC_STATUS_REG = status;
+}

src/libultra_code/osVibrationPack.c

@@ -0,0 +1,123 @@
+#include <ultra64.h>
+#include <global.h>
+
+#include <ultra64/controller.h>
+
+pif_data_buffer_t osPifBuffers[4];
+
+//func_800CF990 in 1.0
+s32 osSetVibration(unk_controller_t *arg0, u32 vibrate) //func_80100780
+{
+    s32 i;
+    s32 ret;
+    u8 *buf;
+    
+    buf = (u8*)&osPifBuffers[arg0->ctrlridx];
+    if (!(arg0->unk0 & 8)){
+        return 5;
+    }
+    __osSiGetAccess();
+    osPifBuffers[arg0->ctrlridx].status_control = 1;
+    buf += arg0->ctrlridx;
+    for(i=0; i<0x20; i++){
+        ((PIF_mempak_wr_t*)buf)->data[i+2] = vibrate;
+    }
+    
+    _osCont_lastPollType = (u8)0xfe; //last controller poll type?
+    __osSiRawStartDma(OS_WRITE, &osPifBuffers[arg0->ctrlridx]);
+    osRecvMesg(arg0->ctrlrqueue, NULL, OS_MESG_BLOCK);
+    __osSiRawStartDma(OS_READ, &osPifBuffers[arg0->ctrlridx]);
+    osRecvMesg(arg0->ctrlrqueue, NULL, OS_MESG_BLOCK);
+    ret = ((PIF_mempak_wr_t*)buf)->hdr.status_hi_bytes_rec_lo & 0xc0;
+    if (!ret) {
+        if (!vibrate) {
+            if (((PIF_mempak_wr_t*)buf)->data[0x22] != 0){
+                ret = 4;
+            }
+        }else{
+            if (((PIF_mempak_wr_t*)buf)->data[0x22] != 0xeb){
+                ret = 4;
+            }
+        }
+    }
+    __osSiRelAccess();
+    return ret;
+}
+
+void osSetUpMempakWrite(s32 ctrlridx, pif_data_buffer_t* buf) //func_801009F4
+{
+    u8* buf_ptr = (u8*)buf;
+    PIF_mempak_wr_t mempakwr;
+    s32 i;
+    mempakwr.hdr.slot_type = 0xFF;
+    mempakwr.hdr.bytes_send = 0x23;
+    mempakwr.hdr.status_hi_bytes_rec_lo = 1;
+    mempakwr.hdr.command = 3; //write mempak
+    mempakwr.data[0] = 0xC0;
+    mempakwr.data[1] = (u8)(func_80106170(0x600) | 0xC000); //yes, this is correct
+    if(ctrlridx != 0){
+        for(i=0; i<ctrlridx; ++i){
+            *buf_ptr++ = 0;
+        }
+    }
+    *(PIF_mempak_wr_t*)buf_ptr = mempakwr;
+    buf_ptr += 0x27;
+    *buf_ptr = 0xFE;
+}
+
+typedef struct {
+    u8 unk[0x20];
+} unk_sp24_t;
+
+s32 osProbeVibrationPack(OSMesgQueue* ctrlrqueue, unk_controller_t *unk_controller, u32 ctrlridx) //func_80100B50
+{
+    s32 ret;
+    unk_sp24_t sp24;
+    
+    unk_controller->ctrlrqueue = ctrlrqueue;
+    unk_controller->ctrlridx = ctrlridx;
+    unk_controller->bytes[0x65] = (u8)0xff;
+    unk_controller->unk0 = 0;
+    
+    ret = func_80104C80(unk_controller, 0xfe);
+    if (ret == 2){
+        ret = func_80104C80(unk_controller, 0x80);
+    }
+    if (ret != 0){
+        return ret;
+    }
+    ret = func_80105F40(ctrlrqueue, ctrlridx, 0x400, &sp24);
+    ret = ret;
+    if (ret == 2){
+        ret = 4; //"Controller pack communication error"
+    }
+    if (ret != 0){
+        return ret;
+    }
+    if (sp24.unk[0x1F] == 0xfe){
+        return 0xb; //possibly controller pack? (Some other valid return value other than vibration pack)
+    }
+    ret = func_80104C80(unk_controller, 0x80);
+    if (ret == 2){
+        ret = 4; //"Controller pack communication error"
+    }
+    if (ret != 0){
+        return ret;
+    }
+    ret = func_80105F40(ctrlrqueue, ctrlridx, 0x400, &sp24);
+    if (ret == 2){
+        ret = 4; //"Controller pack communication error"
+    }
+    if (ret != 0){
+        return ret;
+    }
+    if (sp24.unk[0x1F] != 0x80){
+        return 0xb; //possibly controller pack? (Some other valid return value other than vibration pack)
+    }
+    if ((unk_controller->unk0 & 8) == 0){
+        osSetUpMempakWrite(ctrlridx, &osPifBuffers[ctrlridx]);
+    }
+    unk_controller->unk0 = 8;
+    return 0; //"Recognized vibration pack"
+}
+ 

src/libultra_code/sqrtf.c

@@ -0,0 +1,12 @@
+#include <math.h>
+
+float sqrtf(float f);
+
+#ifndef __GNUC__
+#pragma intrinsic(sqrtf)
+#define __builtin_sqrtf sqrtf
+#endif
+
+float sqrtf(float f) {
+    return __builtin_sqrtf(f);
+}