From 3654db351793fddfcd15a606424f05d4972e5839 Mon Sep 17 00:00:00 2001
From: Jukka Laitinen <jukkax@ssrc.tii.ae>
Date: Wed, 12 May 2021 08:37:31 +0300
Subject: [PATCH] mpfs: Modify IRQ handling to support also HART0 on PF

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>
---
 arch/risc-v/src/mpfs/mpfs_irq.c          | 69 +++++++++++++++++++-----
 arch/risc-v/src/mpfs/mpfs_irq_dispatch.c | 13 ++++-
 2 files changed, 66 insertions(+), 16 deletions(-)

diff --git a/arch/risc-v/src/mpfs/mpfs_irq.c b/arch/risc-v/src/mpfs/mpfs_irq.c
index cde61155b06..653e7260a85 100755
--- a/arch/risc-v/src/mpfs/mpfs_irq.c
+++ b/arch/risc-v/src/mpfs/mpfs_irq.c
@@ -69,12 +69,16 @@ void up_irqinitialize(void)
 
   uint64_t hart_id = READ_CSR(mhartid);
 
-  /* hart0 is E51 we can't run on that (need different irq handling) */
-
-  DEBUGASSERT(hart_id != 0);
-
-  uint32_t *miebase = (uint32_t *)(MPFS_PLIC_H1_MIE0 +
-                                  (hart_id - 1) * MPFS_HART_MIE_OFFSET);
+  uint32_t *miebase;
+  if (hart_id == 0)
+    {
+      miebase = (uint32_t *)MPFS_PLIC_H0_MIE0;
+    }
+  else
+    {
+      miebase = (uint32_t *)(MPFS_PLIC_H1_MIE0 +
+                             (hart_id - 1)  * MPFS_HART_MIE_OFFSET);
+    }
 
   putreg32(0x0, miebase + 0);
   putreg32(0x0, miebase + 1);
@@ -85,8 +89,17 @@ void up_irqinitialize(void)
 
   /* Clear pendings in PLIC (for current hart) */
 
-  uintptr_t claim_address = MPFS_PLIC_H1_MCLAIM +
-                            ((hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET);
+  uintptr_t claim_address;
+  if (hart_id == 0)
+    {
+      claim_address = MPFS_PLIC_H0_MCLAIM;
+    }
+  else
+    {
+      claim_address = MPFS_PLIC_H1_MCLAIM +
+        ((hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET);
+    }
+
   uint32_t val = getreg32(claim_address);
   putreg32(val, claim_address);
 
@@ -108,8 +121,18 @@ void up_irqinitialize(void)
 
   /* Set irq threshold to 0 (permits all global interrupts) */
 
-  uint32_t *threshold_address = (uint32_t *)(MPFS_PLIC_H1_MTHRESHOLD +
-                                ((hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET));
+  uint32_t *threshold_address;
+  if (hart_id == 0)
+    {
+      threshold_address = (uint32_t *)MPFS_PLIC_H0_MTHRESHOLD;
+    }
+  else
+    {
+      threshold_address = (uint32_t *)(MPFS_PLIC_H1_MTHRESHOLD +
+                                       ((hart_id - 1) *
+                                        MPFS_PLIC_NEXTHART_OFFSET));
+    }
+
   putreg32(0, threshold_address);
 
   /* currents_regs is non-NULL only while processing an interrupt */
@@ -164,8 +187,17 @@ void up_disable_irq(int irq)
       /* Clear enable bit for the irq */
 
       uint64_t hart_id = READ_CSR(mhartid);
-      uintptr_t miebase = MPFS_PLIC_H1_MIE0 +
-                          ((hart_id - 1) * MPFS_HART_MIE_OFFSET);
+      uintptr_t miebase;
+
+      if (hart_id == 0)
+        {
+          miebase =  MPFS_PLIC_H0_MIE0;
+        }
+      else
+        {
+          miebase = MPFS_PLIC_H1_MIE0 +
+            ((hart_id - 1) * MPFS_HART_MIE_OFFSET);
+        }
 
       if (0 <= extirq && extirq <= NR_IRQS - MPFS_IRQ_EXT_START)
         {
@@ -210,8 +242,17 @@ void up_enable_irq(int irq)
       /* Set enable bit for the irq */
 
       uint64_t hart_id = READ_CSR(mhartid);
-      uintptr_t miebase = MPFS_PLIC_H1_MIE0 +
-                          ((hart_id - 1) * MPFS_HART_MIE_OFFSET);
+      uintptr_t miebase;
+
+      if (hart_id == 0)
+        {
+          miebase = MPFS_PLIC_H0_MIE0;
+        }
+      else
+        {
+          miebase = MPFS_PLIC_H1_MIE0 +
+            ((hart_id - 1) * MPFS_HART_MIE_OFFSET);
+        }
 
       if (0 <= extirq && extirq <= NR_IRQS - MPFS_IRQ_EXT_START)
         {
diff --git a/arch/risc-v/src/mpfs/mpfs_irq_dispatch.c b/arch/risc-v/src/mpfs/mpfs_irq_dispatch.c
index 2632aa6589c..67bc4d82bf8 100755
--- a/arch/risc-v/src/mpfs/mpfs_irq_dispatch.c
+++ b/arch/risc-v/src/mpfs/mpfs_irq_dispatch.c
@@ -79,8 +79,17 @@ void *mpfs_dispatch_irq(uint64_t vector, uint64_t *regs)
   /* Firstly, check if the irq is machine external interrupt */
 
   uint64_t hart_id = READ_CSR(mhartid);
-  uintptr_t claim_address = MPFS_PLIC_H1_MCLAIM +
-                            ((hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET);
+  uintptr_t claim_address;
+
+  if (hart_id == 0)
+    {
+      claim_address = MPFS_PLIC_H0_MCLAIM;
+    }
+  else
+    {
+      claim_address = MPFS_PLIC_H1_MCLAIM +
+        ((hart_id - 1) * MPFS_PLIC_NEXTHART_OFFSET);
+    }
 
   if (irq == MPFS_IRQ_MEXT)
     {