diff --git a/src/lib.rs b/src/lib.rs index 34170a26..5e79e6e4 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);