--- /dev/null
+/* $OpenBSD: vecast.S,v 1.1 2022/10/22 00:58:56 gkoehler Exp $ */
+
+/*
+ * Copyright (c) 2022 George Koehler <gkoehler@openbsd.org>
+ *
+ * Permission to use, copy, modify, and distribute this software for any
+ * purpose with or without fee is hereby granted, provided that the above
+ * copyright notice and this permission notice appear in all copies.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+ * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+ * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+ * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include <machine/asm.h>
+#include <machine/psl.h>
+
+/*
+ * To load or store an arbitrary AltiVec register, we extract its
+ * number from the instruction and multiply it by 8. We do both using
+ * rlwinm to rotate it left into bits 24 to 28.
+ *
+ * 0 10 15 20 24 28
+ * | | | | | |
+ * 000100dddddaaaaabbbbbcccccxxxxxx
+ */
+#define VD_ROTATE 14, 24, 28
+#define VA_ROTATE 19, 24, 28
+#define VB_ROTATE 24, 24, 28
+#define VC_ROTATE 29, 24, 28
+
+/*
+ * vctuxs, vctsxs have an unsigned immediate UI in bits 11 to 15. We
+ * extract it into bits 4 to 8, then add FLOAT_1_IS to make 2**UI.
+ */
+#define UI_ROTATE 7, 4, 8
+#define FLOAT_1_IS 0x3f80 /* (float 1) >> 16 */
+
+ .rodata
+ .balign 4
+.Lzero: .float 0
+.Lone: .float 1
+.Ln126: .float 126
+.Ltwo63: .float 0x1p63
+.Ltwo126: .float 0x1p126
+.Lmin: .float 0x1p-126 /* FLT_MIN */
+
+ .text
+
+/* This is the stack frame for vecast_asm. */
+#define s_size 128
+#define s_f31 120
+#define s_f30 112
+#define s_f29 104
+#define s_f28 96
+#define s_f27 88
+#define s_f26 80
+#define s_f25 72
+#define s_f24 64
+#define s_vc 48
+#define s_vb 32
+#define s_va 16
+
+/*
+ * vecast_asm(insn r3, label r4) emulates an AltiVec instruction when
+ * it traps a denormal or subnormal float (with an AltiVec assist
+ * exception). Such a float f has 0 < |f| < FLT_MIN 2**-126.
+ *
+ * MPC7450 RISC Microprocessor Family Reference Manual, 7.1.2.5 Java
+ * Mode, NaNs, Denormalized Numbers, and Zeros, has a list of trapping
+ * instructions: vaddfp, vsubfp, vmaddfp, vnmsubfp, vrefp, vrsqrtefp,
+ * vlogefp, vexptefp, vctsxs, vctuxs.
+ */
+ENTRY(vecast_asm)
+ mflr %r0 /* r0 = return address */
+ RETGUARD_SETUP_LATE(vecast_asm, %r9, %r0)
+ stwu %r1, -s_size(%r1)
+ mfmsr %r5 /* r5 = old msr */
+
+ /*
+ * Borrow the vector and floating-point units. We must
+ * preserve all float and most vector registers.
+ */
+ rlwinm %r6, %r5, 0, 17, 15 /* r6 = r5 & ~PSL_EE */
+ oris %r6, %r6, PSL_VEC >> 16
+ ori %r6, %r6, PSL_FP
+ mtmsr %r6
+ isync
+
+ stfd %f31, s_f31(%r1)
+ stfd %f30, s_f30(%r1)
+ stfd %f29, s_f29(%r1)
+ stfd %f28, s_f28(%r1)
+ stfd %f27, s_f27(%r1)
+ stfd %f26, s_f26(%r1)
+ stfd %f25, s_f25(%r1)
+ stfd %f24, s_f24(%r1)
+ mffs %f31 /* f31 = old fpscr */
+
+ lis %r6, .Lzero@ha
+ la %r6, .Lzero@l(%r6) /* r6 = address of .Lzero */
+
+ /* fpscr = zero (round to nearest, no traps) */
+ lfs %f30, 0(%r6) /* f30 = zero */
+ mtfsf 255, %f30
+
+ /* All instructions do s_vb = VB now; VD = s_va at finish. */
+ rlwinm %r7, %r3, VB_ROTATE
+ la %r8, s_vb(%r1)
+ bl vecast_store_vector
+
+ mtctr %r4
+ li %r4, 4 /* r4 = 4 loop iterations */
+ bctr /* Branch to our instruction's label. */
+
+/*
+ * vaddfp: d = a + b
+ */
+ .globl vecast_vaddfp
+vecast_vaddfp:
+ rlwinm %r7, %r3, VA_ROTATE
+ la %r8, s_va(%r1)
+ bl vecast_store_vector
+
+ /* s_va = s_va + s_vb */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7) /* r7 += 4, then load (r7). */
+ lfs %f29, (s_vb - s_va)(%r7)
+ fadds %f30, %f30, %f29
+ stfs %f30, 0(%r7)
+ bdnz 1b /* Loop 4 times. */
+ b vecast_finish
+
+/*
+ * vsubfp: d = a + b
+ */
+ .globl vecast_vsubfp
+vecast_vsubfp:
+ rlwinm %r7, %r3, VA_ROTATE
+ la %r8, s_va(%r1)
+ bl vecast_store_vector
+
+ /* s_va = s_va - s_vb */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ lfs %f29, (s_vb - s_va)(%r7)
+ fsubs %f30, %f30, %f29
+ stfs %f30, 0(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vmaddfp: d = a * c + b
+ */
+ .globl vecast_vmaddfp
+vecast_vmaddfp:
+ rlwinm %r7, %r3, VA_ROTATE
+ la %r8, s_va(%r1)
+ bl vecast_store_vector
+ rlwinm %r7, %r3, VC_ROTATE
+ la %r8, s_vc(%r1)
+ bl vecast_store_vector
+
+ /* s_va = s_va * s_vc + s_vb */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ lfs %f29, (s_vb - s_va)(%r7)
+ lfs %f28, (s_vc - s_va)(%r7)
+ fmadds %f30, %f30, %f28, %f29
+ stfs %f30, 0(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vnmsubfp: d = b - a * c
+ */
+ .globl vecast_vnmsubfp
+vecast_vnmsubfp:
+ rlwinm %r7, %r3, VA_ROTATE
+ la %r8, s_va(%r1)
+ bl vecast_store_vector
+ rlwinm %r7, %r3, VC_ROTATE
+ la %r8, s_vc(%r1)
+ bl vecast_store_vector
+
+ /* s_va = -(s_va * s_vc - s_vb) */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ lfs %f29, (s_vb - s_va)(%r7)
+ lfs %f28, (s_vc - s_va)(%r7)
+ fnmsubs %f30, %f30, %f28, %f29
+ stfs %f30, 0(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vrefp: d = estimate 1 / b
+ */
+ .globl vecast_vrefp
+vecast_vrefp:
+ /* s_va = estimate 1 / s_vb */
+ mtctr %r4
+ la %r7, (s_vb - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ fres %f30, %f30
+ stfs %f30, (s_va - s_vb)(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vrsqrtefp: d = estimate 1 / sqrt(b)
+ * 1 / sqrt(b) = 1 / sqrt(b * 2**126) * 2**63 when b < 2**-126
+ *
+ * MPC7455's frsqrte does 1 / sqrt(1) = 0.984375, relative error 1/64.
+ * AltiVec must not err over 1/4096, so avoid frsqrte.
+ */
+ .globl vecast_vrsqrtefp
+vecast_vrsqrtefp:
+ /* f30 = 1; f29 = 2**63, f28 = 2**126; f27 = 2**-126 */
+ lfs %f30, (.Lone - .Lzero)(%r6)
+ lfs %f29, (.Ltwo63 - .Lzero)(%r6)
+ lfs %f28, (.Ltwo126 - .Lzero)(%r6)
+ lfs %f27, (.Lmin - .Lzero)(%r6)
+
+ /*
+ * s_vb = s_vb * 2**126, s_va = 2**63 when b < 2**-126
+ * s_va = 1 when b >= 2**-126
+ */
+ mtctr %r4
+ la %r7, (s_vb - 4)(%r1)
+1: lfsu %f26, 4(%r7)
+ fmuls %f25, %f26, %f28
+ fsubs %f24, %f26, %f27 /* f24 selects b >= 2**-126 */
+ fsel %f26, %f24, %f26, %f25 /* f26 = b or b * 2**126 */
+ stfs %f26, 0(%r7)
+ fsel %f25, %f24, %f30, %f29 /* f25 = 1 or 2**63 */
+ stfs %f25, (s_va - s_vb)(%r7)
+ bdnz 1b
+
+ /* s_vb = estimate 1 / sqrt(s_vb) */
+ la %r7, s_vc(%r1)
+ la %r8, s_vb(%r1)
+ stvx %v31, 0, %r7 /* Save v31 in s_vc. */
+ lvx %v31, 0, %r8
+ vrsqrtefp %v31, %v31
+ stvx %v31, 0, %r8
+ lvx %v31, 0, %r7
+
+ /* s_va = s_vb * s_va */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ lfs %f29, (s_vb - s_va)(%r7)
+ fmuls %f30, %f29, %f30
+ stfs %f30, 0(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vlogefp: d = estimate log2(b)
+ * log2(b) = log2(b * 2**126) - 126 when b < 2**-126
+ */
+ .globl vecast_vlogefp
+vecast_vlogefp:
+ /* f30 = 0; f29 = 126; f28 = 2**126; f27 = 2**-126 */
+ lfs %f29, (.Ln126 - .Lzero)(%r6)
+ lfs %f28, (.Ltwo126 - .Lzero)(%r6)
+ lfs %f27, (.Lmin - .Lzero)(%r6)
+
+ /*
+ * s_vb = s_vb * 2**126, s_va = 126 when s_vb < 2**-126
+ * s_va = 0 when s_vb >= 2**-126
+ */
+ mtctr %r4
+ la %r7, (s_vb - 4)(%r1)
+1: lfsu %f26, 4(%r7)
+ fmuls %f25, %f26, %f28
+ fsubs %f24, %f26, %f27 /* f24 selects b >= 2**-126 */
+ fsel %f26, %f24, %f26, %f25 /* f26 = b or b * 2**126 */
+ stfs %f26, 0(%r7)
+ fsel %f25, %f24, %f30, %f29 /* f25 = 0 or 126 */
+ stfs %f25, (s_va - s_vb)(%r7)
+ bdnz 1b
+
+ /* s_vb = estimate log2(s_vb) */
+ la %r7, s_vc(%r1)
+ la %r8, s_vb(%r1)
+ stvx %v31, 0, %r7
+ lvx %v31, 0, %r8
+ vlogefp %v31, %v31
+ stvx %v31, 0, %r8
+ lvx %v31, 0, %r7
+
+ /* s_va = s_vb - s_va */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ lfs %f29, (s_vb - s_va)(%r7)
+ fsubs %f30, %f29, %f30
+ stfs %f30, 0(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vexptefp: d = estimate 2**b
+ * 2**b = 2**(b + 126) * 2**-126 when -252 <= b < -126
+ */
+ .globl vecast_vexptefp
+vecast_vexptefp:
+ /* f30 = 1; f29 = 126; f28 = 2**-126 */
+ lfs %f30, (.Lone - .Lzero)(%r6)
+ lfs %f29, (.Ln126 - .Lzero)(%r6)
+ lfs %f28, (.Lmin - .Lzero)(%r6)
+
+ /*
+ * s_vb = s_vb + 126 when -252 <= b < -126
+ * s_va = 2**-126 when b < -126
+ * s_va = 1 when b >= -126
+ *
+ * If b < -252, we avoid a possibly subnormal 2**(b + 126)
+ * by calculating 2**b * 2**-126 = 0 * 2**-126 = 0.
+ */
+ mtctr %r4
+ la %r7, (s_vb - 4)(%r1)
+1: lfsu %f27, 4(%r7)
+ fadds %f26, %f27, %f29 /* f26 selects b >= -126 */
+ fadds %f25, %f26, %f29 /* f25 selects b >= -252 */
+ fsel %f24, %f26, %f27, %f26
+ fsel %f24, %f25, %f24, %f27 /* f24 = b or b + 126 */
+ stfs %f24, 0(%r7)
+ fsel %f27, %f26, %f30, %f28 /* f27 = 1 or 2**-126 */
+ stfs %f27, (s_va - s_vb)(%r7)
+ bdnz 1b
+
+ /* s_vb = estimate 2**s_vb */
+ la %r7, s_vc(%r1)
+ la %r8, s_vb(%r1)
+ stvx %v31, 0, %r7
+ lvx %v31, 0, %r8
+ vexptefp %v31, %v31
+ stvx %v31, 0, %r8
+ lvx %v31, 0, %r7
+
+ /* s_va = s_vb * s_va */
+ mtctr %r4
+ la %r7, (s_va - 4)(%r1)
+1: lfsu %f30, 4(%r7)
+ lfs %f29, (s_vb - s_va)(%r7)
+ fmuls %f30, %f29, %f30
+ stfs %f30, 0(%r7)
+ bdnz 1b
+ b vecast_finish
+
+/*
+ * vctsxs: d = (int32_t)(b * 2**u) where 0 <= u < 32
+ * d = 0 when |b| < 2**-126
+ */
+ .globl vecast_vctsxs
+vecast_vctsxs:
+ /* f30 = 0; f29 = 2**-126; f28 = 2**u */
+ lfs %f29, (.Lmin - .Lzero)(%r6)
+ rlwinm %r7, %r3, UI_ROTATE
+ addis %r7, %r7, FLOAT_1_IS
+ stw %r7, s_va(%r1)
+ lfs %f28, s_va(%r1)
+
+ /* s_va = s_vb * 2**u, unless b is tiny. */
+ mtctr %r4
+ la %r7, (s_vb - 4)(%r1)
+1: lfsu %f27, 4(%r7)
+ fmuls %f26, %f27, %f28
+ fabs %f27, %f27
+ fsubs %f27, %f27, %f29 /* f27 selects |b| >= 2**-126 */
+ fsel %f26, %f27, %f26, %f30 /* f26 = b * 2**u or 0 */
+ stfs %f26, (s_va - s_vb)(%r7)
+ bdnz 1b
+
+ /* s_va = (int32_t)b */
+ la %r7, s_vc(%r1)
+ la %r8, s_va(%r1)
+ stvx %v31, 0, %r7
+ lvx %v31, 0, %r8
+ vctsxs %v31, %v31, 0 /* May set SAT in vscr. */
+ stvx %v31, 0, %r8
+ lvx %v31, 0, %r7
+ b vecast_finish
+
+/*
+ * vctuxs: d = (uint32_t)(b * 2**u) where 0 <= u < 32
+ * d = 0 when |b| < 2**-126
+ */
+ .globl vecast_vctuxs
+vecast_vctuxs:
+ /* f30 = 0; f29 = 2**-126; f28 = 2**u */
+ lfs %f29, (.Lmin - .Lzero)(%r6)
+ rlwinm %r7, %r3, UI_ROTATE
+ addis %r7, %r7, FLOAT_1_IS
+ stw %r7, s_va(%r1)
+ lfs %f28, s_va(%r1)
+
+ /* s_va = s_vb * 2**u, unless b is tiny. */
+ mtctr %r4
+ la %r7, (s_vb - 4)(%r1)
+1: lfsu %f27, 4(%r7)
+ fmuls %f26, %f27, %f28
+ fabs %f27, %f27
+ fsubs %f27, %f27, %f29 /* f27 selects |b| >= 2**-126 */
+ fsel %f26, %f27, %f26, %f30 /* f26 = b * 2**u or 0 */
+ stfs %f26, (s_va - s_vb)(%r7)
+ bdnz 1b
+
+ /* s_va = (uint32_t)b */
+ la %r7, s_vc(%r1)
+ la %r8, s_va(%r1)
+ stvx %v31, 0, %r7
+ lvx %v31, 0, %r8
+ vctuxs %v31, %v31, 0 /* May set SAT in vscr. */
+ stvx %v31, 0, %r8
+ lvx %v31, 0, %r7
+ /* b vecast_finish */
+
+vecast_finish:
+ /* VD = s_va */
+ rlwinm %r7, %r3, VD_ROTATE
+ addis %r7, %r7, 1f@ha
+ addi %r7, %r7, 1f@l
+ mtctr %r7
+ la %r8, s_va(%r1)
+ bctr
+#define M(n) lvx %v##n, 0, %r8; b 2f
+1: M( 0); M( 1); M( 2); M( 3); M( 4); M( 5); M( 6); M( 7)
+ M( 8); M( 9); M(10); M(11); M(12); M(13); M(14); M(15)
+ M(16); M(17); M(18); M(19); M(20); M(21); M(22); M(23)
+ M(24); M(25); M(26); M(27); M(28); M(29); M(30); M(31)
+#undef M
+2: mtlr %r0
+ mtfsf 255, %f31 /* Restore old fpscr. */
+ lfd %f24, s_f24(%r1)
+ lfd %f25, s_f25(%r1)
+ lfd %f26, s_f26(%r1)
+ lfd %f27, s_f27(%r1)
+ lfd %f28, s_f28(%r1)
+ lfd %f29, s_f29(%r1)
+ lfd %f30, s_f30(%r1)
+ lfd %f31, s_f31(%r1)
+ mtmsr %r5 /* Restore old msr. */
+ isync
+ addi %r1, %r1, s_size
+ RETGUARD_CHECK(vecast_asm, %r9, %r0)
+ blr
+
+/*
+ * Stores vector v(r7 / 8) to address r8.
+ */
+vecast_store_vector:
+ RETGUARD_SETUP(vecast_store_vector, %r11, %r12)
+ addis %r7, %r7, 1f@ha
+ addi %r7, %r7, 1f@l
+ mtctr %r7
+ bctr
+#define M(n) stvx %v##n, 0, %r8; b 2f
+1: M( 0); M( 1); M( 2); M( 3); M( 4); M( 5); M( 6); M( 7)
+ M( 8); M( 9); M(10); M(11); M(12); M(13); M(14); M(15)
+ M(16); M(17); M(18); M(19); M(20); M(21); M(22); M(23)
+ M(24); M(25); M(26); M(27); M(28); M(29); M(30); M(31)
+#undef M
+2: RETGUARD_CHECK(vecast_store_vector, %r11, %r12)
+ blr
projects / openbsd / commitdiff