3 files changed, 38 insertions, 4 deletions
diff --git a/mpm/include/mpm/spi/spi_iface.hpp b/mpm/include/mpm/spi/spi_iface.hpp
index d3f387ff3..1fc5f828c 100644
--- a/mpm/include/mpm/spi/spi_iface.hpp
+++ b/mpm/include/mpm/spi/spi_iface.hpp
@@ -39,6 +39,16 @@ namespace mpm { namespace spi {
             const uint32_t data
         ) = 0;
 
+        /*! Convenience function: SPI xfer is 24 bits write, 16 bits read.
+         *
+         * \param data The write data for this xfer
+         *
+         * \return 16 bits worth of the return xfer
+         */
+        virtual uint32_t transfer24_16(
+            const uint32_t data
+        ) = 0;
+
         /*!
          * \param device The path to the spidev used (e.g. "/dev/spidev0.0")
          * \param speed_hz Transaction speed in Hz
diff --git a/mpm/lib/spi/spi_regs_iface.cpp b/mpm/lib/spi/spi_regs_iface.cpp
index 40b376ee1..3f69d89c8 100644
--- a/mpm/lib/spi/spi_regs_iface.cpp
+++ b/mpm/lib/spi/spi_regs_iface.cpp
@@ -82,7 +82,7 @@ public:
             | _read_flags
         ;
 
-        uint32_t data = _spi_iface->transfer24_8(transaction);
+        uint32_t data = _spi_iface->transfer24_16(transaction);
         if ((data & 0xFFFF0000) != 0) {
             throw mpm::runtime_error("SPI read returned too much data");
         }
@@ -100,7 +100,7 @@ public:
             | (data << _data_shift)
         ;
 
-        _spi_iface->transfer24_8(transaction);
+        _spi_iface->transfer24_16(transaction);
     }
 
 private:
diff --git a/mpm/lib/spi/spidev_iface.cpp b/mpm/lib/spi/spidev_iface.cpp
index 74cb6bc88..522238e19 100644
--- a/mpm/lib/spi/spidev_iface.cpp
+++ b/mpm/lib/spi/spidev_iface.cpp
@@ -89,11 +89,35 @@ public:
             ));
         }
 
-         // Assumes that only a single byte is being read.
-         // TODO the function does not advertise this. Should probably fix.
         return uint32_t(rx[2]);
     }
 
+    uint32_t transfer24_16(
+            const uint32_t data_
+    ) {
+        int ret(0);
+
+        uint32_t data = data_;
+        uint8_t *tx_data = reinterpret_cast<uint8_t *>(&data);
+
+        // Create tx and rx buffers:
+        uint8_t tx[] = {tx_data[2], tx_data[1], tx_data[0]}; // FIXME guarantee endianness
+        uint8_t rx[3]; // Buffer length must match tx buffer
+
+        if (transfer(
+            _fd,
+            &tx[0], &rx[0],
+            3,
+            _speed, _bits, _delay
+        ) != 0) {
+            throw mpm::runtime_error(str(
+                    boost::format("SPI Transaction failed!")
+            ));
+        }
+
+        return uint32_t(rx[1] << 8 | rx[2]);
+    }
+
 private:
     int _fd;
     const uint32_t _mode;