// See LICENSE for license details.

#include "emulation.h"
#include "fp_emulation.h"
#include "unprivileged_memory.h"
#include "mtrap.h"
#include "config.h"
#include "pk.h"

union byte_array {
  uint8_t bytes[8];
  uintptr_t intx;
  uint16_t int16;
  uint32_t int32;
  uint64_t int64;
};

void misaligned_load_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
{
  union byte_array val;
  uintptr_t mstatus;
  insn_t insn = get_insn(mepc, &mstatus);
  uintptr_t npc = mepc + insn_len(insn);
  uintptr_t addr = read_csr(mtval);

  int shift = 0, fp = 0, len;
  if ((insn & MASK_LW) == MATCH_LW)
    len = 4, shift = 8*(sizeof(uintptr_t) - len);
#if __riscv_xlen == 64
  else if ((insn & MASK_LD) == MATCH_LD)
    len = 8, shift = 8*(sizeof(uintptr_t) - len);
  else if ((insn & MASK_LWU) == MATCH_LWU)
    len = 4;
#endif
#ifdef PK_ENABLE_FP_EMULATION
  else if ((insn & MASK_FLD) == MATCH_FLD)
    fp = 1, len = 8;
  else if ((insn & MASK_FLW) == MATCH_FLW)
    fp = 1, len = 4;
  else if ((insn & MASK_FLH) == MATCH_FLH)
    fp = 1, len = 2;
#endif
  else if ((insn & MASK_LH) == MATCH_LH)
    len = 2, shift = 8*(sizeof(uintptr_t) - len);
  else if ((insn & MASK_LHU) == MATCH_LHU)
    len = 2;
#ifdef __riscv_vector
  else if ((insn & (MASK_VLE8_V & 0x707f)) == (MATCH_VLE8_V & 0x707f)
           || (insn & (MASK_VLE16_V & 0x707f)) == (MATCH_VLE16_V & 0x707f)
           || (insn & (MASK_VLE32_V & 0x707f)) == (MATCH_VLE32_V & 0x707f)
           || (insn & (MASK_VLE64_V & 0x707f)) == (MATCH_VLE64_V & 0x707f))
    return misaligned_vec_ldst(regs, mcause, mepc, mstatus, insn);
#endif
#ifdef __riscv_compressed
# if __riscv_xlen >= 64
  else if ((insn & MASK_C_LD) == MATCH_C_LD)
    len = 8, shift = 8*(sizeof(uintptr_t) - len), insn = RVC_RS2S(insn) << SH_RD;
  else if ((insn & MASK_C_LDSP) == MATCH_C_LDSP && ((insn >> SH_RD) & 0x1f))
    len = 8, shift = 8*(sizeof(uintptr_t) - len);
# endif
  else if ((insn & MASK_C_LW) == MATCH_C_LW)
    len = 4, shift = 8*(sizeof(uintptr_t) - len), insn = RVC_RS2S(insn) << SH_RD;
  else if ((insn & MASK_C_LWSP) == MATCH_C_LWSP && ((insn >> SH_RD) & 0x1f))
    len = 4, shift = 8*(sizeof(uintptr_t) - len);
# ifdef PK_ENABLE_FP_EMULATION
  else if ((insn & MASK_C_FLD) == MATCH_C_FLD)
    fp = 1, len = 8, insn = RVC_RS2S(insn) << SH_RD;
  else if ((insn & MASK_C_FLDSP) == MATCH_C_FLDSP)
    fp = 1, len = 8;
#  if __riscv_xlen == 32
  else if ((insn & MASK_C_FLW) == MATCH_C_FLW)
    fp = 1, len = 4, insn = RVC_RS2S(insn) << SH_RD;
  else if ((insn & MASK_C_FLWSP) == MATCH_C_FLWSP)
    fp = 1, len = 4;
#  endif
# endif
#endif
  else {
    mcause = CAUSE_LOAD_ACCESS;
    write_csr(mcause, mcause);
    return truly_illegal_insn(regs, mcause, mepc, mstatus, insn);
  }

  val.int64 = 0;
  for (intptr_t i = 0; i < len; i++)
    val.bytes[i] = load_uint8_t((void *)(addr + i), mepc);

  if (!fp)
#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
    SET_RD(insn, regs, (intptr_t)val.intx << shift >> shift);
#else
    SET_RD(insn, regs, (intptr_t)val.intx >> shift);
#endif
  else if (len == 8)
    SET_F64_RD(insn, regs, val.int64);
  else if (len == 4)
    SET_F32_RD(insn, regs, val.int32);
  else
    SET_F32_RD(insn, regs, val.int16 | 0xffff0000U);

  write_csr(mepc, npc);
}

