diff --git a/drivers/st/clk/stm32mp1_clk.c b/drivers/st/clk/stm32mp1_clk.c index 2b6683dfa..af8b71e32 100644 --- a/drivers/st/clk/stm32mp1_clk.c +++ b/drivers/st/clk/stm32mp1_clk.c @@ -15,7 +15,6 @@ #include #include #include -#include #include #include #include @@ -1767,50 +1766,6 @@ static void stm32mp1_set_rtcsrc(unsigned int clksrc, bool lse_css) } } -static void stm32mp1_stgen_config(void) -{ - uint32_t cntfid0; - unsigned long rate; - unsigned long long counter; - - cntfid0 = mmio_read_32(STGEN_BASE + CNTFID_OFF); - rate = get_clock_rate(stm32mp1_clk_get_parent(STGEN_K)); - - if (cntfid0 == rate) { - return; - } - - mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); - counter = (unsigned long long)mmio_read_32(STGEN_BASE + CNTCVL_OFF); - counter |= ((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF)) << 32; - counter = (counter * rate / cntfid0); - - mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter); - mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32)); - mmio_write_32(STGEN_BASE + CNTFID_OFF, rate); - mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); - - write_cntfrq((u_register_t)rate); - - /* Need to update timer with new frequency */ - generic_delay_timer_init(); -} - -void stm32mp1_stgen_increment(unsigned long long offset_in_ms) -{ - unsigned long long cnt; - - cnt = ((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) | - mmio_read_32(STGEN_BASE + CNTCVL_OFF); - - cnt += (offset_in_ms * mmio_read_32(STGEN_BASE + CNTFID_OFF)) / 1000U; - - mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); - mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)cnt); - mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(cnt >> 32)); - mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); -} - static void stm32mp1_pkcs_config(uint32_t pkcs) { uintptr_t address = stm32mp_rcc_base() + ((pkcs >> 4) & 0xFFFU); @@ -2006,7 +1961,8 @@ int stm32mp1_clk_init(void) if (ret != 0) { return ret; } - stm32mp1_stgen_config(); + + stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K)); } /* Select DIV */ @@ -2180,7 +2136,8 @@ int stm32mp1_clk_init(void) if (stm32mp1_osc[_HSI] == 0U) { stm32mp1_hsi_set(false); } - stm32mp1_stgen_config(); + + stm32mp_stgen_config(stm32mp_clk_get_rate(STGEN_K)); /* Software Self-Refresh mode (SSR) during DDR initilialization */ mmio_clrsetbits_32(rcc_base + RCC_DDRITFCR, diff --git a/drivers/st/clk/stm32mp_clkfunc.c b/drivers/st/clk/stm32mp_clkfunc.c index 5ba64fd4e..a013a82c0 100644 --- a/drivers/st/clk/stm32mp_clkfunc.c +++ b/drivers/st/clk/stm32mp_clkfunc.c @@ -6,10 +6,13 @@ #include +#include #include #include +#include #include #include +#include #include #include @@ -318,3 +321,60 @@ unsigned long fdt_get_uart_clock_freq(uintptr_t instance) return clk_get_rate((unsigned long)clk_id); } + +/******************************************************************************* + * This function configures and restores the STGEN counter depending on the + * connected clock. + ******************************************************************************/ +void stm32mp_stgen_config(unsigned long rate) +{ + uint32_t cntfid0; + unsigned long long counter; + + cntfid0 = mmio_read_32(STGEN_BASE + CNTFID_OFF); + + if (cntfid0 == rate) { + return; + } + + mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); + counter = stm32mp_stgen_get_counter() * rate / cntfid0; + + mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)counter); + mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(counter >> 32)); + mmio_write_32(STGEN_BASE + CNTFID_OFF, rate); + mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); + + write_cntfrq_el0(rate); + + /* Need to update timer with new frequency */ + generic_delay_timer_init(); +} + +/******************************************************************************* + * This function returns the STGEN counter value. + ******************************************************************************/ +unsigned long long stm32mp_stgen_get_counter(void) +{ + return (((unsigned long long)mmio_read_32(STGEN_BASE + CNTCVU_OFF) << 32) | + mmio_read_32(STGEN_BASE + CNTCVL_OFF)); +} + +/******************************************************************************* + * This function restores the STGEN counter value. + * It takes a first input value as a counter backup value to be restored and a + * offset in ms to be added. + ******************************************************************************/ +void stm32mp_stgen_restore_counter(unsigned long long value, + unsigned long long offset_in_ms) +{ + unsigned long long cnt; + + cnt = value + ((offset_in_ms * + mmio_read_32(STGEN_BASE + CNTFID_OFF)) / 1000U); + + mmio_clrbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); + mmio_write_32(STGEN_BASE + CNTCVL_OFF, (uint32_t)cnt); + mmio_write_32(STGEN_BASE + CNTCVU_OFF, (uint32_t)(cnt >> 32)); + mmio_setbits_32(STGEN_BASE + CNTCR_OFF, CNTCR_EN); +} diff --git a/include/drivers/st/stm32mp1_clk.h b/include/drivers/st/stm32mp1_clk.h index 59c7c0bd6..e2395bc5c 100644 --- a/include/drivers/st/stm32mp1_clk.h +++ b/include/drivers/st/stm32mp1_clk.h @@ -32,8 +32,6 @@ bool stm32mp1_rcc_is_mckprot(void); void stm32mp1_clk_rcc_regs_lock(void); void stm32mp1_clk_rcc_regs_unlock(void); -void stm32mp1_stgen_increment(unsigned long long offset_in_ms); - #ifdef STM32MP_SHARED_RESOURCES void stm32mp1_register_clock_parents_secure(unsigned long id); #endif diff --git a/include/drivers/st/stm32mp_clkfunc.h b/include/drivers/st/stm32mp_clkfunc.h index 4876213a7..9df38d685 100644 --- a/include/drivers/st/stm32mp_clkfunc.h +++ b/include/drivers/st/stm32mp_clkfunc.h @@ -28,4 +28,9 @@ bool fdt_get_rcc_secure_status(void); int fdt_get_clock_id(int node); unsigned long fdt_get_uart_clock_freq(uintptr_t instance); +void stm32mp_stgen_config(unsigned long rate); +void stm32mp_stgen_restore_counter(unsigned long long value, + unsigned long long offset_in_ms); +unsigned long long stm32mp_stgen_get_counter(void); + #endif /* STM32MP_CLKFUNC_H */