From 118014dfcb4f27753d52c8941cb44b3505a16cac Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Sun, 16 May 2021 17:39:04 +0000
Subject: [PATCH] Fixed wrong sysclk check, added wait states check.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@14382 27425a3e-05d8-49a3-a47f-9c15f0e5edd8
---
 os/hal/ports/STM32/STM32G4xx/hal_lld.c | 21 +++++++++++++++++----
 1 file changed, 17 insertions(+), 4 deletions(-)

diff --git a/os/hal/ports/STM32/STM32G4xx/hal_lld.c b/os/hal/ports/STM32/STM32G4xx/hal_lld.c
index 876307c5b..a0a1b6dec 100644
--- a/os/hal/ports/STM32/STM32G4xx/hal_lld.c
+++ b/os/hal/ports/STM32/STM32G4xx/hal_lld.c
@@ -28,6 +28,11 @@
 /* Driver local definitions.                                                 */
 /*===========================================================================*/
 
+/**
+ * @brief   Number of thresholds in the wait states array.
+ */
+#define STM32_WS_THRESHOLDS             9
+
 /*===========================================================================*/
 /* Driver exported variables.                                                */
 /*===========================================================================*/
@@ -116,7 +121,7 @@ typedef struct {
   halfreq_t     pllq_min;
   halfreq_t     pllr_max;
   halfreq_t     pllr_min;
-  halfreq_t     flash_thresholds[9];
+  halfreq_t     flash_thresholds[STM32_WS_THRESHOLDS];
 } system_limits_t;
 
 /**
@@ -187,7 +192,7 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   halfreq_t hsi16clk = 0U, hseclk = 0U, pllselclk;
   halfreq_t pllpclk = 0U, pllqclk = 0U, pllrclk = 0U;
   halfreq_t sysclk, hclk, pclk1, pclk2, pclk1tim, pclk2tim, mcoclk;
-  uint32_t mcodiv;
+  uint32_t mcodiv, flashws;
 
   /* System limits based on desired VOS settings.*/
   if ((ccp->pwr_cr1 & PWR_CR1_VOS_Msk) == PWR_CR1_VOS_1) {
@@ -301,12 +306,12 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   }
 
   if ((ccp->pwr_cr5 & PWR_CR5_R1MODE) == 0U) {
-    if (sysclk < slp->sysclk_max_boost) {
+    if (sysclk > slp->sysclk_max_boost) {
       return true;
     }
   }
   else {
-    if (sysclk < slp->sysclk_max_noboost) {
+    if (sysclk > slp->sysclk_max_noboost) {
       return true;
     }
   }
@@ -367,6 +372,14 @@ static bool hal_lld_clock_check_tree(const halclkcfg_t *ccp) {
   }
   mcoclk /= mcodiv;
 
+  /* Flash settings.*/
+  flashws = ((ccp->flash_acr & FLASH_ACR_LATENCY_Msk) >> FLASH_ACR_LATENCY_Pos);
+  if (flashws >= STM32_WS_THRESHOLDS) {
+    return true;
+  }  if (hclk > slp->flash_thresholds[flashws]) {
+    return true;
+  }
+
   /* Writing out results.*/
   clock_points[CLK_SYSCLK]   = sysclk;
   clock_points[CLK_PLLPCLK]  = pllpclk;