#include <riscv_vector.h>
#include <softrvv.h>
#include <springbok.h>
#include <stdio.h>
#include <stdlib.h>

#include "pw_unit_test/framework.h"
#include "softrvv.h"

namespace vec_disable_test {
namespace {

int exception_occurred = false;
int mcause = 0x0;
const int MCAUSE_ILLEGAL_INSTRUCTION = 0x2;
const uint32_t AVL_CONST = 15;

class VecDisableTest : public ::testing::Test {
 protected:
  void SetUp() override { softrvv::disable_rvv(); }
  void TearDown() override { softrvv::enable_rvv(); }
};

TEST_F(VecDisableTest, vec_disable) {
  uint32_t vl = 0;
  __asm__ volatile("vsetivli %[VL], %[AVL], e32, m1, tu, mu"
                   : [VL] "=r"(vl)
                   : [AVL] "n"(AVL_CONST));
  ASSERT_EQ(exception_occurred, true);
  ASSERT_EQ(mcause, MCAUSE_ILLEGAL_INSTRUCTION);
}

extern "C" void __attribute__((interrupt("machine"))) exception_handler(void) {
  uint32_t mepc;
  __asm__ volatile("csrr %[MCAUSE], mcause" : [MCAUSE] "=r"(mcause) :);
  if (mcause == MCAUSE_ILLEGAL_INSTRUCTION) {
    exception_occurred = true;
  }
  // Read MEPC and increment to skip offending instruction
  __asm__ volatile("csrr %[MEPC], mepc" : [MEPC] "=r"(mepc) :);
  mepc += 4;
  __asm__ volatile("csrw mepc, %[MEPC]" ::[MEPC] "r"(mepc));
  return;
}

}  // namespace
}  // namespace vec_disable_test