Commit | Line | Data |
---|---|---|
5f6c6430 SH |
1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* | |
3 | * System Control and Management Interface (SCMI) Clock Protocol | |
4 | * | |
23136bff | 5 | * Copyright (C) 2018-2022 ARM Ltd. |
5f6c6430 SH |
6 | */ |
7 | ||
f5800e0b | 8 | #include <linux/module.h> |
7ad6b6cc | 9 | #include <linux/limits.h> |
dccec73d SH |
10 | #include <linux/sort.h> |
11 | ||
23136bff | 12 | #include "protocols.h" |
7aa75496 | 13 | #include "notify.h" |
5f6c6430 SH |
14 | |
15 | enum scmi_clock_protocol_cmd { | |
16 | CLOCK_ATTRIBUTES = 0x3, | |
17 | CLOCK_DESCRIBE_RATES = 0x4, | |
18 | CLOCK_RATE_SET = 0x5, | |
19 | CLOCK_RATE_GET = 0x6, | |
20 | CLOCK_CONFIG_SET = 0x7, | |
b260fcca | 21 | CLOCK_NAME_GET = 0x8, |
7aa75496 CM |
22 | CLOCK_RATE_NOTIFY = 0x9, |
23 | CLOCK_RATE_CHANGE_REQUESTED_NOTIFY = 0xA, | |
5f6c6430 SH |
24 | }; |
25 | ||
26 | struct scmi_msg_resp_clock_protocol_attributes { | |
27 | __le16 num_clocks; | |
28 | u8 max_async_req; | |
29 | u8 reserved; | |
30 | }; | |
31 | ||
32 | struct scmi_msg_resp_clock_attributes { | |
33 | __le32 attributes; | |
34 | #define CLOCK_ENABLE BIT(0) | |
7aa75496 CM |
35 | #define SUPPORTS_RATE_CHANGED_NOTIF(x) ((x) & BIT(31)) |
36 | #define SUPPORTS_RATE_CHANGE_REQUESTED_NOTIF(x) ((x) & BIT(30)) | |
37 | #define SUPPORTS_EXTENDED_NAMES(x) ((x) & BIT(29)) | |
b260fcca | 38 | u8 name[SCMI_SHORT_NAME_MAX_SIZE]; |
18f295b7 | 39 | __le32 clock_enable_latency; |
5f6c6430 SH |
40 | }; |
41 | ||
42 | struct scmi_clock_set_config { | |
43 | __le32 id; | |
44 | __le32 attributes; | |
45 | }; | |
46 | ||
47 | struct scmi_msg_clock_describe_rates { | |
48 | __le32 id; | |
49 | __le32 rate_index; | |
50 | }; | |
51 | ||
52 | struct scmi_msg_resp_clock_describe_rates { | |
53 | __le32 num_rates_flags; | |
54 | #define NUM_RETURNED(x) ((x) & 0xfff) | |
55 | #define RATE_DISCRETE(x) !((x) & BIT(12)) | |
56 | #define NUM_REMAINING(x) ((x) >> 16) | |
57 | struct { | |
58 | __le32 value_low; | |
59 | __le32 value_high; | |
f1ad601d | 60 | } rate[]; |
5f6c6430 SH |
61 | #define RATE_TO_U64(X) \ |
62 | ({ \ | |
63 | typeof(X) x = (X); \ | |
64 | le32_to_cpu((x).value_low) | (u64)le32_to_cpu((x).value_high) << 32; \ | |
65 | }) | |
66 | }; | |
67 | ||
68 | struct scmi_clock_set_rate { | |
69 | __le32 flags; | |
70 | #define CLOCK_SET_ASYNC BIT(0) | |
2bc06ffa | 71 | #define CLOCK_SET_IGNORE_RESP BIT(1) |
5f6c6430 SH |
72 | #define CLOCK_SET_ROUND_UP BIT(2) |
73 | #define CLOCK_SET_ROUND_AUTO BIT(3) | |
74 | __le32 id; | |
75 | __le32 value_low; | |
76 | __le32 value_high; | |
77 | }; | |
78 | ||
c7e223f5 CM |
79 | struct scmi_msg_resp_set_rate_complete { |
80 | __le32 id; | |
81 | __le32 rate_low; | |
82 | __le32 rate_high; | |
83 | }; | |
84 | ||
7aa75496 CM |
85 | struct scmi_msg_clock_rate_notify { |
86 | __le32 clk_id; | |
87 | __le32 notify_enable; | |
88 | }; | |
89 | ||
90 | struct scmi_clock_rate_notify_payld { | |
91 | __le32 agent_id; | |
92 | __le32 clock_id; | |
93 | __le32 rate_low; | |
94 | __le32 rate_high; | |
95 | }; | |
96 | ||
5f6c6430 | 97 | struct clock_info { |
b55b06b7 | 98 | u32 version; |
5f6c6430 SH |
99 | int num_clocks; |
100 | int max_async_req; | |
2bc06ffa | 101 | atomic_t cur_async_req; |
5f6c6430 SH |
102 | struct scmi_clock_info *clk; |
103 | }; | |
104 | ||
7aa75496 CM |
105 | static enum scmi_clock_protocol_cmd evt_2_cmd[] = { |
106 | CLOCK_RATE_NOTIFY, | |
107 | CLOCK_RATE_CHANGE_REQUESTED_NOTIFY, | |
108 | }; | |
109 | ||
887281c7 CM |
110 | static int |
111 | scmi_clock_protocol_attributes_get(const struct scmi_protocol_handle *ph, | |
112 | struct clock_info *ci) | |
5f6c6430 SH |
113 | { |
114 | int ret; | |
115 | struct scmi_xfer *t; | |
116 | struct scmi_msg_resp_clock_protocol_attributes *attr; | |
117 | ||
887281c7 CM |
118 | ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES, |
119 | 0, sizeof(*attr), &t); | |
5f6c6430 SH |
120 | if (ret) |
121 | return ret; | |
122 | ||
123 | attr = t->rx.buf; | |
124 | ||
887281c7 | 125 | ret = ph->xops->do_xfer(ph, t); |
5f6c6430 SH |
126 | if (!ret) { |
127 | ci->num_clocks = le16_to_cpu(attr->num_clocks); | |
128 | ci->max_async_req = attr->max_async_req; | |
129 | } | |
130 | ||
887281c7 | 131 | ph->xops->xfer_put(ph, t); |
5f6c6430 SH |
132 | return ret; |
133 | } | |
134 | ||
887281c7 | 135 | static int scmi_clock_attributes_get(const struct scmi_protocol_handle *ph, |
b260fcca CM |
136 | u32 clk_id, struct scmi_clock_info *clk, |
137 | u32 version) | |
5f6c6430 SH |
138 | { |
139 | int ret; | |
b260fcca | 140 | u32 attributes; |
5f6c6430 SH |
141 | struct scmi_xfer *t; |
142 | struct scmi_msg_resp_clock_attributes *attr; | |
143 | ||
887281c7 CM |
144 | ret = ph->xops->xfer_get_init(ph, CLOCK_ATTRIBUTES, |
145 | sizeof(clk_id), sizeof(*attr), &t); | |
5f6c6430 SH |
146 | if (ret) |
147 | return ret; | |
148 | ||
aa90ac45 | 149 | put_unaligned_le32(clk_id, t->tx.buf); |
5f6c6430 SH |
150 | attr = t->rx.buf; |
151 | ||
887281c7 | 152 | ret = ph->xops->do_xfer(ph, t); |
18f295b7 | 153 | if (!ret) { |
7ad6b6cc | 154 | u32 latency = 0; |
b260fcca | 155 | attributes = le32_to_cpu(attr->attributes); |
4314f9f4 | 156 | strscpy(clk->name, attr->name, SCMI_SHORT_NAME_MAX_SIZE); |
df3576d1 CM |
157 | /* clock_enable_latency field is present only since SCMI v3.1 */ |
158 | if (PROTOCOL_REV_MAJOR(version) >= 0x2) | |
7ad6b6cc SH |
159 | latency = le32_to_cpu(attr->clock_enable_latency); |
160 | clk->enable_latency = latency ? : U32_MAX; | |
18f295b7 | 161 | } |
5f6c6430 | 162 | |
887281c7 | 163 | ph->xops->xfer_put(ph, t); |
b260fcca CM |
164 | |
165 | /* | |
166 | * If supported overwrite short name with the extended one; | |
167 | * on error just carry on and use already provided short name. | |
168 | */ | |
7aa75496 CM |
169 | if (!ret && PROTOCOL_REV_MAJOR(version) >= 0x2) { |
170 | if (SUPPORTS_EXTENDED_NAMES(attributes)) | |
171 | ph->hops->extended_name_get(ph, CLOCK_NAME_GET, clk_id, | |
172 | clk->name, | |
173 | SCMI_MAX_STR_SIZE); | |
174 | ||
175 | if (SUPPORTS_RATE_CHANGED_NOTIF(attributes)) | |
176 | clk->rate_changed_notifications = true; | |
177 | if (SUPPORTS_RATE_CHANGE_REQUESTED_NOTIF(attributes)) | |
178 | clk->rate_change_requested_notifications = true; | |
179 | } | |
b260fcca | 180 | |
5f6c6430 SH |
181 | return ret; |
182 | } | |
183 | ||
dccec73d SH |
184 | static int rate_cmp_func(const void *_r1, const void *_r2) |
185 | { | |
186 | const u64 *r1 = _r1, *r2 = _r2; | |
187 | ||
188 | if (*r1 < *r2) | |
189 | return -1; | |
190 | else if (*r1 == *r2) | |
191 | return 0; | |
192 | else | |
193 | return 1; | |
194 | } | |
195 | ||
7bc7caaf | 196 | struct scmi_clk_ipriv { |
754f04ca | 197 | struct device *dev; |
7bc7caaf CM |
198 | u32 clk_id; |
199 | struct scmi_clock_info *clk; | |
200 | }; | |
5f6c6430 | 201 | |
7bc7caaf CM |
202 | static void iter_clk_describe_prepare_message(void *message, |
203 | const unsigned int desc_index, | |
204 | const void *priv) | |
205 | { | |
206 | struct scmi_msg_clock_describe_rates *msg = message; | |
207 | const struct scmi_clk_ipriv *p = priv; | |
5f6c6430 | 208 | |
7bc7caaf CM |
209 | msg->id = cpu_to_le32(p->clk_id); |
210 | /* Set the number of rates to be skipped/already read */ | |
211 | msg->rate_index = cpu_to_le32(desc_index); | |
212 | } | |
5f6c6430 | 213 | |
7bc7caaf CM |
214 | static int |
215 | iter_clk_describe_update_state(struct scmi_iterator_state *st, | |
216 | const void *response, void *priv) | |
217 | { | |
218 | u32 flags; | |
219 | struct scmi_clk_ipriv *p = priv; | |
220 | const struct scmi_msg_resp_clock_describe_rates *r = response; | |
5f6c6430 | 221 | |
7bc7caaf CM |
222 | flags = le32_to_cpu(r->num_rates_flags); |
223 | st->num_remaining = NUM_REMAINING(flags); | |
224 | st->num_returned = NUM_RETURNED(flags); | |
225 | p->clk->rate_discrete = RATE_DISCRETE(flags); | |
5f6c6430 | 226 | |
754f04ca CM |
227 | /* Warn about out of spec replies ... */ |
228 | if (!p->clk->rate_discrete && | |
229 | (st->num_returned != 3 || st->num_remaining != 0)) { | |
230 | dev_warn(p->dev, | |
231 | "Out-of-spec CLOCK_DESCRIBE_RATES reply for %s - returned:%d remaining:%d rx_len:%zd\n", | |
232 | p->clk->name, st->num_returned, st->num_remaining, | |
233 | st->rx_len); | |
234 | ||
235 | /* | |
236 | * A known quirk: a triplet is returned but num_returned != 3 | |
237 | * Check for a safe payload size and fix. | |
238 | */ | |
239 | if (st->num_returned != 3 && st->num_remaining == 0 && | |
240 | st->rx_len == sizeof(*r) + sizeof(__le32) * 2 * 3) { | |
241 | st->num_returned = 3; | |
242 | st->num_remaining = 0; | |
243 | } else { | |
244 | dev_err(p->dev, | |
245 | "Cannot fix out-of-spec reply !\n"); | |
246 | return -EPROTO; | |
247 | } | |
248 | } | |
249 | ||
7bc7caaf CM |
250 | return 0; |
251 | } | |
5f6c6430 | 252 | |
7bc7caaf CM |
253 | static int |
254 | iter_clk_describe_process_response(const struct scmi_protocol_handle *ph, | |
255 | const void *response, | |
256 | struct scmi_iterator_state *st, void *priv) | |
257 | { | |
258 | int ret = 0; | |
259 | struct scmi_clk_ipriv *p = priv; | |
260 | const struct scmi_msg_resp_clock_describe_rates *r = response; | |
261 | ||
262 | if (!p->clk->rate_discrete) { | |
263 | switch (st->desc_index + st->loop_idx) { | |
264 | case 0: | |
265 | p->clk->range.min_rate = RATE_TO_U64(r->rate[0]); | |
5f6c6430 | 266 | break; |
7bc7caaf CM |
267 | case 1: |
268 | p->clk->range.max_rate = RATE_TO_U64(r->rate[1]); | |
269 | break; | |
270 | case 2: | |
271 | p->clk->range.step_size = RATE_TO_U64(r->rate[2]); | |
272 | break; | |
273 | default: | |
274 | ret = -EINVAL; | |
5f6c6430 SH |
275 | break; |
276 | } | |
7bc7caaf CM |
277 | } else { |
278 | u64 *rate = &p->clk->list.rates[st->desc_index + st->loop_idx]; | |
5f6c6430 | 279 | |
7bc7caaf CM |
280 | *rate = RATE_TO_U64(r->rate[st->loop_idx]); |
281 | p->clk->list.num_rates++; | |
7bc7caaf | 282 | } |
5f6c6430 | 283 | |
7bc7caaf CM |
284 | return ret; |
285 | } | |
9724722f | 286 | |
7bc7caaf CM |
287 | static int |
288 | scmi_clock_describe_rates_get(const struct scmi_protocol_handle *ph, u32 clk_id, | |
289 | struct scmi_clock_info *clk) | |
290 | { | |
291 | int ret; | |
7bc7caaf | 292 | void *iter; |
7bc7caaf CM |
293 | struct scmi_iterator_ops ops = { |
294 | .prepare_message = iter_clk_describe_prepare_message, | |
295 | .update_state = iter_clk_describe_update_state, | |
296 | .process_response = iter_clk_describe_process_response, | |
297 | }; | |
298 | struct scmi_clk_ipriv cpriv = { | |
299 | .clk_id = clk_id, | |
300 | .clk = clk, | |
754f04ca | 301 | .dev = ph->dev, |
7bc7caaf CM |
302 | }; |
303 | ||
304 | iter = ph->hops->iter_response_init(ph, &ops, SCMI_MAX_NUM_RATES, | |
305 | CLOCK_DESCRIBE_RATES, | |
d0c94bef CM |
306 | sizeof(struct scmi_msg_clock_describe_rates), |
307 | &cpriv); | |
7bc7caaf CM |
308 | if (IS_ERR(iter)) |
309 | return PTR_ERR(iter); | |
310 | ||
311 | ret = ph->hops->iter_response_run(iter); | |
312 | if (ret) | |
313 | return ret; | |
5f6c6430 | 314 | |
7bc7caaf CM |
315 | if (!clk->rate_discrete) { |
316 | dev_dbg(ph->dev, "Min %llu Max %llu Step %llu Hz\n", | |
317 | clk->range.min_rate, clk->range.max_rate, | |
318 | clk->range.step_size); | |
319 | } else if (clk->list.num_rates) { | |
320 | sort(clk->list.rates, clk->list.num_rates, | |
321 | sizeof(clk->list.rates[0]), rate_cmp_func, NULL); | |
322 | } | |
c0759b9b | 323 | |
5f6c6430 SH |
324 | return ret; |
325 | } | |
326 | ||
327 | static int | |
887281c7 CM |
328 | scmi_clock_rate_get(const struct scmi_protocol_handle *ph, |
329 | u32 clk_id, u64 *value) | |
5f6c6430 SH |
330 | { |
331 | int ret; | |
332 | struct scmi_xfer *t; | |
333 | ||
887281c7 CM |
334 | ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_GET, |
335 | sizeof(__le32), sizeof(u64), &t); | |
5f6c6430 SH |
336 | if (ret) |
337 | return ret; | |
338 | ||
aa90ac45 | 339 | put_unaligned_le32(clk_id, t->tx.buf); |
5f6c6430 | 340 | |
887281c7 | 341 | ret = ph->xops->do_xfer(ph, t); |
aa90ac45 SH |
342 | if (!ret) |
343 | *value = get_unaligned_le64(t->rx.buf); | |
5f6c6430 | 344 | |
887281c7 | 345 | ph->xops->xfer_put(ph, t); |
5f6c6430 SH |
346 | return ret; |
347 | } | |
348 | ||
887281c7 CM |
349 | static int scmi_clock_rate_set(const struct scmi_protocol_handle *ph, |
350 | u32 clk_id, u64 rate) | |
5f6c6430 SH |
351 | { |
352 | int ret; | |
2bc06ffa | 353 | u32 flags = 0; |
5f6c6430 SH |
354 | struct scmi_xfer *t; |
355 | struct scmi_clock_set_rate *cfg; | |
887281c7 | 356 | struct clock_info *ci = ph->get_priv(ph); |
5f6c6430 | 357 | |
887281c7 | 358 | ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_SET, sizeof(*cfg), 0, &t); |
5f6c6430 SH |
359 | if (ret) |
360 | return ret; | |
361 | ||
2bc06ffa SH |
362 | if (ci->max_async_req && |
363 | atomic_inc_return(&ci->cur_async_req) < ci->max_async_req) | |
364 | flags |= CLOCK_SET_ASYNC; | |
365 | ||
5f6c6430 | 366 | cfg = t->tx.buf; |
2bc06ffa | 367 | cfg->flags = cpu_to_le32(flags); |
5f6c6430 SH |
368 | cfg->id = cpu_to_le32(clk_id); |
369 | cfg->value_low = cpu_to_le32(rate & 0xffffffff); | |
370 | cfg->value_high = cpu_to_le32(rate >> 32); | |
371 | ||
c7e223f5 | 372 | if (flags & CLOCK_SET_ASYNC) { |
887281c7 | 373 | ret = ph->xops->do_xfer_with_response(ph, t); |
c7e223f5 CM |
374 | if (!ret) { |
375 | struct scmi_msg_resp_set_rate_complete *resp; | |
376 | ||
377 | resp = t->rx.buf; | |
378 | if (le32_to_cpu(resp->id) == clk_id) | |
379 | dev_dbg(ph->dev, | |
380 | "Clk ID %d set async to %llu\n", clk_id, | |
381 | get_unaligned_le64(&resp->rate_low)); | |
382 | else | |
383 | ret = -EPROTO; | |
384 | } | |
385 | } else { | |
887281c7 | 386 | ret = ph->xops->do_xfer(ph, t); |
c7e223f5 | 387 | } |
2bc06ffa SH |
388 | |
389 | if (ci->max_async_req) | |
390 | atomic_dec(&ci->cur_async_req); | |
5f6c6430 | 391 | |
887281c7 | 392 | ph->xops->xfer_put(ph, t); |
5f6c6430 SH |
393 | return ret; |
394 | } | |
395 | ||
396 | static int | |
887281c7 | 397 | scmi_clock_config_set(const struct scmi_protocol_handle *ph, u32 clk_id, |
b7bd36f2 | 398 | u32 config, bool atomic) |
5f6c6430 SH |
399 | { |
400 | int ret; | |
401 | struct scmi_xfer *t; | |
402 | struct scmi_clock_set_config *cfg; | |
403 | ||
887281c7 CM |
404 | ret = ph->xops->xfer_get_init(ph, CLOCK_CONFIG_SET, |
405 | sizeof(*cfg), 0, &t); | |
5f6c6430 SH |
406 | if (ret) |
407 | return ret; | |
408 | ||
b7bd36f2 CM |
409 | t->hdr.poll_completion = atomic; |
410 | ||
5f6c6430 SH |
411 | cfg = t->tx.buf; |
412 | cfg->id = cpu_to_le32(clk_id); | |
413 | cfg->attributes = cpu_to_le32(config); | |
414 | ||
887281c7 | 415 | ret = ph->xops->do_xfer(ph, t); |
5f6c6430 | 416 | |
887281c7 | 417 | ph->xops->xfer_put(ph, t); |
5f6c6430 SH |
418 | return ret; |
419 | } | |
420 | ||
887281c7 CM |
421 | static int scmi_clock_enable(const struct scmi_protocol_handle *ph, u32 clk_id) |
422 | { | |
b7bd36f2 | 423 | return scmi_clock_config_set(ph, clk_id, CLOCK_ENABLE, false); |
887281c7 CM |
424 | } |
425 | ||
887281c7 | 426 | static int scmi_clock_disable(const struct scmi_protocol_handle *ph, u32 clk_id) |
5f6c6430 | 427 | { |
b7bd36f2 CM |
428 | return scmi_clock_config_set(ph, clk_id, 0, false); |
429 | } | |
430 | ||
431 | static int scmi_clock_enable_atomic(const struct scmi_protocol_handle *ph, | |
432 | u32 clk_id) | |
433 | { | |
434 | return scmi_clock_config_set(ph, clk_id, CLOCK_ENABLE, true); | |
435 | } | |
436 | ||
437 | static int scmi_clock_disable_atomic(const struct scmi_protocol_handle *ph, | |
438 | u32 clk_id) | |
439 | { | |
440 | return scmi_clock_config_set(ph, clk_id, 0, true); | |
5f6c6430 SH |
441 | } |
442 | ||
887281c7 | 443 | static int scmi_clock_count_get(const struct scmi_protocol_handle *ph) |
5f6c6430 | 444 | { |
887281c7 | 445 | struct clock_info *ci = ph->get_priv(ph); |
5f6c6430 SH |
446 | |
447 | return ci->num_clocks; | |
448 | } | |
449 | ||
450 | static const struct scmi_clock_info * | |
887281c7 | 451 | scmi_clock_info_get(const struct scmi_protocol_handle *ph, u32 clk_id) |
5f6c6430 | 452 | { |
887281c7 | 453 | struct clock_info *ci = ph->get_priv(ph); |
5f6c6430 SH |
454 | struct scmi_clock_info *clk = ci->clk + clk_id; |
455 | ||
d27a3c34 | 456 | if (!clk->name[0]) |
5f6c6430 SH |
457 | return NULL; |
458 | ||
459 | return clk; | |
460 | } | |
461 | ||
887281c7 | 462 | static const struct scmi_clk_proto_ops clk_proto_ops = { |
5f6c6430 SH |
463 | .count_get = scmi_clock_count_get, |
464 | .info_get = scmi_clock_info_get, | |
465 | .rate_get = scmi_clock_rate_get, | |
466 | .rate_set = scmi_clock_rate_set, | |
467 | .enable = scmi_clock_enable, | |
468 | .disable = scmi_clock_disable, | |
b7bd36f2 CM |
469 | .enable_atomic = scmi_clock_enable_atomic, |
470 | .disable_atomic = scmi_clock_disable_atomic, | |
5f6c6430 SH |
471 | }; |
472 | ||
7aa75496 CM |
473 | static int scmi_clk_rate_notify(const struct scmi_protocol_handle *ph, |
474 | u32 clk_id, int message_id, bool enable) | |
475 | { | |
476 | int ret; | |
477 | struct scmi_xfer *t; | |
478 | struct scmi_msg_clock_rate_notify *notify; | |
479 | ||
480 | ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*notify), 0, &t); | |
481 | if (ret) | |
482 | return ret; | |
483 | ||
484 | notify = t->tx.buf; | |
485 | notify->clk_id = cpu_to_le32(clk_id); | |
486 | notify->notify_enable = enable ? cpu_to_le32(BIT(0)) : 0; | |
487 | ||
488 | ret = ph->xops->do_xfer(ph, t); | |
489 | ||
490 | ph->xops->xfer_put(ph, t); | |
491 | return ret; | |
492 | } | |
493 | ||
494 | static int scmi_clk_set_notify_enabled(const struct scmi_protocol_handle *ph, | |
495 | u8 evt_id, u32 src_id, bool enable) | |
496 | { | |
497 | int ret, cmd_id; | |
498 | ||
499 | if (evt_id >= ARRAY_SIZE(evt_2_cmd)) | |
500 | return -EINVAL; | |
501 | ||
502 | cmd_id = evt_2_cmd[evt_id]; | |
503 | ret = scmi_clk_rate_notify(ph, src_id, cmd_id, enable); | |
504 | if (ret) | |
505 | pr_debug("FAIL_ENABLED - evt[%X] dom[%d] - ret:%d\n", | |
506 | evt_id, src_id, ret); | |
507 | ||
508 | return ret; | |
509 | } | |
510 | ||
511 | static void *scmi_clk_fill_custom_report(const struct scmi_protocol_handle *ph, | |
512 | u8 evt_id, ktime_t timestamp, | |
513 | const void *payld, size_t payld_sz, | |
514 | void *report, u32 *src_id) | |
515 | { | |
516 | const struct scmi_clock_rate_notify_payld *p = payld; | |
517 | struct scmi_clock_rate_notif_report *r = report; | |
518 | ||
519 | if (sizeof(*p) != payld_sz || | |
520 | (evt_id != SCMI_EVENT_CLOCK_RATE_CHANGED && | |
521 | evt_id != SCMI_EVENT_CLOCK_RATE_CHANGE_REQUESTED)) | |
522 | return NULL; | |
523 | ||
524 | r->timestamp = timestamp; | |
525 | r->agent_id = le32_to_cpu(p->agent_id); | |
526 | r->clock_id = le32_to_cpu(p->clock_id); | |
527 | r->rate = get_unaligned_le64(&p->rate_low); | |
528 | *src_id = r->clock_id; | |
529 | ||
530 | return r; | |
531 | } | |
532 | ||
533 | static int scmi_clk_get_num_sources(const struct scmi_protocol_handle *ph) | |
534 | { | |
535 | struct clock_info *ci = ph->get_priv(ph); | |
536 | ||
537 | if (!ci) | |
538 | return -EINVAL; | |
539 | ||
540 | return ci->num_clocks; | |
541 | } | |
542 | ||
543 | static const struct scmi_event clk_events[] = { | |
544 | { | |
545 | .id = SCMI_EVENT_CLOCK_RATE_CHANGED, | |
546 | .max_payld_sz = sizeof(struct scmi_clock_rate_notify_payld), | |
547 | .max_report_sz = sizeof(struct scmi_clock_rate_notif_report), | |
548 | }, | |
549 | { | |
550 | .id = SCMI_EVENT_CLOCK_RATE_CHANGE_REQUESTED, | |
551 | .max_payld_sz = sizeof(struct scmi_clock_rate_notify_payld), | |
552 | .max_report_sz = sizeof(struct scmi_clock_rate_notif_report), | |
553 | }, | |
554 | }; | |
555 | ||
556 | static const struct scmi_event_ops clk_event_ops = { | |
557 | .get_num_sources = scmi_clk_get_num_sources, | |
558 | .set_notify_enabled = scmi_clk_set_notify_enabled, | |
559 | .fill_custom_report = scmi_clk_fill_custom_report, | |
560 | }; | |
561 | ||
562 | static const struct scmi_protocol_events clk_protocol_events = { | |
563 | .queue_sz = SCMI_PROTO_QUEUE_SZ, | |
564 | .ops = &clk_event_ops, | |
565 | .evts = clk_events, | |
566 | .num_events = ARRAY_SIZE(clk_events), | |
567 | }; | |
568 | ||
887281c7 | 569 | static int scmi_clock_protocol_init(const struct scmi_protocol_handle *ph) |
5f6c6430 SH |
570 | { |
571 | u32 version; | |
572 | int clkid, ret; | |
573 | struct clock_info *cinfo; | |
574 | ||
4de1b36f CM |
575 | ret = ph->xops->version_get(ph, &version); |
576 | if (ret) | |
577 | return ret; | |
5f6c6430 | 578 | |
887281c7 | 579 | dev_dbg(ph->dev, "Clock Version %d.%d\n", |
5f6c6430 SH |
580 | PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version)); |
581 | ||
887281c7 | 582 | cinfo = devm_kzalloc(ph->dev, sizeof(*cinfo), GFP_KERNEL); |
5f6c6430 SH |
583 | if (!cinfo) |
584 | return -ENOMEM; | |
585 | ||
4de1b36f CM |
586 | ret = scmi_clock_protocol_attributes_get(ph, cinfo); |
587 | if (ret) | |
588 | return ret; | |
5f6c6430 | 589 | |
887281c7 | 590 | cinfo->clk = devm_kcalloc(ph->dev, cinfo->num_clocks, |
5f6c6430 SH |
591 | sizeof(*cinfo->clk), GFP_KERNEL); |
592 | if (!cinfo->clk) | |
593 | return -ENOMEM; | |
594 | ||
595 | for (clkid = 0; clkid < cinfo->num_clocks; clkid++) { | |
596 | struct scmi_clock_info *clk = cinfo->clk + clkid; | |
597 | ||
b260fcca | 598 | ret = scmi_clock_attributes_get(ph, clkid, clk, version); |
5f6c6430 | 599 | if (!ret) |
887281c7 | 600 | scmi_clock_describe_rates_get(ph, clkid, clk); |
5f6c6430 SH |
601 | } |
602 | ||
b55b06b7 | 603 | cinfo->version = version; |
887281c7 | 604 | return ph->set_priv(ph, cinfo); |
5f6c6430 SH |
605 | } |
606 | ||
48dc16e2 CM |
607 | static const struct scmi_protocol scmi_clock = { |
608 | .id = SCMI_PROTOCOL_CLOCK, | |
f5800e0b | 609 | .owner = THIS_MODULE, |
887281c7 CM |
610 | .instance_init = &scmi_clock_protocol_init, |
611 | .ops = &clk_proto_ops, | |
7aa75496 | 612 | .events = &clk_protocol_events, |
48dc16e2 CM |
613 | }; |
614 | ||
615 | DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(clock, scmi_clock) |