Add support for SWD sequence

Cargo.toml

@@ -1,6 +1,6 @@
 [package]
 name = "dap-rs"
-version = "0.1.0"
+version = "0.2.0"
 edition = "2021"
 authors = [
     "Emil Fresk <emil.fresk@gmail.com>",
@@ -20,6 +20,9 @@ embedded-hal = "1.0.0"
 replace_with = { version = "0.1.7", default-features = false, features = ["panic_abort"] }
 bitflags = "1.3.2"
 
+[dev-dependencies]
+mockall = "0.13.0"
+
 [dependencies.defmt]
 optional = true
 version = "0.3"

src/dap.rs

@@ -374,8 +374,46 @@ where
         }
     }
 
-    fn process_swd_sequence(&self, _req: Request, _resp: &mut ResponseWriter) {
-        // TODO: Needs implementing
+    fn process_swd_sequence(&mut self, mut req: Request, resp: &mut ResponseWriter) {
+        self.state.to_swd();
+        let swd = match &mut self.state {
+            State::Swd(swd) => swd,
+            _ => {
+                resp.write_err();
+                return;
+            }
+        };
+
+        resp.write_ok(); // assume ok until we finish
+        let sequence_count = req.next_u8();
+        let success = (0..sequence_count).map(|_| {
+            // parse the seqnence info
+            let sequence_info = req.next_u8();
+            let nbits: usize = match sequence_info & 0x3F {
+                // CMSIS-DAP says 0 means 64 bits
+                0 => 64,
+                // Other integers are normal.
+                n => n as usize,
+            };
+            let nbytes = (nbits + 7) / 8;
+            let output = (sequence_info & 0x80) == 0;
+
+            if output {
+                let output_data = req.data.get(..nbytes).ok_or(())?;
+                swd.write_sequence(nbits, output_data).or(Err(()))?;
+                req.consume(nbytes);
+                Ok(0)
+            } else {
+                let input_data = resp.remaining().get_mut(..nbytes).ok_or(())?;
+                swd.read_sequence(nbits, input_data).or(Err(()))?;
+                resp.skip(nbytes);
+                Ok(nbytes)
+            }
+        }).all(|r: Result<usize, ()>| r.is_ok());
+
+        if !success {
+            resp.write_u8_at(0, 0xFF);
+        }
     }
 
     fn process_swo_transport(&mut self, mut req: Request, resp: &mut ResponseWriter) {
@@ -792,3 +830,204 @@ impl<T> CheckResult<T> for swd::Result<T> {
         }
     }
 }
