From 008737ed19603185c1a3df15917f9c0dc93d049c Mon Sep 17 00:00:00 2001
From: David Given <dg@cowlark.com>
Date: Mon, 10 Sep 2018 23:20:52 +0200
Subject: [PATCH] Added some more very untested helper functions.

---
 mach/mips/libem/fif8.s | 69 ++++++++++++++++++++++++++++++++++++++++++
 mach/mips/libem/sar4.s | 24 +++++++++++++++
 mach/mips/libem/sts4.s | 57 ++++++++++++++++++++++++++++++++++
 3 files changed, 150 insertions(+)
 create mode 100644 mach/mips/libem/fif8.s
 create mode 100644 mach/mips/libem/sar4.s
 create mode 100644 mach/mips/libem/sts4.s

diff --git a/mach/mips/libem/fif8.s b/mach/mips/libem/fif8.s
new file mode 100644
index 000000000..6b18ee746
--- /dev/null
+++ b/mach/mips/libem/fif8.s
@@ -0,0 +1,69 @@
+#
+.sect .text; .sect .rom; .sect .data; .sect .bss
+
+/* Multiplies two double-precision floats, then splits the product into
+ *  fraction and integer, both as floats, like modf(3) in C,
+ *  http://en.cppreference.com/w/c/numeric/math/modf
+ *
+ * Stack: ( a b -- fraction integer )
+ */
+
+.sect .text
+.define .fif8
+.fif8:
+	ldc1 f0, 8(sp)          ! f0 = a
+	ldc1 f2, 0(sp)          ! f2 = b
+	mul.d f0, f0, f2        ! f0 = a * b
+	abs.d f2, f0            ! f2 = abs(f0)
+
+	li at, ha16[max_power_of_two]
+	ldc1 f4, lo16[max_power_of_two] (at) ! f4 = max power of two
+
+	mov.d f6, f2            ! we're going to assemble the integer part in f6
+	c.lt.d 0, f4, f2        ! if absolute value too big, it must be integral
+	bc1t 0, return
+	nop
+
+	! Crudely strip off the fractional part.
+
+	add.d f6, f2, f4        ! f6 = absolute value + max power of two
+	sub.d f6, f6, f4        ! f6 -= max_power_of_two
+
+	! The above might round, so correct that.
+
+	li at, ha16[one]
+	ldc1 f8, lo16[one] (at)   ! f8 = 1.0
+1:
+	c.le.d 0, f6, f2        ! if result <= absolute value, stop
+	bc1t 0, 2f
+	nop
+
+	sub.d f6, f6, f8        ! result -= 1.0
+	b 1b
+	nop
+2:
+
+	! Correct the sign of the result.
+
+	mtc1 zero, f8
+	mthc1 zero, f8          ! f8 = 0.0
+	c.lt.d 0, f0, f8        ! if original value was negative
+	bc1f 0, 1f
+	nop
+	neg.d f6, f6            ! negate the result
+1:
+
+return:
+	sdc1 f6, 0(sp)          ! store integer part
+	sub.d f6, f0, f6        ! calculate fractional part
+	sdc1 f6, 8(sp)          ! store fractional part
+	jr ra
+	nop
+
+! doubles >= MAXPOWTWO are already integers
+.sect .rom
+max_power_of_two:
+	.dataf8 4.503599627370496000E+15
+
+one:
+	.dataf8 1.0
diff --git a/mach/mips/libem/sar4.s b/mach/mips/libem/sar4.s
new file mode 100644
index 000000000..a1ddea33c
--- /dev/null
+++ b/mach/mips/libem/sar4.s
@@ -0,0 +1,24 @@
+#
+.sect .text; .sect .rom; .sect .data; .sect .bss
+
+/* Store to bounds-checked array.
+ *
+ * Stack: ( element array-adr index descriptor-adr -- )
+ */
+
+.sect .text
+.define .sar4
+.sar4:
+	mov r25, ra
+
+	jal .aar4
+	nop
+
+	/* pass r2 = size from .aar4 to .sts4
+
+	jal .sts4
+	nop
+
+	jr r25
+	nop
+
diff --git a/mach/mips/libem/sts4.s b/mach/mips/libem/sts4.s
new file mode 100644
index 000000000..56ea9efa8
--- /dev/null
+++ b/mach/mips/libem/sts4.s
@@ -0,0 +1,57 @@
+#
+.sect .text; .sect .rom; .sect .data; .sect .bss
+
+/* Stores a variable-sized block from the stack.
+ *
+ * On entry: r2 = size
+ * Stack: ( block address -- )
+ * Preserves r25 for .lar4 and .sar4
+ */
+
+.sect .text
+.define .sts4
+.sts4:
+	lw r4, 0(sp)            ! r4 = address
+	addiu sp, sp, 4         ! sp = pointer to block
+
+	! Sizes 1 and 2 are handled specially.
+
+	li at, 1
+	beq r2, at, byte_sized
+	nop
+
+	li at, 2
+	beq r2, at, word_sized
+	nop
+
+	! Else the size must be a multiple of 4.
+
+	srl r5, r2, 2           ! r5 = number of words
+
+	! Copy.
+
+1:
+	lw at, 0(sp)
+	sw at, 0(r4)
+	addiu sp, sp, 4
+	addiu r4, r4, 4
+	addiu r5, r5, -1
+	bne r5, zero, 1b
+	nop
+
+	jr ra
+	nop
+
+byte_sized:
+	lw at, 0(sp)
+	sb at, 0(r4)
+	addiu sp, sp, 4
+	jr ra
+	nop
+
+word_sized:
+	lw at, 0(sp)
+	sh at, 0(r4)
+	addiu sp, sp, 4
+	jr ra
+	nop
-- 
2.34.1