fix: core was not recognized and 1-step lock did not work

Author: ckrenslehner

embassy-stm32/src/hsem/mod.rs

@@ -1,10 +1,11 @@
 //! Hardware Semaphore (HSEM)
 
 use embassy_hal_internal::PeripheralType;
+use stm32_metapac::hsem::regs::Rlr;
 
 use crate::pac;
 use crate::rcc::RccPeripheral;
-// TODO: This code works for all HSEM implemenations except for the STM32WBA52/4/5xx MCUs.
+// TODO: This code works for all HSEM implementations except for the STM32WBA52/4/5xx MCUs.
 // Those MCUs have a different HSEM implementation (Secure semaphore lock support,
 // Privileged / unprivileged semaphore lock support, Semaphore lock protection via semaphore attribute),
 // which is not yet supported by this code.
@@ -17,47 +18,135 @@ pub enum HsemError {
     LockFailed,
 }
 
+/// HSEM identifier
+/// Copied from `hw_conf.h` of the STM32WB55 WPAN stack
+#[cfg(any(stm32wb))]
+#[derive(Debug, PartialEq, Eq, Clone, Copy, Hash)]
+#[repr(u8)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum HsemIdStm32Wb {
+    /// Semaphore to lock the RNG access
+    Rng = 0,
+    /// Semaphore to lock the PKA access
+    Pka = 1,
+    /// Semaphore to lock the FLASH access
+    Flash = 2,
+    /// Semaphore to lock the RCC access
+    Rcc = 3,
+    /// Semaphore to synchronize the entry stop mode
+    EntryStopMode = 4,
+    /// Semaphore to lock the Clk48 access
+    Clk48Config = 5,
+    /// Semaphore to sync blockwise flash access CPU1
+    BlockFlashRequestByCpu1 = 6,
+    /// Semaphore to sync blockwise flash access CPU2
+    BlockFlashRequestByCpu2 = 7,
+    /// Semaphore to lock the BLE NVM access
+    BleNvmSram = 8,
+    /// Semaphore to lock the Thread NVM access
+    ThreadNvmSram = 9,
+}
+
+#[cfg(any(stm32wb))]
+impl From<HsemIdStm32Wb> for u8 {
+    fn from(id: HsemIdStm32Wb) -> Self {
+        id as u8
+    }
+}
+
 /// CPU core.
 /// The enum values are identical to the bus master IDs / core Ids defined for each
 /// chip family (i.e. stm32h747 see rm0399 table 95)
 #[derive(Debug, PartialEq, Eq, Clone, Copy, Hash)]
 #[repr(u8)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub enum CoreId {
+    /// Main processor
+    Core0,
+    /// Coprocessor
+    Core1,
+}
+
+/// The core ID used in the HSEM_RLRx register.
+#[derive(Debug, PartialEq, Eq, Clone, Copy, Hash)]
+#[repr(u8)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum RlrCoreId {
+    /// Main processor (Cortex M4) core ID
+    #[cfg(any(stm32wb, stm32wl))]
+    Core0 = 0x4,
+
+    /// Main processor (Cortex M7) core ID
     #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
-    /// Cortex-M7, core 1.
     Core0 = 0x3,
 
+    /// Coprocessor (Cortex M0) core ID
+    #[cfg(any(stm32wb, stm32wl))]
+    Core1 = 0x8,
+
+    /// Coprocessor (Cortex M4) core ID
     #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
-    /// Cortex-M4, core 2.
     Core1 = 0x1,
+}
 
-    #[cfg(not(any(stm32h745, stm32h747, stm32h755, stm32h757)))]
-    /// Cortex-M4, core 1
-    Core0 = 0x4,
+impl TryFrom<u8> for RlrCoreId {
+    type Error = ();
 
-    #[cfg(any(stm32wb, stm32wl))]
-    /// Cortex-M0+, core 2.
-    Core1 = 0x8,
+    fn try_from(value: u8) -> Result<Self, Self::Error> {
+        #[cfg(any(stm32wb, stm32wl))]
+        match value {
+            0x4 => Ok(RlrCoreId::Core0),
+            0x8 => Ok(RlrCoreId::Core1),
+            _ => Err(()),
+        }
+
+        #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
+        match value {
+            0x3 => Ok(RlrCoreId::Core0),
+            0x1 => Ok(RlrCoreId::Core1),
+            _ => Err(()),
+        }
+    }
 }
 
