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

@@ -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"

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