From aa0c69b21e6cd792abec310b4fbc8b37306dab7c Mon Sep 17 00:00:00 2001
From: Jonah Dahlquist <jonah@jonah.name>
Date: Wed, 7 Aug 2019 20:29:23 -0500
Subject: [PATCH] Fixed some masking issues with FourWireBus, added
 implementation for ThreeWireBus

---
 src/bus/four_wire.rs  | 25 +++++++++++++-----
 src/bus/mod.rs        |  7 ++---
 src/bus/three_wire.rs | 61 ++++++++++++++++++++++++++++++++++++++++---
 3 files changed, 80 insertions(+), 13 deletions(-)

diff --git a/src/bus/four_wire.rs b/src/bus/four_wire.rs
index 0859a44..e486b9e 100644
--- a/src/bus/four_wire.rs
+++ b/src/bus/four_wire.rs
@@ -4,6 +4,9 @@ use embedded_hal::spi::FullDuplex;
 
 use crate::bus::{ActiveBus, Bus};
 
+const WRITE_MODE_MASK: u8 = 0b11111_1_11;
+const READ_MODE_MASK: u8 = 0b_11111_0_11;
+
 pub struct FourWire<ChipSelect: OutputPin> {
     cs: ChipSelect,
 }
@@ -34,22 +37,32 @@ impl<Spi: FullDuplex<u8>, ChipSelect: OutputPin> ActiveBus for ActiveFourWire<Sp
     type Error = FourWireError<Spi::Error, ChipSelect::Error>;
     fn transfer_frame<'a>(
         &mut self,
-        address_phase: u16,
-        mut control_phase: u8,
-        data_phase: &'a mut [u8],
+        address: u16,
+        block: u8,
+        is_write: bool,
+        data: &'a mut [u8],
     ) -> Result<&'a mut [u8], nb::Error<Self::Error>> {
-        let mut address_bytes = [0u8; 2];
-        BigEndian::write_u16(&mut address_bytes, address_phase);
+        let mut control_phase = block << 3;
+        if is_write {
+            control_phase &= WRITE_MODE_MASK;
+        } else {
+            control_phase &= READ_MODE_MASK;
+        }
+        let data_phase = data;
+        let mut address_phase = [0u8; 2];
+        BigEndian::write_u16(&mut address_phase, address);
+
         self.cs
             .set_high()
             .map_err(|e| Self::Error::ChipSelectError(e))?;
-        block!(Self::transfer_bytes(&mut self.spi, &mut address_bytes)
+        block!(Self::transfer_bytes(&mut self.spi, &mut address_phase)
             .and_then(|_| Self::transfer_byte(&mut self.spi, &mut control_phase))
             .and_then(|_| Self::transfer_bytes(&mut self.spi, data_phase)))
         .map_err(|e| Self::Error::SpiError(e))?;
         self.cs
             .set_low()
             .map_err(|e| Self::Error::ChipSelectError(e))?;
+
         Ok(data_phase)
     }
 }
diff --git a/src/bus/mod.rs b/src/bus/mod.rs
index dce0f72..128f0c3 100644
--- a/src/bus/mod.rs
+++ b/src/bus/mod.rs
@@ -16,9 +16,10 @@ pub trait ActiveBus {
 
     fn transfer_frame<'a>(
         &mut self,
-        address_phase: u16,
-        control_phase: u8,
-        data_phase: &'a mut [u8],
+        address: u16,
+        block: u8,
+        is_write: bool,
+        data: &'a mut [u8],
     ) -> Result<&'a mut [u8], Self::Error>;
 
     fn transfer_bytes<'a, Spi: FullDuplex<u8>>(
diff --git a/src/bus/three_wire.rs b/src/bus/three_wire.rs
index 347338b..01b2a5f 100644
--- a/src/bus/three_wire.rs
+++ b/src/bus/three_wire.rs
@@ -1,7 +1,15 @@
+use byteorder::{BigEndian, ByteOrder};
 use embedded_hal::spi::FullDuplex;
 
 use crate::bus::{ActiveBus, Bus};
 
+const WRITE_MODE_MASK: u8 = 0b11111_1_11;
+const READ_MODE_MASK: u8 = 0b_11111_0_11;
+
+const FIXED_DATA_LENGTH_MODE_1: u8 = 0b111111_01;
+const FIXED_DATA_LENGTH_MODE_2: u8 = 0b111111_10;
+const FIXED_DATA_LENGTH_MODE_4: u8 = 0b111111_11;
+
 pub struct ThreeWire {}
 
 impl ThreeWire {
@@ -24,13 +32,58 @@ pub struct ActiveThreeWire<Spi: FullDuplex<u8>> {
 
 impl<Spi: FullDuplex<u8>> ActiveBus for ActiveThreeWire<Spi> {
     type Error = Spi::Error;
+
+    /// Transfers a frame with an arbitrary data length in FDM
+    ///
+    /// This is done by passing chunks of fixed length 4, 2, or 1.  For example if a frame looks like this:
+    ///
+    /// (address 23) 0xF0 0xAB 0x83 0xB2 0x44 0x2C 0xAA
+    ///
+    /// This will be sent as separate frames in the chunks
+    ///
+    /// (address 23) 0xF0 0xAB 0x83 0xB2
+    /// (address 27) 44 2C
+    /// (address 29) AA
     fn transfer_frame<'a>(
         &mut self,
-        address_phase: u16,
-        control_phase: u8,
-        data_phase: &'a mut [u8],
+        mut address: u16,
+        block: u8,
+        is_write: bool,
+        data: &'a mut [u8],
     ) -> Result<&'a mut [u8], nb::Error<Self::Error>> {
-        // TODO implement transfer
+        let mut control_phase = block << 3;
+        if is_write {
+            control_phase &= WRITE_MODE_MASK;
+        } else {
+            control_phase &= READ_MODE_MASK;
+        }
+
+        let mut data_phase = &mut data[..];
+        let mut last_length_written: u16;
+        while data_phase.len() > 0 {
+            if data_phase.len() >= 4 {
+                control_phase &= FIXED_DATA_LENGTH_MODE_4;
+                last_length_written = 4;
+            } else if data_phase.len() >= 2 {
+                control_phase &= FIXED_DATA_LENGTH_MODE_2;
+                last_length_written = 2;
+            } else {
+                control_phase &= FIXED_DATA_LENGTH_MODE_1;
+                last_length_written = 1;
+            }
+
+            let mut address_phase = [0u8; 2];
+            BigEndian::write_u16(&mut address_phase, address);
+            block!(Self::transfer_bytes(&mut self.spi, &mut address_phase)
+                .and_then(|_| Self::transfer_byte(&mut self.spi, &mut control_phase))
+                .and_then(|_| Self::transfer_bytes(
+                    &mut self.spi,
+                    &mut data_phase[..last_length_written as usize]
+                )))?;
+
+            address += last_length_written;
+            data_phase = &mut data_phase[last_length_written as usize..];
+        }
         Ok(data_phase)
     }
 }