nixos-riscv/opensbi-test-plic.patch

diff --git a/lib/sbi/sbi_irqchip.c b/lib/sbi/sbi_irqchip.c
index 0ae604a..54449c7 100644
--- a/lib/sbi/sbi_irqchip.c
+++ b/lib/sbi/sbi_irqchip.c
@@ -9,6 +9,9 @@
 
 #include <sbi/sbi_irqchip.h>
 #include <sbi/sbi_platform.h>
+#include <sbi/sbi_console.h>
+
+static void do_plic_test(void);
 
 static int default_irqfn(void)
 {
@@ -37,8 +40,10 @@ int sbi_irqchip_init(struct sbi_scratch *scratch, bool cold_boot)
 	if (rc)
 		return rc;
 
-	if (ext_irqfn != default_irqfn)
-		csr_set(CSR_MIE, MIP_MEIP);
+	//csr_set(CSR_MIE, MIP_SEIP);
+	//csr_set(CSR_MSTATUS, MSTATUS_SIE);
+
+	do_plic_test();
 
 	return 0;
 }
@@ -47,8 +52,161 @@ void sbi_irqchip_exit(struct sbi_scratch *scratch)
 {
 	const struct sbi_platform *plat = sbi_platform_ptr(scratch);
 
-	if (ext_irqfn != default_irqfn)
-		csr_clear(CSR_MIE, MIP_MEIP);
+	//csr_clear(CSR_MIE, MIP_SEIP);
+	//csr_clear(CSR_MSTATUS, MSTATUS_SIE);
 
 	sbi_platform_irqchip_exit(plat);
 }
