1 files changed, 51 insertions, 8 deletions

diff --git a/mpm/lib/spi/spi_regs_iface.cpp b/mpm/lib/spi/spi_regs_iface.cpp
index 842f45d42..d4776d859 100644
--- a/mpm/lib/spi/spi_regs_iface.cpp
+++ b/mpm/lib/spi/spi_regs_iface.cpp
@@ -21,8 +21,8 @@ public:
     spi_regs_iface_impl(mpm::spi::spi_iface::sptr spi_iface,
         uint32_t addr_shift,
         uint32_t data_shift,
-        uint32_t read_flags,
-        uint32_t write_flags = 0)
+        uint64_t read_flags,
+        uint64_t write_flags = 0)
         : _spi_iface(spi_iface)
         , _addr_shift(addr_shift)
         , _data_shift(data_shift)
@@ -72,20 +72,63 @@ public:
         _spi_iface->transfer24_16(transaction);
     }
 
+    uint32_t peek32(const uint64_t addr)
+    {
+        /* Note: _addr_shift and _read_flags will be offset from the
+         * TX portion of the message (first 2 bytes) rather than the
+         * end of the message as is done with smaller transfers.
+         */
+        uint64_t transaction = 0 | (addr << _addr_shift) | _read_flags;
+
+        uint64_t data = _spi_iface->transfer64_40(transaction);
+
+        // Actual RX data is the last 5 bytes
+        if ((data & 0xFFFFFF00000000F8) != 0) {
+            throw mpm::runtime_error("SPI read returned too much data");
+        }
+
+        // Status data is the last byte
+        bool ack = (data >> 2) & 0x1;
+        uint8_t status = data & 0x3;
+        if (!ack) {
+            throw mpm::runtime_error("Ctrlport SPI read had no ACK");
+        }
+        if (status != 0) {
+            // TODO: Differentiate error codes
+            throw mpm::runtime_error("Ctrlport SPI error");
+        }
+
+        // Register data is the 4 bytes above the last one
+        data = (data >> 8) & 0xFFFFFFFF;
+        return data;
+    }
+
+    void poke32(const uint64_t addr, const uint32_t data)
+    {
+        /* Note: _addr_shift and _write_flags will be offset from the
+         * TX portion of the message (first 6 bytes) rather than the
+         * end of the message as is done with smaller transfers.
+         */
+        uint64_t transaction = 0 | _write_flags | (addr << _addr_shift)
+                               | (data << _data_shift);
+
+        _spi_iface->transfer64_40(transaction);
+    }
+
 private:
     mpm::spi::spi_iface::sptr _spi_iface;
 
     uint32_t _addr_shift;
     uint32_t _data_shift;
-    uint32_t _read_flags;
-    uint32_t _write_flags;
+    uint64_t _read_flags;
+    uint64_t _write_flags;
 };
 
 regs_iface::sptr mpm::spi::make_spi_regs_iface(mpm::spi::spi_iface::sptr spi_iface,
     uint32_t addr_shift,
     uint32_t data_shift,
-    uint32_t read_flags,
-    uint32_t write_flags)
+    uint64_t read_flags,
+    uint64_t write_flags)
 {
     return std::make_shared<spi_regs_iface_impl>(
         spi_iface, addr_shift, data_shift, read_flags, write_flags);
@@ -96,8 +139,8 @@ mpm::types::regs_iface::sptr mpm::spi::make_spidev_regs_iface(const std::string&
     const int spi_mode,
     uint32_t addr_shift,
     uint32_t data_shift,
-    uint32_t read_flags,
-    uint32_t write_flags)
+    uint64_t read_flags,
+    uint64_t write_flags)
 {
     auto spi_iface_sptr = mpm::spi::spi_iface::make_spidev(device, speed_hz, spi_mode);
     return std::make_shared<spi_regs_iface_impl>(