1 files changed, 11 insertions, 20 deletions

diff --git a/embassy-stm32/src/dsihost.rs b/embassy-stm32/src/dsihost.rs
index deda956af..b8945820c 100644
--- a/embassy-stm32/src/dsihost.rs
+++ b/embassy-stm32/src/dsihost.rs
@@ -5,17 +5,10 @@ use core::marker::PhantomData;
 use embassy_hal_internal::PeripheralType;
 
 //use crate::gpio::{AnyPin, SealedPin};
+use crate::block_for_us;
 use crate::gpio::{AfType, AnyPin, OutputType, Speed};
 use crate::rcc::{self, RccPeripheral};
-use crate::{peripherals, Peri};
-
-/// Performs a busy-wait delay for a specified number of microseconds.
-pub fn blocking_delay_ms(ms: u32) {
-    #[cfg(feature = "time")]
-    embassy_time::block_for(embassy_time::Duration::from_millis(ms as u64));
-    #[cfg(not(feature = "time"))]
-    cortex_m::asm::delay(unsafe { crate::rcc::get_freqs() }.sys.to_hertz().unwrap().0 / 1_000 * ms);
-}
+use crate::{Peri, peripherals};
 
 /// PacketTypes extracted from CubeMX
 #[repr(u8)]
@@ -121,17 +114,15 @@ impl<'d, T: Instance> DsiHost<'d, T> {
 
     /// DCS or Generic short/long write command
     pub fn write_cmd(&mut self, channel_id: u8, address: u8, data: &[u8]) -> Result<(), Error> {
-        assert!(data.len() > 0);
-
-        if data.len() == 1 {
-            self.short_write(channel_id, PacketType::DcsShortPktWriteP1, address, data[0])
-        } else {
-            self.long_write(
+        match data.len() {
+            0 => self.short_write(channel_id, PacketType::DcsShortPktWriteP0, address, 0),
+            1 => self.short_write(channel_id, PacketType::DcsShortPktWriteP1, address, data[0]),
+            _ => self.long_write(
                 channel_id,
                 PacketType::DcsLongPktWrite, // FIXME: This might be a generic long packet, as well...
                 address,
                 data,
-            )
+            ),
         }
     }
 
@@ -336,7 +327,7 @@ impl<'d, T: Instance> DsiHost<'d, T> {
             if T::regs().gpsr().read().cmdfe() {
                 return Ok(());
             }
-            blocking_delay_ms(1);
+            block_for_us(1_000);
         }
         Err(Error::FifoTimeout)
     }
@@ -347,7 +338,7 @@ impl<'d, T: Instance> DsiHost<'d, T> {
             if !T::regs().gpsr().read().cmdff() {
                 return Ok(());
             }
-            blocking_delay_ms(1);
+            block_for_us(1_000);
         }
         Err(Error::FifoTimeout)
     }
@@ -358,7 +349,7 @@ impl<'d, T: Instance> DsiHost<'d, T> {
             if !self.read_busy() {
                 return Ok(());
             }
-            blocking_delay_ms(1);
+            block_for_us(1_000);
         }
         Err(Error::ReadTimeout)
     }
@@ -369,7 +360,7 @@ impl<'d, T: Instance> DsiHost<'d, T> {
             if !T::regs().gpsr().read().prdfe() {
                 return Ok(());
             }
-            blocking_delay_ms(1);
+            block_for_us(1_000);
         }
         Err(Error::FifoTimeout)
     }