padmgr and related (#71)

* In process of moving changes over from old repo

* Merged in changes

* Finished import of padmgr changes from old repo

* Adjusted some volatile

* Improving padmgr volatile situation

* Almost matched osReadMempak

* Working on osMempakDataCRC

* Explanations and equivalents but no matches for osMempakAddrCRC and osMempakDataCRC

* OK after merge

* Matched osMempakAddrCRC and osMempakDataCRC

* Matched osReadMempak

* Updated PadMgr function names to be less like original code

* Changed variable names to make them further from original code

* Changed names and it stopped matching

* Undid clang-format steamrollering whitespace memes

* Cleaned up Input names

* More formatting changes

* Moved padmgr to z64.h, deleted padmgr.h

4 files changed, 145 insertions, 20 deletions

diff --git a/src/libultra_code/osContStartReadData.c b/src/libultra_code/osContStartReadData.c
index 9bd0bfbd2..60e415028 100644
--- a/src/libultra_code/osContStartReadData.c
+++ b/src/libultra_code/osContStartReadData.c
@@ -26,9 +26,9 @@ void osContGetReadData(OSContPad* 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;
+            pad->button = slot.input.button;
+            pad->stick_x = slot.input.x;
+            pad->stick_y = slot.input.y;
         }
     };
 }
@@ -46,9 +46,9 @@ void __osPackReadData() {
     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;
+    slot.input.button = 0xFFFF;
+    slot.input.x = 0xFF;
+    slot.input.y = 0xFF;
     for (i = 0; i < _osCont_numControllers; i++) {
         *slot_ptr++ = slot;
     }
diff --git a/src/libultra_code/osMempakAddrCRC.c b/src/libultra_code/osMempakAddrCRC.c
new file mode 100644
index 000000000..4565ff4ee
--- /dev/null
+++ b/src/libultra_code/osMempakAddrCRC.c
@@ -0,0 +1,68 @@
+#include <ultra64.h>
+#include <global.h>
+
+#include <ultra64/controller.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 osMempakAddrCRC(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 osMempakDataCRC(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;
+}
diff --git a/src/libultra_code/osReadMempak.c b/src/libultra_code/osReadMempak.c
new file mode 100644
index 000000000..fd1b3b0e4
--- /dev/null
+++ b/src/libultra_code/osReadMempak.c
@@ -0,0 +1,57 @@
+#include <ultra64.h>
+#include <global.h>
+
+#include <ultra64/controller.h>
+
+s32 osReadMempak(OSMesgQueue* ctrlrqueue, s32 ctrlridx, u16 addr, PIF_mempak_data_t* data) {
+    s32 ret;
+    s32 i;
+    u8* bufptr;
+    s32 read_try_count = 2;
+
+    __osSiGetAccess();
+    do {
+        if ((_osCont_lastPollType != 2) || (ctrlridx != D_80134D20)) {
+            bufptr = &pifMempakBuf.bytes[0];
+            _osCont_lastPollType = 2;
+            D_80134D20 = ctrlridx;
+            // clang-format off
+            // NOLINTNEXTLINE
+            for (i = 0; i < ctrlridx; i++) *bufptr++ = 0;
+            // clang-format on
+            pifMempakBuf.status_control = 1;
+            ((PIF_header_t*)bufptr)->slot_type = 0xff;
+            ((PIF_header_t*)bufptr)->bytes_send = 3;
+            ((PIF_header_t*)bufptr)->status_hi_bytes_rec_lo = 0x21;
+            ((PIF_header_t*)bufptr)->command = 2; // read mempak; send byte 0
+            // Received bytes are 6-26 inclusive
+            bufptr[0x26] = 0xff; // last byte of receive
+            bufptr[0x27] = 0xfe; // End of commands
+        } else {
+            bufptr = &pifMempakBuf.bytes[ctrlridx];
+        }
+        bufptr[4] = addr >> 3;                                 // send byte 1
+        bufptr[5] = (s8)(osMempakAddrCRC(addr) | (addr << 5)); // send byte 2
+        __osSiRawStartDma(1, &pifMempakBuf);
+        osRecvMesg(ctrlrqueue, 0, 1);
+        __osSiRawStartDma(0, &pifMempakBuf);
+        osRecvMesg(ctrlrqueue, 0, 1);
+        ret = (((PIF_header_t*)bufptr)->status_hi_bytes_rec_lo & 0xc0) >> 4;
+        if (!ret) {
+            if (bufptr[0x26] != osMempakDataCRC(bufptr + 6)) {
+                ret = func_80101910(ctrlrqueue, ctrlridx);
+                if (ret)
+                    break;
+                ret = 4; // Retry
+            } else {
+                bcopy(bufptr + 6, data, 0x20);
+            }
+        } else {
+            ret = 1; // Error
+        }
+        if (ret != 4)
+            break;
+    } while (0 <= read_try_count--);
+    __osSiRelAccess();
+    return ret;
+}
diff --git a/src/libultra_code/osVibrationPack.c b/src/libultra_code/osRumblePak.c
index 033f04500..d2b47f2c6 100644
--- a/src/libultra_code/osVibrationPack.c
+++ b/src/libultra_code/osRumblePak.c
@@ -6,7 +6,7 @@
 pif_data_buffer_t osPifBuffers[4];
 
 // func_800CF990 in 1.0
