From 9ecd43238f70b93882eced9e40d39ca96ef962e8 Mon Sep 17 00:00:00 2001
From: Thomas Harte <thomas.harte@gmail.com>
Date: Sat, 30 Oct 2021 12:02:43 -0700
Subject: [PATCH] Correct 8520 TOD setting and getting.

---
 .../6526/Implementation/6526Storage.hpp       | 43 +++++++++----------
 1 file changed, 21 insertions(+), 22 deletions(-)

diff --git a/Components/6526/Implementation/6526Storage.hpp b/Components/6526/Implementation/6526Storage.hpp
index 9b4c36a83..a04954885 100644
--- a/Components/6526/Implementation/6526Storage.hpp
+++ b/Components/6526/Implementation/6526Storage.hpp
@@ -123,7 +123,7 @@ template <> class TODStorage<true>: public TODBase {
 		uint32_t increment_mask_ = uint32_t(~0);
 		uint32_t latch_ = 0;
 		uint32_t value_ = 0;
-		uint32_t alarm_ = 0xffffff;
+		uint32_t alarm_ = 0xff'ffff;
 
 	public:
 		template <int byte> void write(uint8_t v) {
@@ -135,43 +135,42 @@ template <> class TODStorage<true>: public TODBase {
 			// Write to either the alarm or the current value as directed;
 			// writing to any part of the current value other than the LSB
 			// pauses incrementing until the LSB is written.
+			const uint32_t mask = uint32_t(~(0xff << shift));
 			if(write_alarm) {
-				alarm_ = (alarm_ & (0x00ff'ffff >> (24 - shift))) | uint32_t(v << shift);
+				alarm_ = (alarm_ & mask) | uint32_t(v << shift);
 			} else {
-				value_ = (alarm_ & (0x00ff'ffff >> (24 - shift))) | uint32_t(v << shift);
-				increment_mask_ = (shift == 0) ? uint32_t(~0) : 0;
+				value_ = (value_ & mask) | uint32_t(v << shift);
+				increment_mask_ = (byte == 0) ? uint32_t(~0) : 0;
 			}
 		}
 
 		template <int byte> uint8_t read() {
 			if constexpr (byte == 3) {
-				return 0xff;	// Assumed. Just a guss.
+				return 0xff;	// Assumed. Just a guess.
 			}
 			constexpr int shift = byte << 3;
 
-			if(latch_) {
-				// Latching: if this is a latched read from the LSB,
-				// empty the latch.
-				const uint8_t result = uint8_t((latch_ >> shift) & 0xff);
-				if constexpr (shift == 0) {
-					latch_ = 0;
-				}
-				return result;
-			} else {
-				// Latching: if this is a read from the MSB, latch now.
-				if constexpr (shift == 16) {
-					latch_ = value_ | 0xff00'0000;
-				}
-				return uint8_t((value_ >> shift) & 0xff);
+			if constexpr (byte == 2) {
+				latch_ = value_ | 0xff00'0000;
 			}
+
+			const uint32_t source = latch_ ? latch_ : value_;
+			const uint8_t result = uint8_t((source >> shift) & 0xff);
+
+			if constexpr (byte == 0) {
+				latch_ = 0;
+			}
+
+			return result;
 		}
 
 		bool advance(int count) {
 			// The 8250 uses a simple binary counter to replace the
 			// 6526's time-of-day clock. So this is easy.
-			const uint32_t distance_to_alarm = (alarm_ - value_) & 0xffffff;
-			value_ += uint32_t(count) & increment_mask_;
-			return distance_to_alarm <= uint32_t(count);
+			const uint32_t distance_to_alarm = (alarm_ - value_) & 0xff'ffff;
+			const auto increment = uint32_t(count) & increment_mask_;
+			value_ = (value_ + increment) & 0xff'ffff;
+			return distance_to_alarm <= increment;
 		}
 };