dev: Modify LupIO-TMR for SMP support

Added a new LupioTimer struct, as well as a timer event function for
SMP support.

Change-Id: Idbcc549dfa3c5f8d5342d7e2250337a7482a1ac0
Reviewed-on: https://gem5-review.googlesource.com/c/public/gem5/+/53039
Maintainer: Bobby Bruce <bbruce@ucdavis.edu>
Tested-by: kokoro <noreply+kokoro@google.com>
Reviewed-by: Jason Lowe-Power <power.jg@gmail.com>
diff --git a/src/dev/lupio/lupio_pic.cc b/src/dev/lupio/lupio_pic.cc
index 010a5d8..d83ffcf 100644
--- a/src/dev/lupio/lupio_pic.cc
+++ b/src/dev/lupio/lupio_pic.cc
@@ -34,8 +34,6 @@
 #include "params/LupioPIC.hh"
 #include "sim/system.hh"
 
-#define LUPIO_PIC_NSRC      32
-
 namespace gem5
 {
 
@@ -44,23 +42,26 @@
     system(params.system),
     nSrc(params.n_src),
     nThread(params.num_threads),
-    intType(params.int_type)
+    intType(params.int_type),
+    mask{0},
+    enable{0}
 {
+    // CPU0 receives all IRQ sources by default
+    enable[0] = 0xFFFFFFFF;
     DPRINTF(LupioPIC, "LupioPIC initalized\n");
 }
 
 void
 LupioPIC::lupioPicUpdateIRQ()
 {
-    if (nThread > 1 ) {
-        panic("This device currently does not have SMP support\n");
-    }
+    for (int cpu = 0; cpu < nThread; cpu++) {
+        auto tc = system->threads[cpu];
 
-    auto tc = system->threads[0];
-    if (pending & mask) {
-        tc->getCpuPtr()->postInterrupt(tc->threadId(), intType, 0);
-    } else {
-        tc->getCpuPtr()->clearInterrupt(tc->threadId(), intType, 0);
+        if (enable[cpu] & mask[cpu] & pending) {
+            tc->getCpuPtr()->postInterrupt(tc->threadId(), intType, 0);
+        } else {
+            tc->getCpuPtr()->clearInterrupt(tc->threadId(), intType, 0);
+        }
     }
 }
 
@@ -89,20 +90,26 @@
 {
     uint32_t r = 0;
 
-    switch (addr >> 2) {
+    int cpu = addr >> LUPIO_PIC_MAX;
+    int reg = (addr >> 2) & (LUPIO_PIC_MAX - 1);
+
+    switch (reg) {
         case LUPIO_PIC_PRIO:
             // Value will be 32 if there is no unmasked pending IRQ
-            r = ctz32(pending & mask);
+            r = ctz32(pending & mask[cpu] & enable[cpu]);
             DPRINTF(LupioPIC, "Read PIC_PRIO: %d\n", r);
             break;
         case LUPIO_PIC_MASK:
-            r = mask;
+            r = mask[cpu];
             DPRINTF(LupioPIC, "Read PIC_MASK: %d\n", r);
             break;
         case LUPIO_PIC_PEND:
-			r = pending;
+            r = (enable[cpu] & pending);
             DPRINTF(LupioPIC, "Read PIC_PEND: %d\n", r);
             break;
+         case LUPIO_PIC_ENAB:
+            r = enable[cpu];
+            break;
 
         default:
             panic("Unexpected read to the LupioPIC device at address %#llx!",
@@ -117,10 +124,18 @@
 {
     uint32_t val = val64;
 
-    switch (addr >> 2) {
+    int cpu = addr >> LUPIO_PIC_MAX;
+    int reg = (addr >> 2) & (LUPIO_PIC_MAX - 1);
+
+    switch (reg) {
         case LUPIO_PIC_MASK:
-            mask = val;
-            DPRINTF(LupioPIC, "Write PIC_MASK: %d\n", mask);
+            mask[cpu] = val;
+            DPRINTF(LupioPIC, "Write PIC_MASK: %d\n", mask[cpu]);
+            lupioPicUpdateIRQ();
+            break;
+        case LUPIO_PIC_ENAB:
+            enable[cpu] = val;
+            DPRINTF(LupioPIC, "Write PIC_ENAB: %d\n", enable[cpu]);
             lupioPicUpdateIRQ();
             break;
 
diff --git a/src/dev/lupio/lupio_pic.hh b/src/dev/lupio/lupio_pic.hh
index 2ed1d17..ce4815c 100644
--- a/src/dev/lupio/lupio_pic.hh
+++ b/src/dev/lupio/lupio_pic.hh
@@ -35,6 +35,8 @@
 #include "params/LupioPIC.hh"
 #include "sim/system.hh"
 
+#define LUPIO_PIC_NSRC 32
+
 namespace gem5
 {
 
@@ -62,13 +64,18 @@
         LUPIO_PIC_PRIO,
         LUPIO_PIC_MASK,
         LUPIO_PIC_PEND,
+        LUPIO_PIC_ENAB,
 
         // Max offset
         LUPIO_PIC_MAX,
     };
 
     uint32_t pending = 0;
-    uint32_t mask = 0;
+    // Register for masking or unmasking up to 32 sources
+    uint32_t mask[LUPIO_PIC_NSRC];
+    // Regitser to determine which input IRQ is routed to the
+    // corresponding processor
+    uint32_t enable[LUPIO_PIC_NSRC];
 
   protected:
     /**
diff --git a/src/dev/lupio/lupio_tmr.cc b/src/dev/lupio/lupio_tmr.cc
index b440015..63c0021 100644
--- a/src/dev/lupio/lupio_tmr.cc
+++ b/src/dev/lupio/lupio_tmr.cc
@@ -34,11 +34,11 @@
 #include "params/LupioTMR.hh"
 
 // Specific fields for CTRL
-#define LUPIO_TMR_IE    0x1
-#define LUPIO_TMR_PD    0x2
+#define LUPIO_TMR_IRQE    0x1
+#define LUPIO_TMR_PRDC    0x2
 
 // Specific fields for STAT
-#define LUPIO_TMR_EX    0x1
+#define LUPIO_TMR_EXPD 0x1
 
 namespace gem5
 {
@@ -47,26 +47,37 @@
     BasicPioDevice(params, params.pio_size),
     system(params.system),
     nThread(params.num_threads),
-    tmrEvent([this]{ lupioTMRCallback(); }, name()),
     intType(params.int_type)
 {
+    timers.resize(nThread);
+
+    for (int cpu = 0; cpu < nThread; cpu++) {
+        timers[cpu].tmrEvent = new EventFunctionWrapper(
+            [=]{
+                lupioTMRCallback(cpu);
+            }, name()+"done"
+        );
+    }
+
     DPRINTF(LupioTMR, "LupioTMR initalized\n");
 }
 
-void
-LupioTMR::updateIRQ(int level)
+LupioTMR::~LupioTMR()
 {
-    if (nThread > 1) {
-        panic("This device currently does not offer SMP support\n");
+    for (int cpu = 0; cpu < nThread; cpu++) {
+        delete timers[cpu].tmrEvent;
     }
+}
 
-    auto tc = system->threads[0];
+void
+LupioTMR::updateIRQ(int level, int cpu)
+{
+    auto tc = system->threads[cpu];
     // post an interrupt
     if (level) {
         tc->getCpuPtr()->postInterrupt(tc->threadId(), intType, 0);
-    }
-    // clear the interrupt
-    else {
+    } else {
+        // clear the interrupt
         tc->getCpuPtr()->clearInterrupt(tc->threadId(), intType, 0);
     }
 }
@@ -78,52 +89,61 @@
 }
 
 void
-LupioTMR::lupioTMRSet()
+LupioTMR::lupioTMRSet(int cpu)
 {
-    startTime = curTick();
-    if (!tmrEvent.scheduled()) {
-        schedule(tmrEvent, (reload * sim_clock::as_int::ns) + curTick());
+    // Start the timer
+    timers[cpu].startTime = curTick();
+
+    // Schedule the timer to fire at the number of ticks stored
+    // in the reload register from the current tick
+    if (!timers[cpu].tmrEvent->scheduled()) {
+        // Convert the reload value to ticks from nanoseconds
+        schedule(*(timers[cpu].tmrEvent),
+                (timers[cpu].reload * sim_clock::as_int::ns) + curTick());
     }
 }
 
 void
-LupioTMR::lupioTMRCallback()
+LupioTMR::lupioTMRCallback(int cpu)
 {
     // Signal expiration
-    expired = true;
-    if (ie) {
-        updateIRQ(1);
+    timers[cpu].expired = true;
+    if (timers[cpu].ie) {
+        updateIRQ(1, cpu);
     }
 
     // If periodic timer, reload
-    if (pd && reload) {
-        lupioTMRSet();
+    if (timers[cpu].pd && timers[cpu].reload) {
+        lupioTMRSet(cpu);
     }
 }
 
 uint64_t
 LupioTMR::lupioTMRRead(uint8_t addr, int size)
 {
-    uint64_t r = 0;
+    uint32_t r = 0;
 
-    switch (addr >> 2) {
+    size_t cpu = addr >> LUPIO_TMR_MAX;
+    size_t reg = (addr >> 2) & (LUPIO_TMR_MAX - 1);
+
+    switch (reg) {
         case LUPIO_TMR_TIME:
             r = lupioTMRCurrentTime();
             DPRINTF(LupioTMR, "Read LUPIO_TMR_TME: %d\n", r);
             break;
         case LUPIO_TMR_LOAD:
-            r = reload;
+            r = timers[cpu].reload;
             DPRINTF(LupioTMR, "Read LUPIO_TMR_LOAD: %d\n", r);
             break;
         case LUPIO_TMR_STAT:
-            if (expired) {
-                r |= LUPIO_TMR_EX;
+            if (timers[cpu].expired) {
+                r |= LUPIO_TMR_EXPD;
             }
 
             // Acknowledge expiration
-            expired = false;
+            timers[cpu].expired = false;
             DPRINTF(LupioTMR, "Read LUPIO_TMR_STAT: %d\n", r);
-            updateIRQ(0);
+            updateIRQ(0, cpu);
             break;
 
         default:
@@ -139,26 +159,31 @@
 {
     uint32_t val = val64;
 
-    switch (addr >> 2) {
+    size_t cpu = addr >> LUPIO_TMR_MAX;
+    size_t reg = (addr >> 2) & (LUPIO_TMR_MAX - 1);
+
+    switch (reg) {
         case LUPIO_TMR_LOAD:
-            reload = val;
-            DPRINTF(LupioTMR, "Write LUPIO_TMR_LOAD: %d\n", reload);
+            timers[cpu].reload = val;
+            DPRINTF(LupioTMR, "Write LUPIO_TMR_LOAD: %d\n",
+                    timers[cpu].reload);
             break;
 
         case LUPIO_TMR_CTRL:
-                    ie = val & LUPIO_TMR_IE;
-                    pd = val & LUPIO_TMR_PD;
+            timers[cpu].ie = val & LUPIO_TMR_IRQE;
+            timers[cpu].pd = val & LUPIO_TMR_PRDC;
             DPRINTF(LupioTMR, "Write LUPIO_TMR_CTRL\n");
 
             // Stop current timer if any
-            if (curTick() < startTime + (reload * sim_clock::as_int::ns)
-                && tmrEvent.scheduled()) {
-                deschedule(tmrEvent);
+            if (curTick() < timers[cpu].startTime +
+                (timers[cpu].reload * sim_clock::as_int::ns) &&
+                (timers[cpu].tmrEvent)->scheduled()) {
+                deschedule(*(timers[cpu].tmrEvent));
             }
 
             // If reload isn't 0, start a new one
-            if (reload) {
-                lupioTMRSet();
+            if (timers[cpu].reload) {
+                lupioTMRSet(cpu);
             }
             break;
 
diff --git a/src/dev/lupio/lupio_tmr.hh b/src/dev/lupio/lupio_tmr.hh
index 0097f22..feea5ab 100644
--- a/src/dev/lupio/lupio_tmr.hh
+++ b/src/dev/lupio/lupio_tmr.hh
@@ -49,10 +49,8 @@
     const ByteOrder byteOrder = ByteOrder::little;
     System *system;
     int nThread;
-    EventFunctionWrapper tmrEvent;
     int intType;
-
-    Tick startTime = 0;
+    int nCPUs = 0;
 
     // Register map
     enum
@@ -66,15 +64,17 @@
         LUPIO_TMR_MAX,
     };
 
-    // Timer registers
-    uint64_t reload = 0;
+    struct LupioTimer
+    {
+        Event *tmrEvent = nullptr;
+        uint64_t reload = 0;
+        bool ie = false;            // Control
+        bool pd = false;
+        bool expired = false;       // Status
+        Tick startTime = 0;
+    };
 
-    // Control
-    bool ie = false;
-    bool pd = false;
-
-    // Status
-    bool expired = false;
+    std::vector<LupioTimer> timers;
 
     /**
      * Function to return data pertaining to the timer, such as the simulated
@@ -93,20 +93,21 @@
     /**
      * Schedule the next timer event
      */
-    void lupioTMRSet();
+    void lupioTMRSet(int cpu);
     /**
      * Process the timer's event
      */
-    void lupioTMRCallback();
+    void lupioTMRCallback(int cpu);
 
     /**
      * Post or clear timer interrupts
      */
-    void updateIRQ(int level);
+    void updateIRQ(int level, int cpu);
 
   public:
     PARAMS(LupioTMR);
     LupioTMR(const Params &params);
+    ~LupioTMR();
 
     /**
      * Implement BasicPioDevice virtual functions