sensors.c

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