From fcd6b7b0ea88e98a6b096220a803eeaf505e94d1 Mon Sep 17 00:00:00 2001
From: Thomas Harte <thomas.harte@gmail.com>
Date: Sat, 24 Jul 2021 16:06:49 -0400
Subject: [PATCH] Takes further aim at the conters.

I think test cases are needed, probably.
---
 .../Implementation/6526Implementation.hpp     | 33 ++++---------------
 .../6526/Implementation/6526Storage.hpp       | 19 +++++++++++
 2 files changed, 25 insertions(+), 27 deletions(-)

diff --git a/Components/6526/Implementation/6526Implementation.hpp b/Components/6526/Implementation/6526Implementation.hpp
index 8415425db..93ab2ba12 100644
--- a/Components/6526/Implementation/6526Implementation.hpp
+++ b/Components/6526/Implementation/6526Implementation.hpp
@@ -68,16 +68,10 @@ void MOS6526<BusHandlerT, personality>::write(int address, uint8_t value) {
 		break;
 
 		// Counters; writes set the reload values.
-		case 4:	counter_[0].reload = (counter_[0].reload & 0xff00) | uint16_t(value << 0);	break;
-		case 5:
-			counter_[0].reload = (counter_[0].reload & 0x00ff) | uint16_t(value << 8);
-			counter_[0].is_counting = true;
-		break;
-		case 6:	counter_[1].reload = (counter_[1].reload & 0xff00) | uint16_t(value << 0);	break;
-		case 7:
-			counter_[1].reload = (counter_[1].reload & 0x00ff) | uint16_t(value << 8);
-			counter_[1].is_counting = true;
-		break;
+		case 4:	counter_[0].template set_reload<0>(value);	break;
+		case 5:	counter_[0].template set_reload<8>(value);	break;
+		case 6:	counter_[1].template set_reload<0>(value);	break;
+		case 7:	counter_[1].template set_reload<8>(value);	break;
 
 		// Time-of-day clock.
 		//
@@ -141,23 +135,8 @@ void MOS6526<BusHandlerT, personality>::write(int address, uint8_t value) {
 		} break;
 
 		// Control.
-		case 14:
-			if(value & 0x10) {
-				counter_[0].value = counter_[0].reload;
-				counter_[0].is_counting = true;
-			}
-			counter_[0].control = value & 0xef;
-			printf("Ignoring control A write: %02x\n", value);
-		break;
-
-		case 15:
-			if(value & 0x10) {
-				counter_[1].value = counter_[1].reload;
-				counter_[1].is_counting = true;
-			}
-			counter_[1].control = value;
-			printf("Ignoring control B write: %02x\n", value);
-		break;
+		case 14:	counter_[0].template set_control<false>(value);	break;
+		case 15:	counter_[1].template set_control<true>(value);	break;
 
 		default:
 			printf("Unhandled 6526 write: %02x to %d\n", value, address);
diff --git a/Components/6526/Implementation/6526Storage.hpp b/Components/6526/Implementation/6526Storage.hpp
index efaa0638a..d177108b4 100644
--- a/Components/6526/Implementation/6526Storage.hpp
+++ b/Components/6526/Implementation/6526Storage.hpp
@@ -32,6 +32,25 @@ struct MOS6526Storage {
 		uint8_t control = 0;
 		bool is_counting = false;
 
+		template <int shift> void set_reload(uint8_t v) {
+			reload = (reload & (0xff00 >> shift)) | uint16_t(v << shift);
+
+			if constexpr (shift == 8) {
+				if(!is_counting) {
+					is_counting = true;
+					value = reload;
+				}
+			}
+		}
+
+		template <bool is_counter_2> void set_control(uint8_t v) {
+			control = v & 0xef;
+			if(v & 0x10) {
+				value = reload;
+			}
+			is_counting |= (v & 0x18) == 0x10;
+		}
+
 		int subtract(int count) {
 			if(control & 8) {
 				// One-shot.