1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (C) 2022-2024, STMicroelectronics 4 */ 5 6 #include <config.h> 7 #include <drivers/clk.h> 8 #include <drivers/clk_dt.h> 9 #include <drivers/rtc.h> 10 #include <drivers/stm32_stgen.h> 11 #include <io.h> 12 #include <keep.h> 13 #include <kernel/delay.h> 14 #include <kernel/dt.h> 15 #include <kernel/dt_driver.h> 16 #include <kernel/pm.h> 17 #include <kernel/thread.h> 18 #include <libfdt.h> 19 #include <mm/core_memprot.h> 20 #include <stm32_util.h> 21 #include <util.h> 22 23 #define STGENC_CNTCR U(0x0) 24 #define STGENC_CNTSR U(0x4) 25 #define STGENC_CNTCVL U(0x8) 26 #define STGENC_CNTCVU U(0xC) 27 #define STGENC_CNTFID0 U(0x20) 28 29 /* STGENC_CNTCR register */ 30 #define STGENC_CNTCR_EN BIT(0) 31 32 /* 33 * SMC function ID for STM32MP/TF-A monitor to set CNTFRQ to STGEN frequency. 34 * No arguments. 35 */ 36 #define STM32_SIP_SMC_STGEN_SET_RATE 0x82000000 37 38 /* 39 * struct stgen_pdata - STGEN instance 40 * @stgen_clock: STGEN subsystem clock 41 * @stgen_bus: STGEN interface bus clock 42 * @calendar: RTC calendar data at suspend time 43 * @cnt_h: STGEN high counter value at suspend time 44 * @cnt_l: STGEN low counter value at suspend time 45 * @base: STGEN IOMEM base address 46 */ 47 struct stgen_pdata { 48 struct clk *stgen_clock; 49 struct clk *bus_clock; 50 struct optee_rtc_time *calendar; 51 uint32_t cnt_h; 52 uint32_t cnt_l; 53 vaddr_t base; 54 }; 55 56 static struct stgen_pdata stgen_d; 57 58 static void set_counter_value(uint64_t counter) 59 { 60 io_write64(stgen_d.base + STGENC_CNTCVL, counter); 61 } 62 63 uint64_t stm32_stgen_get_counter_value(void) 64 { 65 return io_read64(stgen_d.base + STGENC_CNTCVL); 66 } 67 68 /* 69 * This function invokes EL3 monitor to update CNTFRQ with STGEN 70 * frequency. Updating the CP15 register can only be done by the 71 * highest EL on Armv8-A. 72 */ 73 static TEE_Result stm32mp_stgen_smc_config(void) 74 { 75 /* 76 * STM32_SIP_SMC_STGEN_SET_RATE call API 77 * 78 * Argument a0: (input) SMCC ID 79 * (output) status return code 80 */ 81 struct thread_smc_args stgen_conf_args = { 82 .a0 = STM32_SIP_SMC_STGEN_SET_RATE 83 }; 84 85 thread_smccc(&stgen_conf_args); 86 87 if (stgen_conf_args.a0) { 88 EMSG("STGEN configuration failed, error code: %#llx", 89 (unsigned long long)stgen_conf_args.a0); 90 return TEE_ERROR_BAD_PARAMETERS; 91 } 92 93 return TEE_SUCCESS; 94 } 95 96 static void stm32_stgen_pm_suspend(void) 97 { 98 TEE_Result res = TEE_ERROR_GENERIC; 99 100 /* 101 * Disable STGEN counter. At this point, it should be stopped by 102 * software. 103 */ 104 io_clrbits32(stgen_d.base + STGENC_CNTCR, STGENC_CNTCR_EN); 105 106 /* Save current time from the RTC */ 107 res = rtc_get_time(stgen_d.calendar); 108 if (res) 109 panic("Could not get RTC calendar at suspend"); 110 111 /* Save current counter value */ 112 reg_pair_from_64(stm32_stgen_get_counter_value(), 113 &stgen_d.cnt_h, &stgen_d.cnt_l); 114 115 /* Re-enable STGEN as generic timer can be used later */ 116 io_setbits32(stgen_d.base + STGENC_CNTCR, STGENC_CNTCR_EN); 117 } 118 119 static void stm32_stgen_pm_resume(void) 120 { 121 uint64_t counter_val = reg_pair_to_64(stgen_d.cnt_h, stgen_d.cnt_l); 122 unsigned long clock_src_rate = clk_get_rate(stgen_d.stgen_clock); 123 struct optee_rtc_time cur_calendar = { }; 124 signed long long nb_pm_count_ticks = 0; 125 TEE_Result res = TEE_ERROR_GENERIC; 126 127 /* Disable STGEN counter while we're updating its value */ 128 io_clrbits32(stgen_d.base + STGENC_CNTCR, STGENC_CNTCR_EN); 129 130 /* Read the current time from the RTC to update system counter */ 131 res = rtc_get_time(&cur_calendar); 132 if (res) 133 panic("Could not get RTC calendar at resume"); 134 135 nb_pm_count_ticks = rtc_diff_calendar_tick(&cur_calendar, 136 stgen_d.calendar, 137 clock_src_rate); 138 if (nb_pm_count_ticks < 0 || nb_pm_count_ticks == LLONG_MAX) 139 panic(); 140 141 /* Update the counter value with the number of pm ticks */ 142 if (ADD_OVERFLOW(counter_val, nb_pm_count_ticks, &counter_val)) 143 panic("STGEN counter overflow"); 144 145 io_write32(stgen_d.base + STGENC_CNTFID0, clock_src_rate); 146 if (io_read32(stgen_d.base + STGENC_CNTFID0) != clock_src_rate) 147 panic("Couldn't modify STGEN clock rate"); 148 149 set_counter_value(counter_val); 150 151 /* Set the correct clock frequency in the ARM CP15 register */ 152 if (IS_ENABLED(CFG_WITH_ARM_TRUSTED_FW)) 153 stm32mp_stgen_smc_config(); 154 155 io_setbits32(stgen_d.base + STGENC_CNTCR, STGENC_CNTCR_EN); 156 157 DMSG("Time spent in low-power: %lld ms", 158 (nb_pm_count_ticks * 1000) / clock_src_rate); 159 } 160 161 static TEE_Result 162 stm32_stgen_pm(enum pm_op op, unsigned int pm_hint __unused, 163 const struct pm_callback_handle *pm_handle __unused) 164 { 165 if (op == PM_OP_RESUME) 166 stm32_stgen_pm_resume(); 167 else 168 stm32_stgen_pm_suspend(); 169 170 return TEE_SUCCESS; 171 } 172 173 static TEE_Result parse_dt(const void *fdt, int node) 174 { 175 TEE_Result res = TEE_ERROR_GENERIC; 176 struct io_pa_va base = { }; 177 size_t reg_size = 0; 178 179 if (fdt_reg_info(fdt, node, &base.pa, ®_size)) 180 panic(); 181 182 stgen_d.base = io_pa_or_va_secure(&base, reg_size); 183 assert(stgen_d.base != 0); 184 185 res = clk_dt_get_by_name(fdt, node, "bus", &stgen_d.bus_clock); 186 if (res == TEE_ERROR_DEFER_DRIVER_INIT) 187 return TEE_ERROR_DEFER_DRIVER_INIT; 188 if (res) 189 panic("No stgen bus clock available in device tree"); 190 191 res = clk_dt_get_by_name(fdt, node, "stgen_clk", &stgen_d.stgen_clock); 192 if (res == TEE_ERROR_DEFER_DRIVER_INIT) 193 return TEE_ERROR_DEFER_DRIVER_INIT; 194 if (res) 195 panic("No stgen clock available in device tree"); 196 197 return TEE_SUCCESS; 198 } 199 200 static TEE_Result stgen_probe(const void *fdt, int node, 201 const void *compat_data __unused) 202 { 203 unsigned long previous_source_rate = 0; 204 TEE_Result res = TEE_ERROR_GENERIC; 205 unsigned long stgen_rate = 0; 206 uint64_t stgen_counter = 0; 207 208 res = parse_dt(fdt, node); 209 if (res) 210 return res; 211 212 stgen_d.calendar = calloc(1, sizeof(*stgen_d.calendar)); 213 if (!stgen_d.calendar) 214 return TEE_ERROR_OUT_OF_MEMORY; 215 216 res = clk_enable(stgen_d.bus_clock); 217 if (res) 218 panic(); 219 220 /* 221 * Read current source rate before configuration of the flexgen to 222 * update STGEN counter. 223 */ 224 previous_source_rate = clk_get_rate(stgen_d.stgen_clock); 225 226 /* 227 * Disable STGEN counter. At cold boot, we accept the counter delta 228 * due to STGEN reconfiguration 229 */ 230 io_clrbits32(stgen_d.base + STGENC_CNTCR, STGENC_CNTCR_EN); 231 232 /* 233 * Flexgen for stgen_clock is skipped during RCC probe. 234 * Enabling stgen_clock configure the flexgen. 235 */ 236 res = clk_enable(stgen_d.stgen_clock); 237 if (res) 238 panic(); 239 240 stgen_rate = clk_get_rate(stgen_d.stgen_clock); 241 242 io_write32(stgen_d.base + STGENC_CNTFID0, stgen_rate); 243 if (io_read32(stgen_d.base + STGENC_CNTFID0) != stgen_rate) 244 panic("Couldn't modify STGEN clock rate"); 245 246 /* Update counter value according to the new STGEN clock frequency */ 247 stgen_counter = (stm32_stgen_get_counter_value() * stgen_rate) / 248 previous_source_rate; 249 set_counter_value(stgen_counter); 250 251 if (IS_ENABLED(CFG_WITH_ARM_TRUSTED_FW)) 252 stm32mp_stgen_smc_config(); 253 254 io_setbits32(stgen_d.base + STGENC_CNTCR, STGENC_CNTCR_EN); 255 256 if (IS_ENABLED(CFG_STM32_RTC)) 257 register_pm_core_service_cb(stm32_stgen_pm, NULL, 258 "stm32-stgen"); 259 260 return TEE_SUCCESS; 261 } 262 263 static const struct dt_device_match stm32_stgen_match_table[] = { 264 { .compatible = "st,stm32mp25-stgen" }, 265 { } 266 }; 267 268 DEFINE_DT_DRIVER(stm32_stgen_dt_driver) = { 269 .name = "stm32_stgen", 270 .match_table = stm32_stgen_match_table, 271 .probe = stgen_probe, 272 }; 273