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