From c35340ecce38ab2af0cc4444ee920251129f941e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20L=C3=B6ffler?= <me@fatalerrorcoded.eu>
Date: Wed, 25 Jan 2023 23:26:16 +0100
Subject: [PATCH] register pair manipulation functions

---
 src/emulator/instructions/arithmetic.rs | 15 ++-----
 src/emulator/structs.rs                 | 55 +++++++++++++++++++++++++
 2 files changed, 59 insertions(+), 11 deletions(-)

diff --git a/src/emulator/instructions/arithmetic.rs b/src/emulator/instructions/arithmetic.rs
index a310d9c..517c3c3 100644
--- a/src/emulator/instructions/arithmetic.rs
+++ b/src/emulator/instructions/arithmetic.rs
@@ -1,4 +1,5 @@
-use crate::{get_register, structs::set_register, EmulatorState, Register};
+use crate::structs::{get_register_pair, set_register, set_register_pair};
+use crate::{get_register, EmulatorState, Register};
 
 /// Sets the condition code flags according to `result`. `flags` parameter
 /// indicates which flags will be set, 0b1111 will set all (Z, S, C, P)
@@ -48,18 +49,10 @@ pub fn aci(byte: u8, state: &mut EmulatorState) {
 
 /// Double precision add - Add B&C, D&E or H&L to H&L
 pub fn dad(register: Register, state: &mut EmulatorState) {
-    let num = match register {
-        Register::B => u16::from_le_bytes([state.c, state.b]),
-        Register::D => u16::from_le_bytes([state.e, state.d]),
-        Register::H => u16::from_le_bytes([state.l, state.h]),
-        Register::SP => state.sp,
-        _ => panic!("Cannot perform DAD on register {:?}", register),
-    };
-
+    let num = get_register_pair(register, state);
     let (result, overflow) = num.overflowing_add(u16::from_le_bytes([state.l, state.h]));
     state.cc.c = overflow;
-    state.h = (result >> 8) as u8;
-    state.l = result as u8;
+    set_register_pair(register, result, state);
 }
 
 /// Increase register
diff --git a/src/emulator/structs.rs b/src/emulator/structs.rs
index d5858c6..2a4171f 100644
--- a/src/emulator/structs.rs
+++ b/src/emulator/structs.rs
@@ -93,6 +93,61 @@ pub fn set_register(register: Register, value: u8, state: &mut EmulatorState) {
     };
 }
 
+pub fn get_register_pair(register: Register, state: &mut EmulatorState) -> u16 {
+    match register {
+        Register::B => u16::from_le_bytes([state.c, state.b]),
+        Register::D => u16::from_le_bytes([state.e, state.d]),
+        Register::H => u16::from_le_bytes([state.l, state.h]),
+        Register::A => {
+            // the PSW looks like this: SZ0A0P1C
+            let flags: u8 = u8::from(state.cc.s) << 7 // bit 7
+                | u8::from(state.cc.z) << 6 // bit 6
+                //| u8::from(state.cc.a) << 4 // bit 4
+                | u8::from(state.cc.p) << 2 // bit 2
+                | 0x02 // bit 1
+                | u8::from(state.cc.c); // bit 0
+
+            u16::from_le_bytes([flags, state.a])
+        }
+        Register::SP => state.sp,
+        _ => unreachable!(),
+    }
+}
+
+pub fn set_register_pair(register: Register, value: u16, state: &mut EmulatorState) {
+    let arr = value.to_le_bytes();
+    let high = arr[1];
+    let low = arr[0];
+    match register {
+        Register::B => {
+            state.b = high;
+            state.c = low;
+        }
+        Register::D => {
+            state.d = high;
+            state.e = low;
+        }
+        Register::H => {
+            state.h = high;
+            state.l = low;
+        }
+        Register::A => {
+            state.a = high;
+            // the PSW looks like this: SZ0A0P1C
+            state.cc.s = low & 0b1000_0000 > 0;
+            state.cc.z = low & 0b0100_0000 > 0;
+            debug_assert!(low & 0b0010_0000 == 0, "malformed PSW");
+            //state.cc.a = low & 0b0001_0000 > 0;
+            debug_assert!(low & 0b0000_1000 == 0, "malformed PSW");
+            state.cc.p = low & 0b0000_0100 > 0;
+            debug_assert!(low & 0b0000_0010 > 0, "malformed PSW");
+            state.cc.c = low & 0b0000_0001 > 0;
+        }
+        Register::SP => state.sp = value,
+        _ => unreachable!(),
+    }
+}
+
 /// Print values of registers and flags to stdout
 pub fn print_state(state: &EmulatorState) {
     // State