aboutsummaryrefslogtreecommitdiffstats
path: root/roms/opensbi/lib/sbi/sbi_misaligned_ldst.c
diff options
context:
space:
mode:
Diffstat (limited to 'roms/opensbi/lib/sbi/sbi_misaligned_ldst.c')
-rw-r--r--roms/opensbi/lib/sbi/sbi_misaligned_ldst.c243
1 files changed, 243 insertions, 0 deletions
diff --git a/roms/opensbi/lib/sbi/sbi_misaligned_ldst.c b/roms/opensbi/lib/sbi/sbi_misaligned_ldst.c
new file mode 100644
index 000000000..5057cb5ec
--- /dev/null
+++ b/roms/opensbi/lib/sbi/sbi_misaligned_ldst.c
@@ -0,0 +1,243 @@
+/*
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2019 Western Digital Corporation or its affiliates.
+ *
+ * Authors:
+ * Anup Patel <anup.patel@wdc.com>
+ */
+
+#include <sbi/riscv_asm.h>
+#include <sbi/riscv_encoding.h>
+#include <sbi/riscv_fp.h>
+#include <sbi/sbi_error.h>
+#include <sbi/sbi_misaligned_ldst.h>
+#include <sbi/sbi_trap.h>
+#include <sbi/sbi_unpriv.h>
+
+union reg_data {
+ u8 data_bytes[8];
+ ulong data_ulong;
+ u64 data_u64;
+};
+
+int sbi_misaligned_load_handler(ulong addr, ulong tval2, ulong tinst,
+ struct sbi_trap_regs *regs)
+{
+ ulong insn, insn_len;
+ union reg_data val;
+ struct sbi_trap_info uptrap;
+ int i, fp = 0, shift = 0, len = 0;
+
+ if (tinst & 0x1) {
+ /*
+ * Bit[0] == 1 implies trapped instruction value is
+ * transformed instruction or custom instruction.
+ */
+ insn = tinst | INSN_16BIT_MASK;
+ insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
+ } else {
+ /*
+ * Bit[0] == 0 implies trapped instruction value is
+ * zero or special value.
+ */
+ insn = sbi_get_insn(regs->mepc, &uptrap);
+ if (uptrap.cause) {
+ uptrap.epc = regs->mepc;
+ return sbi_trap_redirect(regs, &uptrap);
+ }
+ insn_len = INSN_LEN(insn);
+ }
+
+ if ((insn & INSN_MASK_LW) == INSN_MATCH_LW) {
+ len = 4;
+ shift = 8 * (sizeof(ulong) - len);
+#if __riscv_xlen == 64
+ } else if ((insn & INSN_MASK_LD) == INSN_MATCH_LD) {
+ len = 8;
+ shift = 8 * (sizeof(ulong) - len);
+ } else if ((insn & INSN_MASK_LWU) == INSN_MATCH_LWU) {
+ len = 4;
+#endif
+#ifdef __riscv_flen
+ } else if ((insn & INSN_MASK_FLD) == INSN_MATCH_FLD) {
+ fp = 1;
+ len = 8;
+ } else if ((insn & INSN_MASK_FLW) == INSN_MATCH_FLW) {
+ fp = 1;
+ len = 4;
+#endif
+ } else if ((insn & INSN_MASK_LH) == INSN_MATCH_LH) {
+ len = 2;
+ shift = 8 * (sizeof(ulong) - len);
+ } else if ((insn & INSN_MASK_LHU) == INSN_MATCH_LHU) {
+ len = 2;
+#if __riscv_xlen >= 64
+ } else if ((insn & INSN_MASK_C_LD) == INSN_MATCH_C_LD) {
+ len = 8;
+ shift = 8 * (sizeof(ulong) - len);
+ insn = RVC_RS2S(insn) << SH_RD;
+ } else if ((insn & INSN_MASK_C_LDSP) == INSN_MATCH_C_LDSP &&
+ ((insn >> SH_RD) & 0x1f)) {
+ len = 8;
+ shift = 8 * (sizeof(ulong) - len);
+#endif
+ } else if ((insn & INSN_MASK_C_LW) == INSN_MATCH_C_LW) {
+ len = 4;
+ shift = 8 * (sizeof(ulong) - len);
+ insn = RVC_RS2S(insn) << SH_RD;
+ } else if ((insn & INSN_MASK_C_LWSP) == INSN_MATCH_C_LWSP &&
+ ((insn >> SH_RD) & 0x1f)) {
+ len = 4;
+ shift = 8 * (sizeof(ulong) - len);
+#ifdef __riscv_flen
+ } else if ((insn & INSN_MASK_C_FLD) == INSN_MATCH_C_FLD) {
+ fp = 1;
+ len = 8;
+ insn = RVC_RS2S(insn) << SH_RD;
+ } else if ((insn & INSN_MASK_C_FLDSP) == INSN_MATCH_C_FLDSP) {
+ fp = 1;
+ len = 8;
+#if __riscv_xlen == 32
+ } else if ((insn & INSN_MASK_C_FLW) == INSN_MATCH_C_FLW) {
+ fp = 1;
+ len = 4;
+ insn = RVC_RS2S(insn) << SH_RD;
+ } else if ((insn & INSN_MASK_C_FLWSP) == INSN_MATCH_C_FLWSP) {
+ fp = 1;
+ len = 4;
+#endif
+#endif
+ } else {
+ uptrap.epc = regs->mepc;
+ uptrap.cause = CAUSE_MISALIGNED_LOAD;
+ uptrap.tval = addr;
+ uptrap.tval2 = tval2;
+ uptrap.tinst = tinst;
+ return sbi_trap_redirect(regs, &uptrap);
+ }
+
+ val.data_u64 = 0;
+ for (i = 0; i < len; i++) {
+ val.data_bytes[i] = sbi_load_u8((void *)(addr + i),
+ &uptrap);
+ if (uptrap.cause) {
+ uptrap.epc = regs->mepc;
+ return sbi_trap_redirect(regs, &uptrap);
+ }
+ }
+
+ if (!fp)
+ SET_RD(insn, regs, ((long)(val.data_ulong << shift)) >> shift);
+#ifdef __riscv_flen
+ else if (len == 8)
+ SET_F64_RD(insn, regs, val.data_u64);
+ else
+ SET_F32_RD(insn, regs, val.data_ulong);
+#endif
+
+ regs->mepc += insn_len;
+
+ return 0;
+}
+
+int sbi_misaligned_store_handler(ulong addr, ulong tval2, ulong tinst,
+ struct sbi_trap_regs *regs)
+{
+ ulong insn, insn_len;
+ union reg_data val;
+ struct sbi_trap_info uptrap;
+ int i, len = 0;
+
+ if (tinst & 0x1) {
+ /*
+ * Bit[0] == 1 implies trapped instruction value is
+ * transformed instruction or custom instruction.
+ */
+ insn = tinst | INSN_16BIT_MASK;
+ insn_len = (tinst & 0x2) ? INSN_LEN(insn) : 2;
+ } else {
+ /*
+ * Bit[0] == 0 implies trapped instruction value is
+ * zero or special value.
+ */
+ insn = sbi_get_insn(regs->mepc, &uptrap);
+ if (uptrap.cause) {
+ uptrap.epc = regs->mepc;
+ return sbi_trap_redirect(regs, &uptrap);
+ }
+ insn_len = INSN_LEN(insn);
+ }
+
+ val.data_ulong = GET_RS2(insn, regs);
+
+ if ((insn & INSN_MASK_SW) == INSN_MATCH_SW) {
+ len = 4;
+#if __riscv_xlen == 64
+ } else if ((insn & INSN_MASK_SD) == INSN_MATCH_SD) {
+ len = 8;
+#endif
+#ifdef __riscv_flen
+ } else if ((insn & INSN_MASK_FSD) == INSN_MATCH_FSD) {
+ len = 8;
+ val.data_u64 = GET_F64_RS2(insn, regs);
+ } else if ((insn & INSN_MASK_FSW) == INSN_MATCH_FSW) {
+ len = 4;
+ val.data_ulong = GET_F32_RS2(insn, regs);
+#endif
+ } else if ((insn & INSN_MASK_SH) == INSN_MATCH_SH) {
+ len = 2;
+#if __riscv_xlen >= 64
+ } else if ((insn & INSN_MASK_C_SD) == INSN_MATCH_C_SD) {
+ len = 8;
+ val.data_ulong = GET_RS2S(insn, regs);
+ } else if ((insn & INSN_MASK_C_SDSP) == INSN_MATCH_C_SDSP &&
+ ((insn >> SH_RD) & 0x1f)) {
+ len = 8;
+ val.data_ulong = GET_RS2C(insn, regs);
+#endif
+ } else if ((insn & INSN_MASK_C_SW) == INSN_MATCH_C_SW) {
+ len = 4;
+ val.data_ulong = GET_RS2S(insn, regs);
+ } else if ((insn & INSN_MASK_C_SWSP) == INSN_MATCH_C_SWSP &&
+ ((insn >> SH_RD) & 0x1f)) {
+ len = 4;
+ val.data_ulong = GET_RS2C(insn, regs);
+#ifdef __riscv_flen
+ } else if ((insn & INSN_MASK_C_FSD) == INSN_MATCH_C_FSD) {
+ len = 8;
+ val.data_u64 = GET_F64_RS2S(insn, regs);
+ } else if ((insn & INSN_MASK_C_FSDSP) == INSN_MATCH_C_FSDSP) {
+ len = 8;
+ val.data_u64 = GET_F64_RS2C(insn, regs);
+#if __riscv_xlen == 32
+ } else if ((insn & INSN_MASK_C_FSW) == INSN_MATCH_C_FSW) {
+ len = 4;
+ val.data_ulong = GET_F32_RS2S(insn, regs);
+ } else if ((insn & INSN_MASK_C_FSWSP) == INSN_MATCH_C_FSWSP) {
+ len = 4;
+ val.data_ulong = GET_F32_RS2C(insn, regs);
+#endif
+#endif
+ } else {
+ uptrap.epc = regs->mepc;
+ uptrap.cause = CAUSE_MISALIGNED_STORE;
+ uptrap.tval = addr;
+ uptrap.tval2 = tval2;
+ uptrap.tinst = tinst;
+ return sbi_trap_redirect(regs, &uptrap);
+ }
+
+ for (i = 0; i < len; i++) {
+ sbi_store_u8((void *)(addr + i), val.data_bytes[i],
+ &uptrap);
+ if (uptrap.cause) {
+ uptrap.epc = regs->mepc;
+ return sbi_trap_redirect(regs, &uptrap);
+ }
+ }
+
+ regs->mepc += insn_len;
+
+ return 0;
+}