From 474f9da3c25da6b85cc6f242efe2aad37afd3640 Mon Sep 17 00:00:00 2001
From: Thomas Harte <thomas.harte@gmail.com>
Date: Wed, 28 Feb 2024 14:09:05 -0500
Subject: [PATCH] Add banked registers.

---
 InstructionSets/ARM/Registers.hpp | 59 +++++++++++++++++++++++++++++--
 1 file changed, 57 insertions(+), 2 deletions(-)

diff --git a/InstructionSets/ARM/Registers.hpp b/InstructionSets/ARM/Registers.hpp
index 828a1b272..e5ae541a7 100644
--- a/InstructionSets/ARM/Registers.hpp
+++ b/InstructionSets/ARM/Registers.hpp
@@ -10,6 +10,7 @@
 
 #include "OperationMapper.hpp"
 
+#include <array>
 #include <cstdint>
 
 namespace InstructionSet::ARM {
@@ -90,7 +91,7 @@ struct Registers {
 			zero_result_ = ~status & ConditionCode::Zero;
 
 			if(mode_ != Mode::User) {
-				mode_ = Mode(status & 3);
+				set_mode(Mode(status & 3));
 				interrupt_flags_ = status & (ConditionCode::IRQDisable | ConditionCode::FIQDisable);
 			}
 		}
@@ -150,7 +151,7 @@ struct Registers {
 			}
 		}
 
-		uint32_t active[16];	// TODO: register swaps with mode.
+		std::array<uint32_t, 16> active;
 
 	private:
 		Mode mode_ = Mode::Supervisor;
@@ -160,6 +161,60 @@ struct Registers {
 		uint32_t interrupt_flags_ = 0;
 		uint32_t carry_flag_ = 0;
 		uint32_t overflow_flag_ = 0;
+
+		// Various shadow registers.
+		std::array<uint32_t, 7> user_registers_;
+		std::array<uint32_t, 7> fiq_registers_;
+		std::array<uint32_t, 2> irq_registers_;
+		std::array<uint32_t, 2> supervisor_registers_;
+
+		void set_mode(Mode target_mode) {
+			if(mode_ == target_mode) {
+				return;
+			}
+
+			// For outgoing modes other than FIQ, only save the final two registers for now;
+			// if the incoming mode is FIQ then the other five will be saved in the next switch.
+			switch(mode_) {
+				case Mode::FIQ:
+					std::copy(active.begin() + 8, active.begin() + 15, fiq_registers_.begin());
+				break;
+				case Mode::User:
+					std::copy(active.begin() + 13, active.begin() + 15, user_registers_.begin() + 5);
+				break;
+				case Mode::Supervisor:
+					std::copy(active.begin() + 13, active.begin() + 15, supervisor_registers_.begin());
+				break;
+				case Mode::IRQ:
+					std::copy(active.begin() + 13, active.begin() + 15, irq_registers_.begin());
+				break;
+			}
+
+			// For all modes except FIQ: restore the final two registers to their appropriate values.
+			// For FIQ: save an additional five, then overwrite seven.
+			switch(target_mode) {
+				case Mode::FIQ:
+					std::copy(active.begin() + 8, active.begin() + 13, user_registers_.begin());
+					std::copy(fiq_registers_.begin(), fiq_registers_.end(), active.begin() + 8);
+				break;
+				case Mode::User:
+					std::copy(user_registers_.begin() + 5, user_registers_.end(), active.begin() + 13);
+				break;
+				case Mode::Supervisor:
+					std::copy(supervisor_registers_.begin(), supervisor_registers_.end(), active.begin() + 13);
+				break;
+				case Mode::IRQ:
+					std::copy(irq_registers_.begin(), irq_registers_.end(), active.begin() + 13);
+				break;
+			}
+
+			// If FIQ is outgoing then there's another five registers to restore.
+			if(mode_ == Mode::FIQ) {
+				std::copy(user_registers_.begin(), user_registers_.begin() + 5, active.begin() + 8);
+			}
+
+			mode_ = target_mode;
+	}
 };
 
 }