From 1fc21b2ee672f2243653b2272548119b1488acfe Mon Sep 17 00:00:00 2001
From: Andrew Kay <git@ajk.me>
Date: Fri, 10 Jul 2020 22:44:26 -0500
Subject: [PATCH] Teensy is fast, get rid of some premature optimization

---
 interface2/firmware/include/NewCoax.h |  2 ++
 interface2/firmware/src/NewCoax.cpp   | 15 ++++-----------
 2 files changed, 6 insertions(+), 11 deletions(-)

diff --git a/interface2/firmware/include/NewCoax.h b/interface2/firmware/include/NewCoax.h
index 2d9b8c8..dc1cdc4 100644
--- a/interface2/firmware/include/NewCoax.h
+++ b/interface2/firmware/include/NewCoax.h
@@ -73,6 +73,8 @@ public:
     void setMode(int mode);
     uint16_t read();
     void write(uint16_t word);
+
+private:
     uint16_t encode(uint16_t word);
     uint16_t decode(uint16_t word);
 };
diff --git a/interface2/firmware/src/NewCoax.cpp b/interface2/firmware/src/NewCoax.cpp
index b16bdc6..a559d13 100644
--- a/interface2/firmware/src/NewCoax.cpp
+++ b/interface2/firmware/src/NewCoax.cpp
@@ -81,13 +81,11 @@ int NewCoaxTransmitter::transmit(uint16_t *buffer, size_t bufferCount)
     _dataBus.setMode(OUTPUT);
 
     for (int index = 0; index < bufferCount; index++) {
-        uint16_t word = _dataBus.encode(buffer[index]);
-
         while (digitalRead(TX_FULL_PIN)) {
             // NOP
         }
 
-        write(word);
+        write(buffer[index]);
 
         // TODO: the Teensy, too fast!
         delayMicroseconds(2);
@@ -202,11 +200,6 @@ int NewCoaxReceiver::receive(uint16_t *buffer, size_t bufferSize, uint16_t timeo
         return ERROR_RX_OVERFLOW;
     }
 
-    // Unscramble the data.
-    for (int index = 0; index < count; index++) {
-        buffer[index] = _dataBus.decode(buffer[index]);
-    }
-
     return count;
 }
 
@@ -249,7 +242,7 @@ void NewCoaxReceiver::dataAvailableInterrupt()
 
 void NewCoaxReceiver::errorInterrupt()
 {
-    _error = _dataBus.decode(read());
+    _error = _dataBus.read();
 
     if (_state == Receiving) {
         _state = Received;
@@ -287,12 +280,12 @@ void NewCoaxDataBus::setMode(int mode)
 
 inline uint16_t NewCoaxDataBus::read()
 {
-    return (GPIO6_DR & DATA_BUS_MASK) >> 16;
+    return decode((GPIO6_DR & DATA_BUS_MASK) >> 16);
 }
 
 inline void NewCoaxDataBus::write(uint16_t word)
 {
-    uint32_t bus = (word << 16) & DATA_BUS_MASK;
+    uint32_t bus = (encode(word) << 16) & DATA_BUS_MASK;
 
     GPIO6_DR_SET = bus;
     GPIO6_DR_CLEAR = ~bus;