+
+
+/* ----------------- PLIC tests ---------------- */
+
+
+#define MIE_MEIE        (1UL << 11)  // Machine External Interrupt Enable
+#define SIE_SEIE	(1UL << 9)
+#define MIDELEG_SEIE	(1UL << 9) // Delegate Machine External Interrupt to Supervisor
+#define PLIC_TIMER_PORT	4
+// Base address of PLIC
+#define PLIC_BASE               0x0000000040800000UL
+#define PLIC_PRIORITY_OFFSET    0x0UL
+#define PLIC_PENDING_OFFSET     0x1000UL
+#define PLIC_ENABLE_OFFSET      0x2000UL
+#define PLIC_THRESHOLD_OFFSET   0x200000UL
+#define PLIC_CLAIM_OFFSET       0x200004UL
+
+// Aux timer
+#define AUX_TIMER_BASE   0x40010000UL
+#define MTIMECMP_OFFSET  0x4000UL
+#define MTIME_OFFSET     0xBFF8UL
+
+#define MSTATUS_MPP_MASK (3 << 11)
+#define MSTATUS_MPP_SUPERVISOR (1 << 11)
+
+static void dumpregs(int machine)
+{
+	char *prefix = "\t";
+	char *suffix = "\t";
+	sbi_printf("Registers:\n");
+	if (machine) {
+		sbi_printf("%sMIE%s: 0x%" PRILX "\n",
+			   prefix, suffix, csr_read(CSR_MIE));
+		sbi_printf("%sMIP%s: 0x%" PRILX "\n",
+			   prefix, suffix, csr_read(CSR_MIP));
+		sbi_printf("%sMSTATUS%s: 0x%" PRILX "\n",
+			   prefix, suffix, csr_read(CSR_MSTATUS));
+		sbi_printf("%sMIDELEG%s: 0x%" PRILX "\n",
+			   prefix, suffix, csr_read(CSR_MIDELEG));
+	}
+	sbi_printf("%sSIE%s: 0x%" PRILX "\n",
+		   prefix, suffix, csr_read(CSR_SIE));
+	sbi_printf("%sSIP%s: 0x%" PRILX "\n",
+		   prefix, suffix, csr_read(CSR_SIP));
+	sbi_printf("%sSSTATUS%s: 0x%" PRILX "\n",
+		   prefix, suffix, csr_read(CSR_SSTATUS));
+}
+
+static void __attribute__((optimize("O0"))) switch_to_supervisor_mode(int (*target_address)(void))
+{
+    unsigned long mstatus;
+
+    // Read the current mstatus
+    asm volatile("csrr %0, mstatus" : "=r"(mstatus));
+
+    // Set the MPP field to supervisor mode
+    mstatus = (mstatus & ~MSTATUS_MPP_MASK) | MSTATUS_MPP_SUPERVISOR;
+
+    // Write back the modified mstatus
+    asm volatile("csrw mstatus, %0" : : "r"(mstatus));
+
+    // Set the mepc to the target address
+    asm volatile("csrw mepc, %0" : : "r"(target_address));
+
+    // Use mret to return to the specified address in supervisor mode
+    asm volatile("mret");
+}
+
+static int supervisor_mode_code(void)
+{
+	sbi_printf("Hello from supervisor\n");
+	dumpregs(0);
+	sbi_printf("Waiting for interrupt...\n");
+	int i = 0;
+	char *s = "-\\|/";
+	while (1) {
+		for (volatile unsigned long j = 0; j < 100000; j++);
+		sbi_printf("\r%c", s[i++]);
+		if (i >= 4)
+			i = 0;
+	}
+	return 0;
+}
+
+static void supervisor_trap_entry(void)
+{
+    sbi_printf("\nSupervisor Trap Entry Reached!\n");
+}
+
+static void do_plic_test(void)
+{
+	sbi_printf("--- TESTING PLIC ---\n");
+
+	volatile unsigned long *mtime    = (unsigned long *)(AUX_TIMER_BASE + MTIME_OFFSET);
+	volatile unsigned long *mtimecmp = (unsigned long *)(AUX_TIMER_BASE + MTIMECMP_OFFSET);
+
+	/* Disable auxiliar timer interrupt */
+	*mtimecmp = 0xffffffffUL;
+	sbi_printf("Timer interrupt disabled\n");
+
+
+	/* Enable supervisor interrupt delegation */
+	csr_set(CSR_SIE, SIE_SEIE); // Enable supervisor external interrupts
+	csr_set(CSR_SSTATUS, SSTATUS_SIE); // Enable global interrupts in supervisor mode
+	csr_set(CSR_MIDELEG, MIDELEG_SEIE); // Delegate machine interrupts to supervisor mode
+	csr_set(CSR_STVEC, &supervisor_trap_entry);
+	sbi_printf("Enabled supervisor delegation:\n");
+
+	dumpregs(1);
+
+	/* Configure PLIC aux timer input  */
+	volatile unsigned *plic_priority  = (unsigned *)(PLIC_BASE + PLIC_PRIORITY_OFFSET + PLIC_TIMER_PORT * 4);
+	volatile unsigned *plic_enable    = (unsigned *)(PLIC_BASE + PLIC_ENABLE_OFFSET);
+	volatile unsigned *plic_threshold = (unsigned *)(PLIC_BASE + PLIC_THRESHOLD_OFFSET);
+	volatile unsigned *plic_claim     = (unsigned *)(PLIC_BASE + PLIC_CLAIM_OFFSET);
+	volatile unsigned *plic_pending   = (unsigned *)(PLIC_BASE + PLIC_PENDING_OFFSET);
+
+	sbi_printf("Enabling timer in PLIC\n");
+	*plic_priority = PLIC_TIMER_PORT;
+	*plic_threshold = PLIC_TIMER_PORT - 1;
+	*plic_enable |= (1 << PLIC_TIMER_PORT);
+
+	/* Clear interrupt */
+	sbi_printf("Pending: %d\n", *plic_pending);
+	unsigned claim = *plic_claim;
+	sbi_printf("Claim: %d\n", claim);
+	*plic_claim = claim;
+	sbi_printf("Pending: %d\n", *plic_pending);
+
+	sbi_printf("Clearing MIP\n");
+	csr_write(CSR_MIP, 0);
+
+	/* Enable external timer interrupts */
+	//sbi_printf("Enabling MEIE in MIE register\n");
+	//csr_set(CSR_MIE, MIE_MEIE); /* Needed? */
+	//sbi_printf("Enabling MIE in MSTATUS register\n");
+	//csr_set(CSR_MSTATUS, MSTATUS_MIE); /* Needed? */
+
+	/* Enable timer interrupt */
+	*mtimecmp = *mtime + 10000;
+
+	sbi_printf("Timer alarm programmed\n");
+	sbi_printf("Switching to supervisor\n");
+
+	dumpregs(1);
+
+	switch_to_supervisor_mode(&supervisor_mode_code);
+
+	/* Never reached */
+	while (1);
+}
+
+