[WB] Add hardware semaphore management

Signed-off-by: Frederic Pillon <[email protected]>
stm32duino · Feb 3, 2021 · 959bc1f · 959bc1f
1 parent 96801d8
commit 959bc1f
Show file tree

Hide file tree

Showing 2 changed files with 16 additions and 8 deletions.
diff --git a/cores/arduino/stm32/lock_resource.h b/cores/arduino/stm32/lock_resource.h
@@ -136,14 +136,14 @@ typedef enum {
 static inline void hsem_lock(uint32_t semID, uint32_t retry)
 {
 #if defined(STM32MP1xx) || defined(STM32WBxx)
-	while (LL_HSEM_1StepLock(HSEM, semID)) {
-		if (retry != HSEM_LOCK_WAIT_FOREVER) {
+  while (LL_HSEM_1StepLock(HSEM, semID)) {
+    if (retry != HSEM_LOCK_WAIT_FOREVER) {
       retry--;
-			if (retry == 0) {
-				Error_Handler();
-			}
-		}
-	}
+      if (retry == 0) {
+        Error_Handler();
+      }
+    }
+  }
 #endif /* STM32MP1xx || STM32WBxx */
 }
 
@@ -155,7 +155,7 @@ static inline void hsem_lock(uint32_t semID, uint32_t retry)
 static inline void hsem_unlock(uint32_t semID)
 {
 #if defined(STM32MP1xx) || defined(STM32WBxx)
-	LL_HSEM_ReleaseLock(HSEM, semID, 0);
+  LL_HSEM_ReleaseLock(HSEM, semID, 0);
 #endif /* STM32MP1xx || STM32WBxx */
 }
 

diff --git a/variants/PNUCLEO_WB55RG/variant.cpp b/variants/PNUCLEO_WB55RG/variant.cpp
@@ -17,6 +17,7 @@
 */
 
 #include "pins_arduino.h"
+#include "lock_resource.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -99,9 +100,15 @@ WEAK void SystemClock_Config(void)
   RCC_ClkInitTypeDef RCC_ClkInitStruct = {};
   RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {};
 
+  /* This prevents the CPU2 (M0+) to configure RCC */
+  hsem_lock(CFG_HW_RCC_SEMID, HSEM_LOCK_DEFAULT_RETRY);
+
   __HAL_RCC_LSEDRIVE_CONFIG(RCC_LSEDRIVE_LOW);
   __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
 
+  /* This prevents the CPU2 (M0+) to disable the HSI48 oscillator */
+  hsem_lock(CFG_HW_CLK48_CONFIG_SEMID, HSEM_LOCK_DEFAULT_RETRY);
+
   /* Initializes the CPU, AHB and APB busses clocks */
   RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_HSI48
                                      | RCC_OSCILLATORTYPE_HSE | RCC_OSCILLATORTYPE_LSE;
@@ -149,6 +156,7 @@ WEAK void SystemClock_Config(void)
   /* Select HSI as system clock source after Wake Up from Stop mode */
   LL_RCC_SetClkAfterWakeFromStop(LL_RCC_STOP_WAKEUPCLOCK_HSI);
 
+  hsem_unlock(CFG_HW_RCC_SEMID);
 }
 
 #ifdef __cplusplus