Fix SWJ_Pins

Change the SWJ_Pins command to wait until a desired state is reached
instead of unconditionally wait for the maximum duration.
This will make so that SWJ_Pins does not panic anymore.

Author: korken89

src/dap.rs

@@ -132,7 +132,29 @@ impl swj::Dependencies<Swd, Jtag> for Context {
             }
         }
 
-        self.delay.delay_ticks(self.cycles_per_us * wait_us);
+        // Delay until desired output state or timeout.
+        let mut last = self.delay.get_current();
+        for _ in 0..wait_us {
+            last = self.delay.delay_ticks_from_last(self.cycles_per_us, last);
+
+            // If a pin is selected, make sure its output equals the desired output state, or else
+            // continue waiting.
+            let swclk_not_in_desired_state = mask.contains(swj::Pins::SWCLK)
+                && output.contains(swj::Pins::SWCLK) != self.swclk.is_high();
+            let swdio_not_in_desired_state = mask.contains(swj::Pins::SWDIO)
+                && output.contains(swj::Pins::SWDIO) != self.swdio.is_high();
+            let nreset_not_in_desired_state = mask.contains(swj::Pins::NRESET)
+                && output.contains(swj::Pins::NRESET) != self.nreset.is_high();
+
+            if swclk_not_in_desired_state
+                || swdio_not_in_desired_state
+                || nreset_not_in_desired_state
+            {
+                continue;
+            }
+
+            break;
+        }
 
         let mut ret = swj::Pins::empty();
         ret.set(swj::Pins::SWCLK, self.swclk.is_high());

src/setup.rs

@@ -4,7 +4,7 @@ use crate::systick_delay::Delay;
 use crate::{dap, usb::ProbeUsb};
 use core::mem::MaybeUninit;
 use dap_rs::usb_device::class_prelude::UsbBusAllocator;
-use embedded_hal::digital::{InputPin, OutputPin};
+use embedded_hal::digital::{InputPin, OutputPin, StatefulOutputPin};
 use embedded_hal::pwm::SetDutyCycle;
 use embedded_hal_02::adc::OneShot;
 use replace_with::replace_with_or_abort_unchecked;
@@ -303,13 +303,11 @@ where
         }
     }
 
-    /// Note: This function panics if the pin is in the wrong mode.
+    /// Note: If the pin is an output this will only reflect the set state, NOT electrical state.
     pub fn is_high(&mut self) -> bool {
         match self {
             DynPin::Input(i) => i.is_high() == Ok(true),
-            DynPin::Output(_) => {
-                defmt::panic!("Input operation on output pin");
-            }
+            DynPin::Output(o) => o.is_set_high() == Ok(true),
         }
     }
 }