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