1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2021, Microchip
4 */
5
6 #include <drivers/clk.h>
7 #include <drivers/scmi.h>
8 #include <drivers/scmi-msg.h>
9 #include <kernel/boot.h>
10 #include <string.h>
11 #include <sys/queue.h>
12
13 #include "clock.h"
14
15 /**
16 * struct scmi_clk - Binds an SCMI channel/clock to a core clk reference
17 * @clk: Core clock reference
18 * @channel_id: SCMI server channel handle exposing the clock
19 * @scmi_id: SCMI clock domain ID
20 * @enabled: SCMI clock state
21 * @link: Reference in SCMI server clock list
22 */
23 struct scmi_clk {
24 struct clk *clk;
25 unsigned int channel_id;
26 unsigned int scmi_id;
27 bool enabled;
28 SLIST_ENTRY(scmi_clk) link;
29 };
30
31 static bool scmi_clk_init_done;
32 static SLIST_HEAD(, scmi_clk) scmi_clk_list =
33 SLIST_HEAD_INITIALIZER(scmi_clk_list);
34
plat_scmi_clock_count(unsigned int channel_id)35 size_t plat_scmi_clock_count(unsigned int channel_id)
36 {
37 unsigned int count = 0;
38 unsigned int max_id = 0;
39 struct scmi_clk *clk = NULL;
40
41 SLIST_FOREACH(clk, &scmi_clk_list, link) {
42 if (clk->channel_id == channel_id) {
43 count++;
44 max_id = MAX(max_id, clk->scmi_id);
45 }
46 }
47
48 if (!count)
49 return 0;
50
51 /* IDs are starting from 0 so we need to return max_id + 1 for count */
52 return max_id + 1;
53 }
54
clk_scmi_get_by_id(unsigned int channel_id,unsigned int scmi_id)55 static struct scmi_clk *clk_scmi_get_by_id(unsigned int channel_id,
56 unsigned int scmi_id)
57 {
58 struct scmi_clk *clk = NULL;
59
60 SLIST_FOREACH(clk, &scmi_clk_list, link)
61 if (clk->channel_id == channel_id && clk->scmi_id == scmi_id)
62 return clk;
63
64 return NULL;
65 }
66
plat_scmi_clock_get_name(unsigned int channel_id,unsigned int scmi_id)67 const char *plat_scmi_clock_get_name(unsigned int channel_id,
68 unsigned int scmi_id)
69 {
70 struct scmi_clk *clk = NULL;
71
72 clk = clk_scmi_get_by_id(channel_id, scmi_id);
73 if (!clk)
74 return "dummy";
75
76 return clk_get_name(clk->clk);
77 }
78
plat_scmi_clock_rates_array(unsigned int channel_id,unsigned int scmi_id,size_t start_index,unsigned long * rates,size_t * nb_elts)79 int32_t plat_scmi_clock_rates_array(unsigned int channel_id,
80 unsigned int scmi_id,
81 size_t start_index,
82 unsigned long *rates,
83 size_t *nb_elts)
84 {
85 TEE_Result res = TEE_ERROR_GENERIC;
86 struct scmi_clk *clk = NULL;
87
88 clk = clk_scmi_get_by_id(channel_id, scmi_id);
89 if (!clk)
90 return SCMI_DENIED;
91
92 res = clk_get_rates_array(clk->clk, start_index, rates, nb_elts);
93 if (res == TEE_SUCCESS)
94 return SCMI_SUCCESS;
95 else if (res == TEE_ERROR_NOT_SUPPORTED)
96 return SCMI_NOT_SUPPORTED;
97 else
98 return SCMI_GENERIC_ERROR;
99 }
100
plat_scmi_clock_get_rate(unsigned int channel_id,unsigned int scmi_id)101 unsigned long plat_scmi_clock_get_rate(unsigned int channel_id,
102 unsigned int scmi_id)
103 {
104 struct scmi_clk *clk = NULL;
105
106 clk = clk_scmi_get_by_id(channel_id, scmi_id);
107 if (!clk)
108 return 0;
109
110 return clk_get_rate(clk->clk);
111 }
112
plat_scmi_clock_set_rate(unsigned int channel_id,unsigned int scmi_id,unsigned long rate)113 int32_t plat_scmi_clock_set_rate(unsigned int channel_id,
114 unsigned int scmi_id,
115 unsigned long rate)
116 {
117 TEE_Result res = TEE_ERROR_GENERIC;
118 struct scmi_clk *clk = NULL;
119
120 clk = clk_scmi_get_by_id(channel_id, scmi_id);
121 if (!clk)
122 return SCMI_DENIED;
123
124 res = clk_set_rate(clk->clk, rate);
125 if (res)
126 return SCMI_GENERIC_ERROR;
127
128 return SCMI_SUCCESS;
129 }
130
plat_scmi_clock_get_state(unsigned int channel_id,unsigned int scmi_id)131 int32_t plat_scmi_clock_get_state(unsigned int channel_id,
132 unsigned int scmi_id)
133 {
134 struct scmi_clk *clk = NULL;
135
136 clk = clk_scmi_get_by_id(channel_id, scmi_id);
137 if (!clk)
138 return false;
139
140 return clk->enabled;
141 }
142
plat_scmi_clock_set_state(unsigned int channel_id,unsigned int scmi_id,bool enable_not_disable)143 int32_t plat_scmi_clock_set_state(unsigned int channel_id,
144 unsigned int scmi_id,
145 bool enable_not_disable)
146 {
147 struct scmi_clk *clk = NULL;
148
149 clk = clk_scmi_get_by_id(channel_id, scmi_id);
150 if (!clk) {
151 if (enable_not_disable)
152 return SCMI_DENIED;
153 else
154 return SCMI_SUCCESS;
155 }
156
157 if (enable_not_disable) {
158 if (!clk->enabled) {
159 if (clk_enable(clk->clk))
160 return SCMI_GENERIC_ERROR;
161 clk->enabled = true;
162 }
163 } else {
164 if (clk->enabled) {
165 clk_disable(clk->clk);
166 clk->enabled = false;
167 }
168 }
169
170 return SCMI_SUCCESS;
171 }
172
clk_check_scmi_id(struct clk * new_clk,unsigned int channel_id,unsigned int scmi_id)173 static TEE_Result clk_check_scmi_id(struct clk *new_clk,
174 unsigned int channel_id,
175 unsigned int scmi_id)
176 {
177 struct scmi_clk *clk = NULL;
178
179 SLIST_FOREACH(clk, &scmi_clk_list, link) {
180 if (clk->channel_id == channel_id && clk->scmi_id == scmi_id) {
181 EMSG("SCMI channel %u, clock %u already registered",
182 channel_id, scmi_id);
183 return TEE_ERROR_BAD_PARAMETERS;
184 }
185 }
186
187 if (strlen(clk_get_name(new_clk)) >= SCMI_CLOCK_NAME_LENGTH_MAX)
188 return TEE_ERROR_BAD_PARAMETERS;
189
190 return TEE_SUCCESS;
191 }
192
scmi_clk_add(struct clk * clk,unsigned int channel_id,unsigned int scmi_id)193 TEE_Result scmi_clk_add(struct clk *clk, unsigned int channel_id,
194 unsigned int scmi_id)
195 {
196 TEE_Result res = TEE_ERROR_GENERIC;
197 struct scmi_clk *scmi_clk = NULL;
198
199 if (scmi_clk_init_done)
200 return TEE_ERROR_BAD_STATE;
201
202 res = clk_check_scmi_id(clk, channel_id, scmi_id);
203 if (res)
204 return res;
205
206 scmi_clk = calloc(1, sizeof(*scmi_clk));
207 if (!scmi_clk)
208 return TEE_ERROR_OUT_OF_MEMORY;
209
210 scmi_clk->clk = clk;
211 scmi_clk->channel_id = channel_id;
212 scmi_clk->scmi_id = scmi_id;
213 scmi_clk->enabled = false;
214
215 SLIST_INSERT_HEAD(&scmi_clk_list, scmi_clk, link);
216
217 return TEE_SUCCESS;
218 }
219
scmi_clk_init_fini(void)220 static TEE_Result scmi_clk_init_fini(void)
221 {
222 scmi_clk_init_done = true;
223
224 return TEE_SUCCESS;
225 }
226
227 release_init_resource(scmi_clk_init_fini);
228