Handwritten asm: gu and mgu (#1816)

* Handwritten asm: gu and mgu

* Remove some unnecessary pad_texts

* Consistent add/and instructions

8 files changed, 298 insertions, 0 deletions

diff --git a/src/libultra/gu/sqrtf.s b/src/libultra/gu/sqrtf.s
new file mode 100644
index 000000000..e1252d82f
--- /dev/null
+++ b/src/libultra/gu/sqrtf.s
@@ -0,0 +1,9 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+
+LEAF(sqrtf)
+    sqrt.s  fv0, fa0
+    j       ra
+END(sqrtf)
diff --git a/src/libultra/mgu/mtxf2l.s b/src/libultra/mgu/mtxf2l.s
new file mode 100644
index 000000000..2c6f1e3e8
--- /dev/null
+++ b/src/libultra/mgu/mtxf2l.s
@@ -0,0 +1,36 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+#define MTX_INTPART  0
+#define MTX_FRACPART 0x20
+
+LEAF(guMtxF2L)
+    li.s    fv0, 65536.0
+    li      t9, 0xFFFF0000
+    addu    t8, a1, MTX_FRACPART
+1:
+    lwc1    ft0, 0(a0)
+    mul.s   ft1, ft0, fv0
+    trunc.w.s ft2, ft1
+    lwc1    ft3, 4(a0)
+    mul.s   ft4, ft3, fv0
+    trunc.w.s ft5, ft4
+    mfc1    t0, ft2
+    mfc1    t1, ft5
+    and     t2, t0, t9
+    srl     t3, t1, 0x10
+    or      t4, t2, t3
+    sw      t4, (MTX_INTPART)(a1)
+    sll     t5, t0, 0x10
+    and     t6, t1, 0xFFFF
+    or      t7, t5, t6
+    sw      t7, (MTX_FRACPART)(a1)
+    addu    a1, a1, 4
+    addu    a0, a0, 8
+    bne     a1, t8, 1b
+
+    jr      ra
+END(guMtxF2L)
diff --git a/src/libultra/mgu/mtxident.s b/src/libultra/mgu/mtxident.s
new file mode 100644
index 000000000..596abd269
--- /dev/null
+++ b/src/libultra/mgu/mtxident.s
@@ -0,0 +1,27 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+LEAF(guMtxIdent)
+    add     t0, zero, 1
+    sll     t1, t0, 0x10
+    sw      t1,   0x00(a0)
+    sw      zero, 0x04(a0)
+    sw      t0,   0x08(a0)
+    sw      zero, 0x0C(a0)
+    sw      zero, 0x10(a0)
+    sw      t1,   0x14(a0)
+    sw      zero, 0x18(a0)
+    sw      t0,   0x1C(a0)
+    sw      zero, 0x20(a0)
+    sw      zero, 0x24(a0)
+    sw      zero, 0x28(a0)
+    sw      zero, 0x2c(a0)
+    sw      zero, 0x30(a0)
+    sw      zero, 0x34(a0)
+    sw      zero, 0x38(a0)
+    sw      zero, 0x3C(a0)
+    jr      ra
+END(guMtxIdent)
diff --git a/src/libultra/mgu/mtxidentf.s b/src/libultra/mgu/mtxidentf.s
new file mode 100644
index 000000000..cce0f97b8
--- /dev/null
+++ b/src/libultra/mgu/mtxidentf.s
@@ -0,0 +1,26 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+LEAF(guMtxIdentF)
+    li.s    t0,   1.0
+    sw      t0,   0x00(a0)
+    sw      zero, 0x04(a0)
+    sw      zero, 0x08(a0)
+    sw      zero, 0x0C(a0)
+    sw      zero, 0x10(a0)
+    sw      t0,   0x14(a0)
+    sw      zero, 0x18(a0)
+    sw      zero, 0x1C(a0)
+    sw      zero, 0x20(a0)
+    sw      zero, 0x24(a0)
+    sw      t0,   0x28(a0)
+    sw      zero, 0x2C(a0)
+    sw      zero, 0x30(a0)
+    sw      zero, 0x34(a0)
+    sw      zero, 0x38(a0)
+    sw      t0,   0x3C(a0)
+    jr      ra
+END(guMtxIdentF)
diff --git a/src/libultra/mgu/mtxl2f.s b/src/libultra/mgu/mtxl2f.s
new file mode 100644
index 000000000..5d9bc9b0e
--- /dev/null
+++ b/src/libultra/mgu/mtxl2f.s
@@ -0,0 +1,43 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+#define MTX_INTPART  0
+#define MTX_FRACPART 0x20
+
+#define FIXTOF 0.0000152587890625 /* 1.0f / 65536.0f */
+
+LEAF(guMtxL2F)
+    li.s    fv0, FIXTOF
+    li      t9, 0xFFFF0000
+    addu    t8, a1, MTX_FRACPART
+1:
+    lw      t0, MTX_INTPART(a1)
+    lw      t1, MTX_FRACPART(a1)
+
+    and     t2, t0, t9
+    srl     t3, t1, 0x10
+    or      t4, t2, t3
+
+    sll     t5, t0, 0x10
+    and     t6, t1, 0xFFFF
+    or      t7, t5, t6
+
+    mtc1    t4, ft0
+    cvt.s.w ft1, ft0
+    mul.s   ft2, ft1, fv0
+
+    mtc1    t7, ft3
+    cvt.s.w ft4, ft3
+    mul.s   ft5, ft4, fv0
+
+    swc1    ft2, 0(a0)
+    swc1    ft5, 4(a0)
+    addu    a0, a0, 8
+    addu    a1, a1, 4
+    bne     a1, t8, 1b
+
+    jr      ra
+END(guMtxL2F)
diff --git a/src/libultra/mgu/normalize.s b/src/libultra/mgu/normalize.s
new file mode 100644
index 000000000..99eccb6ae
--- /dev/null
+++ b/src/libultra/mgu/normalize.s
@@ -0,0 +1,33 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+LEAF(guNormalize)
+    lwc1    ft0, (a0)
+    lwc1    ft1, (a1)
+    lwc1    ft2, (a2)
+.set noreorder
+    mul.s   ft3, ft0, ft0
+    li.s    t0, 1.0
+    mul.s   ft4, ft1, ft1
+    add.s   ft5, ft3, ft4
+    mul.s   ft4, ft2, ft2
+.set reorder
+    add.s   ft3, ft4, ft5
+    mtc1    t0, ft5
+    sqrt.s  ft4, ft3
+    div.s   ft3, ft5, ft4
+.set noreorder
+    mul.s   ft4, ft0, ft3
+    nop
+    mul.s   ft5, ft1, ft3
+    nop
+    mul.s   ft0, ft2, ft3
+.set reorder
+    swc1    ft4, (a0)
+    swc1    ft5, (a1)
+    swc1    ft0, (a2)
+    jr      ra
+END(guNormalize)
diff --git a/src/libultra/mgu/scale.s b/src/libultra/mgu/scale.s
new file mode 100644
index 000000000..e0a964459
--- /dev/null
+++ b/src/libultra/mgu/scale.s
@@ -0,0 +1,56 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+LEAF(guScale)
+    li.s    ft0, 65536.0
+
+    mtc1    a1, ft1
+    mul.s   ft2, ft1, ft0
+    trunc.w.s ft3, ft2
+    mfc1    t1, ft3
+
+    srl     t2, t1, 0x10
+    sll     t0, t2, 0x10
+    sw      t0, 0x00(a0)
+    sll     t2, t1, 0x10
+    sw      t2, 0x20(a0)
+
+    mtc1    a2, ft1
+    mul.s   ft2, ft1, ft0
+    trunc.w.s ft3, ft2
+    mfc1    t1, ft3
+
+    srl     t0, t1, 0x10
+    sw      t0, 0x08(a0)
+    and     t2, t1, 0xFFFF
+    sw      t2, 0x28(a0)
+
+    mtc1    a3, ft1
+    mul.s   ft2, ft1, ft0
+    trunc.w.s ft3, ft2
+    mfc1    t1, ft3
+
+    srl     t2, t1, 0x10
+    sll     t0, t2, 0x10
+    sw      t0, 0x14(a0)
+    sll     t2, t1, 0x10
+    sw      t2, 0x34(a0)
+
+    li      t0, 1
+    sw      t0, 0x1C(a0)
+
+    sw      zero, 0x04(a0)
+    sw      zero, 0x0C(a0)
+    sw      zero, 0x10(a0)
+    sw      zero, 0x18(a0)
+    sw      zero, 0x24(a0)
+    sw      zero, 0x2C(a0)
+    sw      zero, 0x30(a0)
+    sw      zero, 0x38(a0)
+    sw      zero, 0x3C(a0)
+
+    jr      ra
+END(guScale)
diff --git a/src/libultra/mgu/translate.s b/src/libultra/mgu/translate.s
new file mode 100644
index 000000000..db0c858f0
--- /dev/null
+++ b/src/libultra/mgu/translate.s
@@ -0,0 +1,68 @@
+#include "PR/asm.h"
+#include "PR/regdef.h"
+
+.text
+.align 5
+
+LEAF(guTranslate)
+    li.s    ft0, 65536.0
+
+    mtc1    a1, ft1
+    mul.s   ft2, ft1, ft0
+    trunc.w.s ft3, ft2
+    mfc1    t1, ft3
+
+    mtc1    a2, ft1
+    mul.s   ft2, ft1, ft0
+    trunc.w.s ft3, ft2
+    mfc1    t3, ft3
+
+    srl     t2, t1, 0x10
+    sll     t0, t2, 0x10
+    srl     t2, t3, 0x10
+    or      t0, t0, t2
+    sw      t0, 0x18(a0)
+
+    sll     t0, t1, 0x10
+    sll     t2, t3, 0x10
+    srl     t2, t2, 0x10
+    or      t0, t0, t2
+    sw      t0, 0x38(a0)
+
+    mtc1    a3, ft1
+    mul.s   ft2, ft1, ft0
+    trunc.w.s ft3, ft2
+    mfc1    t1, ft3
+
+    srl     t2, t1, 0x10
+    sll     t0, t2, 0x10
+    addu    t0, t0, 1
+    sw      t0, 0x1C(a0)
+
+    sll     t2, t1, 0x10
+    sw      t2, 0x3C(a0)
+
+    sw      zero, 0x00(a0)
+    sw      zero, 0x04(a0)
+    sw      zero, 0x08(a0)
+    sw      zero, 0x0C(a0)
+    sw      zero, 0x10(a0)
+    sw      zero, 0x14(a0)
+    sw      zero, 0x20(a0)
+    sw      zero, 0x24(a0)
+    sw      zero, 0x28(a0)
+    sw      zero, 0x2C(a0)
+    sw      zero, 0x30(a0)
+    sw      zero, 0x34(a0)
+
+    lui     t0, (0x00010000 >> 16)
+    ori     t0, (0x00010000 & 0xFFFF)
+    sw      t0, (a0)
+    sw      t0, 0x14(a0)
+
+    lui     t0, (0x00000001 >> 16)
+    ori     t0, (0x00000001 & 0xFFFF)
+    sw      t0, 8(a0)
+
+    jr      ra
+END(guTranslate)