Handwritten asm: gu and mgu (#1816)

* Handwritten asm: gu and mgu

* Remove some unnecessary pad_texts

* Consistent add/and instructions

9 files changed, 306 insertions, 14 deletions

diff --git a/spec/spec b/spec/spec
index 03a2911fd..3ad80ab38 100644
--- a/spec/spec
+++ b/spec/spec
@@ -60,7 +60,7 @@ beginseg
     include "$(BUILD_DIR)/src/libultra/voice/voicesetword.o"
     include "$(BUILD_DIR)/src/libultra/vimodes/vimodentschpf1.o"
     include "$(BUILD_DIR)/src/libultra/vimodes/vimodepallan1.o"
-    include "$(BUILD_DIR)/asm/boot/guScale.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/scale.o"
     include "$(BUILD_DIR)/src/libultra/gu/sinf.o"
     include "$(BUILD_DIR)/src/libultra/gu/sins.o"
     include "$(BUILD_DIR)/src/libultra/io/sptask.o"
@@ -94,8 +94,7 @@ beginseg
     include "$(BUILD_DIR)/src/libultra/io/sptaskyield.o"
     include "$(BUILD_DIR)/src/libultra/io/pfsreadwritefile.o"
     include "$(BUILD_DIR)/src/libultra/io/pfsgetstatus.o"
-    pad_text
-    include "$(BUILD_DIR)/asm/boot/guMtxIdentF.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/mtxidentf.o"
     include "$(BUILD_DIR)/src/libultra/io/visetmode.o"
     include "$(BUILD_DIR)/src/libultra/os/getconfig.o"
     include "$(BUILD_DIR)/src/libultra/os/setconfig.o"
@@ -105,18 +104,16 @@ beginseg
     include "$(BUILD_DIR)/asm/boot/probetlb.text.o"
     include "$(BUILD_DIR)/src/libultra/io/pimgr.o"
     include "$(BUILD_DIR)/src/libultra/io/piacs.o"
-    pad_text
     include "$(BUILD_DIR)/src/libultra/io/devmgr.o"
     include "$(BUILD_DIR)/src/libultra/io/pirawdma.o"
     include "$(BUILD_DIR)/src/libultra/vimodes/vimodentschpn1.o"
     include "$(BUILD_DIR)/src/libultra/io/contpfs.o"
     include "$(BUILD_DIR)/src/libultra/os/getcount.o"
-    pad_text
-    include "$(BUILD_DIR)/asm/boot/guMtxL2F.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/mtxl2f.o"
     include "$(BUILD_DIR)/src/libultra/os/getmemsize.o"
     include "$(BUILD_DIR)/src/libultra/io/pfssearchfile.o"
     include "$(BUILD_DIR)/src/libultra/os/seteventmesg.o"
-    include "$(BUILD_DIR)/asm/boot/sqrtf.text.o"
+    include "$(BUILD_DIR)/src/libultra/gu/sqrtf.o"
     include "$(BUILD_DIR)/src/libultra/os/afterprenmi.o"
     include "$(BUILD_DIR)/src/libultra/io/contquery.o"
     include "$(BUILD_DIR)/src/libultra/gu/lookathil.o"
@@ -134,7 +131,7 @@ beginseg
     include "$(BUILD_DIR)/src/libultra/voice/voicecontread36.o"
     include "$(BUILD_DIR)/src/libultra/io/sp.o"
     include "$(BUILD_DIR)/src/libultra/io/si.o"
-    include "$(BUILD_DIR)/asm/boot/guMtxIdent.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/mtxident.o"
     include "$(BUILD_DIR)/src/libultra/os/jammesg.o"
     include "$(BUILD_DIR)/src/libultra/os/setthreadpri.o"
     include "$(BUILD_DIR)/src/libultra/os/getthreadpri.o"
@@ -155,7 +152,7 @@ beginseg
     include "$(BUILD_DIR)/src/libultra/voice/voicecontwrite20.o"
     include "$(BUILD_DIR)/src/libultra/io/crc.o"
     include "$(BUILD_DIR)/src/libultra/os/getactivequeue.o"
-    include "$(BUILD_DIR)/asm/boot/normalize.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/normalize.o"
     include "$(BUILD_DIR)/src/libultra/os/setcompare.o"
     include "$(BUILD_DIR)/src/libultra/os/getcompare.o"
     include "$(BUILD_DIR)/src/libultra/io/dpgetstat.o"
@@ -193,8 +190,7 @@ beginseg
     include "$(BUILD_DIR)/src/libultra/io/epiwrite.o"
     include "$(BUILD_DIR)/asm/boot/maptlbrdb.text.o"
     include "$(BUILD_DIR)/src/libultra/os/yieldthread.o"
-    pad_text
-    include "$(BUILD_DIR)/asm/boot/guTranslate.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/translate.o"
     include "$(BUILD_DIR)/src/libultra/os/getcause.o"
     include "$(BUILD_DIR)/src/libultra/io/contramwrite.o"
     include "$(BUILD_DIR)/src/libultra/io/epirawwrite.o"
@@ -206,14 +202,12 @@ beginseg
     include "$(BUILD_DIR)/src/libultra/io/sirawwrite.o"
     include "$(BUILD_DIR)/src/libultra/io/spgetstat.o"
     include "$(BUILD_DIR)/src/libultra/io/spsetstat.o"
-    pad_text
     include "$(BUILD_DIR)/src/libultra/io/vimgr.o"
     include "$(BUILD_DIR)/src/libultra/io/vigetcurrcontext.o"
     include "$(BUILD_DIR)/asm/boot/writebackdcacheall.text.o"
     include "$(BUILD_DIR)/src/libultra/os/getcurrfaultthread.o"
     include "$(BUILD_DIR)/src/libultra/voice/voicemaskdictionary.o"
-    pad_text
-    include "$(BUILD_DIR)/asm/boot/guMtxF2L.text.o"
+    include "$(BUILD_DIR)/src/libultra/mgu/mtxf2l.o"
     include "$(BUILD_DIR)/src/libultra/os/startthread.o"
     include "$(BUILD_DIR)/src/libultra/io/visetyscale.o"
     include "$(BUILD_DIR)/src/libultra/io/visetxscale.o"
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)