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
set_counter_value(uint64_t counter)58 static void set_counter_value(uint64_t counter)
59 {
60 io_write64(stgen_d.base + STGENC_CNTCVL, counter);
61 }
62
stm32_stgen_get_counter_value(void)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 */
stm32mp_stgen_smc_config(void)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
stm32_stgen_pm_suspend(void)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
stm32_stgen_pm_resume(void)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
stm32_stgen_pm(enum pm_op op,unsigned int pm_hint __unused,const struct pm_callback_handle * pm_handle __unused)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
parse_dt(const void * fdt,int node)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
stgen_probe(const void * fdt,int node,const void * compat_data __unused)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