From d68a5af55e2d19b0c1d780557b27647d1d28028c Mon Sep 17 00:00:00 2001
From: folkert van heusden <mail@vanheusden.com>
Date: Sat, 11 Mar 2023 21:54:18 +0100
Subject: [PATCH] writePhysical

---
 bus.cpp |  6 ++++++
 bus.h   |  2 ++
 cpu.cpp | 59 ++++++++++++++++++++++++++++++++++++++++++++++++++++++---
 cpu.h   |  4 +---
 4 files changed, 65 insertions(+), 6 deletions(-)

diff --git a/bus.cpp b/bus.cpp
index 668c9db..dcf7897 100644
--- a/bus.cpp
+++ b/bus.cpp
@@ -864,6 +864,12 @@ void bus::write(const uint16_t a, const bool word_mode, uint16_t value, const bo
 		m->writeWord(m_offset, value);
 }
 
+void bus::writePhysical(const uint32_t a, const uint16_t value)
+{
+	DOLOG(debug, true, "physicalWRITE %06o to %o", value, a);
+	m->writeWord(a, value);
+}
+
 uint16_t bus::readWord(const uint16_t a, const d_i_space_t s)
 {
 	return read(a, false, false, false, s);
diff --git a/bus.h b/bus.h
index 4670622..9a07fef 100644
--- a/bus.h
+++ b/bus.h
@@ -130,6 +130,8 @@ public:
 	void writeByte(const uint16_t a, const uint8_t value) { return write(a, true, value, false); }
 	void writeWord(const uint16_t a, const uint16_t value);
 
+	void writePhysical(const uint32_t a, const uint16_t value);
+
 	void writeUnibusByte(const uint16_t a, const uint8_t value);
 
 	uint16_t getMMR0() { return MMR0; }
diff --git a/cpu.cpp b/cpu.cpp
index 2356c3d..629b7d0 100644
--- a/cpu.cpp
+++ b/cpu.cpp
@@ -90,7 +90,6 @@ void cpu::reset()
 	pc = 0;
 	psw = 7 << 5;
 	fpsr = 0;
-	runMode = false;
 	init_interrupt_queue();
 }
 
@@ -449,6 +448,50 @@ uint16_t cpu::getGAMAddress(const uint8_t mode, const int reg, const bool word_m
 	return -1;
 }
 
+uint16_t cpu::getGAMAddressDI(const uint8_t mode, const int reg, const bool word_mode, const bool prev_mode)
+{
+	uint16_t next_word = 0;
+	uint16_t temp      = 0;
+
+	int      set       = getBitPSW(11);
+
+	switch(mode) {
+		case 0:
+			// registers are also mapped in memory
+			return 0177700 + reg;
+		case 1:
+			return getRegister(reg, set, prev_mode);
+		case 2:
+			temp = getRegister(reg, set, prev_mode);
+			addRegister(reg, prev_mode, !word_mode || reg == 6 || reg == 7 ? 2 : 1);
+			return temp;
+		case 3:
+			printf("hier001\n");
+			temp = b -> readWord(getRegister(reg, set, prev_mode));
+			addRegister(reg, prev_mode, 2);
+			return temp;
+		case 4:
+			addRegister(reg, prev_mode, !word_mode || reg == 6 || reg == 7 ? -2 : -1);
+			return getRegister(reg, set, prev_mode);
+		case 5:
+			addRegister(reg, prev_mode, -2);
+			printf("hier002\n");
+			return b -> readWord(getRegister(reg, set, prev_mode));
+		case 6:
+			printf("hier003\n");
+			next_word = b -> readWord(getPC());
+			addRegister(7, prev_mode, 2);
+			return getRegister(reg, set, prev_mode) + next_word;
+		case 7:
+			printf("hier004\n");
+			next_word = b -> readWord(getPC());
+			addRegister(7, prev_mode, 2);
+			return b -> readWord(getRegister(reg, set, prev_mode) + next_word);
+	}
+
+	return -1;
+}
+
 bool cpu::double_operand_instructions(const uint16_t instr)
 {
 	const bool    word_mode = !!(instr & 0x8000);
@@ -1334,11 +1377,21 @@ bool cpu::single_operand_instructions(const uint16_t instr)
 					 if (dst_mode == 0)
 						setRegister(dst_reg, true, v);
 					 else {
-						uint16_t a = getGAMAddress(dst_mode, dst_reg, false, false);
+						uint16_t a = getGAMAddressDI(dst_mode, dst_reg, false, false);
 
 						set_flags = a != ADDR_PSW;
 
-						b->write(a, false, v, true);  // put in '13/12' address space
+						if (a >= 0160000)
+							b->write(a, false, v, true);  // put in '13/12' address space
+						else {
+							auto phys = b->calculate_physical_address((getPSW() >> 12) & 3, a);
+
+							DOLOG(debug, true, "MTPI/D %06o -> %o / %o", a, phys.physical_instruction, phys.physical_data);
+
+							b->check_address(true, true, phys, false, word_mode, (getPSW() >> 12) & 3);
+
+							b->writePhysical(word_mode ? phys.physical_data : phys.physical_instruction, v);
+						}
 					 }
 
 					 if (set_flags) {
diff --git a/cpu.h b/cpu.h
index 00ced1b..b5355f9 100644
--- a/cpu.h
+++ b/cpu.h
@@ -21,7 +21,6 @@ private:
 	uint16_t fpsr  { 0 };
 	uint16_t stackLimitRegister { 0 };
 	uint8_t  scheduled_trap     { 0 };
-	bool     runMode     { false };
 	bool     emulateMFPT { false };
 	uint64_t instruction_count { 0 };
 	uint64_t running_since     { 0 };
@@ -43,6 +42,7 @@ private:
 
 	void     addToMMR1(const uint8_t mode, const uint8_t reg, const bool word_mode);
 	uint16_t getGAMAddress(const uint8_t mode, const int reg, const bool word_mode, const bool MF_MT);
+	uint16_t getGAMAddressDI(const uint8_t mode, const int reg, const bool word_mode, const bool MF_MT);
 	uint16_t getGAM(const uint8_t mode, const uint8_t reg, const bool word_mode, const bool MF_MT);
 	// returns false when flag registers should not be updated
 	bool     putGAM(const uint8_t mode, const int reg, const bool word_mode, const uint16_t value, const bool MF_FT);
@@ -100,8 +100,6 @@ public:
 
 	void setEmulateMFPT(const bool v) { emulateMFPT = v; }
 
-	bool getRunMode() { return runMode; }
-
 	bool getPSW_c() const;
 	bool getPSW_v() const;
 	bool getPSW_z() const;