From 01ebcda726726bc34ef907bfc777e518db13f12f Mon Sep 17 00:00:00 2001
From: William Patty <wpatty24@gmail.com>
Date: Mon, 8 Nov 2021 15:36:54 -0800
Subject: [PATCH] soc: riscv: riscv-privilege: add support for mtvec vectored
 mode

Enable support for mtvec vectored mode for privilege SOCs. This
is an optional feature and takes up addtional code space. It is
necessary to support this feature for privilege SOCs that only
support mtvec vectored mode.

Change was tested on qemu_riscv32 and qemu_riscv64 boards with
CONFIG_RISCV_MTVEC_VECTORED_MODE enabled.

Signed-off-by: William Patty <wpatty24@gmail.com>
---
 soc/riscv/riscv-privilege/Kconfig         |  5 ++++
 soc/riscv/riscv-privilege/common/vector.S | 28 +++++++++++++++++++++++
 2 files changed, 33 insertions(+)

diff --git a/soc/riscv/riscv-privilege/Kconfig b/soc/riscv/riscv-privilege/Kconfig
index 88128d06e27..1d9a137290d 100644
--- a/soc/riscv/riscv-privilege/Kconfig
+++ b/soc/riscv/riscv-privilege/Kconfig
@@ -18,5 +18,10 @@ config RISCV_HAS_PLIC
 	help
 	  Does the SOC provide support for a Platform Level Interrupt Controller
 
+config RISCV_MTVEC_VECTORED_MODE
+	bool "Should the SOC use mtvec in vectored mode"
+	depends on SOC_FAMILY_RISCV_PRIVILEGE
+	help
+	  Should the SOC use mtvec in vectored mode
 
 source "soc/riscv/riscv-privilege/*/Kconfig.soc"
diff --git a/soc/riscv/riscv-privilege/common/vector.S b/soc/riscv/riscv-privilege/common/vector.S
index d0634908339..dfa2a41ab91 100644
--- a/soc/riscv/riscv-privilege/common/vector.S
+++ b/soc/riscv/riscv-privilege/common/vector.S
@@ -25,12 +25,40 @@ SECTION_FUNC(vectors, __start)
 
 	.option norvc;
 
+#if defined(CONFIG_RISCV_MTVEC_VECTORED_MODE)
+	/*
+	 * Set mtvec (Machine Trap-Vector Base-Address Register)
+	 * to __ivt (interrupt vector table). Add 1 to base
+	 * address of __ivt to indicate that vectored mode
+	 * is used (LSB = 0x1). CPU will mask the LSB out of
+	 * the address so that base address of __ivt is used.
+	 *
+	 * NOTE: __ivt is 256-byte aligned. Incorrect alignment
+	 *        of __ivt breaks this code.
+	 */
+	la t0, __ivt /* Load address of interrupt vector table */
+	addi t0, t0, 1 /* Enable vectored mode by setting LSB */
+
+/* MTVEC_DIRECT_MODE */
+#else
 	/*
 	 * Set mtvec (Machine Trap-Vector Base-Address Register)
 	 * to __irq_wrapper.
 	 */
 	la t0, __irq_wrapper
+#endif
+
 	csrw mtvec, t0
 
 	/* Jump to __reset */
 	tail __reset
+
+#if defined(CONFIG_RISCV_MTVEC_VECTORED_MODE)
+SECTION_FUNC(reset,  __ivt)
+	.option push
+	.option norvc
+	.balign 0x100 /* must be 256 byte aligned per specification */
+	.rept (CONFIG_NUM_IRQS)
+	j __irq_wrapper
+	.endr
+#endif