From 5d1d94848c39064d72ea72a57adeeaf9a4345ebc Mon Sep 17 00:00:00 2001
From: Thomas Harte <thomas.harte@gmail.com>
Date: Wed, 4 May 2022 08:26:11 -0400
Subject: [PATCH] Take a bash at LINK and UNLK.

---
 InstructionSets/M68k/Executor.hpp             |  3 +++
 .../Implementation/ExecutorImplementation.hpp | 15 ++++++++++++
 .../Implementation/PerformImplementation.hpp  | 24 +++++--------------
 3 files changed, 24 insertions(+), 18 deletions(-)

diff --git a/InstructionSets/M68k/Executor.hpp b/InstructionSets/M68k/Executor.hpp
index 9f38c7b23..64e44b44d 100644
--- a/InstructionSets/M68k/Executor.hpp
+++ b/InstructionSets/M68k/Executor.hpp
@@ -45,8 +45,11 @@ template <Model model, typename BusHandler> class Executor {
 		void add_pc(uint32_t);
 		void decline_branch() {}
 		void did_update_status();
+
 		void bsr(uint32_t offset);
 		void jsr(uint32_t offset);
+		void link(uint32_t &address, uint32_t offset);
+		void unlink(uint32_t &address);
 
 		// TODO: ownership of this shouldn't be here.
 		struct Registers {
diff --git a/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp b/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp
index 1f30d5cf3..e6d500885 100644
--- a/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp
+++ b/InstructionSets/M68k/Implementation/ExecutorImplementation.hpp
@@ -378,6 +378,21 @@ void Executor<model, BusHandler>::jsr(uint32_t address) {
 	program_counter_.l = address;
 }
 
+template <Model model, typename BusHandler>
+void Executor<model, BusHandler>::link(uint32_t &address, uint32_t offset) {
+	address_[7].l -= 4;
+	bus_handler_.template write<uint32_t>(address_[7].l, address);
+	address = address_[7].l;
+	address_[7].l += offset;
+}
+
+template <Model model, typename BusHandler>
+void Executor<model, BusHandler>::unlink(uint32_t &address) {
+	address_[7].l = address;
+	address = bus_handler_.template read<uint32_t>(address_[7].l);
+	address_[7].l += 4;
+}
+
 }
 }
 
diff --git a/InstructionSets/M68k/Implementation/PerformImplementation.hpp b/InstructionSets/M68k/Implementation/PerformImplementation.hpp
index 877bb731f..9940b3589 100644
--- a/InstructionSets/M68k/Implementation/PerformImplementation.hpp
+++ b/InstructionSets/M68k/Implementation/PerformImplementation.hpp
@@ -805,25 +805,13 @@ template <
 			amount of stack space to be allocated or deallocated.
 		*/
 
-//		case Operation::LINK:
-//			// Make space for the new long-word value, and set up
-//			// the proper target address for the stack operations to follow.
-//			address_[7].l -= 4;
-//			effective_address_[1].l = address_[7].l;
-//
-//			// The current value of the address register will be pushed.
-//			destination_bus_data_.l = src.l;
-//
-//			// The address register will then contain the bottom of the stack,
-//			// and the stack pointer will be offset.
-//			src.l = address_[7].l;
-//			address_[7].l += u_extend16(prefetch_queue_.w);
-//		break;
+		case Operation::LINKw:
+			flow_controller.link(src.l, int16_t(dest.w));
+		break;
 
-//		case Operation::UNLINK:
-//			address_[7].l = effective_address_[1].l + 2;
-//			dest.l = destination_bus_data_.l;
-//		break;
+		case Operation::UNLINK:
+			flow_controller.unlink(src.l);
+		break;
 
 		/*
 			TAS: sets zero and negative depending on the current value of the destination,