Main: refactor fetch_and_execute

No functional change

1 files changed, 128 insertions, 115 deletions

diff --git a/aarch64/elfmain.sail b/aarch64/elfmain.sail
index 5fe95705..aaa76d21 100644
--- a/aarch64/elfmain.sail
+++ b/aarch64/elfmain.sail
@@ -13,66 +13,132 @@ let CNT_IRQ  = [0x0000_000d,       0x0000_000a,        0x0000_03ff,        0x000
 // SGI Interrupts are 0-15, PPI interrupts are 16-31, so SPI interrupts have an offset of 32.
 let SPI_OFFSET = 32
 
+register cycle_counter : int
+
+// Advance CPU by one cycle
+val Step_CPU : unit -> unit effect {escape, undef, wreg, rreg, rmem, wmem}
+
+function Step_CPU() = {
+  // Check for pending interrupts
+  var interrupt_req : InterruptReq = undefined;
+  interrupt_req.take_SE  = true; interrupt_req.take_vSE  = true;
+  interrupt_req.take_IRQ = true; interrupt_req.take_vIRQ = true;
+  interrupt_req.take_FIQ = true; interrupt_req.take_vFIQ = true;
+  try {
+      var interrupted = false;
+      interrupted = TakePendingInterrupts(interrupt_req);
+      if interrupted then {
+          print("Pending interrupt taken\n");
+      }
+  } catch {
+      _ => {
+        print("Unhandled exception while pending exceptions\n");
+      }
+  };
+
+  var fetch_ok = false;
+  try {
+      __currentInstr       = __fetchA64();
+      __currentInstrLength = 4;
+      fetch_ok = true;
+  } catch {
+      Error_ExceptionTaken() => {
+        print(concat_str("Exception taken during IFetch from PC=", concat_str(HexStr(UInt(aget_PC())), "\n")));
+      },
+      _ => {
+        print("Exiting due to unhandled exception during fetch\n");
+        exit()
+      }
+  };
+
+  if fetch_ok then {
+      try {
+          __PC_changed = false;
+          ShouldAdvanceIT = (PSTATE.nRW == [bitone]) & (PSTATE.T == [bitone]);
+          decode(__currentInstr);
+      } catch {
+          // note: if supporting AArch32 as well, call _UndefinedFault() instead
+          Error_Undefined()         => try { AArch64_UndefinedFault() } catch { _ => print("Exception during Undefined recovery\n") },
+          Error_See(_)              => try { AArch64_UndefinedFault() } catch { _ => print("Exception during SEE recovery\n") },
+          Error_ReservedEncoding(_) => try { AArch64_UndefinedFault() } catch { _ => print("Exception during ReservedEncoding recovery\n") },
+          Error_ExceptionTaken() => {
+            print(concat_str("ExceptionTaken during Decode/Execute from PC=", concat_str(HexStr(UInt(aget_PC())), concat_str(" opcode=", concat_str(HexStr(UInt(__currentInstr)), "\n")))));
+            // print("  This might just be a HINT like 0xd50320df\n");
+            ()
+          },
+          Error_Implementation_Defined(s) => {
+            print(concat_str("Ignoring IMPLEMENTATION_DEFINED", concat_str(s, "\n")));
+          }
+      }
+  };
+  if ~(__PC_changed) then _PC = _PC + __currentInstrLength else ();
+  if ShouldAdvanceIT then AArch32_ITAdvance() else ();
+  SSAdvance();
+  __UpdateSystemCounter(); // should this happen even if sleeping?
+}
+
+// Advance Timers by one cycle
+val Step_Timers : unit -> unit effect {escape, undef, wreg, rreg, rmem, wmem}
+
+function Step_Timers() = {
+  // Increment the counter
+  CNTPCT_EL0 = CNTPCT_EL0 + 1;
+  CNTVCT_EL0 = CNTPCT_EL0 - CNTVOFF_EL2;
+
+  // Timer value view for each timer
+  foreach (timer from 0 to 4) {
+    let TVAL = CNT_TVAL[timer];
+    let nextTVAL = read_register(TVAL) - 1;
+    (*TVAL) = nextTVAL;
+
+    if nextTVAL == 0x0000_0000 then {
+      _IRQPending = true;
+      AArch64_TakePhysicalIRQException()
+    }
+  };
+
+  // Handle the Kernel event stream, if enabled by CNTKCTL_EL1[2].
+  if CNTKCTL_EL1[2] == bitone then {
+    let mask = LSL(ZeroExtend(0x1, 64), UInt(CNTKCTL_EL1[7 .. 4]));
+
+    // Trigger event stream on either rising bit (0-1, bitzero) or falling (1-0, bitone)
+    let trigger : bool = if CNTKCTL_EL1[3] == bitzero then {
+      ((CNTVCT_EL0 - 1) & mask) == Zeros() & (CNTVCT_EL0 & mask) != Zeros()
+    } else {
+      ((CNTVCT_EL0 - 1) & mask) != Zeros() & (CNTVCT_EL0 & mask) == Zeros()
+    };
+
+    if trigger & PSTATE.I == bitzero then {
+      _IRQPending = true;
+      AArch64_TakePhysicalIRQException()
+    }
+  };
+
+  // Handle the Hypervisor event stream, if enabled by CNTHCTL_EL2[2].
+  if CNTHCTL_EL2[2] == bitone then {
+    let mask = LSL(ZeroExtend(0x1, 64), UInt(CNTHCTL_EL2[7 .. 4]));
+
+    // Trigger event stream on either rising bit (0-1, bitzero) or falling (1-0, bitone)
+    let trigger : bool = if CNTHCTL_EL2[3] == bitzero then {
+      ((CNTPCT_EL0 - 1) & mask) == Zeros() & (CNTPCT_EL0 & mask) != Zeros()
+    } else {
+      ((CNTPCT_EL0 - 1) & mask) != Zeros() & (CNTPCT_EL0 & mask) == Zeros()
+    };
+
+    if trigger & PSTATE.I == bitzero then {
+      prerr("[Clock] Tick\n");
+      _IRQPending = true;
+      AArch64_TakePhysicalIRQException()
+    }
+  }
+}
+
 // Simple top level fetch and execute loop.
 val fetch_and_execute : unit -> unit effect {escape, undef, wreg, rreg, rmem, wmem}
 
-register cycle_counter : int
-
 function fetch_and_execute () = {
-  cycle_counter = 0;
-  while true do {
-    cycle_counter = cycle_counter + 1;
     try {
-      // Increment the counter
-      CNTPCT_EL0 = CNTPCT_EL0 + 1;
-      CNTVCT_EL0 = CNTPCT_EL0 - CNTVOFF_EL2;
-
-      // Timer value view for each timer
-      foreach (timer from 0 to 4) {
-        let TVAL = CNT_TVAL[timer];
-        let nextTVAL = read_register(TVAL) - 1;
-        (*TVAL) = nextTVAL;
-
-        if nextTVAL == 0x0000_0000 then {
-	  _IRQPending = true;
-          AArch64_TakePhysicalIRQException()
-        }
-      };
-
-      // Handle the Kernel event stream, if enabled by CNTKCTL_EL1[2].
-      if CNTKCTL_EL1[2] == bitone then {
-        let mask = LSL(ZeroExtend(0x1, 64), UInt(CNTKCTL_EL1[7 .. 4]));
-
-        // Trigger event stream on either rising bit (0-1, bitzero) or falling (1-0, bitone)
-        let trigger : bool = if CNTKCTL_EL1[3] == bitzero then {
-          ((CNTVCT_EL0 - 1) & mask) == Zeros() & (CNTVCT_EL0 & mask) != Zeros()
-        } else {
-          ((CNTVCT_EL0 - 1) & mask) != Zeros() & (CNTVCT_EL0 & mask) == Zeros()
-        };
-
-        if trigger & PSTATE.I == bitzero then {
-	  _IRQPending = true;
-          AArch64_TakePhysicalIRQException()
-        }
-      };
-
-      // Handle the Hypervisor event stream, if enabled by CNTHCTL_EL2[2].
-      if CNTHCTL_EL2[2] == bitone then {
-	let mask = LSL(ZeroExtend(0x1, 64), UInt(CNTHCTL_EL2[7 .. 4]));
-
-        // Trigger event stream on either rising bit (0-1, bitzero) or falling (1-0, bitone)
-        let trigger : bool = if CNTHCTL_EL2[3] == bitzero then {
-          ((CNTPCT_EL0 - 1) & mask) == Zeros() & (CNTPCT_EL0 & mask) != Zeros()
-        } else {
-          ((CNTPCT_EL0 - 1) & mask) != Zeros() & (CNTPCT_EL0 & mask) == Zeros()
-        };
-
-	if trigger & PSTATE.I == bitzero then {
-	  prerr("[Clock] Tick\n");
-	  _IRQPending = true;
-          AArch64_TakePhysicalIRQException()
-        }
-      };
+      Step_Timers();
 
       var prevEL = PSTATE.EL;
       var prevI = PSTATE.I;
@@ -83,63 +149,7 @@ function fetch_and_execute () = {
       var prevCNTHCTL_EL2 = CNTHCTL_EL2;
 
       if ~(__Sleeping()) then {
-
-          // Check for pending interrupts
-          var interrupt_req : InterruptReq = undefined;
-          interrupt_req.take_SE  = true; interrupt_req.take_vSE  = true;
-          interrupt_req.take_IRQ = true; interrupt_req.take_vIRQ = true;
-          interrupt_req.take_FIQ = true; interrupt_req.take_vFIQ = true;
-          var interrupted = false;
-          try {
-              interrupted = TakePendingInterrupts(interrupt_req);
-              if interrupted then {
-                  print("Pending interrupt taken\n");
-              }
-          } catch {
-              _ => {
-                print("Unhandled exception while pending exceptions\n");
-              }
-          };
-          var ok = true;
-          if ok then {
-              try {
-                  __currentInstr       = __fetchA64();
-                  __currentInstrLength = 4;
-                  ok = true;
-              } catch {
-                  Error_ExceptionTaken() => {
-                    print(concat_str("Exception taken during IFetch from PC=", concat_str(HexStr(UInt(aget_PC())), "\n")));
-                  },
-                  _ => {
-                    print("Exiting due to unhandled exception during fetch\n");
-                    exit()
-                  }
-              };
-              if ok then {
-                  try {
-                      __PC_changed = false;
-                      ShouldAdvanceIT = (PSTATE.nRW == [bitone]) & (PSTATE.T == [bitone]);
-                      decode(__currentInstr);
-                  } catch {
-                      // note: if supporting AArch32 as well, call _UndefinedFault() instead
-                      Error_Undefined()         => try { AArch64_UndefinedFault() } catch { _ => print("Exception during Undefined recovery\n") },
-                      Error_See(_)              => try { AArch64_UndefinedFault() } catch { _ => print("Exception during SEE recovery\n") },
-                      Error_ReservedEncoding(_) => try { AArch64_UndefinedFault() } catch { _ => print("Exception during ReservedEncoding recovery\n") },
-                      Error_ExceptionTaken() => {
-                        print(concat_str("ExceptionTaken during Decode/Execute from PC=", concat_str(HexStr(UInt(aget_PC())), concat_str(" opcode=", concat_str(HexStr(UInt(__currentInstr)), "\n")))));
-                        // print("  This might just be a HINT like 0xd50320df\n");
-                        ()
-                      },
-                      Error_Implementation_Defined(s) => {
-                        print(concat_str("Ignoring IMPLEMENTATION_DEFINED", concat_str(s, "\n")));
-                      }
-                  }
-              };
-              if ~(__PC_changed) then _PC = _PC + __currentInstrLength else ();
-              if ShouldAdvanceIT then AArch32_ITAdvance() else ();
-              SSAdvance();
-              __UpdateSystemCounter(); // should this happen even if sleeping?
-          }
+          Step_CPU();
       };
 
       // We want to keep track of what exception level we are in for debugging purposes.
@@ -168,8 +178,8 @@ function fetch_and_execute () = {
         exit()
       }
     };
-    __EndCycle() // advance state of non-sleeping parts of the system
-  }
+    __EndCycle(); // advance state of non-sleeping parts of the system
+    cycle_counter = cycle_counter + 1;
 }
 
 let COLD_RESET : bool = true
@@ -188,5 +198,8 @@ val main : unit -> unit effect {escape, undef, wreg, rreg, rmem, wmem}
 
 function main() = {
   init();
-  fetch_and_execute()
+  cycle_counter = 0;
+  while true do {
+    fetch_and_execute()
+  }
 }