1*7f41506fSGatien Chevallier /* 2*7f41506fSGatien Chevallier * Copyright (c) 2024, STMicroelectronics - All Rights Reserved 3*7f41506fSGatien Chevallier * 4*7f41506fSGatien Chevallier * SPDX-License-Identifier: BSD-3-Clause 5*7f41506fSGatien Chevallier */ 6*7f41506fSGatien Chevallier 7*7f41506fSGatien Chevallier #include <common/debug.h> 8*7f41506fSGatien Chevallier #include <drivers/generic_delay_timer.h> 9*7f41506fSGatien Chevallier #include <drivers/st/stm32mp_clkfunc.h> 10*7f41506fSGatien Chevallier #include <lib/mmio.h> 11*7f41506fSGatien Chevallier #include <plat/common/platform.h> 12*7f41506fSGatien Chevallier #include <platform_def.h> 13*7f41506fSGatien Chevallier 14*7f41506fSGatien Chevallier #include "stgen_svc.h" 15*7f41506fSGatien Chevallier #include <stm32mp2_smc.h> 16*7f41506fSGatien Chevallier #include <stm32mp_common.h> 17*7f41506fSGatien Chevallier #include <stm32mp_svc_setup.h> 18*7f41506fSGatien Chevallier 19*7f41506fSGatien Chevallier /* 20*7f41506fSGatien Chevallier * This function reads and applies the STGEN frequency value in the STGENC base frequency register, 21*7f41506fSGatien Chevallier * which is the frequency the system base counter use for our platforms. 22*7f41506fSGatien Chevallier */ stgen_svc_handler(void)23*7f41506fSGatien Chevallieruint32_t stgen_svc_handler(void) 24*7f41506fSGatien Chevallier { 25*7f41506fSGatien Chevallier unsigned long freq_to_set = mmio_read_32(STGEN_BASE + CNTFID_OFF); 26*7f41506fSGatien Chevallier 27*7f41506fSGatien Chevallier VERBOSE("STGEN frequency set to %lu\n", freq_to_set); 28*7f41506fSGatien Chevallier 29*7f41506fSGatien Chevallier /* 30*7f41506fSGatien Chevallier * Update the system counter frequency according to STGEN's base 31*7f41506fSGatien Chevallier * counter frequency register 32*7f41506fSGatien Chevallier */ 33*7f41506fSGatien Chevallier write_cntfrq_el0((u_register_t)freq_to_set); 34*7f41506fSGatien Chevallier 35*7f41506fSGatien Chevallier /* Need to update timer with new frequency */ 36*7f41506fSGatien Chevallier generic_delay_timer_init(); 37*7f41506fSGatien Chevallier 38*7f41506fSGatien Chevallier return STM32_SMC_OK; 39*7f41506fSGatien Chevallier } 40