Merge pull request #4324 from plorefice/hsem-fixes

embassy-stm32: fix HSEM initialization and core detection

1 files changed, 4 insertions, 2 deletions

diff --git a/embassy-stm32/src/hsem/mod.rs b/embassy-stm32/src/hsem/mod.rs
index 31527bcdb..573a1851d 100644
--- a/embassy-stm32/src/hsem/mod.rs
+++ b/embassy-stm32/src/hsem/mod.rs
@@ -3,7 +3,7 @@
 use embassy_hal_internal::PeripheralType;
 
 use crate::pac;
-use crate::rcc::RccPeripheral;
+use crate::rcc::{self, RccPeripheral};
 // TODO: This code works for all HSEM implemenations 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),
@@ -46,7 +46,7 @@ pub enum CoreId {
 #[inline(always)]
 pub fn get_current_coreid() -> CoreId {
     let cpuid = unsafe { cortex_m::peripheral::CPUID::PTR.read_volatile().base.read() };
-    match cpuid & 0x000000F0 {
+    match (cpuid & 0x000000F0) >> 4 {
         #[cfg(any(stm32wb, stm32wl))]
         0x0 => CoreId::Core1,
 
@@ -80,6 +80,8 @@ pub struct HardwareSemaphore<'d, T: Instance> {
 impl<'d, T: Instance> HardwareSemaphore<'d, T> {
     /// Creates a new HardwareSemaphore instance.
     pub fn new(peripheral: Peri<'d, T>) -> Self {
+        rcc::enable_and_reset::<T>();
+
         HardwareSemaphore { _peri: peripheral }
     }