Merge pull request #11 from jannic-dev-forks/minimal_flow_control

Implement minimal tx flow control

1 files changed, 35 insertions, 8 deletions

diff --git a/src/lib.rs b/src/lib.rs
index 34170a266..5e79e6e40 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -525,6 +525,8 @@ pub struct Runner<'a, PWR, SPI> {
     ioctl_id: u16,
     sdpcm_seq: u8,
     backplane_window: u32,
+
+    sdpcm_seq_max: u8,
 }
 
 pub async fn new<'a, PWR, SPI>(
@@ -546,6 +548,8 @@ where
         ioctl_id: 0,
         sdpcm_seq: 0,
         backplane_window: 0xAAAA_AAAA,
+
+        sdpcm_seq_max: 1,
     };
 
     runner.init(firmware).await;
@@ -669,14 +673,21 @@ where
         let mut buf = [0; 512];
         loop {
             // Send stuff
-            // TODO flow control
-            if let IoctlState::Pending { kind, cmd, iface, buf } = self.state.ioctl_state.get() {
-                self.send_ioctl(kind, cmd, iface, unsafe { &*buf }).await;
-                self.state.ioctl_state.set(IoctlState::Sent { buf });
-            }
-
-            if let Ok(p) = self.state.tx_channel.try_recv() {
-                self.send_packet(&p).await;
+            // TODO flow control not yet complete
+            if !self.has_credit() {
+                warn!("TX stalled");
+            } else {
+                if let IoctlState::Pending { kind, cmd, iface, buf } = self.state.ioctl_state.get() {
+                    self.send_ioctl(kind, cmd, iface, unsafe { &*buf }).await;
+                    self.state.ioctl_state.set(IoctlState::Sent { buf });
+                }
+                if !self.has_credit() {
+                    warn!("TX stalled");
+                } else {
+                    if let Ok(p) = self.state.tx_channel.try_recv() {
+                        self.send_packet(&p).await;
+                    }
+                }
             }
 
             // Receive stuff
@@ -788,6 +799,8 @@ where
             return;
         }
 
+        self.update_credit(&sdpcm_header);
+
         let channel = sdpcm_header.channel_and_flags & 0x0f;
 
         let payload = &packet[sdpcm_header.header_length as _..];
@@ -894,6 +907,20 @@ where
         }
     }
 
+    fn update_credit(&mut self, sdpcm_header: &SdpcmHeader) {
+        if sdpcm_header.channel_and_flags & 0xf < 3 {
+            let mut sdpcm_seq_max = sdpcm_header.bus_data_credit;
+            if sdpcm_seq_max - self.sdpcm_seq > 0x40 {
+                sdpcm_seq_max = self.sdpcm_seq + 2;
+            }
+            self.sdpcm_seq_max = sdpcm_seq_max;
+        }
+    }
+
+    fn has_credit(&mut self) -> bool {
+        self.sdpcm_seq != self.sdpcm_seq_max && self.sdpcm_seq_max.wrapping_sub(self.sdpcm_seq) & 0x80 == 0
+    }
+
     async fn send_ioctl(&mut self, kind: u32, cmd: u32, iface: u32, data: &[u8]) {
         let mut buf = [0; 512];
         let buf8 = slice8_mut(&mut buf);