drivers/firmware/arm_scmi/perf.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * System Control and Management Interface (SCMI) Performance Protocol
   4  *
   5  * Copyright (C) 2018-2021 ARM Ltd.
   6  */
   7
   8 #define pr_fmt(fmt) "SCMI Notifications PERF - " fmt
   9
  10 #include <linux/bits.h>
  11 #include <linux/of.h>
  12 #include <linux/io.h>
  13 #include <linux/io-64-nonatomic-hi-lo.h>
  14 #include <linux/module.h>
  15 #include <linux/platform_device.h>
  16 #include <linux/pm_opp.h>
  17 #include <linux/scmi_protocol.h>
  18 #include <linux/sort.h>
  19
  20 #include "common.h"
  21 #include "notify.h"
  22
  23 enum scmi_performance_protocol_cmd {
  24         PERF_DOMAIN_ATTRIBUTES = 0x3,
  25         PERF_DESCRIBE_LEVELS = 0x4,
  26         PERF_LIMITS_SET = 0x5,
  27         PERF_LIMITS_GET = 0x6,
  28         PERF_LEVEL_SET = 0x7,
  29         PERF_LEVEL_GET = 0x8,
  30         PERF_NOTIFY_LIMITS = 0x9,
  31         PERF_NOTIFY_LEVEL = 0xa,
  32         PERF_DESCRIBE_FASTCHANNEL = 0xb,
  33 };
  34
  35 struct scmi_opp {
  36         u32 perf;
  37         u32 power;
  38         u32 trans_latency_us;
  39 };
  40
  41 struct scmi_msg_resp_perf_attributes {
  42         __le16 num_domains;
  43         __le16 flags;
  44 #define POWER_SCALE_IN_MILLIWATT(x)     ((x) & BIT(0))
  45         __le32 stats_addr_low;
  46         __le32 stats_addr_high;
  47         __le32 stats_size;
  48 };
  49
  50 struct scmi_msg_resp_perf_domain_attributes {
  51         __le32 flags;
  52 #define SUPPORTS_SET_LIMITS(x)          ((x) & BIT(31))
  53 #define SUPPORTS_SET_PERF_LVL(x)        ((x) & BIT(30))
  54 #define SUPPORTS_PERF_LIMIT_NOTIFY(x)   ((x) & BIT(29))
  55 #define SUPPORTS_PERF_LEVEL_NOTIFY(x)   ((x) & BIT(28))
  56 #define SUPPORTS_PERF_FASTCHANNELS(x)   ((x) & BIT(27))
  57         __le32 rate_limit_us;
  58         __le32 sustained_freq_khz;
  59         __le32 sustained_perf_level;
  60             u8 name[SCMI_MAX_STR_SIZE];
  61 };
  62
  63 struct scmi_msg_perf_describe_levels {
  64         __le32 domain;
  65         __le32 level_index;
  66 };
  67
  68 struct scmi_perf_set_limits {
  69         __le32 domain;
  70         __le32 max_level;
  71         __le32 min_level;
  72 };
  73
  74 struct scmi_perf_get_limits {
  75         __le32 max_level;
  76         __le32 min_level;
  77 };
  78
  79 struct scmi_perf_set_level {
  80         __le32 domain;
  81         __le32 level;
  82 };
  83
  84 struct scmi_perf_notify_level_or_limits {
  85         __le32 domain;
  86         __le32 notify_enable;
  87 };
  88
  89 struct scmi_perf_limits_notify_payld {
  90         __le32 agent_id;
  91         __le32 domain_id;
  92         __le32 range_max;
  93         __le32 range_min;
  94 };
  95
  96 struct scmi_perf_level_notify_payld {
  97         __le32 agent_id;
  98         __le32 domain_id;
  99         __le32 performance_level;
 100 };
 101
 102 struct scmi_msg_resp_perf_describe_levels {
 103         __le16 num_returned;
 104         __le16 num_remaining;
 105         struct {
 106                 __le32 perf_val;
 107                 __le32 power;
 108                 __le16 transition_latency_us;
 109                 __le16 reserved;
 110         } opp[];
 111 };
 112
 113 struct scmi_perf_get_fc_info {
 114         __le32 domain;
 115         __le32 message_id;
 116 };
 117
 118 struct scmi_msg_resp_perf_desc_fc {
 119         __le32 attr;
 120 #define SUPPORTS_DOORBELL(x)            ((x) & BIT(0))
 121 #define DOORBELL_REG_WIDTH(x)           FIELD_GET(GENMASK(2, 1), (x))
 122         __le32 rate_limit;
 123         __le32 chan_addr_low;
 124         __le32 chan_addr_high;
 125         __le32 chan_size;
 126         __le32 db_addr_low;
 127         __le32 db_addr_high;
 128         __le32 db_set_lmask;
 129         __le32 db_set_hmask;
 130         __le32 db_preserve_lmask;
 131         __le32 db_preserve_hmask;
 132 };
 133
 134 struct scmi_fc_db_info {
 135         int width;
 136         u64 set;
 137         u64 mask;
 138         void __iomem *addr;
 139 };
 140
 141 struct scmi_fc_info {
 142         void __iomem *level_set_addr;
 143         void __iomem *limit_set_addr;
 144         void __iomem *level_get_addr;
 145         void __iomem *limit_get_addr;
 146         struct scmi_fc_db_info *level_set_db;
 147         struct scmi_fc_db_info *limit_set_db;
 148 };
 149
 150 struct perf_dom_info {
 151         bool set_limits;
 152         bool set_perf;
 153         bool perf_limit_notify;
 154         bool perf_level_notify;
 155         bool perf_fastchannels;
 156         u32 opp_count;
 157         u32 sustained_freq_khz;
 158         u32 sustained_perf_level;
 159         u32 mult_factor;
 160         char name[SCMI_MAX_STR_SIZE];
 161         struct scmi_opp opp[MAX_OPPS];
 162         struct scmi_fc_info *fc_info;
 163 };
 164
 165 struct scmi_perf_info {
 166         u32 version;
 167         int num_domains;
 168         bool power_scale_mw;
 169         u64 stats_addr;
 170         u32 stats_size;
 171         struct perf_dom_info *dom_info;
 172 };
 173
 174 static enum scmi_performance_protocol_cmd evt_2_cmd[] = {
 175         PERF_NOTIFY_LIMITS,
 176         PERF_NOTIFY_LEVEL,
 177 };
 178
 179 static int scmi_perf_attributes_get(const struct scmi_protocol_handle *ph,
 180                                     struct scmi_perf_info *pi)
 181 {
 182         int ret;
 183         struct scmi_xfer *t;
 184         struct scmi_msg_resp_perf_attributes *attr;
 185
 186         ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES, 0,
 187                                       sizeof(*attr), &t);
 188         if (ret)
 189                 return ret;
 190
 191         attr = t->rx.buf;
 192
 193         ret = ph->xops->do_xfer(ph, t);
 194         if (!ret) {
 195                 u16 flags = le16_to_cpu(attr->flags);
 196
 197                 pi->num_domains = le16_to_cpu(attr->num_domains);
 198                 pi->power_scale_mw = POWER_SCALE_IN_MILLIWATT(flags);
 199                 pi->stats_addr = le32_to_cpu(attr->stats_addr_low) |
 200                                 (u64)le32_to_cpu(attr->stats_addr_high) << 32;
 201                 pi->stats_size = le32_to_cpu(attr->stats_size);
 202         }
 203
 204         ph->xops->xfer_put(ph, t);
 205         return ret;
 206 }
 207
 208 static int
 209 scmi_perf_domain_attributes_get(const struct scmi_protocol_handle *ph,
 210                                 u32 domain, struct perf_dom_info *dom_info)
 211 {
 212         int ret;
 213         struct scmi_xfer *t;
 214         struct scmi_msg_resp_perf_domain_attributes *attr;
 215
 216         ret = ph->xops->xfer_get_init(ph, PERF_DOMAIN_ATTRIBUTES,
 217                                      sizeof(domain), sizeof(*attr), &t);
 218         if (ret)
 219                 return ret;
 220
 221         put_unaligned_le32(domain, t->tx.buf);
 222         attr = t->rx.buf;
 223
 224         ret = ph->xops->do_xfer(ph, t);
 225         if (!ret) {
 226                 u32 flags = le32_to_cpu(attr->flags);
 227
 228                 dom_info->set_limits = SUPPORTS_SET_LIMITS(flags);
 229                 dom_info->set_perf = SUPPORTS_SET_PERF_LVL(flags);
 230                 dom_info->perf_limit_notify = SUPPORTS_PERF_LIMIT_NOTIFY(flags);
 231                 dom_info->perf_level_notify = SUPPORTS_PERF_LEVEL_NOTIFY(flags);
 232                 dom_info->perf_fastchannels = SUPPORTS_PERF_FASTCHANNELS(flags);
 233                 dom_info->sustained_freq_khz =
 234                                         le32_to_cpu(attr->sustained_freq_khz);
 235                 dom_info->sustained_perf_level =
 236                                         le32_to_cpu(attr->sustained_perf_level);
 237                 if (!dom_info->sustained_freq_khz ||
 238                     !dom_info->sustained_perf_level)
 239                         /* CPUFreq converts to kHz, hence default 1000 */
 240                         dom_info->mult_factor = 1000;
 241                 else
 242                         dom_info->mult_factor =
 243                                         (dom_info->sustained_freq_khz * 1000) /
 244                                         dom_info->sustained_perf_level;
 245                 strlcpy(dom_info->name, attr->name, SCMI_MAX_STR_SIZE);
 246         }
 247
 248         ph->xops->xfer_put(ph, t);
 249         return ret;
 250 }
 251
 252 static int opp_cmp_func(const void *opp1, const void *opp2)
 253 {
 254         const struct scmi_opp *t1 = opp1, *t2 = opp2;
 255
 256         return t1->perf - t2->perf;
 257 }
 258
 259 static int
 260 scmi_perf_describe_levels_get(const struct scmi_protocol_handle *ph, u32 domain,
 261                               struct perf_dom_info *perf_dom)
 262 {
 263         int ret, cnt;
 264         u32 tot_opp_cnt = 0;
 265         u16 num_returned, num_remaining;
 266         struct scmi_xfer *t;
 267         struct scmi_opp *opp;
 268         struct scmi_msg_perf_describe_levels *dom_info;
 269         struct scmi_msg_resp_perf_describe_levels *level_info;
 270
 271         ret = ph->xops->xfer_get_init(ph, PERF_DESCRIBE_LEVELS,
 272                                       sizeof(*dom_info), 0, &t);
 273         if (ret)
 274                 return ret;
 275
 276         dom_info = t->tx.buf;
 277         level_info = t->rx.buf;
 278
 279         do {
 280                 dom_info->domain = cpu_to_le32(domain);
 281                 /* Set the number of OPPs to be skipped/already read */
 282                 dom_info->level_index = cpu_to_le32(tot_opp_cnt);
 283
 284                 ret = ph->xops->do_xfer(ph, t);
 285                 if (ret)
 286                         break;
 287
 288                 num_returned = le16_to_cpu(level_info->num_returned);
 289                 num_remaining = le16_to_cpu(level_info->num_remaining);
 290                 if (tot_opp_cnt + num_returned > MAX_OPPS) {
 291                         dev_err(ph->dev, "No. of OPPs exceeded MAX_OPPS");
 292                         break;
 293                 }
 294
 295                 opp = &perf_dom->opp[tot_opp_cnt];
 296                 for (cnt = 0; cnt < num_returned; cnt++, opp++) {
 297                         opp->perf = le32_to_cpu(level_info->opp[cnt].perf_val);
 298                         opp->power = le32_to_cpu(level_info->opp[cnt].power);
 299                         opp->trans_latency_us = le16_to_cpu
 300                                 (level_info->opp[cnt].transition_latency_us);
 301
 302                         dev_dbg(ph->dev, "Level %d Power %d Latency %dus\n",
 303                                 opp->perf, opp->power, opp->trans_latency_us);
 304                 }
 305
 306                 tot_opp_cnt += num_returned;
 307
 308                 ph->xops->reset_rx_to_maxsz(ph, t);
 309                 /*
 310                  * check for both returned and remaining to avoid infinite
 311                  * loop due to buggy firmware
 312                  */
 313         } while (num_returned && num_remaining);
 314
 315         perf_dom->opp_count = tot_opp_cnt;
 316         ph->xops->xfer_put(ph, t);
 317
 318         sort(perf_dom->opp, tot_opp_cnt, sizeof(*opp), opp_cmp_func, NULL);
 319         return ret;
 320 }
 321
 322 #define SCMI_PERF_FC_RING_DB(w)                         \
 323 do {                                                    \
 324         u##w val = 0;                                   \
 325                                                         \
 326         if (db->mask)                                   \
 327                 val = ioread##w(db->addr) & db->mask;   \
 328         iowrite##w((u##w)db->set | val, db->addr);      \
 329 } while (0)
 330
 331 static void scmi_perf_fc_ring_db(struct scmi_fc_db_info *db)
 332 {
 333         if (!db || !db->addr)
 334                 return;
 335
 336         if (db->width == 1)
 337                 SCMI_PERF_FC_RING_DB(8);
 338         else if (db->width == 2)
 339                 SCMI_PERF_FC_RING_DB(16);
 340         else if (db->width == 4)
 341                 SCMI_PERF_FC_RING_DB(32);
 342         else /* db->width == 8 */
 343 #ifdef CONFIG_64BIT
 344                 SCMI_PERF_FC_RING_DB(64);
 345 #else
 346         {
 347                 u64 val = 0;
 348
 349                 if (db->mask)
 350                         val = ioread64_hi_lo(db->addr) & db->mask;
 351                 iowrite64_hi_lo(db->set | val, db->addr);
 352         }
 353 #endif
 354 }
 355
 356 static int scmi_perf_mb_limits_set(const struct scmi_protocol_handle *ph,
 357                                    u32 domain, u32 max_perf, u32 min_perf)
 358 {
 359         int ret;
 360         struct scmi_xfer *t;
 361         struct scmi_perf_set_limits *limits;
 362
 363         ret = ph->xops->xfer_get_init(ph, PERF_LIMITS_SET,
 364                                       sizeof(*limits), 0, &t);
 365         if (ret)
 366                 return ret;
 367
 368         limits = t->tx.buf;
 369         limits->domain = cpu_to_le32(domain);
 370         limits->max_level = cpu_to_le32(max_perf);
 371         limits->min_level = cpu_to_le32(min_perf);
 372
 373         ret = ph->xops->do_xfer(ph, t);
 374
 375         ph->xops->xfer_put(ph, t);
 376         return ret;
 377 }
 378
 379 static int scmi_perf_limits_set(const struct scmi_protocol_handle *ph,
 380                                 u32 domain, u32 max_perf, u32 min_perf)
 381 {
 382         struct scmi_perf_info *pi = ph->get_priv(ph);
 383         struct perf_dom_info *dom = pi->dom_info + domain;
 384
 385         if (dom->fc_info && dom->fc_info->limit_set_addr) {
 386                 iowrite32(max_perf, dom->fc_info->limit_set_addr);
 387                 iowrite32(min_perf, dom->fc_info->limit_set_addr + 4);
 388                 scmi_perf_fc_ring_db(dom->fc_info->limit_set_db);
 389                 return 0;
 390         }
 391
 392         return scmi_perf_mb_limits_set(ph, domain, max_perf, min_perf);
 393 }
 394
 395 static int scmi_perf_mb_limits_get(const struct scmi_protocol_handle *ph,
 396                                    u32 domain, u32 *max_perf, u32 *min_perf)
 397 {
 398         int ret;
 399         struct scmi_xfer *t;
 400         struct scmi_perf_get_limits *limits;
 401
 402         ret = ph->xops->xfer_get_init(ph, PERF_LIMITS_GET,
 403                                       sizeof(__le32), 0, &t);
 404         if (ret)
 405                 return ret;
 406
 407         put_unaligned_le32(domain, t->tx.buf);
 408
 409         ret = ph->xops->do_xfer(ph, t);
 410         if (!ret) {
 411                 limits = t->rx.buf;
 412
 413                 *max_perf = le32_to_cpu(limits->max_level);
 414                 *min_perf = le32_to_cpu(limits->min_level);
 415         }
 416
 417         ph->xops->xfer_put(ph, t);
 418         return ret;
 419 }
 420
 421 static int scmi_perf_limits_get(const struct scmi_protocol_handle *ph,
 422                                 u32 domain, u32 *max_perf, u32 *min_perf)
 423 {
 424         struct scmi_perf_info *pi = ph->get_priv(ph);
 425         struct perf_dom_info *dom = pi->dom_info + domain;
 426
 427         if (dom->fc_info && dom->fc_info->limit_get_addr) {
 428                 *max_perf = ioread32(dom->fc_info->limit_get_addr);
 429                 *min_perf = ioread32(dom->fc_info->limit_get_addr + 4);
 430                 return 0;
 431         }
 432
 433         return scmi_perf_mb_limits_get(ph, domain, max_perf, min_perf);
 434 }
 435
 436 static int scmi_perf_mb_level_set(const struct scmi_protocol_handle *ph,
 437                                   u32 domain, u32 level, bool poll)
 438 {
 439         int ret;
 440         struct scmi_xfer *t;
 441         struct scmi_perf_set_level *lvl;
 442
 443         ret = ph->xops->xfer_get_init(ph, PERF_LEVEL_SET, sizeof(*lvl), 0, &t);
 444         if (ret)
 445                 return ret;
 446
 447         t->hdr.poll_completion = poll;
 448         lvl = t->tx.buf;
 449         lvl->domain = cpu_to_le32(domain);
 450         lvl->level = cpu_to_le32(level);
 451
 452         ret = ph->xops->do_xfer(ph, t);
 453
 454         ph->xops->xfer_put(ph, t);
 455         return ret;
 456 }
 457
 458 static int scmi_perf_level_set(const struct scmi_protocol_handle *ph,
 459                                u32 domain, u32 level, bool poll)
 460 {
 461         struct scmi_perf_info *pi = ph->get_priv(ph);
 462         struct perf_dom_info *dom = pi->dom_info + domain;
 463
 464         if (dom->fc_info && dom->fc_info->level_set_addr) {
 465                 iowrite32(level, dom->fc_info->level_set_addr);
 466                 scmi_perf_fc_ring_db(dom->fc_info->level_set_db);
 467                 return 0;
 468         }
 469
 470         return scmi_perf_mb_level_set(ph, domain, level, poll);
 471 }
 472
 473 static int scmi_perf_mb_level_get(const struct scmi_protocol_handle *ph,
 474                                   u32 domain, u32 *level, bool poll)
 475 {
 476         int ret;
 477         struct scmi_xfer *t;
 478
 479         ret = ph->xops->xfer_get_init(ph, PERF_LEVEL_GET,
 480                                      sizeof(u32), sizeof(u32), &t);
 481         if (ret)
 482                 return ret;
 483
 484         t->hdr.poll_completion = poll;
 485         put_unaligned_le32(domain, t->tx.buf);
 486
 487         ret = ph->xops->do_xfer(ph, t);
 488         if (!ret)
 489                 *level = get_unaligned_le32(t->rx.buf);
 490
 491         ph->xops->xfer_put(ph, t);
 492         return ret;
 493 }
 494
 495 static int scmi_perf_level_get(const struct scmi_protocol_handle *ph,
 496                                u32 domain, u32 *level, bool poll)
 497 {
 498         struct scmi_perf_info *pi = ph->get_priv(ph);
 499         struct perf_dom_info *dom = pi->dom_info + domain;
 500
 501         if (dom->fc_info && dom->fc_info->level_get_addr) {
 502                 *level = ioread32(dom->fc_info->level_get_addr);
 503                 return 0;
 504         }
 505
 506         return scmi_perf_mb_level_get(ph, domain, level, poll);
 507 }
 508
 509 static int scmi_perf_level_limits_notify(const struct scmi_protocol_handle *ph,
 510                                          u32 domain, int message_id,
 511                                          bool enable)
 512 {
 513         int ret;
 514         struct scmi_xfer *t;
 515         struct scmi_perf_notify_level_or_limits *notify;
 516
 517         ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*notify), 0, &t);
 518         if (ret)
 519                 return ret;
 520
 521         notify = t->tx.buf;
 522         notify->domain = cpu_to_le32(domain);
 523         notify->notify_enable = enable ? cpu_to_le32(BIT(0)) : 0;
 524
 525         ret = ph->xops->do_xfer(ph, t);
 526
 527         ph->xops->xfer_put(ph, t);
 528         return ret;
 529 }
 530
 531 static bool scmi_perf_fc_size_is_valid(u32 msg, u32 size)
 532 {
 533         if ((msg == PERF_LEVEL_GET || msg == PERF_LEVEL_SET) && size == 4)
 534                 return true;
 535         if ((msg == PERF_LIMITS_GET || msg == PERF_LIMITS_SET) && size == 8)
 536                 return true;
 537         return false;
 538 }
 539
 540 static void
 541 scmi_perf_domain_desc_fc(const struct scmi_protocol_handle *ph, u32 domain,
 542                          u32 message_id, void __iomem **p_addr,
 543                          struct scmi_fc_db_info **p_db)
 544 {
 545         int ret;
 546         u32 flags;
 547         u64 phys_addr;
 548         u8 size;
 549         void __iomem *addr;
 550         struct scmi_xfer *t;
 551         struct scmi_fc_db_info *db;
 552         struct scmi_perf_get_fc_info *info;
 553         struct scmi_msg_resp_perf_desc_fc *resp;
 554
 555         if (!p_addr)
 556                 return;
 557
 558         ret = ph->xops->xfer_get_init(ph, PERF_DESCRIBE_FASTCHANNEL,
 559                                       sizeof(*info), sizeof(*resp), &t);
 560         if (ret)
 561                 return;
 562
 563         info = t->tx.buf;
 564         info->domain = cpu_to_le32(domain);
 565         info->message_id = cpu_to_le32(message_id);
 566
 567         ret = ph->xops->do_xfer(ph, t);
 568         if (ret)
 569                 goto err_xfer;
 570
 571         resp = t->rx.buf;
 572         flags = le32_to_cpu(resp->attr);
 573         size = le32_to_cpu(resp->chan_size);
 574         if (!scmi_perf_fc_size_is_valid(message_id, size))
 575                 goto err_xfer;
 576
 577         phys_addr = le32_to_cpu(resp->chan_addr_low);
 578         phys_addr |= (u64)le32_to_cpu(resp->chan_addr_high) << 32;
 579         addr = devm_ioremap(ph->dev, phys_addr, size);
 580         if (!addr)
 581                 goto err_xfer;
 582         *p_addr = addr;
 583
 584         if (p_db && SUPPORTS_DOORBELL(flags)) {
 585                 db = devm_kzalloc(ph->dev, sizeof(*db), GFP_KERNEL);
 586                 if (!db)
 587                         goto err_xfer;
 588
 589                 size = 1 << DOORBELL_REG_WIDTH(flags);
 590                 phys_addr = le32_to_cpu(resp->db_addr_low);
 591                 phys_addr |= (u64)le32_to_cpu(resp->db_addr_high) << 32;
 592                 addr = devm_ioremap(ph->dev, phys_addr, size);
 593                 if (!addr)
 594                         goto err_xfer;
 595
 596                 db->addr = addr;
 597                 db->width = size;
 598                 db->set = le32_to_cpu(resp->db_set_lmask);
 599                 db->set |= (u64)le32_to_cpu(resp->db_set_hmask) << 32;
 600                 db->mask = le32_to_cpu(resp->db_preserve_lmask);
 601                 db->mask |= (u64)le32_to_cpu(resp->db_preserve_hmask) << 32;
 602                 *p_db = db;
 603         }
 604 err_xfer:
 605         ph->xops->xfer_put(ph, t);
 606 }
 607
 608 static void scmi_perf_domain_init_fc(const struct scmi_protocol_handle *ph,
 609                                      u32 domain, struct scmi_fc_info **p_fc)
 610 {
 611         struct scmi_fc_info *fc;
 612
 613         fc = devm_kzalloc(ph->dev, sizeof(*fc), GFP_KERNEL);
 614         if (!fc)
 615                 return;
 616
 617         scmi_perf_domain_desc_fc(ph, domain, PERF_LEVEL_SET,
 618                                  &fc->level_set_addr, &fc->level_set_db);
 619         scmi_perf_domain_desc_fc(ph, domain, PERF_LEVEL_GET,
 620                                  &fc->level_get_addr, NULL);
 621         scmi_perf_domain_desc_fc(ph, domain, PERF_LIMITS_SET,
 622                                  &fc->limit_set_addr, &fc->limit_set_db);
 623         scmi_perf_domain_desc_fc(ph, domain, PERF_LIMITS_GET,
 624                                  &fc->limit_get_addr, NULL);
 625         *p_fc = fc;
 626 }
 627
 628 /* Device specific ops */
 629 static int scmi_dev_domain_id(struct device *dev)
 630 {
 631         struct of_phandle_args clkspec;
 632
 633         if (of_parse_phandle_with_args(dev->of_node, "clocks", "#clock-cells",
 634                                        0, &clkspec))
 635                 return -EINVAL;
 636
 637         return clkspec.args[0];
 638 }
 639
 640 static int scmi_dvfs_device_opps_add(const struct scmi_protocol_handle *ph,
 641                                      struct device *dev)
 642 {
 643         int idx, ret, domain;
 644         unsigned long freq;
 645         struct scmi_opp *opp;
 646         struct perf_dom_info *dom;
 647         struct scmi_perf_info *pi = ph->get_priv(ph);
 648
 649         domain = scmi_dev_domain_id(dev);
 650         if (domain < 0)
 651                 return domain;
 652
 653         dom = pi->dom_info + domain;
 654
 655         for (opp = dom->opp, idx = 0; idx < dom->opp_count; idx++, opp++) {
 656                 freq = opp->perf * dom->mult_factor;
 657
 658                 ret = dev_pm_opp_add(dev, freq, 0);
 659                 if (ret) {
 660                         dev_warn(dev, "failed to add opp %luHz\n", freq);
 661
 662                         while (idx-- > 0) {
 663                                 freq = (--opp)->perf * dom->mult_factor;
 664                                 dev_pm_opp_remove(dev, freq);
 665                         }
 666                         return ret;
 667                 }
 668         }
 669         return 0;
 670 }
 671
 672 static int
 673 scmi_dvfs_transition_latency_get(const struct scmi_protocol_handle *ph,
 674                                  struct device *dev)
 675 {
 676         struct perf_dom_info *dom;
 677         struct scmi_perf_info *pi = ph->get_priv(ph);
 678         int domain = scmi_dev_domain_id(dev);
 679
 680         if (domain < 0)
 681                 return domain;
 682
 683         dom = pi->dom_info + domain;
 684         /* uS to nS */
 685         return dom->opp[dom->opp_count - 1].trans_latency_us * 1000;
 686 }
 687
 688 static int scmi_dvfs_freq_set(const struct scmi_protocol_handle *ph, u32 domain,
 689                               unsigned long freq, bool poll)
 690 {
 691         struct scmi_perf_info *pi = ph->get_priv(ph);
 692         struct perf_dom_info *dom = pi->dom_info + domain;
 693
 694         return scmi_perf_level_set(ph, domain, freq / dom->mult_factor, poll);
 695 }
 696
 697 static int scmi_dvfs_freq_get(const struct scmi_protocol_handle *ph, u32 domain,
 698                               unsigned long *freq, bool poll)
 699 {
 700         int ret;
 701         u32 level;
 702         struct scmi_perf_info *pi = ph->get_priv(ph);
 703         struct perf_dom_info *dom = pi->dom_info + domain;
 704
 705         ret = scmi_perf_level_get(ph, domain, &level, poll);
 706         if (!ret)
 707                 *freq = level * dom->mult_factor;
 708
 709         return ret;
 710 }
 711
 712 static int scmi_dvfs_est_power_get(const struct scmi_protocol_handle *ph,
 713                                    u32 domain, unsigned long *freq,
 714                                    unsigned long *power)
 715 {
 716         struct scmi_perf_info *pi = ph->get_priv(ph);
 717         struct perf_dom_info *dom;
 718         unsigned long opp_freq;
 719         int idx, ret = -EINVAL;
 720         struct scmi_opp *opp;
 721
 722         dom = pi->dom_info + domain;
 723         if (!dom)
 724                 return -EIO;
 725
 726         for (opp = dom->opp, idx = 0; idx < dom->opp_count; idx++, opp++) {
 727                 opp_freq = opp->perf * dom->mult_factor;
 728                 if (opp_freq < *freq)
 729                         continue;
 730
 731                 *freq = opp_freq;
 732                 *power = opp->power;
 733                 ret = 0;
 734                 break;
 735         }
 736
 737         return ret;
 738 }
 739
 740 static bool scmi_fast_switch_possible(const struct scmi_protocol_handle *ph,
 741                                       struct device *dev)
 742 {
 743         struct perf_dom_info *dom;
 744         struct scmi_perf_info *pi = ph->get_priv(ph);
 745
 746         dom = pi->dom_info + scmi_dev_domain_id(dev);
 747
 748         return dom->fc_info && dom->fc_info->level_set_addr;
 749 }
 750
 751 static bool scmi_power_scale_mw_get(const struct scmi_protocol_handle *ph)
 752 {
 753         struct scmi_perf_info *pi = ph->get_priv(ph);
 754
 755         return pi->power_scale_mw;
 756 }
 757
 758 static const struct scmi_perf_proto_ops perf_proto_ops = {
 759         .limits_set = scmi_perf_limits_set,
 760         .limits_get = scmi_perf_limits_get,
 761         .level_set = scmi_perf_level_set,
 762         .level_get = scmi_perf_level_get,
 763         .device_domain_id = scmi_dev_domain_id,
 764         .transition_latency_get = scmi_dvfs_transition_latency_get,
 765         .device_opps_add = scmi_dvfs_device_opps_add,
 766         .freq_set = scmi_dvfs_freq_set,
 767         .freq_get = scmi_dvfs_freq_get,
 768         .est_power_get = scmi_dvfs_est_power_get,
 769         .fast_switch_possible = scmi_fast_switch_possible,
 770         .power_scale_mw_get = scmi_power_scale_mw_get,
 771 };
 772
 773 static int scmi_perf_set_notify_enabled(const struct scmi_protocol_handle *ph,
 774                                         u8 evt_id, u32 src_id, bool enable)
 775 {
 776         int ret, cmd_id;
 777
 778         if (evt_id >= ARRAY_SIZE(evt_2_cmd))
 779                 return -EINVAL;
 780
 781         cmd_id = evt_2_cmd[evt_id];
 782         ret = scmi_perf_level_limits_notify(ph, src_id, cmd_id, enable);
 783         if (ret)
 784                 pr_debug("FAIL_ENABLED - evt[%X] dom[%d] - ret:%d\n",
 785                          evt_id, src_id, ret);
 786
 787         return ret;
 788 }
 789
 790 static void *scmi_perf_fill_custom_report(const struct scmi_protocol_handle *ph,
 791                                           u8 evt_id, ktime_t timestamp,
 792                                           const void *payld, size_t payld_sz,
 793                                           void *report, u32 *src_id)
 794 {
 795         void *rep = NULL;
 796
 797         switch (evt_id) {
 798         case SCMI_EVENT_PERFORMANCE_LIMITS_CHANGED:
 799         {
 800                 const struct scmi_perf_limits_notify_payld *p = payld;
 801                 struct scmi_perf_limits_report *r = report;
 802
 803                 if (sizeof(*p) != payld_sz)
 804                         break;
 805
 806                 r->timestamp = timestamp;
 807                 r->agent_id = le32_to_cpu(p->agent_id);
 808                 r->domain_id = le32_to_cpu(p->domain_id);
 809                 r->range_max = le32_to_cpu(p->range_max);
 810                 r->range_min = le32_to_cpu(p->range_min);
 811                 *src_id = r->domain_id;
 812                 rep = r;
 813                 break;
 814         }
 815         case SCMI_EVENT_PERFORMANCE_LEVEL_CHANGED:
 816         {
 817                 const struct scmi_perf_level_notify_payld *p = payld;
 818                 struct scmi_perf_level_report *r = report;
 819
 820                 if (sizeof(*p) != payld_sz)
 821                         break;
 822
 823                 r->timestamp = timestamp;
 824                 r->agent_id = le32_to_cpu(p->agent_id);
 825                 r->domain_id = le32_to_cpu(p->domain_id);
 826                 r->performance_level = le32_to_cpu(p->performance_level);
 827                 *src_id = r->domain_id;
 828                 rep = r;
 829                 break;
 830         }
 831         default:
 832                 break;
 833         }
 834
 835         return rep;
 836 }
 837
 838 static int scmi_perf_get_num_sources(const struct scmi_protocol_handle *ph)
 839 {
 840         struct scmi_perf_info *pi = ph->get_priv(ph);
 841
 842         if (!pi)
 843                 return -EINVAL;
 844
 845         return pi->num_domains;
 846 }
 847
 848 static const struct scmi_event perf_events[] = {
 849         {
 850                 .id = SCMI_EVENT_PERFORMANCE_LIMITS_CHANGED,
 851                 .max_payld_sz = sizeof(struct scmi_perf_limits_notify_payld),
 852                 .max_report_sz = sizeof(struct scmi_perf_limits_report),
 853         },
 854         {
 855                 .id = SCMI_EVENT_PERFORMANCE_LEVEL_CHANGED,
 856                 .max_payld_sz = sizeof(struct scmi_perf_level_notify_payld),
 857                 .max_report_sz = sizeof(struct scmi_perf_level_report),
 858         },
 859 };
 860
 861 static const struct scmi_event_ops perf_event_ops = {
 862         .get_num_sources = scmi_perf_get_num_sources,
 863         .set_notify_enabled = scmi_perf_set_notify_enabled,
 864         .fill_custom_report = scmi_perf_fill_custom_report,
 865 };
 866
 867 static const struct scmi_protocol_events perf_protocol_events = {
 868         .queue_sz = SCMI_PROTO_QUEUE_SZ,
 869         .ops = &perf_event_ops,
 870         .evts = perf_events,
 871         .num_events = ARRAY_SIZE(perf_events),
 872 };
 873
 874 static int scmi_perf_protocol_init(const struct scmi_protocol_handle *ph)
 875 {
 876         int domain;
 877         u32 version;
 878         struct scmi_perf_info *pinfo;
 879
 880         ph->xops->version_get(ph, &version);
 881
 882         dev_dbg(ph->dev, "Performance Version %d.%d\n",
 883                 PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version));
 884
 885         pinfo = devm_kzalloc(ph->dev, sizeof(*pinfo), GFP_KERNEL);
 886         if (!pinfo)
 887                 return -ENOMEM;
 888
 889         scmi_perf_attributes_get(ph, pinfo);
 890
 891         pinfo->dom_info = devm_kcalloc(ph->dev, pinfo->num_domains,
 892                                        sizeof(*pinfo->dom_info), GFP_KERNEL);
 893         if (!pinfo->dom_info)
 894                 return -ENOMEM;
 895
 896         for (domain = 0; domain < pinfo->num_domains; domain++) {
 897                 struct perf_dom_info *dom = pinfo->dom_info + domain;
 898
 899                 scmi_perf_domain_attributes_get(ph, domain, dom);
 900                 scmi_perf_describe_levels_get(ph, domain, dom);
 901
 902                 if (dom->perf_fastchannels)
 903                         scmi_perf_domain_init_fc(ph, domain, &dom->fc_info);
 904         }
 905
 906         pinfo->version = version;
 907
 908         return ph->set_priv(ph, pinfo);
 909 }
 910
 911 static const struct scmi_protocol scmi_perf = {
 912         .id = SCMI_PROTOCOL_PERF,
 913         .owner = THIS_MODULE,
 914         .instance_init = &scmi_perf_protocol_init,
 915         .ops = &perf_proto_ops,
 916         .events = &perf_protocol_events,
 917 };
 918
 919 DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(perf, scmi_perf)