drivers/firmware/arm_scmi/clock.c

   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         struct device *dev;
 198         u32 clk_id;
 199         struct scmi_clock_info *clk;
 200 };
 201
 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;
 208
 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 }
 213
 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;
 221
 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);
 226
 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
 250         return 0;
 251 }
 252
 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]);
 266                         break;
 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;
 275                         break;
 276                 }
 277         } else {
 278                 u64 *rate = &p->clk->list.rates[st->desc_index + st->loop_idx];
 279
 280                 *rate = RATE_TO_U64(r->rate[st->loop_idx]);
 281                 p->clk->list.num_rates++;
 282         }
 283
 284         return ret;
 285 }
 286
 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;
 292         void *iter;
 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,
 301                 .dev = ph->dev,
 302         };
 303
 304         iter = ph->hops->iter_response_init(ph, &ops, SCMI_MAX_NUM_RATES,
 305                                             CLOCK_DESCRIBE_RATES,
 306                                             sizeof(struct scmi_msg_clock_describe_rates),
 307                                             &cpriv);
 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;
 314
 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         }
 323
 324         return ret;
 325 }
 326
 327 static int
 328 scmi_clock_rate_get(const struct scmi_protocol_handle *ph,
 329                     u32 clk_id, u64 *value)
 330 {
 331         int ret;
 332         struct scmi_xfer *t;
 333
 334         ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_GET,
 335                                       sizeof(__le32), sizeof(u64), &t);
 336         if (ret)
 337                 return ret;
 338
 339         put_unaligned_le32(clk_id, t->tx.buf);
 340
 341         ret = ph->xops->do_xfer(ph, t);
 342         if (!ret)
 343                 *value = get_unaligned_le64(t->rx.buf);
 344
 345         ph->xops->xfer_put(ph, t);
 346         return ret;
 347 }
 348
 349 static int scmi_clock_rate_set(const struct scmi_protocol_handle *ph,
 350                                u32 clk_id, u64 rate)
 351 {
 352         int ret;
 353         u32 flags = 0;
 354         struct scmi_xfer *t;
 355         struct scmi_clock_set_rate *cfg;
 356         struct clock_info *ci = ph->get_priv(ph);
 357
 358         ret = ph->xops->xfer_get_init(ph, CLOCK_RATE_SET, sizeof(*cfg), 0, &t);
 359         if (ret)
 360                 return ret;
 361
 362         if (ci->max_async_req &&
 363             atomic_inc_return(&ci->cur_async_req) < ci->max_async_req)
 364                 flags |= CLOCK_SET_ASYNC;
 365
 366         cfg = t->tx.buf;
 367         cfg->flags = cpu_to_le32(flags);
 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
 372         if (flags & CLOCK_SET_ASYNC) {
 373                 ret = ph->xops->do_xfer_with_response(ph, t);
 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 {
 386                 ret = ph->xops->do_xfer(ph, t);
 387         }
 388
 389         if (ci->max_async_req)
 390                 atomic_dec(&ci->cur_async_req);
 391
 392         ph->xops->xfer_put(ph, t);
 393         return ret;
 394 }
 395
 396 static int
 397 scmi_clock_config_set(const struct scmi_protocol_handle *ph, u32 clk_id,
 398                       u32 config, bool atomic)
 399 {
 400         int ret;
 401         struct scmi_xfer *t;
 402         struct scmi_clock_set_config *cfg;
 403
 404         ret = ph->xops->xfer_get_init(ph, CLOCK_CONFIG_SET,
 405                                       sizeof(*cfg), 0, &t);
 406         if (ret)
 407                 return ret;
 408
 409         t->hdr.poll_completion = atomic;
 410
 411         cfg = t->tx.buf;
 412         cfg->id = cpu_to_le32(clk_id);
 413         cfg->attributes = cpu_to_le32(config);
 414
 415         ret = ph->xops->do_xfer(ph, t);
 416
 417         ph->xops->xfer_put(ph, t);
 418         return ret;
 419 }
 420
 421 static int scmi_clock_enable(const struct scmi_protocol_handle *ph, u32 clk_id)
 422 {
 423         return scmi_clock_config_set(ph, clk_id, CLOCK_ENABLE, false);
 424 }
 425
 426 static int scmi_clock_disable(const struct scmi_protocol_handle *ph, u32 clk_id)
 427 {
 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);
 441 }
 442
 443 static int scmi_clock_count_get(const struct scmi_protocol_handle *ph)
 444 {
 445         struct clock_info *ci = ph->get_priv(ph);
 446
 447         return ci->num_clocks;
 448 }
 449
 450 static const struct scmi_clock_info *
 451 scmi_clock_info_get(const struct scmi_protocol_handle *ph, u32 clk_id)
 452 {
 453         struct scmi_clock_info *clk;
 454         struct clock_info *ci = ph->get_priv(ph);
 455
 456         if (clk_id >= ci->num_clocks)
 457                 return NULL;
 458
 459         clk = ci->clk + clk_id;
 460         if (!clk->name[0])
 461                 return NULL;
 462
 463         return clk;
 464 }
 465
 466 static const struct scmi_clk_proto_ops clk_proto_ops = {
 467         .count_get = scmi_clock_count_get,
 468         .info_get = scmi_clock_info_get,
 469         .rate_get = scmi_clock_rate_get,
 470         .rate_set = scmi_clock_rate_set,
 471         .enable = scmi_clock_enable,
 472         .disable = scmi_clock_disable,
 473         .enable_atomic = scmi_clock_enable_atomic,
 474         .disable_atomic = scmi_clock_disable_atomic,
 475 };
 476
 477 static int scmi_clk_rate_notify(const struct scmi_protocol_handle *ph,
 478                                 u32 clk_id, int message_id, bool enable)
 479 {
 480         int ret;
 481         struct scmi_xfer *t;
 482         struct scmi_msg_clock_rate_notify *notify;
 483
 484         ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*notify), 0, &t);
 485         if (ret)
 486                 return ret;
 487
 488         notify = t->tx.buf;
 489         notify->clk_id = cpu_to_le32(clk_id);
 490         notify->notify_enable = enable ? cpu_to_le32(BIT(0)) : 0;
 491
 492         ret = ph->xops->do_xfer(ph, t);
 493
 494         ph->xops->xfer_put(ph, t);
 495         return ret;
 496 }
 497
 498 static int scmi_clk_set_notify_enabled(const struct scmi_protocol_handle *ph,
 499                                        u8 evt_id, u32 src_id, bool enable)
 500 {
 501         int ret, cmd_id;
 502
 503         if (evt_id >= ARRAY_SIZE(evt_2_cmd))
 504                 return -EINVAL;
 505
 506         cmd_id = evt_2_cmd[evt_id];
 507         ret = scmi_clk_rate_notify(ph, src_id, cmd_id, enable);
 508         if (ret)
 509                 pr_debug("FAIL_ENABLED - evt[%X] dom[%d] - ret:%d\n",
 510                          evt_id, src_id, ret);
 511
 512         return ret;
 513 }
 514
 515 static void *scmi_clk_fill_custom_report(const struct scmi_protocol_handle *ph,
 516                                          u8 evt_id, ktime_t timestamp,
 517                                          const void *payld, size_t payld_sz,
 518                                          void *report, u32 *src_id)
 519 {
 520         const struct scmi_clock_rate_notify_payld *p = payld;
 521         struct scmi_clock_rate_notif_report *r = report;
 522
 523         if (sizeof(*p) != payld_sz ||
 524             (evt_id != SCMI_EVENT_CLOCK_RATE_CHANGED &&
 525              evt_id != SCMI_EVENT_CLOCK_RATE_CHANGE_REQUESTED))
 526                 return NULL;
 527
 528         r->timestamp = timestamp;
 529         r->agent_id = le32_to_cpu(p->agent_id);
 530         r->clock_id = le32_to_cpu(p->clock_id);
 531         r->rate = get_unaligned_le64(&p->rate_low);
 532         *src_id = r->clock_id;
 533
 534         return r;
 535 }
 536
 537 static int scmi_clk_get_num_sources(const struct scmi_protocol_handle *ph)
 538 {
 539         struct clock_info *ci = ph->get_priv(ph);
 540
 541         if (!ci)
 542                 return -EINVAL;
 543
 544         return ci->num_clocks;
 545 }
 546
 547 static const struct scmi_event clk_events[] = {
 548         {
 549                 .id = SCMI_EVENT_CLOCK_RATE_CHANGED,
 550                 .max_payld_sz = sizeof(struct scmi_clock_rate_notify_payld),
 551                 .max_report_sz = sizeof(struct scmi_clock_rate_notif_report),
 552         },
 553         {
 554                 .id = SCMI_EVENT_CLOCK_RATE_CHANGE_REQUESTED,
 555                 .max_payld_sz = sizeof(struct scmi_clock_rate_notify_payld),
 556                 .max_report_sz = sizeof(struct scmi_clock_rate_notif_report),
 557         },
 558 };
 559
 560 static const struct scmi_event_ops clk_event_ops = {
 561         .get_num_sources = scmi_clk_get_num_sources,
 562         .set_notify_enabled = scmi_clk_set_notify_enabled,
 563         .fill_custom_report = scmi_clk_fill_custom_report,
 564 };
 565
 566 static const struct scmi_protocol_events clk_protocol_events = {
 567         .queue_sz = SCMI_PROTO_QUEUE_SZ,
 568         .ops = &clk_event_ops,
 569         .evts = clk_events,
 570         .num_events = ARRAY_SIZE(clk_events),
 571 };
 572
 573 static int scmi_clock_protocol_init(const struct scmi_protocol_handle *ph)
 574 {
 575         u32 version;
 576         int clkid, ret;
 577         struct clock_info *cinfo;
 578
 579         ret = ph->xops->version_get(ph, &version);
 580         if (ret)
 581                 return ret;
 582
 583         dev_dbg(ph->dev, "Clock Version %d.%d\n",
 584                 PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version));
 585
 586         cinfo = devm_kzalloc(ph->dev, sizeof(*cinfo), GFP_KERNEL);
 587         if (!cinfo)
 588                 return -ENOMEM;
 589
 590         ret = scmi_clock_protocol_attributes_get(ph, cinfo);
 591         if (ret)
 592                 return ret;
 593
 594         cinfo->clk = devm_kcalloc(ph->dev, cinfo->num_clocks,
 595                                   sizeof(*cinfo->clk), GFP_KERNEL);
 596         if (!cinfo->clk)
 597                 return -ENOMEM;
 598
 599         for (clkid = 0; clkid < cinfo->num_clocks; clkid++) {
 600                 struct scmi_clock_info *clk = cinfo->clk + clkid;
 601
 602                 ret = scmi_clock_attributes_get(ph, clkid, clk, version);
 603                 if (!ret)
 604                         scmi_clock_describe_rates_get(ph, clkid, clk);
 605         }
 606
 607         cinfo->version = version;
 608         return ph->set_priv(ph, cinfo);
 609 }
 610
 611 static const struct scmi_protocol scmi_clock = {
 612         .id = SCMI_PROTOCOL_CLOCK,
 613         .owner = THIS_MODULE,
 614         .instance_init = &scmi_clock_protocol_init,
 615         .ops = &clk_proto_ops,
 616         .events = &clk_protocol_events,
 617 };
 618
 619 DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(clock, scmi_clock)