fix spi read

Author: almindor

src/spi/bus.rs

@@ -145,15 +145,25 @@ where
     SPI: SpiX,
 {
     fn read(&mut self, words: &mut [u8]) -> Result<(), Infallible> {
-        let rxdata = self.spi.rxdata.read();
-        // TODO: send 00 ??
+        let mut iwrite = 0;
+        let mut iread = 0;
+
+        // Ensure that RX FIFO is empty
+        self.wait_for_rxfifo();
 
-        for word in words {
-            if rxdata.empty().bit_is_set() {
-                break;
+        while iwrite < words.len() || iread < words.len() {
+            if iwrite < words.len() && self.spi.txdata.read().full().bit_is_clear() {
+                iwrite += 1;
+                self.spi.txdata.write(|w| unsafe { w.data().bits(0x00) });
             }
 
-            *word = rxdata.data().bits();
+            if iread < iwrite {
+                let data = self.spi.rxdata.read();
+                if data.empty().bit_is_clear() {
+                    unsafe { *words.get_unchecked_mut(iread) = data.data().bits() };
+                    iread += 1;
+                }
+            }
         }
 
         Ok(())