Extend stm32f769 clock control driver

 * Implement clock rate get proc
 * Add init proc to initialize external xtal/clkgen and pll

3 files changed, 179 insertions, 20 deletions

diff --git a/os/drivers/clock/stm32f769_clock_control.c b/os/drivers/clock/stm32f769_clock_control.c
index d6c0a0f9..d62b5799 100644
--- a/os/drivers/clock/stm32f769_clock_control.c
+++ b/os/drivers/clock/stm32f769_clock_control.c
@@ -6,40 +6,196 @@
 #include "stm32f769_clocks.h"
 
 #define STM32F769_CLOCK_ID_OFFSET(id) (((id) >> 6U) & 0xFFU)
-#define STM32F769_CLOCK_ID_BIT(id)	 ((id)&0x1FU)
+#define STM32F769_CLOCK_ID_BIT(id)   ((id)&0x1FU)
 
-int stm32f7_clock_control_on(uint16_t id) {
-	sys_set_bit(RCC_BASE + STM32F769_CLOCK_ID_OFFSET(id),
+volatile uint64_t uptime_ctr;
+
+void SysTick_Handler(void) {
+  uptime_ctr ++;
+}
+
+int stm32f769_clock_control_on(uint16_t id) {
+  sys_set_bit(RCC_BASE + STM32F769_CLOCK_ID_OFFSET(id),
               STM32F769_CLOCK_ID_BIT(id));
   int ret = sys_test_bit(RCC_BASE + STM32F769_CLOCK_ID_OFFSET(id),
-               STM32F769_CLOCK_ID_BIT(id));
+                         STM32F769_CLOCK_ID_BIT(id));
   (void)ret;
-	return 0;
+  return 0;
 }
 
-int stm32f7_clock_control_off(uint16_t id) {
-	sys_clear_bit(RCC_BASE + STM32F769_CLOCK_ID_OFFSET(id),
-              STM32F769_CLOCK_ID_BIT(id));
+int stm32f769_clock_control_off(uint16_t id) {
+  sys_clear_bit(RCC_BASE + STM32F769_CLOCK_ID_OFFSET(id),
+                STM32F769_CLOCK_ID_BIT(id));
   int ret = sys_test_bit(RCC_BASE + STM32F769_CLOCK_ID_OFFSET(id),
-               STM32F769_CLOCK_ID_BIT(id));
+                         STM32F769_CLOCK_ID_BIT(id));
   (void)ret;
 
-	return 0;
+  return 0;
+}
+
+static uint32_t __get_ahb_div(uint32_t val) {
+  switch (val) {
+  case RCC_CFGR_HPRE_DIV2:
+    return 2;
+  case RCC_CFGR_HPRE_DIV4:
+    return 4;
+  case RCC_CFGR_HPRE_DIV8:
+    return 8;
+  case RCC_CFGR_HPRE_DIV16:
+    return 16;
+  case RCC_CFGR_HPRE_DIV64:
+    return 64;
+  case RCC_CFGR_HPRE_DIV128:
+    return 128;
+  case RCC_CFGR_HPRE_DIV256:
+    return 256;
+  case RCC_CFGR_HPRE_DIV512:
+    return 512;
+  case RCC_CFGR_HPRE_DIV1:
+  default:
+    return 1;
+  }
+}
+
+static uint32_t __get_apb_div(uint32_t val) {
+  switch (val) {
+  case RCC_CFGR_PPRE1_DIV2:
+    return 2;
+  case RCC_CFGR_PPRE1_DIV4:
+    return 4;
+  case RCC_CFGR_PPRE1_DIV8:
+    return 8;
+  case RCC_CFGR_PPRE1_DIV16:
+    return 16;
+  case RCC_CFGR_PPRE1_DIV1:
+  default:
+    return 1;
+  }
 }
 
-int stm32f7_clock_control_get_rate(uint16_t id,
-                                 uint32_t *rate)
+int stm32f769_clock_control_get_rate(uint16_t id, uint32_t *rate)
 {
-	return 0;
+  uint32_t clk_offt = STM32F769_CLOCK_ID_OFFSET(id);
+  uint32_t ahb_div = __get_ahb_div(RCC->CFGR & RCC_CFGR_HPRE_Msk);
+  uint32_t apb1_div = __get_apb_div(RCC->CFGR & RCC_CFGR_PPRE1_Msk);
+  uint32_t apb2_div = __get_apb_div(RCC->CFGR & RCC_CFGR_PPRE2_Msk);
+  uint32_t ahb_rate = CONFIG_BOARD_SYS_CLK / ahb_div;
+
+  switch (clk_offt) {
+  case STM32F769_RCC_SYS_SET_OFFSET:
+    *rate = CONFIG_BOARD_SYS_CLK;
+    break;
+  case STM32F769_RCC_AHB1_SET_OFFSET:
+    *rate = ahb_rate;
+    break;
+  case STM32F769_RCC_AHB2_SET_OFFSET:
+    *rate = ahb_rate;
+    break;
+  case STM32F769_RCC_AHB3_SET_OFFSET:
+    *rate = ahb_rate;
+    break;
+  case STM32F769_RCC_APB1_SET_OFFSET:
+    *rate = ahb_rate / apb1_div;
+    break;
+  case STM32F769_RCC_APB2_SET_OFFSET:
+    *rate = ahb_rate / apb2_div;
+    break;
+  default:
+    return -ENOTSUP;
+  }
+  return 0;
+}
+
+#define __HAL_RCC_GET_SYSCLK_SOURCE() (RCC->CFGR & RCC_CFGR_SWS)
+#define RCC_FLAG_MASK  ((uint8_t)0x1F)
+#define __HAL_RCC_GET_FLAG(__FLAG__) (((((((__FLAG__) >> 5) == 1)? RCC->CR :((((__FLAG__) >> 5) == 2) ? RCC->BDCR :((((__FLAG__) >> 5) == 3)? RCC->CSR :RCC->CIR))) & ((uint32_t)1 << ((__FLAG__) & RCC_FLAG_MASK)))!= 0)? 1 : 0)
+
+#define __HAL_RCC_PLL_CONFIG(__RCC_PLLSource__, __PLLM__, __PLLN__, __PLLP__, __PLLQ__,__PLLR__) \
+  (RCC->PLLCFGR = ((__RCC_PLLSource__) | (__PLLM__)                   | \
+                   ((__PLLN__) << RCC_PLLCFGR_PLLN_Pos)                      | \
+                   ((((__PLLP__) >> 1) -1) << RCC_PLLCFGR_PLLP_Pos)          | \
+                   ((__PLLQ__) << RCC_PLLCFGR_PLLQ_Pos)                      | \
+                   ((__PLLR__) << RCC_PLLCFGR_PLLR_Pos)))
+
+#define __HAL_RCC_PLL_ENABLE() SET_BIT(RCC->CR, RCC_CR_PLLON)
+#define __HAL_RCC_PLL_DISABLE() CLEAR_BIT(RCC->CR, RCC_CR_PLLON)
+
+#define HSE_STARTUP_TIMEOUT    100U   /*!< Time out for HSE start up, in ms */
+#define PLL_TIMEOUT_VALUE 2
+
+int stm32f769_clock_control_init(struct device *dev)
+{
+  uint32_t tickstart;
+  uint32_t pll_config;
+  FlagStatus pwrclkchanged = RESET;
+
+  if ((__HAL_RCC_GET_SYSCLK_SOURCE() == RCC_SYSCLKSOURCE_STATUS_HSE) || ((__HAL_RCC_GET_SYSCLK_SOURCE() == RCC_SYSCLKSOURCE_STATUS_PLLCLK) && ((RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) == RCC_PLLCFGR_PLLSRC_HSE))) {
+    if ((__HAL_RCC_GET_FLAG(RCC_FLAG_HSERDY) != RESET) && (RCC_OscInitStruct->HSEState == RCC_HSE_OFF)) {
+      return -1;
+    }
+  } else {
+
+#if defined(CONFIG_BOARD_HSE_CLK)
+#if CONFIG_BOARD_HSE_BYP == 1
+    SET_BIT(RCC->CR, RCC_CR_HSEBYP);
+    SET_BIT(RCC->CR, RCC_CR_HSEON);
+#else
+    SET_BIT(RCC->CR, RCC_CR_HSEON);
+#endif
+    /* Get Start Tick*/
+    tickstart = uptime_ctr;
+
+    /* Wait till HSE is ready */
+    while (__HAL_RCC_GET_FLAG(RCC_FLAG_HSERDY) == RESET) {
+      if ((uptime_ctr - tickstart) > HSE_STARTUP_TIMEOUT) {
+        return -1;
+      }
+    }
+#endif
+  }
+
+  /* Check if the PLL is used as system clock or not */
+  if (__HAL_RCC_GET_SYSCLK_SOURCE() != RCC_SYSCLKSOURCE_STATUS_PLLCLK) {
+
+    /* Disable the main PLL. */
+    __HAL_RCC_PLL_DISABLE();
+
+    /* Get Start Tick*/
+    tickstart = uptime_ctr;
+
+    /* Wait till PLL is ready */
+    while (__HAL_RCC_GET_FLAG(RCC_FLAG_PLLRDY) != RESET) {
+      if ((HAL_GetTick() - tickstart) > PLL_TIMEOUT_VALUE) {
+        return -1;
+      }
+    }
+
+    /* Configure the main PLL clock source, multiplication and division factors. */
+    __HAL_RCC_PLL_CONFIG(RCC_PLLSOURCE_HSE, 25, 432, RCC_PLLP_DIV2, 9, 7);
+
+    /* Enable the main PLL. */
+    __HAL_RCC_PLL_ENABLE();
+
+    /* Get Start Tick*/
+    tickstart = uptime_ctr;
+
+    /* Wait till PLL is ready */
+    while (__HAL_RCC_GET_FLAG(RCC_FLAG_PLLRDY) == RESET) {
+      if ((HAL_GetTick() - tickstart) > PLL_TIMEOUT_VALUE) {
+        return -1;
+      }
+    }
+  }
+  return 0;
 }
 
 /* static enum clock_control_status */
 /* clock_control_mik32_get_status(uint16_t id) */
 /* { */
-/* 	if (sys_test_bit(PM_BASE_ADDRESS + MIK32_CLOCK_ID_OFFSET(id), */
-/* 			 MIK32_CLOCK_ID_BIT(id)) != 0) { */
-/* 		return CLOCK_CONTROL_STATUS_ON; */
-/* 	} */
+/*    if (sys_test_bit(PM_BASE_ADDRESS + MIK32_CLOCK_ID_OFFSET(id), */
+/*         MIK32_CLOCK_ID_BIT(id)) != 0) { */
+/*      return CLOCK_CONTROL_STATUS_ON; */
+/*    } */
 
-/* 	return CLOCK_CONTROL_STATUS_OFF; */
+/*    return CLOCK_CONTROL_STATUS_OFF; */
 /* } */
diff --git a/os/drivers/clock/stm32f769_clock_control.h b/os/drivers/clock/stm32f769_clock_control.h
index 0fae03a3..9d553f61 100644
--- a/os/drivers/clock/stm32f769_clock_control.h
+++ b/os/drivers/clock/stm32f769_clock_control.h
@@ -6,5 +6,6 @@
 int stm32f769_clock_control_on(uint16_t id);
 int stm32f769_clock_control_off(uint16_t id);
 int stm32f769_clock_control_get_rate(uint16_t id, uint32_t *rate);
+int stm32f769_clock_control_init(struct device *dev);
 
 #endif/*__STM32F769_CLOCK_CONTROL_H__*/
diff --git a/os/drivers/clock/stm32f769_clocks.h b/os/drivers/clock/stm32f769_clocks.h
index 7a0731f7..5ff690bc 100644
--- a/os/drivers/clock/stm32f769_clocks.h
+++ b/os/drivers/clock/stm32f769_clocks.h
@@ -1,8 +1,10 @@
 #ifndef __DD_STM32F769_CLOCKS_H__
 #define __DD_STM32F769_CLOCKS_H__
 
-#define STM32F769_CLOCK_CONFIG(reg, bit) \
-	(((STM32F769_RCC_ ## reg ## _OFFSET) << 6U) | (bit))
+#define STM32F769_CLOCK_CONFIG(reg, bit)                \
+  (((STM32F769_RCC_ ## reg ## _OFFSET) << 6U) | (bit))
+
+#define STM32F769_RCC_SYS_SET_OFFSET        0x80U
 
 #define STM32F769_RCC_AHB1_SET_OFFSET       0x30U
 #define STM32F769_RCC_AHB2_SET_OFFSET       0x34U