-s32 osSetVibration(unk_controller_t* arg0, u32 vibrate) {
+s32 osSetRumble(unk_controller_t* arg0, u32 vibrate) {
     s32 i;
     s32 ret;
     u8* buf;
@@ -22,7 +22,7 @@ s32 osSetVibration(unk_controller_t* arg0, u32 vibrate) {
         ((PIF_mempak_wr_t*)buf)->data[i + 2] = vibrate;
     }
 
-    _osCont_lastPollType = (u8)0xfe; // last controller poll type?
+    _osCont_lastPollType = 0xfe; // last controller poll type?
     __osSiRawStartDma(OS_WRITE, &osPifBuffers[arg0->ctrlridx]);
     osRecvMesg(arg0->ctrlrqueue, NULL, OS_MESG_BLOCK);
     __osSiRawStartDma(OS_READ, &osPifBuffers[arg0->ctrlridx]);
@@ -51,8 +51,8 @@ void osSetUpMempakWrite(s32 ctrlridx, pif_data_buffer_t* buf) {
     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
+    mempakwr.data[0] = 0x600 >> 3;
+    mempakwr.data[1] = (u8)(osMempakAddrCRC(0x600) | (0x600 << 5));
     if (ctrlridx != 0) {
         for (i = 0; i < ctrlridx; ++i) {
             *buf_ptr++ = 0;
@@ -67,13 +67,13 @@ typedef struct {
     u8 unk[0x20];
 } unk_sp24_t;
 
-s32 osProbeVibrationPack(OSMesgQueue* ctrlrqueue, unk_controller_t* unk_controller, u32 ctrlridx) {
+s32 osProbeRumblePak(OSMesgQueue* ctrlrqueue, unk_controller_t* unk_controller, u32 ctrlridx) {
     s32 ret;
     unk_sp24_t sp24;
 
     unk_controller->ctrlrqueue = ctrlrqueue;
     unk_controller->ctrlridx = ctrlridx;
-    unk_controller->bytes[0x65] = (u8)0xff;
+    unk_controller->bytes[0x65] = 0xff;
     unk_controller->unk0 = 0;
 
     ret = func_80104C80(unk_controller, 0xfe);
@@ -83,37 +83,37 @@ s32 osProbeVibrationPack(OSMesgQueue* ctrlrqueue, unk_controller_t* unk_controll
     if (ret != 0) {
         return ret;
     }
-    ret = func_80105F40(ctrlrqueue, ctrlridx, 0x400, &sp24);
+    ret = osReadMempak(ctrlrqueue, ctrlridx, 0x400, &sp24);
     ret = ret;
     if (ret == 2) {
-        ret = 4; //"Controller pack communication error"
+        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)
+        return 0xb; // possibly controller pack? (Some other valid return value other than rumble pak)
     }
     ret = func_80104C80(unk_controller, 0x80);
     if (ret == 2) {
-        ret = 4; //"Controller pack communication error"
+        ret = 4; // "Controller pack communication error"
     }
     if (ret != 0) {
         return ret;
     }
-    ret = func_80105F40(ctrlrqueue, ctrlridx, 0x400, &sp24);
+    ret = osReadMempak(ctrlrqueue, ctrlridx, 0x400, &sp24);
     if (ret == 2) {
-        ret = 4; //"Controller pack communication error"
+        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)
+        return 0xb; // possibly controller pack? (Some other valid return value other than rumble pak)
     }
     if ((unk_controller->unk0 & 8) == 0) {
         osSetUpMempakWrite(ctrlridx, &osPifBuffers[ctrlridx]);
     }
     unk_controller->unk0 = 8;
-    return 0; //"Recognized vibration pack"
+    return 0; // "Recognized rumble pak"
 }