diff --git a/src/lib.rs b/src/lib.rs index 34170a26..f818caf6 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, + + tx_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, + + tx_seq_max: 1, }; runner.init(firmware).await; @@ -670,13 +674,17 @@ where 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 self.sdpcm_seq == self.tx_seq_max || self.tx_seq_max.wrapping_sub(self.sdpcm_seq) & 0x80 != 0 { + 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 let Ok(p) = self.state.tx_channel.try_recv() { - self.send_packet(&p).await; + if let Ok(p) = self.state.tx_channel.try_recv() { + self.send_packet(&p).await; + } } // Receive stuff @@ -788,6 +796,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 +904,16 @@ where } } + fn update_credit(&mut self, sdpcm_header: &SdpcmHeader) { + if sdpcm_header.channel_and_flags & 0xf < 3 { + let mut tx_seq_max = sdpcm_header.bus_data_credit; + if tx_seq_max - self.sdpcm_seq > 0x40 { + tx_seq_max = self.sdpcm_seq + 2; + } + self.tx_seq_max = tx_seq_max; + } + } + 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);