-/// Get the current core id
-/// This code assume that it is only executed on a Cortex-M M0+, M4 or M7 core.
+impl From<CoreId> for RlrCoreId {
+    fn from(core: CoreId) -> Self {
+        match core {
+            CoreId::Core0 => RlrCoreId::Core0,
+            CoreId::Core1 => RlrCoreId::Core1,
+        }
+    }
+}
+
+/// Get the current core id.
 #[inline(always)]
 pub fn get_current_coreid() -> CoreId {
-    let cpuid = unsafe { cortex_m::peripheral::CPUID::PTR.read_volatile().base.read() };
-    match cpuid & 0x000000F0 {
-        #[cfg(any(stm32wb, stm32wl))]
-        0x0 => CoreId::Core1,
+    const CPUID_PARTNO_MASK: u32 = 0x0000FFF0;
+    // From the arm developer manual
+    // https://developer.arm.com/documentation/ddi0439/b/System-Control/Register-descriptions/CPUID-Base-Register--CPUID
+    #[allow(dead_code)]
+    const CPUID_PARTNO_CORTEX_M4: u32 = 0x0000C240;
+    #[allow(dead_code)]
+    const CPUID_PARTNO_CORTEX_M7: u32 = 0x0000C270;
+    #[allow(dead_code)]
+    const CPUID_PARTNO_CORTEX_M0: u32 = 0x0000C600;
+    // const CPUID_PARTNO_CORTEX_M33: u32 = 0x0000D210;
 
-        #[cfg(not(any(stm32h745, stm32h747, stm32h755, stm32h757)))]
-        0x4 => CoreId::Core0,
+    let cpuid = unsafe { cortex_m::peripheral::CPUID::PTR.read_volatile().base.read() };
 
-        #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
-        0x4 => CoreId::Core1,
+    #[cfg(any(stm32wb, stm32wl))]
+    // Cortex M4 as main processor and Cortex M0 as coprocessor
+    match cpuid & CPUID_PARTNO_MASK {
+        CPUID_PARTNO_CORTEX_M4 => CoreId::Core0,
+        CPUID_PARTNO_CORTEX_M0 => CoreId::Core1,
+        _ => panic!("Unknown Cortex-M core"),
+    }
 
-        #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
-        0x7 => CoreId::Core0,
+    #[cfg(any(stm32h745, stm32h747, stm32h755, stm32h757))]
+    // Cortex M7 as main processor and Cortex M4 as coprocessor
+    match cpuid & CPUID_PARTNO_MASK {
+        CPUID_PARTNO_CORTEX_M7 => CoreId::Core0,
+        CPUID_PARTNO_CORTEX_M4 => CoreId::Core1,
         _ => panic!("Unknown Cortex-M core"),
     }
 }
@@ -89,13 +178,13 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     pub fn two_step_lock(&mut self, sem_id: u8, process_id: u8) -> Result<(), HsemError> {
         T::regs().r(sem_id as usize).write(|w| {
             w.set_procid(process_id);
-            w.set_coreid(get_current_coreid() as u8);
+            w.set_coreid(RlrCoreId::from(get_current_coreid()) as u8);
             w.set_lock(true);
         });
         let reg = T::regs().r(sem_id as usize).read();
         match (
             reg.lock(),
-            reg.coreid() == get_current_coreid() as u8,
+            reg.coreid() == RlrCoreId::from(get_current_coreid()) as u8,
             reg.procid() == process_id,
         ) {
             (true, true, true) => Ok(()),
@@ -108,8 +197,13 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     /// carried out from the HSEM_RLRx register.
     pub fn one_step_lock(&mut self, sem_id: u8) -> Result<(), HsemError> {
         let reg = T::regs().rlr(sem_id as usize).read();
-        match (reg.lock(), reg.coreid() == get_current_coreid() as u8, reg.procid()) {
-            (false, true, 0) => Ok(()),
+
+        match (
+            reg.lock(),
+            reg.coreid() == RlrCoreId::from(get_current_coreid()) as u8,
+            reg.procid(),
+        ) {
+            (true, true, 0) => Ok(()),
             _ => Err(HsemError::LockFailed),
         }
     }
@@ -120,7 +214,7 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     pub fn unlock(&mut self, sem_id: u8, process_id: u8) {
         T::regs().r(sem_id as usize).write(|w| {
             w.set_procid(process_id);
-            w.set_coreid(get_current_coreid() as u8);
+            w.set_coreid(RlrCoreId::from(get_current_coreid()) as u8);
             w.set_lock(false);
         });
     }
@@ -129,10 +223,10 @@ impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     /// All semaphores locked by a COREID can be unlocked at once by using the HSEM_CR
     /// register. Write COREID and correct KEY value in HSEM_CR. All locked semaphores with a
     /// matching COREID are unlocked, and may generate an interrupt when enabled.
-    pub fn unlock_all(&mut self, key: u16, core_id: u8) {
+    pub fn unlock_all(&mut self, key: u16, core_id: CoreId) {
         T::regs().cr().write(|w| {
             w.set_key(key);
-            w.set_coreid(core_id);
+            w.set_coreid(RlrCoreId::from(core_id) as u8);
         });
     }