1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2019-2022, STMicroelectronics
4 */
5
6 #include <drivers/clk.h>
7 #include <drivers/stm32mp_dt_bindings.h>
8 #include <initcall.h>
9 #include <kernel/delay.h>
10 #include <mm/core_memprot.h>
11 #include <stm32_util.h>
12 #include <io.h>
13 #include <trace.h>
14 #include <types_ext.h>
15
16 /*
17 * SYSCFG register offsets (base relative)
18 */
19 #define SYSCFG_CMPCR 0x20U
20 #define SYSCFG_CMPENSETR 0x24U
21
22 /*
23 * SYSCFG_CMPCR Register
24 */
25 #define SYSCFG_CMPCR_SW_CTRL BIT(1)
26 #define SYSCFG_CMPCR_READY BIT(8)
27 #define SYSCFG_CMPCR_RANSRC GENMASK_32(19, 16)
28 #define SYSCFG_CMPCR_RANSRC_SHIFT 16
29 #define SYSCFG_CMPCR_RAPSRC GENMASK_32(23, 20)
30 #define SYSCFG_CMPCR_ANSRC_SHIFT 24
31
32 #define SYSCFG_CMPCR_READY_TIMEOUT_US 1000U
33
34 /*
35 * SYSCFG_CMPENSETR Register
36 */
37 #define SYSCFG_CMPENSETR_MPU_EN BIT(0)
38
get_syscfg_base(void)39 static vaddr_t get_syscfg_base(void)
40 {
41 struct io_pa_va base = { .pa = SYSCFG_BASE };
42
43 return io_pa_or_va(&base, 1);
44 }
45
stm32mp_syscfg_enable_io_compensation(void)46 void stm32mp_syscfg_enable_io_compensation(void)
47 {
48 vaddr_t syscfg_base = get_syscfg_base();
49 uint64_t timeout_ref = 0;
50
51 if (clk_enable(stm32mp_rcc_clock_id_to_clk(CK_CSI)) ||
52 clk_enable(stm32mp_rcc_clock_id_to_clk(SYSCFG)))
53 panic();
54
55 io_setbits32(syscfg_base + SYSCFG_CMPENSETR, SYSCFG_CMPENSETR_MPU_EN);
56
57 timeout_ref = timeout_init_us(SYSCFG_CMPCR_READY_TIMEOUT_US);
58
59 while (!(io_read32(syscfg_base + SYSCFG_CMPCR) & SYSCFG_CMPCR_READY))
60 if (timeout_elapsed(timeout_ref)) {
61 EMSG("IO compensation cell not ready");
62 /* Allow an almost silent failure here */
63 break;
64 }
65
66 io_clrbits32(syscfg_base + SYSCFG_CMPCR, SYSCFG_CMPCR_SW_CTRL);
67
68 DMSG("SYSCFG.cmpcr = %#"PRIx32, io_read32(syscfg_base + SYSCFG_CMPCR));
69 }
70
stm32mp_syscfg_disable_io_compensation(void)71 void stm32mp_syscfg_disable_io_compensation(void)
72 {
73 vaddr_t syscfg_base = get_syscfg_base();
74 uint32_t value = 0;
75
76 value = io_read32(syscfg_base + SYSCFG_CMPCR) >>
77 SYSCFG_CMPCR_ANSRC_SHIFT;
78
79 io_clrbits32(syscfg_base + SYSCFG_CMPCR,
80 SYSCFG_CMPCR_RANSRC | SYSCFG_CMPCR_RAPSRC);
81
82 value = io_read32(syscfg_base + SYSCFG_CMPCR) |
83 (value << SYSCFG_CMPCR_RANSRC_SHIFT);
84
85 io_write32(syscfg_base + SYSCFG_CMPCR, value | SYSCFG_CMPCR_SW_CTRL);
86
87 DMSG("SYSCFG.cmpcr = %#"PRIx32, io_read32(syscfg_base + SYSCFG_CMPCR));
88
89 io_clrbits32(syscfg_base + SYSCFG_CMPENSETR, SYSCFG_CMPENSETR_MPU_EN);
90
91 clk_disable(stm32mp_rcc_clock_id_to_clk(CK_CSI));
92 clk_disable(stm32mp_rcc_clock_id_to_clk(SYSCFG));
93 }
94
stm32mp1_iocomp(void)95 static TEE_Result stm32mp1_iocomp(void)
96 {
97 stm32mp_syscfg_enable_io_compensation();
98
99 return TEE_SUCCESS;
100 }
101 driver_init(stm32mp1_iocomp);
102