1 files changed, 26 insertions, 2 deletions
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;