+
+#[cfg(test)]
+mod test {
+    use super::*;
+    use crate::mock_device::*;
+    use mockall::predicate::*;
+
+    struct FakeLEDs {}
+    impl DapLeds for FakeLEDs {
+        fn react_to_host_status(&mut self, _host_status: HostStatus) {}
+    }
+
+    struct StdDelayUs {}
+    impl DelayNs for StdDelayUs {
+        fn delay_ns(&mut self, ns: u32) {
+            std::thread::sleep(std::time::Duration::from_nanos(ns as u64));
+        }
+    }
+
+    type TestDap<'a> = Dap<
+        'a,
+        MockSwdJtagDevice,
+        FakeLEDs,
+        StdDelayUs,
+        MockSwdJtagDevice,
+        MockSwdJtagDevice,
+        swo::MockSwo,
+    >;
+
+    #[test]
+    fn test_swd_output_reset() {
+        let mut dap = TestDap::new(
+            MockSwdJtagDevice::new(),
+            FakeLEDs {},
+            StdDelayUs {},
+            None,
+            "test_dap",
+        );
+
+        let report = [0x1Du8, 1, 52, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0];
+        let mut rbuf = [0u8; 64];
+        dap.state.to_swd();
+        match &mut dap.state {
+            State::Swd(swd) => {
+                swd.expect_write_sequence()
+                    .once()
+                    .with(eq(52), eq([0xFFu8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0]))
+                    .return_const(Ok(()));
+            }
+            _ => assert!(false, "can't switch to swd"),
+        }
+        let rsize = dap.process_command(&report, &mut rbuf, DapVersion::V2);
+        assert_eq!(rsize, 2);
+        assert_eq!(&rbuf[..2], &[0x1Du8, 0x00])
+    }
+
+    #[test]
+    fn test_swd_output_max_size() {
+        let mut dap = TestDap::new(
+            MockSwdJtagDevice::new(),
+            FakeLEDs {},
+            StdDelayUs {},
+            None,
+            "test_dap",
+        );
+
+        let report = [0x1Du8, 1, 0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0, 0x00];
+        let mut rbuf = [0u8; 64];
+        dap.state.to_swd();
+        match &mut dap.state {
+            State::Swd(swd) => {
+                swd.expect_write_sequence()
+                    .once()
+                    .with(
+                        eq(64),
+                        eq([0xFFu8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0, 0x00]),
+                    )
+                    .return_const(Ok(()));
+            }
+            _ => assert!(false, "can't switch to swd"),
+        }
+        let rsize = dap.process_command(&report, &mut rbuf, DapVersion::V2);
+        assert_eq!(rsize, 2);
+        assert_eq!(&rbuf[..2], &[0x1Du8, 0x00])
+    }
+
+    #[test]
+    fn test_swd_input() {
+        let mut dap = TestDap::new(
+            MockSwdJtagDevice::new(),
+            FakeLEDs {},
+            StdDelayUs {},
+            None,
+            "test_dap",
+        );
+
+        let report = [0x1Du8, 1, 0x80 | 52];
+        let mut rbuf = [0u8; 64];
+        dap.state.to_swd();
+        match &mut dap.state {
+            State::Swd(swd) => {
+                swd.expect_read_sequence()
+                    .once()
+                    .withf(|nbits, buf| buf.len() >= 7 && *nbits == 52)
+                    .returning(|_, buf| {
+                        buf[..7].clone_from_slice(&[0xFFu8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0]);
+                        Ok(())
+                    });
+            }
+            _ => assert!(false, "can't switch to swd"),
+        }
+        let rsize = dap.process_command(&report, &mut rbuf, DapVersion::V2);
+        assert_eq!(rsize, 9);
+        assert_eq!(
+            &rbuf[..9],
+            &[0x1Du8, 0x00, 0xFFu8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0]
+        )
+    }
+
+    #[test]
+    fn test_swd_input_max_size() {
+        let mut dap = TestDap::new(
+            MockSwdJtagDevice::new(),
+            FakeLEDs {},
+            StdDelayUs {},
+            None,
+            "test_dap",
+        );
+
+        let report = [0x1Du8, 1, 0x80];
+        let mut rbuf = [0u8; 64];
+        dap.state.to_swd();
+        match &mut dap.state {
+            State::Swd(swd) => {
+                swd.expect_read_sequence()
+                    .once()
+                    .withf(|nbits, buf| buf.len() >= 8 && *nbits == 64)
+                    .returning(|_, buf| {
+                        buf[..8]
+                            .clone_from_slice(&[0xFFu8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0, 0x00]);
+                        Ok(())
+                    });
+            }
+            _ => assert!(false, "can't switch to swd"),
+        }
+        let rsize = dap.process_command(&report, &mut rbuf, DapVersion::V2);
+        assert_eq!(rsize, 10);
+        assert_eq!(
+            &rbuf[..10],
+            &[0x1Du8, 0x00, 0xFFu8, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xC0, 0x00]
+        )
+    }
+
+    #[test]
+    fn test_target_select() {
+        let mut dap = TestDap::new(
+            MockSwdJtagDevice::new(),
+            FakeLEDs {},
+            StdDelayUs {},
+            None,
+            "test_dap",
+        );
+
+        // write 8 bits, read 5 bits, write 33 bits
+        let report = [0x1Du8, 3, 8, 0b10111101, 0x80 | 5, 33, 0x56, 0x83, 0xAB, 0x32, 0x01];
+        let mut rbuf = [0u8; 64];
+        dap.state.to_swd();
+        match &mut dap.state {
+            State::Swd(swd) => {
+                swd.expect_write_sequence()
+                    .once()
+                    .with(
+                        eq(8),
+                        eq([0b10111101u8]),
+                    )
+                    .return_const(Ok(()));
+                swd.expect_read_sequence()
+                    .once()
+                    .withf(|nbits, buf| buf.len() >= 1 && *nbits == 5)
+                    .returning(|_, buf| {
+                        buf[0] = 0x1F;
+                        Ok(())
+                    });
+                swd.expect_write_sequence()
+                    .once()
+                    .with(
+                        eq(33),
+                        eq([0x56, 0x83, 0xAB, 0x32, 0x01]),
+                    )
+                    .return_const(Ok(()));
+            }
+            _ => assert!(false, "can't switch to swd"),
+        }
+        let rsize = dap.process_command(&report, &mut rbuf, DapVersion::V2);
+        assert_eq!(rsize, 3);
+        assert_eq!(
+            &rbuf[..3],
+            &[0x1Du8, 0x00, 0x1F]
+        )
+    }
+}

src/dap/request.rs

@@ -33,6 +33,10 @@ impl<'a> Request<'a> {
         value
     }
 
+    pub fn consume(&mut self, count: usize) {
+        self.data = &self.data[count..];
+    }
+
     pub fn rest(self) -> &'a [u8] {
         &self.data
     }

src/lib.rs

@@ -1,6 +1,6 @@
 //! TODO: Crate docs
 
-#![no_std]
+#![cfg_attr(not(test), no_std)]
 #![warn(missing_docs)]
 
 /// TODO: Dap docs
@@ -11,6 +11,9 @@ pub mod swj;
 pub mod swo;
 pub mod usb;
 
