From cf9811d03a81f07755b1ec6671894ac5fc2593b7 Mon Sep 17 00:00:00 2001
From: joevt <joevt@shaw.ca>
Date: Fri, 26 Apr 2024 18:28:40 -0700
Subject: [PATCH] escc: Read ch_a is missing register pointer reset.

---
 devices/serial/escc.cpp | 29 +++++++++++++++++------------
 devices/serial/escc.h   |  1 +
 2 files changed, 18 insertions(+), 12 deletions(-)

diff --git a/devices/serial/escc.cpp b/devices/serial/escc.cpp
index eebaeac..1af548e 100644
--- a/devices/serial/escc.cpp
+++ b/devices/serial/escc.cpp
@@ -80,20 +80,10 @@ uint8_t EsccController::read(uint8_t reg_offset)
 
     switch(reg_offset) {
     case EsccReg::Port_B_Cmd:
-        if (this->reg_ptr == RR2) {
-            // TODO: implement interrupt vector modifications
-            value = this->int_vec;
-        } else {
-            value = this->ch_b->read_reg(this->reg_ptr);
-        }
-        this->reg_ptr = RR0; // or WR0
+        value = this->read_internal(this->ch_b.get());
         break;
     case EsccReg::Port_A_Cmd:
-        if (this->reg_ptr == RR2) {
-            value = this->int_vec;
-        } else {
-            value = this->ch_a->read_reg(this->reg_ptr);
-        }
+        value = this->read_internal(this->ch_a.get());
         break;
     case EsccReg::Port_B_Data:
         value = this->ch_b->receive_byte();
@@ -141,6 +131,21 @@ void EsccController::write(uint8_t reg_offset, uint8_t value)
     }
 }
 
+uint8_t EsccController::read_internal(EsccChannel *ch)
+{
+    uint8_t value;
+    switch (this->reg_ptr) {
+    case RR2:
+        // TODO: implement interrupt vector modifications
+        value = this->int_vec;
+        break;
+    default:
+        value = ch->read_reg(this->reg_ptr);
+    }
+    this->reg_ptr = RR0; // or WR0
+    return value;
+}
+
 void EsccController::write_internal(EsccChannel *ch, uint8_t value)
 {
     switch (this->reg_ptr) {
diff --git a/devices/serial/escc.h b/devices/serial/escc.h
index 21d6cb4..e496b1d 100644
--- a/devices/serial/escc.h
+++ b/devices/serial/escc.h
@@ -171,6 +171,7 @@ public:
 private:
     void reset();
     void write_internal(EsccChannel* ch, uint8_t value);
+    uint8_t read_internal(EsccChannel* ch);
 
     std::unique_ptr<EsccChannel>    ch_a;
     std::unique_ptr<EsccChannel>    ch_b;