void misaligned_store_trap(uintptr_t* regs, uintptr_t mcause, uintptr_t mepc)
{
  union byte_array val;
  uintptr_t mstatus;
  insn_t insn = get_insn(mepc, &mstatus);
  uintptr_t npc = mepc + insn_len(insn);
  int len;

  val.intx = GET_RS2(insn, regs);
  if ((insn & MASK_SW) == MATCH_SW)
    len = 4;
#if __riscv_xlen == 64
  else if ((insn & MASK_SD) == MATCH_SD)
    len = 8;
#endif
#ifdef PK_ENABLE_FP_EMULATION
  else if ((insn & MASK_FSD) == MATCH_FSD)
    len = 8, val.int64 = GET_F64_RS2(insn, regs);
  else if ((insn & MASK_FSW) == MATCH_FSW)
    len = 4, val.intx = GET_F32_RS2(insn, regs);
  else if ((insn & MASK_FSH) == MATCH_FSH)
    len = 2, val.intx = GET_F32_RS2(insn, regs);
#endif
  else if ((insn & MASK_SH) == MATCH_SH)
    len = 2;
#ifdef __riscv_vector
  else if ((insn & (MASK_VSE8_V & 0x707f)) == (MATCH_VSE8_V & 0x707f)
           || (insn & (MASK_VSE16_V & 0x707f)) == (MATCH_VSE16_V & 0x707f)
           || (insn & (MASK_VSE32_V & 0x707f)) == (MATCH_VSE32_V & 0x707f)
           || (insn & (MASK_VSE64_V & 0x707f)) == (MATCH_VSE64_V & 0x707f))
    return misaligned_vec_ldst(regs, mcause, mepc, mstatus, insn);
#endif
#ifdef __riscv_compressed
# if __riscv_xlen >= 64
  else if ((insn & MASK_C_SD) == MATCH_C_SD)
    len = 8, val.intx = GET_RS2S(insn, regs);
  else if ((insn & MASK_C_SDSP) == MATCH_C_SDSP)
    len = 8, val.intx = GET_RS2C(insn, regs);
# endif
  else if ((insn & MASK_C_SW) == MATCH_C_SW)
    len = 4, val.intx = GET_RS2S(insn, regs);
  else if ((insn & MASK_C_SWSP) == MATCH_C_SWSP)
    len = 4, val.intx = GET_RS2C(insn, regs);
# ifdef PK_ENABLE_FP_EMULATION
  else if ((insn & MASK_C_FSD) == MATCH_C_FSD)
    len = 8, val.int64 = GET_F64_RS2S(insn, regs);
  else if ((insn & MASK_C_FSDSP) == MATCH_C_FSDSP)
    len = 8, val.int64 = GET_F64_RS2C(insn, regs);
#  if __riscv_xlen == 32
  else if ((insn & MASK_C_FSW) == MATCH_C_FSW)
    len = 4, val.intx = GET_F32_RS2S(insn, regs);
  else if ((insn & MASK_C_FSWSP) == MATCH_C_FSWSP)
    len = 4, val.intx = GET_F32_RS2C(insn, regs);
#  endif
# endif
#endif
  else {
    mcause = CAUSE_STORE_ACCESS;
    write_csr(mcause, mcause);
    return truly_illegal_insn(regs, mcause, mepc, mstatus, insn);
  }

  uintptr_t addr = read_csr(mtval);
#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
  intptr_t offs = (len == 8? 0 : sizeof(val.intx) - len);
#else
  intptr_t offs = 0;
#endif
  for (int i = 0; i < len; i++)
    store_uint8_t((void *)(addr + i), val.bytes[offs + i], mepc);

  write_csr(mepc, npc);
}