include: Add hard FP access macros and defines

This patch adds hardware floating-point (hard FP) access
macros and defines.

Signed-off-by: Anup Patel <[email protected]>

5 files changed, 259 insertions, 7 deletions

diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
index 381f3058..a6651a15 100644
--- a/include/sbi/riscv_encoding.h
+++ b/include/sbi/riscv_encoding.h
@@ -446,6 +446,19 @@
 #define INSN_MATCH_SD			0x3023
 #define INSN_MASK_SD			0x707f
 
+#define INSN_MATCH_FLW			0x2007
+#define INSN_MASK_FLW			0x707f
+#define INSN_MATCH_FLD			0x3007
+#define INSN_MASK_FLD			0x707f
+#define INSN_MATCH_FLQ			0x4007
+#define INSN_MASK_FLQ			0x707f
+#define INSN_MATCH_FSW			0x2027
+#define INSN_MASK_FSW			0x707f
+#define INSN_MATCH_FSD			0x3027
+#define INSN_MASK_FSD			0x707f
+#define INSN_MATCH_FSQ			0x4027
+#define INSN_MASK_FSQ			0x707f
+
 #define INSN_MATCH_C_LD			0x6000
 #define INSN_MASK_C_LD			0xe003
 #define INSN_MATCH_C_SD			0xe000
diff --git a/include/sbi/riscv_fp.h b/include/sbi/riscv_fp.h
new file mode 100644
index 00000000..0deb8e4a
--- /dev/null
+++ b/include/sbi/riscv_fp.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <[email protected]>
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#ifndef __RISCV_FP_H__
+#define __RISCV_FP_H__
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/sbi_types.h>
+
+#define GET_PRECISION(insn) (((insn) >> 25) & 3)
+#define GET_RM(insn) (((insn) >> 12) & 7)
+#define PRECISION_S 0
+#define PRECISION_D 1
+
+#ifdef __riscv_flen
+
+#define GET_F32_REG(insn, pos, regs) ({ \
+  register s32 value asm("a0") = SHIFT_RIGHT(insn, (pos)-3) & 0xf8; \
+  ulong tmp; \
+  asm ("1: auipc %0, %%pcrel_hi(get_f32_reg); add %0, %0, %1; jalr t0, %0, %%pcrel_lo(1b)" : "=&r"(tmp), "+&r"(value) :: "t0"); \
+  value; })
+#define SET_F32_REG(insn, pos, regs, val) ({ \
+  register u32 value asm("a0") = (val); \
+  ulong offset = SHIFT_RIGHT(insn, (pos)-3) & 0xf8; \
+  ulong tmp; \
+  asm volatile ("1: auipc %0, %%pcrel_hi(put_f32_reg); add %0, %0, %2; jalr t0, %0, %%pcrel_lo(1b)" : "=&r"(tmp) : "r"(value), "r"(offset) : "t0"); })
+#define init_fp_reg(i) SET_F32_REG((i) << 3, 3, 0, 0)
+#define GET_F64_REG(insn, pos, regs) ({ \
+  register ulong value asm("a0") = SHIFT_RIGHT(insn, (pos)-3) & 0xf8; \
+  ulong tmp; \
+  asm ("1: auipc %0, %%pcrel_hi(get_f64_reg); add %0, %0, %1; jalr t0, %0, %%pcrel_lo(1b)" : "=&r"(tmp), "+&r"(value) :: "t0"); \
+  sizeof(ulong) == 4 ? *(int64_t*)value : (int64_t)value; })
+#define SET_F64_REG(insn, pos, regs, val) ({ \
+  uint64_t __val = (val); \
+  register ulong value asm("a0") = sizeof(ulong) == 4 ? (ulong)&__val : (ulong)__val; \
+  ulong offset = SHIFT_RIGHT(insn, (pos)-3) & 0xf8; \
+  ulong tmp; \
+  asm volatile ("1: auipc %0, %%pcrel_hi(put_f64_reg); add %0, %0, %2; jalr t0, %0, %%pcrel_lo(1b)" : "=&r"(tmp) : "r"(value), "r"(offset) : "t0"); })
+#define GET_FCSR() csr_read(fcsr)
+#define SET_FCSR(value) csr_write(fcsr, (value))
+#define GET_FRM() csr_read(frm)
+#define SET_FRM(value) csr_write(frm, (value))
+#define GET_FFLAGS() csr_read(fflags)
+#define SET_FFLAGS(value) csr_write(fflags, (value))
+
+#define SET_FS_DIRTY() ((void) 0)
+
+#else
+#error "Floating point emulation not supported.\n"
+#endif
+
+#define GET_F32_RS1(insn, regs) (GET_F32_REG(insn, 15, regs))
+#define GET_F32_RS2(insn, regs) (GET_F32_REG(insn, 20, regs))
+#define GET_F32_RS3(insn, regs) (GET_F32_REG(insn, 27, regs))
+#define GET_F64_RS1(insn, regs) (GET_F64_REG(insn, 15, regs))
+#define GET_F64_RS2(insn, regs) (GET_F64_REG(insn, 20, regs))
+#define GET_F64_RS3(insn, regs) (GET_F64_REG(insn, 27, regs))
+#define SET_F32_RD(insn, regs, val) (SET_F32_REG(insn, 7, regs, val), SET_FS_DIRTY())
+#define SET_F64_RD(insn, regs, val) (SET_F64_REG(insn, 7, regs, val), SET_FS_DIRTY())
+
+#define GET_F32_RS2C(insn, regs) (GET_F32_REG(insn, 2, regs))
+#define GET_F32_RS2S(insn, regs) (GET_F32_REG(RVC_RS2S(insn), 0, regs))
+#define GET_F64_RS2C(insn, regs) (GET_F64_REG(insn, 2, regs))
+#define GET_F64_RS2S(insn, regs) (GET_F64_REG(RVC_RS2S(insn), 0, regs))
+
+#endif
diff --git a/lib/objects.mk b/lib/objects.mk
index b70557dd..2ce6328f 100644
--- a/lib/objects.mk
+++ b/lib/objects.mk
@@ -9,6 +9,7 @@
 
 lib-objs-y += riscv_asm.o
 lib-objs-y += riscv_atomic.o
