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