+#[cfg(test)]
+mod mock_device;
+
 // Re-export the usb-device crate, so that crates depending on us can use it without
 // having to track it as a separate dependency.
 pub use usb_device;

src/mock_device.rs

@@ -0,0 +1,76 @@
+use crate::{jtag, swd, swj};
+
+#[mockall::automock]
+pub trait SwdJtagDevice {
+    // swj
+    fn high_impedance_mode(&mut self);
+    fn process_swj_clock(&mut self, max_frequency: u32) -> bool;
+    fn process_swj_pins(&mut self, output: swj::Pins, mask: swj::Pins, wait_us: u32) -> swj::Pins;
+    fn process_swj_sequence(&mut self, data: &[u8], nbits: usize);
+
+    // swd
+    fn read_inner(&mut self, apndp: swd::APnDP, a: swd::DPRegister) -> swd::Result<u32>;
+    fn write_inner(&mut self, apndp: swd::APnDP, a: swd::DPRegister, data: u32) -> swd::Result<()>;
+    fn read_sequence(&mut self, num_bits: usize, data: &mut [u8]) -> swd::Result<()>;
+    fn write_sequence(&mut self, num_bits: usize, data: &[u8]) -> swd::Result<()>;
+
+    // jtag
+    fn sequences(&mut self, data: &[u8], rxbuf: &mut [u8]) -> u32;
+
+    // swd/jtag
+    fn set_clock(&mut self, max_frequency: u32) -> bool;
+}
+
+impl swj::Dependencies<Self, Self> for MockSwdJtagDevice {
+    fn high_impedance_mode(&mut self) {
+        SwdJtagDevice::high_impedance_mode(self)
+    }
+
+    fn process_swj_clock(&mut self, max_frequency: u32) -> bool {
+        SwdJtagDevice::process_swj_clock(self, max_frequency)
+    }
+
+    fn process_swj_pins(&mut self, output: swj::Pins, mask: swj::Pins, wait_us: u32) -> swj::Pins {
+        SwdJtagDevice::process_swj_pins(self, output, mask, wait_us)
+    }
+
+    fn process_swj_sequence(&mut self, data: &[u8], nbits: usize) {
+        SwdJtagDevice::process_swj_sequence(self, data, nbits)
+    }
+}
+
+impl swd::Swd<Self> for MockSwdJtagDevice {
+    const AVAILABLE: bool = true;
+
+    fn set_clock(&mut self, max_frequency: u32) -> bool {
+        SwdJtagDevice::set_clock(self, max_frequency)
+    }
+
+    fn read_inner(&mut self, apndp: swd::APnDP, a: swd::DPRegister) -> swd::Result<u32> {
+        SwdJtagDevice::read_inner(self, apndp, a)
+    }
+
+    fn write_inner(&mut self, apndp: swd::APnDP, a: swd::DPRegister, data: u32) -> swd::Result<()> {
+        SwdJtagDevice::write_inner(self, apndp, a, data)
+    }
+
+    fn read_sequence(&mut self, num_bits: usize, data: &mut [u8]) -> swd::Result<()> {
+        SwdJtagDevice::read_sequence(self, num_bits, data)
+    }
+
+    fn write_sequence(&mut self, num_bits: usize, data: &[u8]) -> swd::Result<()> {
+        SwdJtagDevice::write_sequence(self, num_bits, data)
+    }
+}
+
+impl jtag::Jtag<MockSwdJtagDevice> for MockSwdJtagDevice {
+    const AVAILABLE: bool = true;
+
+    fn set_clock(&mut self, max_frequency: u32) -> bool {
+        SwdJtagDevice::set_clock(self, max_frequency)
+    }
+
+    fn sequences(&mut self, data: &[u8], rxbuf: &mut [u8]) -> u32 {
+        SwdJtagDevice::sequences(self, data, rxbuf)
+    }
+}

src/swd.rs

@@ -152,6 +152,12 @@ pub trait Swd<DEPS>: From<DEPS> {
 
     /// Set the maximum clock frequency, return `true` if it is valid.
     fn set_clock(&mut self, max_frequency: u32) -> bool;
+
+    /// Write a sequence of bits using SWDIO and the clock line running at the configured freq.
+    fn write_sequence(&mut self, num_bits: usize, data: &[u8]) -> Result<()>;
+
+    /// Read a sequence of bits using SWDIO and the clock line running at the configured freq.
+    fn read_sequence(&mut self, num_bits: usize, data: &mut [u8]) -> Result<()>;
 }
 
 /// Helper used by `Swd::read_inner` and `Swd::write_inner` to make the request byte.

src/swo.rs

@@ -42,6 +42,7 @@ pub struct SwoStatus {
     pub bytes_available: u32,
 }
 
+#[cfg_attr(test, mockall::automock)]
 pub trait Swo {
     fn set_transport(&mut self, transport: SwoTransport);
     fn set_mode(&mut self, mode: SwoMode);