+lib-objs-y += riscv_hardfp.o
 lib-objs-y += riscv_locks.o
 
 lib-objs-y += sbi_console.o
diff --git a/lib/riscv_hardfp.S b/lib/riscv_hardfp.S
new file mode 100644
index 00000000..11f3e41e
--- /dev/null
+++ b/lib/riscv_hardfp.S
@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ *   Anup Patel <[email protected]>
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ */
+
+#ifdef __riscv_flen
+
+#if __riscv_flen != 64
+# error single-float only is not supported
+#endif
+
+#define get_f32(which) fmv.x.s a0, which; jr t0
+#define put_f32(which) fmv.s.x which, a0; jr t0
+#if __riscv_xlen == 64
+# define get_f64(which) fmv.x.d a0, which; jr t0
+# define put_f64(which) fmv.d.x which, a0; jr t0
+#else
+# define get_f64(which) fsd which, 0(a0); jr t0
+# define put_f64(which) fld which, 0(a0); jr t0
+#endif
+
+	.text
+	.option norvc
+	.globl get_f32_reg
+	get_f32_reg:
+		get_f32(f0)
+		get_f32(f1)
+		get_f32(f2)
+		get_f32(f3)
+		get_f32(f4)
+		get_f32(f5)
+		get_f32(f6)
+		get_f32(f7)
+		get_f32(f8)
+		get_f32(f9)
+		get_f32(f10)
+		get_f32(f11)
+		get_f32(f12)
+		get_f32(f13)
+		get_f32(f14)
+		get_f32(f15)
+		get_f32(f16)
+		get_f32(f17)
+		get_f32(f18)
+		get_f32(f19)
+		get_f32(f20)
+		get_f32(f21)
+		get_f32(f22)
+		get_f32(f23)
+		get_f32(f24)
+		get_f32(f25)
+		get_f32(f26)
+		get_f32(f27)
+		get_f32(f28)
+		get_f32(f29)
+		get_f32(f30)
+		get_f32(f31)
+
+	.text
+	.globl put_f32_reg
+	put_f32_reg:
+		put_f32(f0)
+		put_f32(f1)
+		put_f32(f2)
+		put_f32(f3)
+		put_f32(f4)
+		put_f32(f5)
+		put_f32(f6)
+		put_f32(f7)
+		put_f32(f8)
+		put_f32(f9)
+		put_f32(f10)
+		put_f32(f11)
+		put_f32(f12)
+		put_f32(f13)
+		put_f32(f14)
+		put_f32(f15)
+		put_f32(f16)
+		put_f32(f17)
+		put_f32(f18)
+		put_f32(f19)
+		put_f32(f20)
+		put_f32(f21)
+		put_f32(f22)
+		put_f32(f23)
+		put_f32(f24)
+		put_f32(f25)
+		put_f32(f26)
+		put_f32(f27)
+		put_f32(f28)
+		put_f32(f29)
+		put_f32(f30)
+		put_f32(f31)
+
+	.text
+	.globl get_f64_reg
+	get_f64_reg:
+		get_f64(f0)
+		get_f64(f1)
+		get_f64(f2)
+		get_f64(f3)
+		get_f64(f4)
+		get_f64(f5)
+		get_f64(f6)
+		get_f64(f7)
+		get_f64(f8)
+		get_f64(f9)
+		get_f64(f10)
+		get_f64(f11)
+		get_f64(f12)
+		get_f64(f13)
+		get_f64(f14)
+		get_f64(f15)
+		get_f64(f16)
+		get_f64(f17)
+		get_f64(f18)
+		get_f64(f19)
+		get_f64(f20)
+		get_f64(f21)
+		get_f64(f22)
+		get_f64(f23)
+		get_f64(f24)
+		get_f64(f25)
+		get_f64(f26)
+		get_f64(f27)
+		get_f64(f28)
+		get_f64(f29)
+		get_f64(f30)
+		get_f64(f31)
+
+	.text
+	.globl put_f64_reg
+	put_f64_reg:
+		put_f64(f0)
+		put_f64(f1)
+		put_f64(f2)
+		put_f64(f3)
+		put_f64(f4)
+		put_f64(f5)
+		put_f64(f6)
+		put_f64(f7)
+		put_f64(f8)
+		put_f64(f9)
+		put_f64(f10)
+		put_f64(f11)
+		put_f64(f12)
+		put_f64(f13)
+		put_f64(f14)
+		put_f64(f15)
+		put_f64(f16)
+		put_f64(f17)
+		put_f64(f18)
+		put_f64(f19)
+		put_f64(f20)
+		put_f64(f21)
+		put_f64(f22)
+		put_f64(f23)
+		put_f64(f24)
+		put_f64(f25)
+		put_f64(f26)
+		put_f64(f27)
+		put_f64(f28)
+		put_f64(f29)
+		put_f64(f30)
+		put_f64(f31)
+
+#endif
diff --git a/lib/sbi_hart.c b/lib/sbi_hart.c
index 1e5465f5..e8a689ed 100644
--- a/lib/sbi_hart.c
+++ b/lib/sbi_hart.c
@@ -10,6 +10,7 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_barrier.h>
 #include <sbi/riscv_encoding.h>
+#include <sbi/riscv_fp.h>
 #include <sbi/riscv_locks.h>
 #include <sbi/sbi_bits.h>
 #include <sbi/sbi_console.h>
@@ -48,13 +49,6 @@ static void mstatus_init(struct sbi_scratch *scratch, u32 hartid)
 		csr_write(sptbr, 0);
 }
 
-#ifdef __riscv_flen
-static void init_fp_reg(int i)
-{
-	/* TODO: */
-}
-#endif
-
 static int fp_init(u32 hartid)
 {
 #ifdef __riscv_flen