From a0885ab7d02b593606921d8649b369422cc16368 Mon Sep 17 00:00:00 2001
From: Thomas Harte <thomas.harte@gmail.com>
Date: Sun, 11 Oct 2020 17:56:55 -0400
Subject: [PATCH] Implements STP and WAI.

Albeit still without fully-implemented reactions to exceptions in general.
---
 .../Implementation/65816Implementation.hpp    | 27 ++++++++++++++++---
 .../65816/Implementation/65816Storage.cpp     | 19 ++++++-------
 .../65816/Implementation/65816Storage.hpp     |  7 +++++
 3 files changed, 38 insertions(+), 15 deletions(-)

diff --git a/Processors/65816/Implementation/65816Implementation.hpp b/Processors/65816/Implementation/65816Implementation.hpp
index 000d0eb38..0f25f1185 100644
--- a/Processors/65816/Implementation/65816Implementation.hpp
+++ b/Processors/65816/Implementation/65816Implementation.hpp
@@ -170,6 +170,19 @@ template <typename BusHandler> void Processor<BusHandler>::run_for(const Cycles
 
 #undef stack_access
 
+			//
+			// STP and WAI.
+			//
+
+			case CycleRepeatingNone:
+				if(pending_exceptions_ & required_exceptions_) {
+					continue;
+				} else {
+					--next_op_;
+					perform_bus(0xffffff, nullptr, MOS6502Esque::None);
+				}
+			break;
+
 			//
 			// Data movement.
 			//
@@ -822,11 +835,17 @@ template <typename BusHandler> void Processor<BusHandler>::run_for(const Cycles
 						LD(a_, result, m_masks_);
 					} break;
 
-					// TODO:
-					//	STP, WAI,
+					//
+					// STP and WAI
+					//
 
-					default:
-						assert(false);
+					case STP:
+						required_exceptions_ = Reset;
+					break;
+
+					case WAI:
+						required_exceptions_ = Reset | IRQ | NMI;
+					break;
 				}
 			continue;
 		}
diff --git a/Processors/65816/Implementation/65816Storage.cpp b/Processors/65816/Implementation/65816Storage.cpp
index 137f3889f..03cb4feca 100644
--- a/Processors/65816/Implementation/65816Storage.cpp
+++ b/Processors/65816/Implementation/65816Storage.cpp
@@ -539,18 +539,15 @@ struct CPU::WDC65816::ProcessorStorageConstructor {
 		target(OperationPerform);
 	}
 
-	// 19c. Stop the Clock.
-	static void stp(AccessType, bool, const std::function<void(MicroOp)> &target) {
-		target(CycleFetchPCThrowaway);		// IO
-		target(CycleFetchPCThrowaway);		// IO
-		target(OperationPerform);
-	}
-
+	// 19c. Stop the Clock; also
 	// 19d. Wait for interrupt.
-	static void wai(AccessType, bool, const std::function<void(MicroOp)> &target) {
+	static void stp_wai(AccessType, bool, const std::function<void(MicroOp)> &target) {
+		target(OperationPerform);			// Establishes the termination condition.
 		target(CycleFetchPCThrowaway);		// IO
 		target(CycleFetchPCThrowaway);		// IO
-		target(OperationPerform);
+		target(CycleRepeatingNone);			// This will first check whether the STP/WAI exit
+											// condition has occurred; if not then it'll issue
+											// a BusOperation::None and then reschedule itself.
 	}
 
 	// 20. Relative; r.
@@ -955,7 +952,7 @@ ProcessorStorage::ProcessorStorage() {
 	/* 0xc8 INY i */			op(implied, INY);
 	/* 0xc9 CMP # */			op(immediate, CMP);
 	/* 0xca DEX i */			op(implied, DEX);
-	/* 0xcb WAI i */			op(wai, WAI);
+	/* 0xcb WAI i */			op(stp_wai, WAI);
 	/* 0xcc CPY a */			op(absolute, CPY);
 	/* 0xcd CMP a */			op(absolute, CMP);
 	/* 0xce DEC a */			op(absolute_rmw, DEC);
@@ -972,7 +969,7 @@ ProcessorStorage::ProcessorStorage() {
 	/* 0xd8 CLD i */			op(implied, CLD);
 	/* 0xd9 CMP a, y */			op(absolute_y, CMP);
 	/* 0xda PHX s */			op(stack_push, STX);
-	/* 0xdb STP i */			op(stp, STP);
+	/* 0xdb STP i */			op(stp_wai, STP);
 	/* 0xdc JML (a) */			op(absolute_indirect_jml, JML);
 	/* 0xdd CMP a, x */			op(absolute_x, CMP);
 	/* 0xde DEC a, x */			op(absolute_x_rmw, DEC);
diff --git a/Processors/65816/Implementation/65816Storage.hpp b/Processors/65816/Implementation/65816Storage.hpp
index b7ba1d467..b4a42de06 100644
--- a/Processors/65816/Implementation/65816Storage.hpp
+++ b/Processors/65816/Implementation/65816Storage.hpp
@@ -47,6 +47,10 @@ enum MicroOp: uint8_t {
 	/// Performs as CyclePull if the 65816 is not in emulation mode; otherwise skips itself.
 	CyclePullIfNotEmulation,
 
+	/// Issues a BusOperation::None and regresses the micro-op counter until an established
+	/// STP or WAI condition is satisfied.
+	CycleRepeatingNone,
+
 	/// Sets the data address by copying the final two bytes of the instruction buffer and
 	/// using the data register as a high byte.
 	OperationConstructAbsolute,
@@ -261,6 +265,9 @@ struct ProcessorStorage {
 	static constexpr int NMI = 1 << 3;
 	int pending_exceptions_ = PowerOn;	// By default.
 
+	/// Sets the required exception flags necessary to exit a STP or WAI.
+	int required_exceptions_ = 0;
+
 	/// Defines a four-byte buffer which can be cleared or filled in single-byte increments from least significant byte
 	/// to most significant.
 	struct Buffer {