1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (C) 2024 Microchip Technology Inc.
4 */
5
6 #include <at91_clk.h>
7 #include <kernel/misc.h>
8 #include <kernel/panic.h>
9 #include <malloc.h>
10 #include <libfdt.h>
11 #include <trace.h>
12
13 #define OPP_RATES_MAX 8
14
15 /* the clock rates for CPU OPP */
16 struct clk_rates {
17 size_t rate_num; /* the number of valid clock rates in @rates */
18 unsigned long rates[OPP_RATES_MAX];
19 };
20
21 static struct clk_rates *opp_rates;
22
get_rates_array(struct clk * clk __unused,size_t start_index,unsigned long * rates,size_t * nb_elts)23 static TEE_Result get_rates_array(struct clk *clk __unused, size_t start_index,
24 unsigned long *rates, size_t *nb_elts)
25 {
26 if (!opp_rates)
27 panic("Invalid CPU OPP Rates Array");
28
29 if (!rates) {
30 *nb_elts = opp_rates->rate_num;
31
32 return TEE_SUCCESS;
33 }
34
35 if (start_index + *nb_elts > opp_rates->rate_num) {
36 EMSG("Bad parameter(s): start_index %zu, nb_elts %zu",
37 start_index, *nb_elts);
38
39 return TEE_ERROR_BAD_PARAMETERS;
40 }
41 memcpy(rates, &opp_rates->rates[start_index],
42 *nb_elts * sizeof(*rates));
43
44 return TEE_SUCCESS;
45 }
46
cpu_opp_clk_set_rate(struct clk * clk,unsigned long rate,unsigned long parent_rate)47 static TEE_Result cpu_opp_clk_set_rate(struct clk *clk, unsigned long rate,
48 unsigned long parent_rate)
49 {
50 size_t n = 0;
51
52 assert(clk->parent);
53
54 for (n = 0; n < opp_rates->rate_num; n++)
55 if (rate == opp_rates->rates[n])
56 break;
57 if (n == opp_rates->rate_num)
58 return TEE_ERROR_BAD_PARAMETERS;
59
60 return clk->parent->ops->set_rate(clk->parent, rate, parent_rate);
61 }
62
63 static const struct clk_ops cpu_opp_clk_ops = {
64 .set_rate = cpu_opp_clk_set_rate,
65 .get_rates_array = get_rates_array,
66 };
67
dt_get_opp_hz(const void * fdt,int node,unsigned long * value)68 static TEE_Result dt_get_opp_hz(const void *fdt, int node, unsigned long *value)
69 {
70 const char *property = "opp-hz";
71 const fdt64_t *p = NULL;
72 int len = 0;
73
74 p = fdt_getprop(fdt, node, property, &len);
75 if (!p)
76 return TEE_ERROR_ITEM_NOT_FOUND;
77
78 if (len != sizeof(*p))
79 return TEE_ERROR_BAD_FORMAT;
80
81 *value = fdt64_ld(p);
82
83 return TEE_SUCCESS;
84 }
85
opp_rates_setup(const void * fdt,int node)86 static TEE_Result opp_rates_setup(const void *fdt, int node)
87 {
88 const char *compatible = "operating-points-v2";
89 const fdt32_t *cuint = NULL;
90 size_t rate_num = 0;
91 int opp_table = 0;
92 int offset = 0;
93
94 cuint = fdt_getprop(fdt, node, compatible, NULL);
95 if (!cuint)
96 return TEE_ERROR_NOT_SUPPORTED;
97
98 opp_rates = calloc(1, sizeof(*opp_rates));
99 if (!opp_rates) {
100 EMSG("Fail to alloc opp_rates");
101
102 return TEE_ERROR_OUT_OF_MEMORY;
103 }
104
105 offset = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
106 if (offset < 0)
107 panic("Invalid offset of opp-table");
108
109 if (fdt_node_check_compatible(fdt, offset, compatible))
110 panic("Invalid opp-table");
111
112 fdt_for_each_subnode(opp_table, fdt, offset) {
113 if (rate_num >= OPP_RATES_MAX)
114 panic("CPU OPP rate array shortage");
115
116 if (dt_get_opp_hz(fdt, opp_table, opp_rates->rates + rate_num))
117 panic("Get opp-hz failed");
118
119 rate_num++;
120 }
121
122 /* Ensure rates are in ascending order */
123 qsort_ul(opp_rates->rates, rate_num);
124
125 opp_rates->rate_num = rate_num;
126
127 return TEE_SUCCESS;
128 }
129
130 static struct clk *cpu_opp_clk;
131
at91_cpu_opp_clk_get(void)132 struct clk *at91_cpu_opp_clk_get(void)
133 {
134 return cpu_opp_clk;
135 }
136
at91_clk_register_cpu_opp(const void * fdt,int node,struct clk * clk)137 TEE_Result at91_clk_register_cpu_opp(const void *fdt, int node, struct clk *clk)
138 {
139 TEE_Result res = TEE_ERROR_GENERIC;
140
141 res = opp_rates_setup(fdt, node);
142 if (res == TEE_ERROR_NOT_SUPPORTED)
143 return TEE_SUCCESS;
144 if (res)
145 return res;
146
147 cpu_opp_clk = clk_alloc("cpu-opp", &cpu_opp_clk_ops, &clk, 1);
148 if (!cpu_opp_clk)
149 panic("CPU OPP clock alloc failed");
150
151 res = clk_register(cpu_opp_clk);
152 if (res) {
153 clk_free(cpu_opp_clk);
154 return res;
155 }
156
157 /* CPU clock is likely always enabled so set its refcount */
158 if (clk_enable(cpu_opp_clk))
159 panic("CPU clock should always enabled");
160
161 return TEE_SUCCESS;
162 }
163