From 296783fd57fb8954b4051754fd88880026a0b0c4 Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Sun, 23 May 2021 10:40:17 +0000
Subject: [PATCH] Fixed HSISYS calculation and missing SWs.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14422 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/hal/ports/STM32/STM32G0xx/hal_lld.c | 33 ++++++++++++++++---------
 os/hal/ports/STM32/STM32G0xx/hal_lld.h | 34 ++++++++++++++------------
 2 files changed, 40 insertions(+), 27 deletions(-)

diff --git a/os/hal/ports/STM32/STM32G0xx/hal_lld.c b/os/hal/ports/STM32/STM32G0xx/hal_lld.c
index 07fbfd2d9..a7e3bdd7a 100644
--- a/os/hal/ports/STM32/STM32G0xx/hal_lld.c
+++ b/os/hal/ports/STM32/STM32G0xx/hal_lld.c
@@ -215,10 +215,10 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
                                        2U, 4U, 8U, 16U, 64U, 128U, 256U, 512U};
   static const uint32_t pprediv[16] = {1U, 1U, 1U, 1U, 2U, 4U, 8U, 16U};
   const system_limits_t *slp;
-  halfreq_t hsi16clk = 0U, hseclk = 0U, pllselclk;
+  halfreq_t hsi16clk = 0U, hseclk = 0U, pllselclk, hsisysclk;
   halfreq_t pllpclk = 0U, pllqclk = 0U, pllrclk = 0U;
   halfreq_t sysclk, hclk, pclk, pclktim, mcoclk;
-  uint32_t mcodiv, flashws;
+  uint32_t mcodiv, flashws, hsidiv;
 
   /* System limits based on desired VOS settings.*/
   if ((ccp->pwr_cr1 & PWR_CR1_VOS_Msk) == PWR_CR1_VOS_1) {
@@ -236,6 +236,10 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
     hsi16clk = STM32_HSI16CLK;
   }
 
+  /* HSISYS clock.*/
+  hsidiv = 1U << ((ccp->pwr_cr1 & RCC_CR_HSIDIV_Msk) >> RCC_CR_HSIDIV_Pos);
+  hsisysclk = hsi16clk / hsidiv;
+
   /* HSE clock.*/
   if ((ccp->rcc_cr & RCC_CR_HSEON) != 0U) {
     hseclk = STM32_HSECLK;
@@ -308,7 +312,7 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   /* SYSCLK frequency.*/
   switch(ccp->rcc_cfgr & RCC_CFGR_SW_Msk) {
   case RCC_CFGR_SW_HSI:
-    sysclk = hsi16clk;
+    sysclk = hsisysclk;
     break;
   case RCC_CFGR_SW_HSE:
     sysclk = hseclk;
@@ -316,6 +320,12 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   case RCC_CFGR_SW_PLL:
     sysclk = pllrclk;
     break;
+  case RCC_CFGR_SW_LSI:
+    sysclk = STM32_LSICLK;
+    break;
+  case RCC_CFGR_SW_LSE:
+    sysclk = STM32_LSECLK;
+    break;
   default:
     sysclk = 0U;
   }
@@ -378,14 +388,15 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   }
 
   /* Writing out results.*/
-  clock_points[CLK_SYSCLK]   = sysclk;
-  clock_points[CLK_PLLPCLK]  = pllpclk;
-  clock_points[CLK_PLLQCLK]  = pllqclk;
-  clock_points[CLK_PLLRCLK]  = pllrclk;
-  clock_points[CLK_HCLK]     = hclk;
-  clock_points[CLK_PCLK]     = pclk;
-  clock_points[CLK_PCLKTIM]  = pclktim;
-  clock_points[CLK_MCO]      = mcoclk;
+  clock_points[CLK_SYSCLK]    = sysclk;
+  clock_points[CLK_HSISYSCLK] = hsisysclk;
+  clock_points[CLK_PLLPCLK]   = pllpclk;
+  clock_points[CLK_PLLQCLK]   = pllqclk;
+  clock_points[CLK_PLLRCLK]   = pllrclk;
+  clock_points[CLK_HCLK]      = hclk;
+  clock_points[CLK_PCLK]      = pclk;
+  clock_points[CLK_PCLKTIM]   = pclktim;
+  clock_points[CLK_MCO]       = mcoclk;
 
   return false;
 }
diff --git a/os/hal/ports/STM32/STM32G0xx/hal_lld.h b/os/hal/ports/STM32/STM32G0xx/hal_lld.h
index 3022b51b4..f295a0efa 100644
--- a/os/hal/ports/STM32/STM32G0xx/hal_lld.h
+++ b/os/hal/ports/STM32/STM32G0xx/hal_lld.h
@@ -73,14 +73,15 @@
  * @{
  */
 #define CLK_SYSCLK              0U
-#define CLK_PLLPCLK             1U
-#define CLK_PLLQCLK             2U
-#define CLK_PLLRCLK             3U
-#define CLK_HCLK                4U
-#define CLK_PCLK                5U
-#define CLK_PCLKTIM             6U
-#define CLK_MCO                 7U
-#define CLK_ARRAY_SIZE          8U
+#define CLK_HSISYSCLK           1U
+#define CLK_PLLPCLK             2U
+#define CLK_PLLQCLK             3U
+#define CLK_PLLRCLK             4U
+#define CLK_HCLK                5U
+#define CLK_PCLK                6U
+#define CLK_PCLKTIM             7U
+#define CLK_MCO                 8U
+#define CLK_ARRAY_SIZE          9U
 /** @} */
 
 /**
@@ -1490,14 +1491,15 @@ typedef struct {
  * @notapi
  */
 #define hal_lld_get_clock_point(clkpt)                                      \
-  ((clkpt) == CLK_SYSCLK   ? STM32_SYSCLK        :                          \
-   (clkpt) == CLK_PLLPCLK  ? STM32_PLL_P_CLKOUT  :                          \
-   (clkpt) == CLK_PLLQCLK  ? STM32_PLL_Q_CLKOUT  :                          \
-   (clkpt) == CLK_PLLRCLK  ? STM32_PLL_R_CLKOUT  :                          \
-   (clkpt) == CLK_HCLK     ? STM32_HCLK          :                          \
-   (clkpt) == CLK_PCLK     ? STM32_PCLK          :                          \
-   (clkpt) == CLK_PCLKTIM  ? STM32_TIMPCLK       :                          \
-   (clkpt) == CLK_MCO      ? STM32_MCOCLK        :                          \
+  ((clkpt) == CLK_SYSCLK    ? STM32_SYSCLK        :                         \
+   (clkpt) == CLK_HSISYSCLK ? STM32_HSISYSCLK     :                         \
+   (clkpt) == CLK_PLLPCLK   ? STM32_PLL_P_CLKOUT  :                         \
+   (clkpt) == CLK_PLLQCLK   ? STM32_PLL_Q_CLKOUT  :                         \
+   (clkpt) == CLK_PLLRCLK   ? STM32_PLL_R_CLKOUT  :                         \
+   (clkpt) == CLK_HCLK      ? STM32_HCLK          :                         \
+   (clkpt) == CLK_PCLK      ? STM32_PCLK          :                         \
+   (clkpt) == CLK_PCLKTIM   ? STM32_TIMPCLK       :                         \
+   (clkpt) == CLK_MCO       ? STM32_MCOCLK        :                         \
    0U)
 #endif /* !defined(HAL_LLD_USE_CLOCK_MANAGEMENT) */