1 files changed, 30 insertions, 1 deletions
diff --git a/mpm/lib/spi/spi_regs_iface.cpp b/mpm/lib/spi/spi_regs_iface.cpp
index ab9e089f8..40b376ee1 100644
--- a/mpm/lib/spi/spi_regs_iface.cpp
+++ b/mpm/lib/spi/spi_regs_iface.cpp
@@ -58,7 +58,7 @@ public:
             throw mpm::runtime_error("SPI read returned too much data");
         }
 
-        return uint8_t(data & 0xFF);
+        return data;
     }
 
     void poke8(
@@ -74,6 +74,35 @@ public:
         _spi_iface->transfer24_8(transaction);
     }
 
+    uint16_t peek16(
+        const uint32_t addr
+    ) {
+        uint32_t transaction = 0
+            | (addr << _addr_shift)
+            | _read_flags
+        ;
+
+        uint32_t data = _spi_iface->transfer24_8(transaction);
+        if ((data & 0xFFFF0000) != 0) {
+            throw mpm::runtime_error("SPI read returned too much data");
+        }
+
+        return data;
+    }
+
+    void poke16(
+        const uint32_t addr,
+        const uint16_t data
+    ) {
+        uint32_t transaction = 0
+            | _write_flags
+            | (addr << _addr_shift)
+            | (data << _data_shift)
+        ;
+
+        _spi_iface->transfer24_8(transaction);
+    }
+
 private:
     mpm::spi::spi_iface::sptr _spi_iface;