drivers/firmware/arm_scmi/sensors.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * System Control and Management Interface (SCMI) Sensor Protocol
   4  *
   5  * Copyright (C) 2018-2021 ARM Ltd.
   6  */
   7
   8 #define pr_fmt(fmt) "SCMI Notifications SENSOR - " fmt
   9
  10 #include <linux/bitfield.h>
  11 #include <linux/module.h>
  12 #include <linux/scmi_protocol.h>
  13
  14 #include "common.h"
  15 #include "notify.h"
  16
  17 #define SCMI_MAX_NUM_SENSOR_AXIS        63
  18 #define SCMIv2_SENSOR_PROTOCOL          0x10000
  19
  20 enum scmi_sensor_protocol_cmd {
  21         SENSOR_DESCRIPTION_GET = 0x3,
  22         SENSOR_TRIP_POINT_NOTIFY = 0x4,
  23         SENSOR_TRIP_POINT_CONFIG = 0x5,
  24         SENSOR_READING_GET = 0x6,
  25         SENSOR_AXIS_DESCRIPTION_GET = 0x7,
  26         SENSOR_LIST_UPDATE_INTERVALS = 0x8,
  27         SENSOR_CONFIG_GET = 0x9,
  28         SENSOR_CONFIG_SET = 0xA,
  29         SENSOR_CONTINUOUS_UPDATE_NOTIFY = 0xB,
  30 };
  31
  32 struct scmi_msg_resp_sensor_attributes {
  33         __le16 num_sensors;
  34         u8 max_requests;
  35         u8 reserved;
  36         __le32 reg_addr_low;
  37         __le32 reg_addr_high;
  38         __le32 reg_size;
  39 };
  40
  41 /* v3 attributes_low macros */
  42 #define SUPPORTS_UPDATE_NOTIFY(x)       FIELD_GET(BIT(30), (x))
  43 #define SENSOR_TSTAMP_EXP(x)            FIELD_GET(GENMASK(14, 10), (x))
  44 #define SUPPORTS_TIMESTAMP(x)           FIELD_GET(BIT(9), (x))
  45 #define SUPPORTS_EXTEND_ATTRS(x)        FIELD_GET(BIT(8), (x))
  46
  47 /* v2 attributes_high macros */
  48 #define SENSOR_UPDATE_BASE(x)           FIELD_GET(GENMASK(31, 27), (x))
  49 #define SENSOR_UPDATE_SCALE(x)          FIELD_GET(GENMASK(26, 22), (x))
  50
  51 /* v3 attributes_high macros */
  52 #define SENSOR_AXIS_NUMBER(x)           FIELD_GET(GENMASK(21, 16), (x))
  53 #define SUPPORTS_AXIS(x)                FIELD_GET(BIT(8), (x))
  54
  55 /* v3 resolution macros */
  56 #define SENSOR_RES(x)                   FIELD_GET(GENMASK(26, 0), (x))
  57 #define SENSOR_RES_EXP(x)               FIELD_GET(GENMASK(31, 27), (x))
  58
  59 struct scmi_msg_resp_attrs {
  60         __le32 min_range_low;
  61         __le32 min_range_high;
  62         __le32 max_range_low;
  63         __le32 max_range_high;
  64 };
  65
  66 struct scmi_msg_resp_sensor_description {
  67         __le16 num_returned;
  68         __le16 num_remaining;
  69         struct scmi_sensor_descriptor {
  70                 __le32 id;
  71                 __le32 attributes_low;
  72 /* Common attributes_low macros */
  73 #define SUPPORTS_ASYNC_READ(x)          FIELD_GET(BIT(31), (x))
  74 #define NUM_TRIP_POINTS(x)              FIELD_GET(GENMASK(7, 0), (x))
  75                 __le32 attributes_high;
  76 /* Common attributes_high macros */
  77 #define SENSOR_SCALE(x)                 FIELD_GET(GENMASK(15, 11), (x))
  78 #define SENSOR_SCALE_SIGN               BIT(4)
  79 #define SENSOR_SCALE_EXTEND             GENMASK(31, 5)
  80 #define SENSOR_TYPE(x)                  FIELD_GET(GENMASK(7, 0), (x))
  81                 u8 name[SCMI_MAX_STR_SIZE];
  82                 /* only for version > 2.0 */
  83                 __le32 power;
  84                 __le32 resolution;
  85                 struct scmi_msg_resp_attrs scalar_attrs;
  86         } desc[];
  87 };
  88
  89 /* Base scmi_sensor_descriptor size excluding extended attrs after name */
  90 #define SCMI_MSG_RESP_SENS_DESCR_BASE_SZ        28
  91
  92 /* Sign extend to a full s32 */
  93 #define S32_EXT(v)                                                      \
  94         ({                                                              \
  95                 int __v = (v);                                          \
  96                                                                         \
  97                 if (__v & SENSOR_SCALE_SIGN)                            \
  98                         __v |= SENSOR_SCALE_EXTEND;                     \
  99                 __v;                                                    \
 100         })
 101
 102 struct scmi_msg_sensor_axis_description_get {
 103         __le32 id;
 104         __le32 axis_desc_index;
 105 };
 106
 107 struct scmi_msg_resp_sensor_axis_description {
 108         __le32 num_axis_flags;
 109 #define NUM_AXIS_RETURNED(x)            FIELD_GET(GENMASK(5, 0), (x))
 110 #define NUM_AXIS_REMAINING(x)           FIELD_GET(GENMASK(31, 26), (x))
 111         struct scmi_axis_descriptor {
 112                 __le32 id;
 113                 __le32 attributes_low;
 114                 __le32 attributes_high;
 115                 u8 name[SCMI_MAX_STR_SIZE];
 116                 __le32 resolution;
 117                 struct scmi_msg_resp_attrs attrs;
 118         } desc[];
 119 };
 120
 121 /* Base scmi_axis_descriptor size excluding extended attrs after name */
 122 #define SCMI_MSG_RESP_AXIS_DESCR_BASE_SZ        28
 123
 124 struct scmi_msg_sensor_list_update_intervals {
 125         __le32 id;
 126         __le32 index;
 127 };
 128
 129 struct scmi_msg_resp_sensor_list_update_intervals {
 130         __le32 num_intervals_flags;
 131 #define NUM_INTERVALS_RETURNED(x)       FIELD_GET(GENMASK(11, 0), (x))
 132 #define SEGMENTED_INTVL_FORMAT(x)       FIELD_GET(BIT(12), (x))
 133 #define NUM_INTERVALS_REMAINING(x)      FIELD_GET(GENMASK(31, 16), (x))
 134         __le32 intervals[];
 135 };
 136
 137 struct scmi_msg_sensor_request_notify {
 138         __le32 id;
 139         __le32 event_control;
 140 #define SENSOR_NOTIFY_ALL       BIT(0)
 141 };
 142
 143 struct scmi_msg_set_sensor_trip_point {
 144         __le32 id;
 145         __le32 event_control;
 146 #define SENSOR_TP_EVENT_MASK    (0x3)
 147 #define SENSOR_TP_DISABLED      0x0
 148 #define SENSOR_TP_POSITIVE      0x1
 149 #define SENSOR_TP_NEGATIVE      0x2
 150 #define SENSOR_TP_BOTH          0x3
 151 #define SENSOR_TP_ID(x)         (((x) & 0xff) << 4)
 152         __le32 value_low;
 153         __le32 value_high;
 154 };
 155
 156 struct scmi_msg_sensor_config_set {
 157         __le32 id;
 158         __le32 sensor_config;
 159 };
 160
 161 struct scmi_msg_sensor_reading_get {
 162         __le32 id;
 163         __le32 flags;
 164 #define SENSOR_READ_ASYNC       BIT(0)
 165 };
 166
 167 struct scmi_resp_sensor_reading_complete {
 168         __le32 id;
 169         __le32 readings_low;
 170         __le32 readings_high;
 171 };
 172
 173 struct scmi_sensor_reading_resp {
 174         __le32 sensor_value_low;
 175         __le32 sensor_value_high;
 176         __le32 timestamp_low;
 177         __le32 timestamp_high;
 178 };
 179
 180 struct scmi_resp_sensor_reading_complete_v3 {
 181         __le32 id;
 182         struct scmi_sensor_reading_resp readings[];
 183 };
 184
 185 struct scmi_sensor_trip_notify_payld {
 186         __le32 agent_id;
 187         __le32 sensor_id;
 188         __le32 trip_point_desc;
 189 };
 190
 191 struct scmi_sensor_update_notify_payld {
 192         __le32 agent_id;
 193         __le32 sensor_id;
 194         struct scmi_sensor_reading_resp readings[];
 195 };
 196
 197 struct sensors_info {
 198         u32 version;
 199         int num_sensors;
 200         int max_requests;
 201         u64 reg_addr;
 202         u32 reg_size;
 203         struct scmi_sensor_info *sensors;
 204 };
 205
 206 static int scmi_sensor_attributes_get(const struct scmi_protocol_handle *ph,
 207                                       struct sensors_info *si)
 208 {
 209         int ret;
 210         struct scmi_xfer *t;
 211         struct scmi_msg_resp_sensor_attributes *attr;
 212
 213         ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES,
 214                                       0, sizeof(*attr), &t);
 215         if (ret)
 216                 return ret;
 217
 218         attr = t->rx.buf;
 219
 220         ret = ph->xops->do_xfer(ph, t);
 221         if (!ret) {
 222                 si->num_sensors = le16_to_cpu(attr->num_sensors);
 223                 si->max_requests = attr->max_requests;
 224                 si->reg_addr = le32_to_cpu(attr->reg_addr_low) |
 225                                 (u64)le32_to_cpu(attr->reg_addr_high) << 32;
 226                 si->reg_size = le32_to_cpu(attr->reg_size);
 227         }
 228
 229         ph->xops->xfer_put(ph, t);
 230         return ret;
 231 }
 232
 233 static inline void scmi_parse_range_attrs(struct scmi_range_attrs *out,
 234                                           struct scmi_msg_resp_attrs *in)
 235 {
 236         out->min_range = get_unaligned_le64((void *)&in->min_range_low);
 237         out->max_range = get_unaligned_le64((void *)&in->max_range_low);
 238 }
 239
 240 static int scmi_sensor_update_intervals(const struct scmi_protocol_handle *ph,
 241                                         struct scmi_sensor_info *s)
 242 {
 243         int ret, cnt;
 244         u32 desc_index = 0;
 245         u16 num_returned, num_remaining;
 246         struct scmi_xfer *ti;
 247         struct scmi_msg_resp_sensor_list_update_intervals *buf;
 248         struct scmi_msg_sensor_list_update_intervals *msg;
 249
 250         ret = ph->xops->xfer_get_init(ph, SENSOR_LIST_UPDATE_INTERVALS,
 251                                       sizeof(*msg), 0, &ti);
 252         if (ret)
 253                 return ret;
 254
 255         buf = ti->rx.buf;
 256         do {
 257                 u32 flags;
 258
 259                 msg = ti->tx.buf;
 260                 /* Set the number of sensors to be skipped/already read */
 261                 msg->id = cpu_to_le32(s->id);
 262                 msg->index = cpu_to_le32(desc_index);
 263
 264                 ret = ph->xops->do_xfer(ph, ti);
 265                 if (ret)
 266                         break;
 267
 268                 flags = le32_to_cpu(buf->num_intervals_flags);
 269                 num_returned = NUM_INTERVALS_RETURNED(flags);
 270                 num_remaining = NUM_INTERVALS_REMAINING(flags);
 271
 272                 /*
 273                  * Max intervals is not declared previously anywhere so we
 274                  * assume it's returned+remaining.
 275                  */
 276                 if (!s->intervals.count) {
 277                         s->intervals.segmented = SEGMENTED_INTVL_FORMAT(flags);
 278                         s->intervals.count = num_returned + num_remaining;
 279                         /* segmented intervals are reported in one triplet */
 280                         if (s->intervals.segmented &&
 281                             (num_remaining || num_returned != 3)) {
 282                                 dev_err(ph->dev,
 283                                         "Sensor ID:%d advertises an invalid segmented interval (%d)\n",
 284                                         s->id, s->intervals.count);
 285                                 s->intervals.segmented = false;
 286                                 s->intervals.count = 0;
 287                                 ret = -EINVAL;
 288                                 break;
 289                         }
 290                         /* Direct allocation when exceeding pre-allocated */
 291                         if (s->intervals.count >= SCMI_MAX_PREALLOC_POOL) {
 292                                 s->intervals.desc =
 293                                         devm_kcalloc(ph->dev,
 294                                                      s->intervals.count,
 295                                                      sizeof(*s->intervals.desc),
 296                                                      GFP_KERNEL);
 297                                 if (!s->intervals.desc) {
 298                                         s->intervals.segmented = false;
 299                                         s->intervals.count = 0;
 300                                         ret = -ENOMEM;
 301                                         break;
 302                                 }
 303                         }
 304                 } else if (desc_index + num_returned > s->intervals.count) {
 305                         dev_err(ph->dev,
 306                                 "No. of update intervals can't exceed %d\n",
 307                                 s->intervals.count);
 308                         ret = -EINVAL;
 309                         break;
 310                 }
 311
 312                 for (cnt = 0; cnt < num_returned; cnt++)
 313                         s->intervals.desc[desc_index + cnt] =
 314                                         le32_to_cpu(buf->intervals[cnt]);
 315
 316                 desc_index += num_returned;
 317
 318                 ph->xops->reset_rx_to_maxsz(ph, ti);
 319                 /*
 320                  * check for both returned and remaining to avoid infinite
 321                  * loop due to buggy firmware
 322                  */
 323         } while (num_returned && num_remaining);
 324
 325         ph->xops->xfer_put(ph, ti);
 326         return ret;
 327 }
 328
 329 static int scmi_sensor_axis_description(const struct scmi_protocol_handle *ph,
 330                                         struct scmi_sensor_info *s)
 331 {
 332         int ret, cnt;
 333         u32 desc_index = 0;
 334         u16 num_returned, num_remaining;
 335         struct scmi_xfer *te;
 336         struct scmi_msg_resp_sensor_axis_description *buf;
 337         struct scmi_msg_sensor_axis_description_get *msg;
 338
 339         s->axis = devm_kcalloc(ph->dev, s->num_axis,
 340                                sizeof(*s->axis), GFP_KERNEL);
 341         if (!s->axis)
 342                 return -ENOMEM;
 343
 344         ret = ph->xops->xfer_get_init(ph, SENSOR_AXIS_DESCRIPTION_GET,
 345                                       sizeof(*msg), 0, &te);
 346         if (ret)
 347                 return ret;
 348
 349         buf = te->rx.buf;
 350         do {
 351                 u32 flags;
 352                 struct scmi_axis_descriptor *adesc;
 353
 354                 msg = te->tx.buf;
 355                 /* Set the number of sensors to be skipped/already read */
 356                 msg->id = cpu_to_le32(s->id);
 357                 msg->axis_desc_index = cpu_to_le32(desc_index);
 358
 359                 ret = ph->xops->do_xfer(ph, te);
 360                 if (ret)
 361                         break;
 362
 363                 flags = le32_to_cpu(buf->num_axis_flags);
 364                 num_returned = NUM_AXIS_RETURNED(flags);
 365                 num_remaining = NUM_AXIS_REMAINING(flags);
 366
 367                 if (desc_index + num_returned > s->num_axis) {
 368                         dev_err(ph->dev, "No. of axis can't exceed %d\n",
 369                                 s->num_axis);
 370                         break;
 371                 }
 372
 373                 adesc = &buf->desc[0];
 374                 for (cnt = 0; cnt < num_returned; cnt++) {
 375                         u32 attrh, attrl;
 376                         struct scmi_sensor_axis_info *a;
 377                         size_t dsize = SCMI_MSG_RESP_AXIS_DESCR_BASE_SZ;
 378
 379                         attrl = le32_to_cpu(adesc->attributes_low);
 380
 381                         a = &s->axis[desc_index + cnt];
 382
 383                         a->id = le32_to_cpu(adesc->id);
 384                         a->extended_attrs = SUPPORTS_EXTEND_ATTRS(attrl);
 385
 386                         attrh = le32_to_cpu(adesc->attributes_high);
 387                         a->scale = S32_EXT(SENSOR_SCALE(attrh));
 388                         a->type = SENSOR_TYPE(attrh);
 389                         strlcpy(a->name, adesc->name, SCMI_MAX_STR_SIZE);
 390
 391                         if (a->extended_attrs) {
 392                                 unsigned int ares =
 393                                         le32_to_cpu(adesc->resolution);
 394
 395                                 a->resolution = SENSOR_RES(ares);
 396                                 a->exponent =
 397                                         S32_EXT(SENSOR_RES_EXP(ares));
 398                                 dsize += sizeof(adesc->resolution);
 399
 400                                 scmi_parse_range_attrs(&a->attrs,
 401                                                        &adesc->attrs);
 402                                 dsize += sizeof(adesc->attrs);
 403                         }
 404
 405                         adesc = (typeof(adesc))((u8 *)adesc + dsize);
 406                 }
 407
 408                 desc_index += num_returned;
 409
 410                 ph->xops->reset_rx_to_maxsz(ph, te);
 411                 /*
 412                  * check for both returned and remaining to avoid infinite
 413                  * loop due to buggy firmware
 414                  */
 415         } while (num_returned && num_remaining);
 416
 417         ph->xops->xfer_put(ph, te);
 418         return ret;
 419 }
 420
 421 static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph,
 422                                        struct sensors_info *si)
 423 {
 424         int ret, cnt;
 425         u32 desc_index = 0;
 426         u16 num_returned, num_remaining;
 427         struct scmi_xfer *t;
 428         struct scmi_msg_resp_sensor_description *buf;
 429
 430         ret = ph->xops->xfer_get_init(ph, SENSOR_DESCRIPTION_GET,
 431                                       sizeof(__le32), 0, &t);
 432         if (ret)
 433                 return ret;
 434
 435         buf = t->rx.buf;
 436
 437         do {
 438                 struct scmi_sensor_descriptor *sdesc;
 439
 440                 /* Set the number of sensors to be skipped/already read */
 441                 put_unaligned_le32(desc_index, t->tx.buf);
 442
 443                 ret = ph->xops->do_xfer(ph, t);
 444                 if (ret)
 445                         break;
 446
 447                 num_returned = le16_to_cpu(buf->num_returned);
 448                 num_remaining = le16_to_cpu(buf->num_remaining);
 449
 450                 if (desc_index + num_returned > si->num_sensors) {
 451                         dev_err(ph->dev, "No. of sensors can't exceed %d",
 452                                 si->num_sensors);
 453                         break;
 454                 }
 455
 456                 sdesc = &buf->desc[0];
 457                 for (cnt = 0; cnt < num_returned; cnt++) {
 458                         u32 attrh, attrl;
 459                         struct scmi_sensor_info *s;
 460                         size_t dsize = SCMI_MSG_RESP_SENS_DESCR_BASE_SZ;
 461
 462                         s = &si->sensors[desc_index + cnt];
 463                         s->id = le32_to_cpu(sdesc->id);
 464
 465                         attrl = le32_to_cpu(sdesc->attributes_low);
 466                         /* common bitfields parsing */
 467                         s->async = SUPPORTS_ASYNC_READ(attrl);
 468                         s->num_trip_points = NUM_TRIP_POINTS(attrl);
 469                         /**
 470                          * only SCMIv3.0 specific bitfield below.
 471                          * Such bitfields are assumed to be zeroed on non
 472                          * relevant fw versions...assuming fw not buggy !
 473                          */
 474                         s->update = SUPPORTS_UPDATE_NOTIFY(attrl);
 475                         s->timestamped = SUPPORTS_TIMESTAMP(attrl);
 476                         if (s->timestamped)
 477                                 s->tstamp_scale =
 478                                         S32_EXT(SENSOR_TSTAMP_EXP(attrl));
 479                         s->extended_scalar_attrs =
 480                                 SUPPORTS_EXTEND_ATTRS(attrl);
 481
 482                         attrh = le32_to_cpu(sdesc->attributes_high);
 483                         /* common bitfields parsing */
 484                         s->scale = S32_EXT(SENSOR_SCALE(attrh));
 485                         s->type = SENSOR_TYPE(attrh);
 486                         /* Use pre-allocated pool wherever possible */
 487                         s->intervals.desc = s->intervals.prealloc_pool;
 488                         if (si->version == SCMIv2_SENSOR_PROTOCOL) {
 489                                 s->intervals.segmented = false;
 490                                 s->intervals.count = 1;
 491                                 /*
 492                                  * Convert SCMIv2.0 update interval format to
 493                                  * SCMIv3.0 to be used as the common exposed
 494                                  * descriptor, accessible via common macros.
 495                                  */
 496                                 s->intervals.desc[0] =
 497                                         (SENSOR_UPDATE_BASE(attrh) << 5) |
 498                                          SENSOR_UPDATE_SCALE(attrh);
 499                         } else {
 500                                 /*
 501                                  * From SCMIv3.0 update intervals are retrieved
 502                                  * via a dedicated (optional) command.
 503                                  * Since the command is optional, on error carry
 504                                  * on without any update interval.
 505                                  */
 506                                 if (scmi_sensor_update_intervals(ph, s))
 507                                         dev_dbg(ph->dev,
 508                                                 "Update Intervals not available for sensor ID:%d\n",
 509                                                 s->id);
 510                         }
 511                         /**
 512                          * only > SCMIv2.0 specific bitfield below.
 513                          * Such bitfields are assumed to be zeroed on non
 514                          * relevant fw versions...assuming fw not buggy !
 515                          */
 516                         s->num_axis = min_t(unsigned int,
 517                                             SUPPORTS_AXIS(attrh) ?
 518                                             SENSOR_AXIS_NUMBER(attrh) : 0,
 519                                             SCMI_MAX_NUM_SENSOR_AXIS);
 520                         strlcpy(s->name, sdesc->name, SCMI_MAX_STR_SIZE);
 521
 522                         if (s->extended_scalar_attrs) {
 523                                 s->sensor_power = le32_to_cpu(sdesc->power);
 524                                 dsize += sizeof(sdesc->power);
 525                                 /* Only for sensors reporting scalar values */
 526                                 if (s->num_axis == 0) {
 527                                         unsigned int sres =
 528                                                 le32_to_cpu(sdesc->resolution);
 529
 530                                         s->resolution = SENSOR_RES(sres);
 531                                         s->exponent =
 532                                                 S32_EXT(SENSOR_RES_EXP(sres));
 533                                         dsize += sizeof(sdesc->resolution);
 534
 535                                         scmi_parse_range_attrs(&s->scalar_attrs,
 536                                                                &sdesc->scalar_attrs);
 537                                         dsize += sizeof(sdesc->scalar_attrs);
 538                                 }
 539                         }
 540                         if (s->num_axis > 0) {
 541                                 ret = scmi_sensor_axis_description(ph, s);
 542                                 if (ret)
 543                                         goto out;
 544                         }
 545
 546                         sdesc = (typeof(sdesc))((u8 *)sdesc + dsize);
 547                 }
 548
 549                 desc_index += num_returned;
 550
 551                 ph->xops->reset_rx_to_maxsz(ph, t);
 552                 /*
 553                  * check for both returned and remaining to avoid infinite
 554                  * loop due to buggy firmware
 555                  */
 556         } while (num_returned && num_remaining);
 557
 558 out:
 559         ph->xops->xfer_put(ph, t);
 560         return ret;
 561 }
 562
 563 static inline int
 564 scmi_sensor_request_notify(const struct scmi_protocol_handle *ph, u32 sensor_id,
 565                            u8 message_id, bool enable)
 566 {
 567         int ret;
 568         u32 evt_cntl = enable ? SENSOR_NOTIFY_ALL : 0;
 569         struct scmi_xfer *t;
 570         struct scmi_msg_sensor_request_notify *cfg;
 571
 572         ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*cfg), 0, &t);
 573         if (ret)
 574                 return ret;
 575
 576         cfg = t->tx.buf;
 577         cfg->id = cpu_to_le32(sensor_id);
 578         cfg->event_control = cpu_to_le32(evt_cntl);
 579
 580         ret = ph->xops->do_xfer(ph, t);
 581
 582         ph->xops->xfer_put(ph, t);
 583         return ret;
 584 }
 585
 586 static int scmi_sensor_trip_point_notify(const struct scmi_protocol_handle *ph,
 587                                          u32 sensor_id, bool enable)
 588 {
 589         return scmi_sensor_request_notify(ph, sensor_id,
 590                                           SENSOR_TRIP_POINT_NOTIFY,
 591                                           enable);
 592 }
 593
 594 static int
 595 scmi_sensor_continuous_update_notify(const struct scmi_protocol_handle *ph,
 596                                      u32 sensor_id, bool enable)
 597 {
 598         return scmi_sensor_request_notify(ph, sensor_id,
 599                                           SENSOR_CONTINUOUS_UPDATE_NOTIFY,
 600                                           enable);
 601 }
 602
 603 static int
 604 scmi_sensor_trip_point_config(const struct scmi_protocol_handle *ph,
 605                               u32 sensor_id, u8 trip_id, u64 trip_value)
 606 {
 607         int ret;
 608         u32 evt_cntl = SENSOR_TP_BOTH;
 609         struct scmi_xfer *t;
 610         struct scmi_msg_set_sensor_trip_point *trip;
 611
 612         ret = ph->xops->xfer_get_init(ph, SENSOR_TRIP_POINT_CONFIG,
 613                                       sizeof(*trip), 0, &t);
 614         if (ret)
 615                 return ret;
 616
 617         trip = t->tx.buf;
 618         trip->id = cpu_to_le32(sensor_id);
 619         trip->event_control = cpu_to_le32(evt_cntl | SENSOR_TP_ID(trip_id));
 620         trip->value_low = cpu_to_le32(trip_value & 0xffffffff);
 621         trip->value_high = cpu_to_le32(trip_value >> 32);
 622
 623         ret = ph->xops->do_xfer(ph, t);
 624
 625         ph->xops->xfer_put(ph, t);
 626         return ret;
 627 }
 628
 629 static int scmi_sensor_config_get(const struct scmi_protocol_handle *ph,
 630                                   u32 sensor_id, u32 *sensor_config)
 631 {
 632         int ret;
 633         struct scmi_xfer *t;
 634
 635         ret = ph->xops->xfer_get_init(ph, SENSOR_CONFIG_GET,
 636                                       sizeof(__le32), sizeof(__le32), &t);
 637         if (ret)
 638                 return ret;
 639
 640         put_unaligned_le32(sensor_id, t->tx.buf);
 641         ret = ph->xops->do_xfer(ph, t);
 642         if (!ret) {
 643                 struct sensors_info *si = ph->get_priv(ph);
 644                 struct scmi_sensor_info *s = si->sensors + sensor_id;
 645
 646                 *sensor_config = get_unaligned_le64(t->rx.buf);
 647                 s->sensor_config = *sensor_config;
 648         }
 649
 650         ph->xops->xfer_put(ph, t);
 651         return ret;
 652 }
 653
 654 static int scmi_sensor_config_set(const struct scmi_protocol_handle *ph,
 655                                   u32 sensor_id, u32 sensor_config)
 656 {
 657         int ret;
 658         struct scmi_xfer *t;
 659         struct scmi_msg_sensor_config_set *msg;
 660
 661         ret = ph->xops->xfer_get_init(ph, SENSOR_CONFIG_SET,
 662                                       sizeof(*msg), 0, &t);
 663         if (ret)
 664                 return ret;
 665
 666         msg = t->tx.buf;
 667         msg->id = cpu_to_le32(sensor_id);
 668         msg->sensor_config = cpu_to_le32(sensor_config);
 669
 670         ret = ph->xops->do_xfer(ph, t);
 671         if (!ret) {
 672                 struct sensors_info *si = ph->get_priv(ph);
 673                 struct scmi_sensor_info *s = si->sensors + sensor_id;
 674
 675                 s->sensor_config = sensor_config;
 676         }
 677
 678         ph->xops->xfer_put(ph, t);
 679         return ret;
 680 }
 681
 682 /**
 683  * scmi_sensor_reading_get  - Read scalar sensor value
 684  * @ph: Protocol handle
 685  * @sensor_id: Sensor ID
 686  * @value: The 64bit value sensor reading
 687  *
 688  * This function returns a single 64 bit reading value representing the sensor
 689  * value; if the platform SCMI Protocol implementation and the sensor support
 690  * multiple axis and timestamped-reads, this just returns the first axis while
 691  * dropping the timestamp value.
 692  * Use instead the @scmi_sensor_reading_get_timestamped to retrieve the array of
 693  * timestamped multi-axis values.
 694  *
 695  * Return: 0 on Success
 696  */
 697 static int scmi_sensor_reading_get(const struct scmi_protocol_handle *ph,
 698                                    u32 sensor_id, u64 *value)
 699 {
 700         int ret;
 701         struct scmi_xfer *t;
 702         struct scmi_msg_sensor_reading_get *sensor;
 703         struct sensors_info *si = ph->get_priv(ph);
 704         struct scmi_sensor_info *s = si->sensors + sensor_id;
 705
 706         ret = ph->xops->xfer_get_init(ph, SENSOR_READING_GET,
 707                                       sizeof(*sensor), 0, &t);
 708         if (ret)
 709                 return ret;
 710
 711         sensor = t->tx.buf;
 712         sensor->id = cpu_to_le32(sensor_id);
 713         if (s->async) {
 714                 sensor->flags = cpu_to_le32(SENSOR_READ_ASYNC);
 715                 ret = ph->xops->do_xfer_with_response(ph, t);
 716                 if (!ret) {
 717                         struct scmi_resp_sensor_reading_complete *resp;
 718
 719                         resp = t->rx.buf;
 720                         if (le32_to_cpu(resp->id) == sensor_id)
 721                                 *value =
 722                                         get_unaligned_le64(&resp->readings_low);
 723                         else
 724                                 ret = -EPROTO;
 725                 }
 726         } else {
 727                 sensor->flags = cpu_to_le32(0);
 728                 ret = ph->xops->do_xfer(ph, t);
 729                 if (!ret)
 730                         *value = get_unaligned_le64(t->rx.buf);
 731         }
 732
 733         ph->xops->xfer_put(ph, t);
 734         return ret;
 735 }
 736
 737 static inline void
 738 scmi_parse_sensor_readings(struct scmi_sensor_reading *out,
 739                            const struct scmi_sensor_reading_resp *in)
 740 {
 741         out->value = get_unaligned_le64((void *)&in->sensor_value_low);
 742         out->timestamp = get_unaligned_le64((void *)&in->timestamp_low);
 743 }
 744
 745 /**
 746  * scmi_sensor_reading_get_timestamped  - Read multiple-axis timestamped values
 747  * @ph: Protocol handle
 748  * @sensor_id: Sensor ID
 749  * @count: The length of the provided @readings array
 750  * @readings: An array of elements each representing a timestamped per-axis
 751  *            reading of type @struct scmi_sensor_reading.
 752  *            Returned readings are ordered as the @axis descriptors array
 753  *            included in @struct scmi_sensor_info and the max number of
 754  *            returned elements is min(@count, @num_axis); ideally the provided
 755  *            array should be of length @count equal to @num_axis.
 756  *
 757  * Return: 0 on Success
 758  */
 759 static int
 760 scmi_sensor_reading_get_timestamped(const struct scmi_protocol_handle *ph,
 761                                     u32 sensor_id, u8 count,
 762                                     struct scmi_sensor_reading *readings)
 763 {
 764         int ret;
 765         struct scmi_xfer *t;
 766         struct scmi_msg_sensor_reading_get *sensor;
 767         struct sensors_info *si = ph->get_priv(ph);
 768         struct scmi_sensor_info *s = si->sensors + sensor_id;
 769
 770         if (!count || !readings ||
 771             (!s->num_axis && count > 1) || (s->num_axis && count > s->num_axis))
 772                 return -EINVAL;
 773
 774         ret = ph->xops->xfer_get_init(ph, SENSOR_READING_GET,
 775                                       sizeof(*sensor), 0, &t);
 776         if (ret)
 777                 return ret;
 778
 779         sensor = t->tx.buf;
 780         sensor->id = cpu_to_le32(sensor_id);
 781         if (s->async) {
 782                 sensor->flags = cpu_to_le32(SENSOR_READ_ASYNC);
 783                 ret = ph->xops->do_xfer_with_response(ph, t);
 784                 if (!ret) {
 785                         int i;
 786                         struct scmi_resp_sensor_reading_complete_v3 *resp;
 787
 788                         resp = t->rx.buf;
 789                         /* Retrieve only the number of requested axis anyway */
 790                         if (le32_to_cpu(resp->id) == sensor_id)
 791                                 for (i = 0; i < count; i++)
 792                                         scmi_parse_sensor_readings(&readings[i],
 793                                                                    &resp->readings[i]);
 794                         else
 795                                 ret = -EPROTO;
 796                 }
 797         } else {
 798                 sensor->flags = cpu_to_le32(0);
 799                 ret = ph->xops->do_xfer(ph, t);
 800                 if (!ret) {
 801                         int i;
 802                         struct scmi_sensor_reading_resp *resp_readings;
 803
 804                         resp_readings = t->rx.buf;
 805                         for (i = 0; i < count; i++)
 806                                 scmi_parse_sensor_readings(&readings[i],
 807                                                            &resp_readings[i]);
 808                 }
 809         }
 810
 811         ph->xops->xfer_put(ph, t);
 812         return ret;
 813 }
 814
 815 static const struct scmi_sensor_info *
 816 scmi_sensor_info_get(const struct scmi_protocol_handle *ph, u32 sensor_id)
 817 {
 818         struct sensors_info *si = ph->get_priv(ph);
 819
 820         return si->sensors + sensor_id;
 821 }
 822
 823 static int scmi_sensor_count_get(const struct scmi_protocol_handle *ph)
 824 {
 825         struct sensors_info *si = ph->get_priv(ph);
 826
 827         return si->num_sensors;
 828 }
 829
 830 static const struct scmi_sensor_proto_ops sensor_proto_ops = {
 831         .count_get = scmi_sensor_count_get,
 832         .info_get = scmi_sensor_info_get,
 833         .trip_point_config = scmi_sensor_trip_point_config,
 834         .reading_get = scmi_sensor_reading_get,
 835         .reading_get_timestamped = scmi_sensor_reading_get_timestamped,
 836         .config_get = scmi_sensor_config_get,
 837         .config_set = scmi_sensor_config_set,
 838 };
 839
 840 static int scmi_sensor_set_notify_enabled(const struct scmi_protocol_handle *ph,
 841                                           u8 evt_id, u32 src_id, bool enable)
 842 {
 843         int ret;
 844
 845         switch (evt_id) {
 846         case SCMI_EVENT_SENSOR_TRIP_POINT_EVENT:
 847                 ret = scmi_sensor_trip_point_notify(ph, src_id, enable);
 848                 break;
 849         case SCMI_EVENT_SENSOR_UPDATE:
 850                 ret = scmi_sensor_continuous_update_notify(ph, src_id, enable);
 851                 break;
 852         default:
 853                 ret = -EINVAL;
 854                 break;
 855         }
 856
 857         if (ret)
 858                 pr_debug("FAIL_ENABLED - evt[%X] dom[%d] - ret:%d\n",
 859                          evt_id, src_id, ret);
 860
 861         return ret;
 862 }
 863
 864 static void *
 865 scmi_sensor_fill_custom_report(const struct scmi_protocol_handle *ph,
 866                                u8 evt_id, ktime_t timestamp,
 867                                const void *payld, size_t payld_sz,
 868                                void *report, u32 *src_id)
 869 {
 870         void *rep = NULL;
 871
 872         switch (evt_id) {
 873         case SCMI_EVENT_SENSOR_TRIP_POINT_EVENT:
 874         {
 875                 const struct scmi_sensor_trip_notify_payld *p = payld;
 876                 struct scmi_sensor_trip_point_report *r = report;
 877
 878                 if (sizeof(*p) != payld_sz)
 879                         break;
 880
 881                 r->timestamp = timestamp;
 882                 r->agent_id = le32_to_cpu(p->agent_id);
 883                 r->sensor_id = le32_to_cpu(p->sensor_id);
 884                 r->trip_point_desc = le32_to_cpu(p->trip_point_desc);
 885                 *src_id = r->sensor_id;
 886                 rep = r;
 887                 break;
 888         }
 889         case SCMI_EVENT_SENSOR_UPDATE:
 890         {
 891                 int i;
 892                 struct scmi_sensor_info *s;
 893                 const struct scmi_sensor_update_notify_payld *p = payld;
 894                 struct scmi_sensor_update_report *r = report;
 895                 struct sensors_info *sinfo = ph->get_priv(ph);
 896
 897                 /* payld_sz is variable for this event */
 898                 r->sensor_id = le32_to_cpu(p->sensor_id);
 899                 if (r->sensor_id >= sinfo->num_sensors)
 900                         break;
 901                 r->timestamp = timestamp;
 902                 r->agent_id = le32_to_cpu(p->agent_id);
 903                 s = &sinfo->sensors[r->sensor_id];
 904                 /*
 905                  * The generated report r (@struct scmi_sensor_update_report)
 906                  * was pre-allocated to contain up to SCMI_MAX_NUM_SENSOR_AXIS
 907                  * readings: here it is filled with the effective @num_axis
 908                  * readings defined for this sensor or 1 for scalar sensors.
 909                  */
 910                 r->readings_count = s->num_axis ?: 1;
 911                 for (i = 0; i < r->readings_count; i++)
 912                         scmi_parse_sensor_readings(&r->readings[i],
 913                                                    &p->readings[i]);
 914                 *src_id = r->sensor_id;
 915                 rep = r;
 916                 break;
 917         }
 918         default:
 919                 break;
 920         }
 921
 922         return rep;
 923 }
 924
 925 static int scmi_sensor_get_num_sources(const struct scmi_protocol_handle *ph)
 926 {
 927         struct sensors_info *si = ph->get_priv(ph);
 928
 929         return si->num_sensors;
 930 }
 931
 932 static const struct scmi_event sensor_events[] = {
 933         {
 934                 .id = SCMI_EVENT_SENSOR_TRIP_POINT_EVENT,
 935                 .max_payld_sz = sizeof(struct scmi_sensor_trip_notify_payld),
 936                 .max_report_sz = sizeof(struct scmi_sensor_trip_point_report),
 937         },
 938         {
 939                 .id = SCMI_EVENT_SENSOR_UPDATE,
 940                 .max_payld_sz =
 941                         sizeof(struct scmi_sensor_update_notify_payld) +
 942                          SCMI_MAX_NUM_SENSOR_AXIS *
 943                          sizeof(struct scmi_sensor_reading_resp),
 944                 .max_report_sz = sizeof(struct scmi_sensor_update_report) +
 945                                   SCMI_MAX_NUM_SENSOR_AXIS *
 946                                   sizeof(struct scmi_sensor_reading),
 947         },
 948 };
 949
 950 static const struct scmi_event_ops sensor_event_ops = {
 951         .get_num_sources = scmi_sensor_get_num_sources,
 952         .set_notify_enabled = scmi_sensor_set_notify_enabled,
 953         .fill_custom_report = scmi_sensor_fill_custom_report,
 954 };
 955
 956 static const struct scmi_protocol_events sensor_protocol_events = {
 957         .queue_sz = SCMI_PROTO_QUEUE_SZ,
 958         .ops = &sensor_event_ops,
 959         .evts = sensor_events,
 960         .num_events = ARRAY_SIZE(sensor_events),
 961 };
 962
 963 static int scmi_sensors_protocol_init(const struct scmi_protocol_handle *ph)
 964 {
 965         u32 version;
 966         int ret;
 967         struct sensors_info *sinfo;
 968
 969         ph->xops->version_get(ph, &version);
 970
 971         dev_dbg(ph->dev, "Sensor Version %d.%d\n",
 972                 PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version));
 973
 974         sinfo = devm_kzalloc(ph->dev, sizeof(*sinfo), GFP_KERNEL);
 975         if (!sinfo)
 976                 return -ENOMEM;
 977         sinfo->version = version;
 978
 979         ret = scmi_sensor_attributes_get(ph, sinfo);
 980         if (ret)
 981                 return ret;
 982         sinfo->sensors = devm_kcalloc(ph->dev, sinfo->num_sensors,
 983                                       sizeof(*sinfo->sensors), GFP_KERNEL);
 984         if (!sinfo->sensors)
 985                 return -ENOMEM;
 986
 987         ret = scmi_sensor_description_get(ph, sinfo);
 988         if (ret)
 989                 return ret;
 990
 991         return ph->set_priv(ph, sinfo);
 992 }
 993
 994 static const struct scmi_protocol scmi_sensors = {
 995         .id = SCMI_PROTOCOL_SENSOR,
 996         .owner = THIS_MODULE,
 997         .instance_init = &scmi_sensors_protocol_init,
 998         .ops = &sensor_proto_ops,
 999         .events = &sensor_protocol_events,
1000 };
1001
1002 DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(sensors, scmi_sensors)