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         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)