Merge pull request #330 from sifive/l2pf-trap-fix

Handle trap if L2PF does not exist for a hart id.
sifive · Aug 28, 2020 · d47cd29 · d47cd29
2 parents f4818b9 + 4671012
commit d47cd29
Showing 1 changed file with 31 additions and 0 deletions.
diff --git a/src/drivers/sifive_l2pf0.c b/src/drivers/sifive_l2pf0.c
@@ -32,35 +32,61 @@
 #define REG_BITSHIFT_21 21
 #define REG_BITSHIFT_28 28
 
+/* Macros to capture trap, if L2PF does not exist for a hart id. */
+#define SIFIVE_L2PF0_TRAP_CAPTURE(exit, mtvec)                                 \
+    __asm__ __volatile__("la %0, 1f \n\t"                                      \
+                         "csrr %1, mtvec \n\t"                                 \
+                         "csrw mtvec, %0 \n\t"                                 \
+                         : "+r"(exit), "+r"(mtvec))
+
+#define SIFIVE_L2PF0_TRAP_RESTORE(mtvec)                                       \
+    __asm__ __volatile__(".align 2 \n\t"                                       \
+                         "1: \n\t"                                             \
+                         "csrw mtvec, %0 \n\t"                                 \
+                         : "+r"(mtvec))
+
 void sifive_l2pf0_enable(void) {
+    volatile uintptr_t exit = 0, mtvec = 0;
     int hartid;
     __asm__ volatile("csrr %0, mhartid" : "=r"(hartid));
 
+    SIFIVE_L2PF0_TRAP_CAPTURE(exit, mtvec);
+
     uint32_t val = REGW(METAL_SIFIVE_L2PF0_BASIC_CONTROL);
 
     /* Enable L2 prefetch unit for current hart */
     val |= REG_MASK_BITWIDTH1;
 
     REGW(METAL_SIFIVE_L2PF0_BASIC_CONTROL) = val;
+
+    SIFIVE_L2PF0_TRAP_RESTORE(mtvec);
 }
 
 void sifive_l2pf0_disable(void) {
+    volatile uintptr_t exit = 0, mtvec = 0;
     int hartid;
     __asm__ volatile("csrr %0, mhartid" : "=r"(hartid));
 
+    SIFIVE_L2PF0_TRAP_CAPTURE(exit, mtvec);
+
     uint32_t val = REGW(METAL_SIFIVE_L2PF0_BASIC_CONTROL);
 
     /* Disable L2 prefetch unit for current hart */
     val &= ~REG_MASK_BITWIDTH1;
 
     REGW(METAL_SIFIVE_L2PF0_BASIC_CONTROL) = val;
+
+    SIFIVE_L2PF0_TRAP_RESTORE(mtvec);
 }
 
 void sifive_l2pf0_get_config(sifive_l2pf0_config *config) {
+    volatile uintptr_t exit = 0, mtvec = 0;
     int hartid;
     __asm__ volatile("csrr %0, mhartid" : "=r"(hartid));
     uint32_t val;
 
+    SIFIVE_L2PF0_TRAP_CAPTURE(exit, mtvec);
+
     if (config) /* Check for NULL */
     {
         /* Get currently active L2 prefetch configuration values */
@@ -89,13 +115,17 @@ void sifive_l2pf0_get_config(sifive_l2pf0_config *config) {
             ((val >> REG_BITSHIFT_9) & REG_MASK_BITWIDTH4);
         config->Window = ((val >> REG_BITSHIFT_13) & REG_MASK_BITWIDTH6);
     }
+    SIFIVE_L2PF0_TRAP_RESTORE(mtvec);
 }
 
 void sifive_l2pf0_set_config(sifive_l2pf0_config *config) {
+    volatile uintptr_t exit = 0, mtvec = 0;
     int hartid;
     __asm__ volatile("csrr %0, mhartid" : "=r"(hartid));
     uint32_t val;
 
+    SIFIVE_L2PF0_TRAP_CAPTURE(exit, mtvec);
+
     if (config) /* Check for NULL */
     {
         /* Get values from configuration to write into register */
@@ -126,6 +156,7 @@ void sifive_l2pf0_set_config(sifive_l2pf0_config *config) {
 
         REGW(METAL_SIFIVE_L2PF0_USER_CONTROL) = val;
     }
+    SIFIVE_L2PF0_TRAP_RESTORE(mtvec);
 }
 
 #endif