1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * System Control and Management Interface (SCMI) Clock Protocol 4 * 5 * Copyright (C) 2018-2022 ARM Ltd. 6 */ 7 8 #include <linux/module.h> 9 #include <linux/limits.h> 10 #include <linux/sort.h> 11 12 #include "protocols.h" 13 #include "notify.h" 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, 21 CLOCK_NAME_GET = 0x8, 22 CLOCK_RATE_NOTIFY = 0x9, 23 CLOCK_RATE_CHANGE_REQUESTED_NOTIFY = 0xA, 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) 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)) 38 u8 name[SCMI_SHORT_NAME_MAX_SIZE]; 39 __le32 clock_enable_latency; 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; 60 } rate[]; 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) 71 #define CLOCK_SET_IGNORE_RESP BIT(1) 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 79 struct scmi_msg_resp_set_rate_complete { 80 __le32 id; 81 __le32 rate_low; 82 __le32 rate_high; 83 }; 84 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 97 struct clock_info { 98 u32 version; 99 int num_clocks; 100 int max_async_req; 101 atomic_t cur_async_req; 102 struct scmi_clock_info *clk; 103 }; 104 105 static enum scmi_clock_protocol_cmd evt_2_cmd[] = { 106 CLOCK_RATE_NOTIFY, 107 CLOCK_RATE_CHANGE_REQUESTED_NOTIFY, 108 }; 109 110 static int 111 scmi_clock_protocol_attributes_get(const struct scmi_protocol_handle *ph, 112 struct clock_info *ci) 113 { 114 int ret; 115 struct scmi_xfer *t; 116 struct scmi_msg_resp_clock_protocol_attributes *attr; 117 118 ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES, 119 0, sizeof(*attr), &t); 120 if (ret) 121 return ret; 122 123 attr = t->rx.buf; 124 125 ret = ph->xops->do_xfer(ph, t); 126 if (!ret) { 127 ci->num_clocks = le16_to_cpu(attr->num_clocks); 128 ci->max_async_req = attr->max_async_req; 129 } 130 131 ph->xops->xfer_put(ph, t); 132 return ret; 133 } 134 135 static int scmi_clock_attributes_get(const struct scmi_protocol_handle *ph, 136 u32 clk_id, struct scmi_clock_info *clk, 137 u32 version) 138 { 139 int ret; 140 u32 attributes; 141 struct scmi_xfer *t; 142 struct scmi_msg_resp_clock_attributes *attr; 143 144 ret = ph->xops->xfer_get_init(ph, CLOCK_ATTRIBUTES, 145 sizeof(clk_id), sizeof(*attr), &t); 146 if (ret) 147 return ret; 148 149 put_unaligned_le32(clk_id, t->tx.buf); 150 attr = t->rx.buf; 151 152 ret = ph->xops->do_xfer(ph, t); 153 if (!ret) { 154 u32 latency = 0; 155 attributes = le32_to_cpu(attr->attributes); 156 strscpy(clk->name, attr->name, SCMI_SHORT_NAME_MAX_SIZE); 157 /* clock_enable_latency field is present only since SCMI v3.1 */ 158 if (PROTOCOL_REV_MAJOR(version) >= 0x2) 159 latency = le32_to_cpu(attr->clock_enable_latency); 160 clk->enable_latency = latency ? : U32_MAX; 161 } 162 163 ph->xops->xfer_put(ph, t); 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 */ 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 } 180 181 return ret; 182 } 183 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 196 struct scmi_clk_ipriv { 197 u32 clk_id; 198 struct scmi_clock_info *clk; 199 }; 200 201 static void iter_clk_describe_prepare_message(void *message, 202 const unsigned int desc_index, 203 const void *priv) 204 { 205 struct scmi_msg_clock_describe_rates *msg = message; 206 const struct scmi_clk_ipriv *p = priv; 207 208 msg->id = cpu_to_le32(p->clk_id); 209 /* Set the number of rates to be skipped/already read */ 210 msg->rate_index = cpu_to_le32(desc_index); 211 } 212 213 static int 214 iter_clk_describe_update_state(struct scmi_iterator_state *st, 215 const void *response, void *priv) 216 { 217 u32 flags; 218 struct scmi_clk_ipriv *p = priv; 219 const struct scmi_msg_resp_clock_describe_rates *r = response; 220 221 flags = le32_to_cpu(r->num_rates_flags); 222 st->num_remaining = NUM_REMAINING(flags); 223 st->num_returned = NUM_RETURNED(flags); 224 p->clk->rate_discrete = RATE_DISCRETE(flags); 225 226 return 0; 227 } 228 229 static int 230 iter_clk_describe_process_response(const struct scmi_protocol_handle *ph, 231 const void *response, 232 struct scmi_iterator_state *st, void *priv) 233 { 234 int ret = 0; 235 struct scmi_clk_ipriv *p = priv; 236 const struct scmi_msg_resp_clock_describe_rates *r = response; 237 238 if (!p->clk->rate_discrete) { 239 switch (st->desc_index + st->loop_idx) { 240 case 0: 241 p->clk->range.min_rate = RATE_TO_U64(r->rate[0]); 242 break; 243 case 1: 244 p->clk->range.max_rate = RATE_TO_U64(r->rate[1]); 245 break; 246 case 2: 247 p->clk->range.step_size = RATE_TO_U64(r->rate[2]); 248 break; 249 default: 250 ret = -EINVAL; 251 break; 252 } 253 } else { 254 u64 *rate = &p->clk->list.rates[st->desc_index + st->loop_idx]; 255 256 *rate = RATE_TO_U64(r->rate[st->loop_idx]); 257 p->clk->list.num_rates++; 258 //XXX dev_dbg(ph->dev, "Rate %llu Hz\n", *rate); 259 } 260 261 return ret; 262 } 263 264 static int 265 scmi_clock_describe_rates_get(const struct scmi_protocol_handle *ph, u32 clk_id, 266 struct scmi_clock_info *clk) 267 { 268 int ret; 269 void *iter; 270 struct scmi_iterator_ops ops = { 271 .prepare_message = iter_clk_describe_prepare_message, 272 .update_state = iter_clk_describe_update_state, 273 .process_response = iter_clk_describe_process_response, 274 }; 275 struct scmi_clk_ipriv cpriv = { 276 .clk_id = clk_id, 277 .clk = clk, 278 }; 279 280 iter = ph->hops->iter_response_init(ph, &ops, SCMI_MAX_NUM_RATES, 281 CLOCK_DESCRIBE_RATES, 282 sizeof(struct scmi_msg_clock_describe_rates), 283 &cpriv); 284 if (IS_ERR(iter)) 285 return PTR_ERR(iter); 286 287 ret = ph->hops->iter_response_run(iter); 288 if (ret) 289 return ret; 290 291 if (!clk->rate_discrete) { 292 dev_dbg(ph->dev, "Min %llu Max %llu Step %llu Hz\n", 293 clk->range.min_rate, clk->range.max_rate, 294 clk->range.step_size); 295 } else if (clk->list.num_rates) { 296 sort(clk->list.rates, clk->list.num_rates, 297 sizeof(clk->list.rates[0]), rate_cmp_func, NULL); 298 } 299 300 return ret; 301 } 302 303 static int 304 scmi_clock_rate_get(const struct scmi_protocol_handle *ph, 305 u32 clk_id, u64 *value) 306 { 307 int ret; 308 struct scmi_xfer *t; 309 310 ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_GET, 311 sizeof(__le32), sizeof(u64), &t); 312 if (ret) 313 return ret; 314 315 put_unaligned_le32(clk_id, t->tx.buf); 316 317 ret = ph->xops->do_xfer(ph, t); 318 if (!ret) 319 *value = get_unaligned_le64(t->rx.buf); 320 321 ph->xops->xfer_put(ph, t); 322 return ret; 323 } 324 325 static int scmi_clock_rate_set(const struct scmi_protocol_handle *ph, 326 u32 clk_id, u64 rate) 327 { 328 int ret; 329 u32 flags = 0; 330 struct scmi_xfer *t; 331 struct scmi_clock_set_rate *cfg; 332 struct clock_info *ci = ph->get_priv(ph); 333 334 ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_SET, sizeof(*cfg), 0, &t); 335 if (ret) 336 return ret; 337 338 if (ci->max_async_req && 339 atomic_inc_return(&ci->cur_async_req) < ci->max_async_req) 340 flags |= CLOCK_SET_ASYNC; 341 342 cfg = t->tx.buf; 343 cfg->flags = cpu_to_le32(flags); 344 cfg->id = cpu_to_le32(clk_id); 345 cfg->value_low = cpu_to_le32(rate & 0xffffffff); 346 cfg->value_high = cpu_to_le32(rate >> 32); 347 348 if (flags & CLOCK_SET_ASYNC) { 349 ret = ph->xops->do_xfer_with_response(ph, t); 350 if (!ret) { 351 struct scmi_msg_resp_set_rate_complete *resp; 352 353 resp = t->rx.buf; 354 if (le32_to_cpu(resp->id) == clk_id) 355 dev_dbg(ph->dev, 356 "Clk ID %d set async to %llu\n", clk_id, 357 get_unaligned_le64(&resp->rate_low)); 358 else 359 ret = -EPROTO; 360 } 361 } else { 362 ret = ph->xops->do_xfer(ph, t); 363 } 364 365 if (ci->max_async_req) 366 atomic_dec(&ci->cur_async_req); 367 368 ph->xops->xfer_put(ph, t); 369 return ret; 370 } 371 372 static int 373 scmi_clock_config_set(const struct scmi_protocol_handle *ph, u32 clk_id, 374 u32 config, bool atomic) 375 { 376 int ret; 377 struct scmi_xfer *t; 378 struct scmi_clock_set_config *cfg; 379 380 ret = ph->xops->xfer_get_init(ph, CLOCK_CONFIG_SET, 381 sizeof(*cfg), 0, &t); 382 if (ret) 383 return ret; 384 385 t->hdr.poll_completion = atomic; 386 387 cfg = t->tx.buf; 388 cfg->id = cpu_to_le32(clk_id); 389 cfg->attributes = cpu_to_le32(config); 390 391 ret = ph->xops->do_xfer(ph, t); 392 393 ph->xops->xfer_put(ph, t); 394 return ret; 395 } 396 397 static int scmi_clock_enable(const struct scmi_protocol_handle *ph, u32 clk_id) 398 { 399 return scmi_clock_config_set(ph, clk_id, CLOCK_ENABLE, false); 400 } 401 402 static int scmi_clock_disable(const struct scmi_protocol_handle *ph, u32 clk_id) 403 { 404 return scmi_clock_config_set(ph, clk_id, 0, false); 405 } 406 407 static int scmi_clock_enable_atomic(const struct scmi_protocol_handle *ph, 408 u32 clk_id) 409 { 410 return scmi_clock_config_set(ph, clk_id, CLOCK_ENABLE, true); 411 } 412 413 static int scmi_clock_disable_atomic(const struct scmi_protocol_handle *ph, 414 u32 clk_id) 415 { 416 return scmi_clock_config_set(ph, clk_id, 0, true); 417 } 418 419 static int scmi_clock_count_get(const struct scmi_protocol_handle *ph) 420 { 421 struct clock_info *ci = ph->get_priv(ph); 422 423 return ci->num_clocks; 424 } 425 426 static const struct scmi_clock_info * 427 scmi_clock_info_get(const struct scmi_protocol_handle *ph, u32 clk_id) 428 { 429 struct clock_info *ci = ph->get_priv(ph); 430 struct scmi_clock_info *clk = ci->clk + clk_id; 431 432 if (!clk->name[0]) 433 return NULL; 434 435 return clk; 436 } 437 438 static const struct scmi_clk_proto_ops clk_proto_ops = { 439 .count_get = scmi_clock_count_get, 440 .info_get = scmi_clock_info_get, 441 .rate_get = scmi_clock_rate_get, 442 .rate_set = scmi_clock_rate_set, 443 .enable = scmi_clock_enable, 444 .disable = scmi_clock_disable, 445 .enable_atomic = scmi_clock_enable_atomic, 446 .disable_atomic = scmi_clock_disable_atomic, 447 }; 448 449 static int scmi_clk_rate_notify(const struct scmi_protocol_handle *ph, 450 u32 clk_id, int message_id, bool enable) 451 { 452 int ret; 453 struct scmi_xfer *t; 454 struct scmi_msg_clock_rate_notify *notify; 455 456 ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*notify), 0, &t); 457 if (ret) 458 return ret; 459 460 notify = t->tx.buf; 461 notify->clk_id = cpu_to_le32(clk_id); 462 notify->notify_enable = enable ? cpu_to_le32(BIT(0)) : 0; 463 464 ret = ph->xops->do_xfer(ph, t); 465 466 ph->xops->xfer_put(ph, t); 467 return ret; 468 } 469 470 static int scmi_clk_set_notify_enabled(const struct scmi_protocol_handle *ph, 471 u8 evt_id, u32 src_id, bool enable) 472 { 473 int ret, cmd_id; 474 475 if (evt_id >= ARRAY_SIZE(evt_2_cmd)) 476 return -EINVAL; 477 478 cmd_id = evt_2_cmd[evt_id]; 479 ret = scmi_clk_rate_notify(ph, src_id, cmd_id, enable); 480 if (ret) 481 pr_debug("FAIL_ENABLED - evt[%X] dom[%d] - ret:%d\n", 482 evt_id, src_id, ret); 483 484 return ret; 485 } 486 487 static void *scmi_clk_fill_custom_report(const struct scmi_protocol_handle *ph, 488 u8 evt_id, ktime_t timestamp, 489 const void *payld, size_t payld_sz, 490 void *report, u32 *src_id) 491 { 492 const struct scmi_clock_rate_notify_payld *p = payld; 493 struct scmi_clock_rate_notif_report *r = report; 494 495 if (sizeof(*p) != payld_sz || 496 (evt_id != SCMI_EVENT_CLOCK_RATE_CHANGED && 497 evt_id != SCMI_EVENT_CLOCK_RATE_CHANGE_REQUESTED)) 498 return NULL; 499 500 r->timestamp = timestamp; 501 r->agent_id = le32_to_cpu(p->agent_id); 502 r->clock_id = le32_to_cpu(p->clock_id); 503 r->rate = get_unaligned_le64(&p->rate_low); 504 *src_id = r->clock_id; 505 506 return r; 507 } 508 509 static int scmi_clk_get_num_sources(const struct scmi_protocol_handle *ph) 510 { 511 struct clock_info *ci = ph->get_priv(ph); 512 513 if (!ci) 514 return -EINVAL; 515 516 return ci->num_clocks; 517 } 518 519 static const struct scmi_event clk_events[] = { 520 { 521 .id = SCMI_EVENT_CLOCK_RATE_CHANGED, 522 .max_payld_sz = sizeof(struct scmi_clock_rate_notify_payld), 523 .max_report_sz = sizeof(struct scmi_clock_rate_notif_report), 524 }, 525 { 526 .id = SCMI_EVENT_CLOCK_RATE_CHANGE_REQUESTED, 527 .max_payld_sz = sizeof(struct scmi_clock_rate_notify_payld), 528 .max_report_sz = sizeof(struct scmi_clock_rate_notif_report), 529 }, 530 }; 531 532 static const struct scmi_event_ops clk_event_ops = { 533 .get_num_sources = scmi_clk_get_num_sources, 534 .set_notify_enabled = scmi_clk_set_notify_enabled, 535 .fill_custom_report = scmi_clk_fill_custom_report, 536 }; 537 538 static const struct scmi_protocol_events clk_protocol_events = { 539 .queue_sz = SCMI_PROTO_QUEUE_SZ, 540 .ops = &clk_event_ops, 541 .evts = clk_events, 542 .num_events = ARRAY_SIZE(clk_events), 543 }; 544 545 static int scmi_clock_protocol_init(const struct scmi_protocol_handle *ph) 546 { 547 u32 version; 548 int clkid, ret; 549 struct clock_info *cinfo; 550 551 ret = ph->xops->version_get(ph, &version); 552 if (ret) 553 return ret; 554 555 dev_dbg(ph->dev, "Clock Version %d.%d\n", 556 PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version)); 557 558 cinfo = devm_kzalloc(ph->dev, sizeof(*cinfo), GFP_KERNEL); 559 if (!cinfo) 560 return -ENOMEM; 561 562 ret = scmi_clock_protocol_attributes_get(ph, cinfo); 563 if (ret) 564 return ret; 565 566 cinfo->clk = devm_kcalloc(ph->dev, cinfo->num_clocks, 567 sizeof(*cinfo->clk), GFP_KERNEL); 568 if (!cinfo->clk) 569 return -ENOMEM; 570 571 for (clkid = 0; clkid < cinfo->num_clocks; clkid++) { 572 struct scmi_clock_info *clk = cinfo->clk + clkid; 573 574 ret = scmi_clock_attributes_get(ph, clkid, clk, version); 575 if (!ret) 576 scmi_clock_describe_rates_get(ph, clkid, clk); 577 } 578 579 cinfo->version = version; 580 return ph->set_priv(ph, cinfo); 581 } 582 583 static const struct scmi_protocol scmi_clock = { 584 .id = SCMI_PROTOCOL_CLOCK, 585 .owner = THIS_MODULE, 586 .instance_init = &scmi_clock_protocol_init, 587 .ops = &clk_proto_ops, 588 .events = &clk_protocol_events, 589 }; 590 591 DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(clock, scmi_clock) 592