text: // SPDX-License-Identifier: GPL-2.0
  /*
   * System Control and Management Interface (SCMI) Sensor Protocol
   *
   * Copyright (C) 2018-2021 ARM Ltd.
   */
  
  #define pr_fmt(fmt) "SCMI Notifications SENSOR - " fmt
  
  #include <linux/bitfield.h>
  #include <linux/module.h>
  #include <linux/scmi_protocol.h>
  
  #include "common.h"
  #include "notify.h"
  
  #define SCMI_MAX_NUM_SENSOR_AXIS	63
  #define	SCMIv2_SENSOR_PROTOCOL		0x10000
  
  enum scmi_sensor_protocol_cmd {
  	SENSOR_DESCRIPTION_GET = 0x3,
  	SENSOR_TRIP_POINT_NOTIFY = 0x4,
  	SENSOR_TRIP_POINT_CONFIG = 0x5,
  	SENSOR_READING_GET = 0x6,
  	SENSOR_AXIS_DESCRIPTION_GET = 0x7,
  	SENSOR_LIST_UPDATE_INTERVALS = 0x8,
  	SENSOR_CONFIG_GET = 0x9,
  	SENSOR_CONFIG_SET = 0xA,
  	SENSOR_CONTINUOUS_UPDATE_NOTIFY = 0xB,
  };
  
  struct scmi_msg_resp_sensor_attributes {
  	__le16 num_sensors;
  	u8 max_requests;
  	u8 reserved;
  	__le32 reg_addr_low;
  	__le32 reg_addr_high;
  	__le32 reg_size;
  };
  
  /* v3 attributes_low macros */
  #define SUPPORTS_UPDATE_NOTIFY(x)	FIELD_GET(BIT(30), (x))
  #define SENSOR_TSTAMP_EXP(x)		FIELD_GET(GENMASK(14, 10), (x))
  #define SUPPORTS_TIMESTAMP(x)		FIELD_GET(BIT(9), (x))
  #define SUPPORTS_EXTEND_ATTRS(x)	FIELD_GET(BIT(8), (x))
  
  /* v2 attributes_high macros */
  #define SENSOR_UPDATE_BASE(x)		FIELD_GET(GENMASK(31, 27), (x))
  #define SENSOR_UPDATE_SCALE(x)		FIELD_GET(GENMASK(26, 22), (x))
  
  /* v3 attributes_high macros */
  #define SENSOR_AXIS_NUMBER(x)		FIELD_GET(GENMASK(21, 16), (x))
  #define SUPPORTS_AXIS(x)		FIELD_GET(BIT(8), (x))
  
  /* v3 resolution macros */
  #define SENSOR_RES(x)			FIELD_GET(GENMASK(26, 0), (x))
  #define SENSOR_RES_EXP(x)		FIELD_GET(GENMASK(31, 27), (x))
  
  struct scmi_msg_resp_attrs {
  	__le32 min_range_low;
  	__le32 min_range_high;
  	__le32 max_range_low;
  	__le32 max_range_high;
  };
  
  struct scmi_msg_resp_sensor_description {
  	__le16 num_returned;
  	__le16 num_remaining;
  	struct scmi_sensor_descriptor {
  		__le32 id;
  		__le32 attributes_low;
  /* Common attributes_low macros */
  #define SUPPORTS_ASYNC_READ(x)		FIELD_GET(BIT(31), (x))
  #define NUM_TRIP_POINTS(x)		FIELD_GET(GENMASK(7, 0), (x))
  		__le32 attributes_high;
  /* Common attributes_high macros */
  #define SENSOR_SCALE(x)			FIELD_GET(GENMASK(15, 11), (x))
  #define SENSOR_SCALE_SIGN		BIT(4)
  #define SENSOR_SCALE_EXTEND		GENMASK(31, 5)
  #define SENSOR_TYPE(x)			FIELD_GET(GENMASK(7, 0), (x))
  		u8 name[SCMI_MAX_STR_SIZE];
  		/* only for version > 2.0 */
  		__le32 power;
  		__le32 resolution;
  		struct scmi_msg_resp_attrs scalar_attrs;
  	} desc[];
  };
  
  /* Base scmi_sensor_descriptor size excluding extended attrs after name */
  #define SCMI_MSG_RESP_SENS_DESCR_BASE_SZ	28
  
  /* Sign extend to a full s32 */
  #define	S32_EXT(v)							\
  	({								\
  		int __v = (v);						\
  									\
  		if (__v & SENSOR_SCALE_SIGN)				\
  			__v |= SENSOR_SCALE_EXTEND;			\
  		__v;							\
  	})
  
  struct scmi_msg_sensor_axis_description_get {
  	__le32 id;
  	__le32 axis_desc_index;
  };
  
  struct scmi_msg_resp_sensor_axis_description {
  	__le32 num_axis_flags;
  #define NUM_AXIS_RETURNED(x)		FIELD_GET(GENMASK(5, 0), (x))
  #define NUM_AXIS_REMAINING(x)		FIELD_GET(GENMASK(31, 26), (x))
  	struct scmi_axis_descriptor {
  		__le32 id;
  		__le32 attributes_low;
  		__le32 attributes_high;
  		u8 name[SCMI_MAX_STR_SIZE];
  		__le32 resolution;
  		struct scmi_msg_resp_attrs attrs;
  	} desc[];
  };
  
  /* Base scmi_axis_descriptor size excluding extended attrs after name */
  #define SCMI_MSG_RESP_AXIS_DESCR_BASE_SZ	28
  
  struct scmi_msg_sensor_list_update_intervals {
  	__le32 id;
  	__le32 index;
  };
  
  struct scmi_msg_resp_sensor_list_update_intervals {
  	__le32 num_intervals_flags;
  #define NUM_INTERVALS_RETURNED(x)	FIELD_GET(GENMASK(11, 0), (x))
  #define SEGMENTED_INTVL_FORMAT(x)	FIELD_GET(BIT(12), (x))
  #define NUM_INTERVALS_REMAINING(x)	FIELD_GET(GENMASK(31, 16), (x))
  	__le32 intervals[];
  };
  
  struct scmi_msg_sensor_request_notify {
  	__le32 id;
  	__le32 event_control;
  #define SENSOR_NOTIFY_ALL	BIT(0)
  };
  
  struct scmi_msg_set_sensor_trip_point {
  	__le32 id;
  	__le32 event_control;
  #define SENSOR_TP_EVENT_MASK	(0x3)
  #define SENSOR_TP_DISABLED	0x0
  #define SENSOR_TP_POSITIVE	0x1
  #define SENSOR_TP_NEGATIVE	0x2
  #define SENSOR_TP_BOTH		0x3
  #define SENSOR_TP_ID(x)		(((x) & 0xff) << 4)
  	__le32 value_low;
  	__le32 value_high;
  };
  
  struct scmi_msg_sensor_config_set {
  	__le32 id;
  	__le32 sensor_config;
  };
  
  struct scmi_msg_sensor_reading_get {
  	__le32 id;
  	__le32 flags;
  #define SENSOR_READ_ASYNC	BIT(0)
  };
  
  struct scmi_resp_sensor_reading_complete {
  	__le32 id;
  	__le32 readings_low;
  	__le32 readings_high;
  };
  
  struct scmi_sensor_reading_resp {
  	__le32 sensor_value_low;
  	__le32 sensor_value_high;
  	__le32 timestamp_low;
  	__le32 timestamp_high;
  };
  
  struct scmi_resp_sensor_reading_complete_v3 {
  	__le32 id;
  	struct scmi_sensor_reading_resp readings[];
  };
  
  struct scmi_sensor_trip_notify_payld {
  	__le32 agent_id;
  	__le32 sensor_id;
  	__le32 trip_point_desc;
  };
  
  struct scmi_sensor_update_notify_payld {
  	__le32 agent_id;
  	__le32 sensor_id;
  	struct scmi_sensor_reading_resp readings[];
  };
  
  struct sensors_info {
  	u32 version;
  	int num_sensors;
  	int max_requests;
  	u64 reg_addr;
  	u32 reg_size;
  	struct scmi_sensor_info *sensors;
  };
  
  static int scmi_sensor_attributes_get(const struct scmi_protocol_handle *ph,
  				      struct sensors_info *si)
  {
  	int ret;
  	struct scmi_xfer *t;
  	struct scmi_msg_resp_sensor_attributes *attr;
  
  	ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES,
  				      0, sizeof(*attr), &t);
  	if (ret)
  		return ret;
  
  	attr = t->rx.buf;
  
  	ret = ph->xops->do_xfer(ph, t);
  	if (!ret) {
  		si->num_sensors = le16_to_cpu(attr->num_sensors);
  		si->max_requests = attr->max_requests;
  		si->reg_addr = le32_to_cpu(attr->reg_addr_low) |
  				(u64)le32_to_cpu(attr->reg_addr_high) << 32;
  		si->reg_size = le32_to_cpu(attr->reg_size);
  	}
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static inline void scmi_parse_range_attrs(struct scmi_range_attrs *out,
  					  struct scmi_msg_resp_attrs *in)
  {
  	out->min_range = get_unaligned_le64((void *)&in->min_range_low);
  	out->max_range = get_unaligned_le64((void *)&in->max_range_low);
  }
  
  static int scmi_sensor_update_intervals(const struct scmi_protocol_handle *ph,
  					struct scmi_sensor_info *s)
  {
  	int ret, cnt;
  	u32 desc_index = 0;
  	u16 num_returned, num_remaining;
  	struct scmi_xfer *ti;
  	struct scmi_msg_resp_sensor_list_update_intervals *buf;
  	struct scmi_msg_sensor_list_update_intervals *msg;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_LIST_UPDATE_INTERVALS,
  				      sizeof(*msg), 0, &ti);
  	if (ret)
  		return ret;
  
  	buf = ti->rx.buf;
  	do {
  		u32 flags;
  
  		msg = ti->tx.buf;
  		/* Set the number of sensors to be skipped/already read */
  		msg->id = cpu_to_le32(s->id);
  		msg->index = cpu_to_le32(desc_index);
  
  		ret = ph->xops->do_xfer(ph, ti);
  		if (ret)
  			break;
  
  		flags = le32_to_cpu(buf->num_intervals_flags);
  		num_returned = NUM_INTERVALS_RETURNED(flags);
  		num_remaining = NUM_INTERVALS_REMAINING(flags);
  
  		/*
  		 * Max intervals is not declared previously anywhere so we
  		 * assume it's returned+remaining.
  		 */
  		if (!s->intervals.count) {
  			s->intervals.segmented = SEGMENTED_INTVL_FORMAT(flags);
  			s->intervals.count = num_returned + num_remaining;
  			/* segmented intervals are reported in one triplet */
  			if (s->intervals.segmented &&
  			    (num_remaining || num_returned != 3)) {
  				dev_err(ph->dev,
  					"Sensor ID:%d advertises an invalid segmented interval (%d)\n",
  					s->id, s->intervals.count);
  				s->intervals.segmented = false;
  				s->intervals.count = 0;
  				ret = -EINVAL;
  				break;
  			}
  			/* Direct allocation when exceeding pre-allocated */
  			if (s->intervals.count >= SCMI_MAX_PREALLOC_POOL) {
  				s->intervals.desc =
  					devm_kcalloc(ph->dev,
  						     s->intervals.count,
  						     sizeof(*s->intervals.desc),
  						     GFP_KERNEL);
  				if (!s->intervals.desc) {
  					s->intervals.segmented = false;
  					s->intervals.count = 0;
  					ret = -ENOMEM;
  					break;
  				}
  			}
  		} else if (desc_index + num_returned > s->intervals.count) {
  			dev_err(ph->dev,
  				"No. of update intervals can't exceed %d\n",
  				s->intervals.count);
  			ret = -EINVAL;
  			break;
  		}
  
  		for (cnt = 0; cnt < num_returned; cnt++)
  			s->intervals.desc[desc_index + cnt] =
  					le32_to_cpu(buf->intervals[cnt]);
  
  		desc_index += num_returned;
  
  		ph->xops->reset_rx_to_maxsz(ph, ti);
  		/*
  		 * check for both returned and remaining to avoid infinite
  		 * loop due to buggy firmware
  		 */
  	} while (num_returned && num_remaining);
  
  	ph->xops->xfer_put(ph, ti);
  	return ret;
  }
  
  static int scmi_sensor_axis_description(const struct scmi_protocol_handle *ph,
  					struct scmi_sensor_info *s)
  {
  	int ret, cnt;
  	u32 desc_index = 0;
  	u16 num_returned, num_remaining;
  	struct scmi_xfer *te;
  	struct scmi_msg_resp_sensor_axis_description *buf;
  	struct scmi_msg_sensor_axis_description_get *msg;
  
  	s->axis = devm_kcalloc(ph->dev, s->num_axis,
  			       sizeof(*s->axis), GFP_KERNEL);
  	if (!s->axis)
  		return -ENOMEM;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_AXIS_DESCRIPTION_GET,
  				      sizeof(*msg), 0, &te);
  	if (ret)
  		return ret;
  
  	buf = te->rx.buf;
  	do {
  		u32 flags;
  		struct scmi_axis_descriptor *adesc;
  
  		msg = te->tx.buf;
  		/* Set the number of sensors to be skipped/already read */
  		msg->id = cpu_to_le32(s->id);
  		msg->axis_desc_index = cpu_to_le32(desc_index);
  
  		ret = ph->xops->do_xfer(ph, te);
  		if (ret)
  			break;
  
  		flags = le32_to_cpu(buf->num_axis_flags);
  		num_returned = NUM_AXIS_RETURNED(flags);
  		num_remaining = NUM_AXIS_REMAINING(flags);
  
  		if (desc_index + num_returned > s->num_axis) {
  			dev_err(ph->dev, "No. of axis can't exceed %d\n",
  				s->num_axis);
  			break;
  		}
  
  		adesc = &buf->desc[0];
  		for (cnt = 0; cnt < num_returned; cnt++) {
  			u32 attrh, attrl;
  			struct scmi_sensor_axis_info *a;
  			size_t dsize = SCMI_MSG_RESP_AXIS_DESCR_BASE_SZ;
  
  			attrl = le32_to_cpu(adesc->attributes_low);
  
  			a = &s->axis[desc_index + cnt];
  
  			a->id = le32_to_cpu(adesc->id);
  			a->extended_attrs = SUPPORTS_EXTEND_ATTRS(attrl);
  
  			attrh = le32_to_cpu(adesc->attributes_high);
  			a->scale = S32_EXT(SENSOR_SCALE(attrh));
  			a->type = SENSOR_TYPE(attrh);
  			strlcpy(a->name, adesc->name, SCMI_MAX_STR_SIZE);
  
  			if (a->extended_attrs) {
  				unsigned int ares =
  					le32_to_cpu(adesc->resolution);
  
  				a->resolution = SENSOR_RES(ares);
  				a->exponent =
  					S32_EXT(SENSOR_RES_EXP(ares));
  				dsize += sizeof(adesc->resolution);
  
  				scmi_parse_range_attrs(&a->attrs,
  						       &adesc->attrs);
  				dsize += sizeof(adesc->attrs);
  			}
  
  			adesc = (typeof(adesc))((u8 *)adesc + dsize);
  		}
  
  		desc_index += num_returned;
  
  		ph->xops->reset_rx_to_maxsz(ph, te);
  		/*
  		 * check for both returned and remaining to avoid infinite
  		 * loop due to buggy firmware
  		 */
  	} while (num_returned && num_remaining);
  
  	ph->xops->xfer_put(ph, te);
  	return ret;
  }
  
  static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph,
  				       struct sensors_info *si)
  {
  	int ret, cnt;
  	u32 desc_index = 0;
  	u16 num_returned, num_remaining;
  	struct scmi_xfer *t;
  	struct scmi_msg_resp_sensor_description *buf;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_DESCRIPTION_GET,
  				      sizeof(__le32), 0, &t);
  	if (ret)
  		return ret;
  
  	buf = t->rx.buf;
  
  	do {
  		struct scmi_sensor_descriptor *sdesc;
  
  		/* Set the number of sensors to be skipped/already read */
  		put_unaligned_le32(desc_index, t->tx.buf);
  
  		ret = ph->xops->do_xfer(ph, t);
  		if (ret)
  			break;
  
  		num_returned = le16_to_cpu(buf->num_returned);
  		num_remaining = le16_to_cpu(buf->num_remaining);
  
  		if (desc_index + num_returned > si->num_sensors) {
  			dev_err(ph->dev, "No. of sensors can't exceed %d",
  				si->num_sensors);
  			break;
  		}
  
  		sdesc = &buf->desc[0];
  		for (cnt = 0; cnt < num_returned; cnt++) {
  			u32 attrh, attrl;
  			struct scmi_sensor_info *s;
  			size_t dsize = SCMI_MSG_RESP_SENS_DESCR_BASE_SZ;
  
  			s = &si->sensors[desc_index + cnt];
  			s->id = le32_to_cpu(sdesc->id);
  
  			attrl = le32_to_cpu(sdesc->attributes_low);
  			/* common bitfields parsing */
  			s->async = SUPPORTS_ASYNC_READ(attrl);
  			s->num_trip_points = NUM_TRIP_POINTS(attrl);
  			/**
  			 * only SCMIv3.0 specific bitfield below.
  			 * Such bitfields are assumed to be zeroed on non
  			 * relevant fw versions...assuming fw not buggy !
  			 */
  			s->update = SUPPORTS_UPDATE_NOTIFY(attrl);
  			s->timestamped = SUPPORTS_TIMESTAMP(attrl);
  			if (s->timestamped)
  				s->tstamp_scale =
  					S32_EXT(SENSOR_TSTAMP_EXP(attrl));
  			s->extended_scalar_attrs =
  				SUPPORTS_EXTEND_ATTRS(attrl);
  
  			attrh = le32_to_cpu(sdesc->attributes_high);
  			/* common bitfields parsing */
  			s->scale = S32_EXT(SENSOR_SCALE(attrh));
  			s->type = SENSOR_TYPE(attrh);
  			/* Use pre-allocated pool wherever possible */
  			s->intervals.desc = s->intervals.prealloc_pool;
  			if (si->version == SCMIv2_SENSOR_PROTOCOL) {
  				s->intervals.segmented = false;
  				s->intervals.count = 1;
  				/*
  				 * Convert SCMIv2.0 update interval format to
  				 * SCMIv3.0 to be used as the common exposed
  				 * descriptor, accessible via common macros.
  				 */
  				s->intervals.desc[0] =
  					(SENSOR_UPDATE_BASE(attrh) << 5) |
  					 SENSOR_UPDATE_SCALE(attrh);
  			} else {
  				/*
  				 * From SCMIv3.0 update intervals are retrieved
  				 * via a dedicated (optional) command.
  				 * Since the command is optional, on error carry
  				 * on without any update interval.
  				 */
  				if (scmi_sensor_update_intervals(ph, s))
  					dev_dbg(ph->dev,
  						"Update Intervals not available for sensor ID:%d\n",
  						s->id);
  			}
  			/**
  			 * only > SCMIv2.0 specific bitfield below.
  			 * Such bitfields are assumed to be zeroed on non
  			 * relevant fw versions...assuming fw not buggy !
  			 */
  			s->num_axis = min_t(unsigned int,
  					    SUPPORTS_AXIS(attrh) ?
  					    SENSOR_AXIS_NUMBER(attrh) : 0,
  					    SCMI_MAX_NUM_SENSOR_AXIS);
  			strlcpy(s->name, sdesc->name, SCMI_MAX_STR_SIZE);
  
  			if (s->extended_scalar_attrs) {
  				s->sensor_power = le32_to_cpu(sdesc->power);
  				dsize += sizeof(sdesc->power);
  				/* Only for sensors reporting scalar values */
  				if (s->num_axis == 0) {
  					unsigned int sres =
  						le32_to_cpu(sdesc->resolution);
  
  					s->resolution = SENSOR_RES(sres);
  					s->exponent =
  						S32_EXT(SENSOR_RES_EXP(sres));
  					dsize += sizeof(sdesc->resolution);
  
  					scmi_parse_range_attrs(&s->scalar_attrs,
  							       &sdesc->scalar_attrs);
  					dsize += sizeof(sdesc->scalar_attrs);
  				}
  			}
  			if (s->num_axis > 0) {
  				ret = scmi_sensor_axis_description(ph, s);
  				if (ret)
  					goto out;
  			}
  
  			sdesc = (typeof(sdesc))((u8 *)sdesc + dsize);
  		}
  
  		desc_index += num_returned;
  
  		ph->xops->reset_rx_to_maxsz(ph, t);
  		/*
  		 * check for both returned and remaining to avoid infinite
  		 * loop due to buggy firmware
  		 */
  	} while (num_returned && num_remaining);
  
  out:
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static inline int
  scmi_sensor_request_notify(const struct scmi_protocol_handle *ph, u32 sensor_id,
  			   u8 message_id, bool enable)
  {
  	int ret;
  	u32 evt_cntl = enable ? SENSOR_NOTIFY_ALL : 0;
  	struct scmi_xfer *t;
  	struct scmi_msg_sensor_request_notify *cfg;
  
  	ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*cfg), 0, &t);
  	if (ret)
  		return ret;
  
  	cfg = t->tx.buf;
  	cfg->id = cpu_to_le32(sensor_id);
  	cfg->event_control = cpu_to_le32(evt_cntl);
  
  	ret = ph->xops->do_xfer(ph, t);
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static int scmi_sensor_trip_point_notify(const struct scmi_protocol_handle *ph,
  					 u32 sensor_id, bool enable)
  {
  	return scmi_sensor_request_notify(ph, sensor_id,
  					  SENSOR_TRIP_POINT_NOTIFY,
  					  enable);
  }
  
  static int
  scmi_sensor_continuous_update_notify(const struct scmi_protocol_handle *ph,
  				     u32 sensor_id, bool enable)
  {
  	return scmi_sensor_request_notify(ph, sensor_id,
  					  SENSOR_CONTINUOUS_UPDATE_NOTIFY,
  					  enable);
  }
  
  static int
  scmi_sensor_trip_point_config(const struct scmi_protocol_handle *ph,
  			      u32 sensor_id, u8 trip_id, u64 trip_value)
  {
  	int ret;
  	u32 evt_cntl = SENSOR_TP_BOTH;
  	struct scmi_xfer *t;
  	struct scmi_msg_set_sensor_trip_point *trip;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_TRIP_POINT_CONFIG,
  				      sizeof(*trip), 0, &t);
  	if (ret)
  		return ret;
  
  	trip = t->tx.buf;
  	trip->id = cpu_to_le32(sensor_id);
  	trip->event_control = cpu_to_le32(evt_cntl | SENSOR_TP_ID(trip_id));
  	trip->value_low = cpu_to_le32(trip_value & 0xffffffff);
  	trip->value_high = cpu_to_le32(trip_value >> 32);
  
  	ret = ph->xops->do_xfer(ph, t);
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static int scmi_sensor_config_get(const struct scmi_protocol_handle *ph,
  				  u32 sensor_id, u32 *sensor_config)
  {
  	int ret;
  	struct scmi_xfer *t;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_CONFIG_GET,
  				      sizeof(__le32), sizeof(__le32), &t);
  	if (ret)
  		return ret;
  
  	put_unaligned_le32(sensor_id, t->tx.buf);
  	ret = ph->xops->do_xfer(ph, t);
  	if (!ret) {
  		struct sensors_info *si = ph->get_priv(ph);
  		struct scmi_sensor_info *s = si->sensors + sensor_id;
  
  		*sensor_config = get_unaligned_le64(t->rx.buf);
  		s->sensor_config = *sensor_config;
  	}
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static int scmi_sensor_config_set(const struct scmi_protocol_handle *ph,
  				  u32 sensor_id, u32 sensor_config)
  {
  	int ret;
  	struct scmi_xfer *t;
  	struct scmi_msg_sensor_config_set *msg;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_CONFIG_SET,
  				      sizeof(*msg), 0, &t);
  	if (ret)
  		return ret;
  
  	msg = t->tx.buf;
  	msg->id = cpu_to_le32(sensor_id);
  	msg->sensor_config = cpu_to_le32(sensor_config);
  
  	ret = ph->xops->do_xfer(ph, t);
  	if (!ret) {
  		struct sensors_info *si = ph->get_priv(ph);
  		struct scmi_sensor_info *s = si->sensors + sensor_id;
  
  		s->sensor_config = sensor_config;
  	}
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  /**
   * scmi_sensor_reading_get  - Read scalar sensor value
   * @ph: Protocol handle
   * @sensor_id: Sensor ID
   * @value: The 64bit value sensor reading
   *
   * This function returns a single 64 bit reading value representing the sensor
   * value; if the platform SCMI Protocol implementation and the sensor support
   * multiple axis and timestamped-reads, this just returns the first axis while
   * dropping the timestamp value.
   * Use instead the @scmi_sensor_reading_get_timestamped to retrieve the array of
   * timestamped multi-axis values.
   *
   * Return: 0 on Success
   */
  static int scmi_sensor_reading_get(const struct scmi_protocol_handle *ph,
  				   u32 sensor_id, u64 *value)
  {
  	int ret;
  	struct scmi_xfer *t;
  	struct scmi_msg_sensor_reading_get *sensor;
  	struct sensors_info *si = ph->get_priv(ph);
  	struct scmi_sensor_info *s = si->sensors + sensor_id;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_READING_GET,
  				      sizeof(*sensor), 0, &t);
  	if (ret)
  		return ret;
  
  	sensor = t->tx.buf;
  	sensor->id = cpu_to_le32(sensor_id);
  	if (s->async) {
  		sensor->flags = cpu_to_le32(SENSOR_READ_ASYNC);
  		ret = ph->xops->do_xfer_with_response(ph, t);
  		if (!ret) {
  			struct scmi_resp_sensor_reading_complete *resp;
  
  			resp = t->rx.buf;
  			if (le32_to_cpu(resp->id) == sensor_id)
  				*value =
  					get_unaligned_le64(&resp->readings_low);
  			else
  				ret = -EPROTO;
  		}
  	} else {
  		sensor->flags = cpu_to_le32(0);
  		ret = ph->xops->do_xfer(ph, t);
  		if (!ret)
  			*value = get_unaligned_le64(t->rx.buf);
  	}
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static inline void
  scmi_parse_sensor_readings(struct scmi_sensor_reading *out,
  			   const struct scmi_sensor_reading_resp *in)
  {
  	out->value = get_unaligned_le64((void *)&in->sensor_value_low);
  	out->timestamp = get_unaligned_le64((void *)&in->timestamp_low);
  }
  
  /**
   * scmi_sensor_reading_get_timestamped  - Read multiple-axis timestamped values
   * @ph: Protocol handle
   * @sensor_id: Sensor ID
   * @count: The length of the provided @readings array
   * @readings: An array of elements each representing a timestamped per-axis
   *	      reading of type @struct scmi_sensor_reading.
   *	      Returned readings are ordered as the @axis descriptors array
   *	      included in @struct scmi_sensor_info and the max number of
   *	      returned elements is min(@count, @num_axis); ideally the provided
   *	      array should be of length @count equal to @num_axis.
   *
   * Return: 0 on Success
   */
  static int
  scmi_sensor_reading_get_timestamped(const struct scmi_protocol_handle *ph,
  				    u32 sensor_id, u8 count,
  				    struct scmi_sensor_reading *readings)
  {
  	int ret;
  	struct scmi_xfer *t;
  	struct scmi_msg_sensor_reading_get *sensor;
  	struct sensors_info *si = ph->get_priv(ph);
  	struct scmi_sensor_info *s = si->sensors + sensor_id;
  
  	if (!count || !readings ||
  	    (!s->num_axis && count > 1) || (s->num_axis && count > s->num_axis))
  		return -EINVAL;
  
  	ret = ph->xops->xfer_get_init(ph, SENSOR_READING_GET,
  				      sizeof(*sensor), 0, &t);
  	if (ret)
  		return ret;
  
  	sensor = t->tx.buf;
  	sensor->id = cpu_to_le32(sensor_id);
  	if (s->async) {
  		sensor->flags = cpu_to_le32(SENSOR_READ_ASYNC);
  		ret = ph->xops->do_xfer_with_response(ph, t);
  		if (!ret) {
  			int i;
  			struct scmi_resp_sensor_reading_complete_v3 *resp;
  
  			resp = t->rx.buf;
  			/* Retrieve only the number of requested axis anyway */
  			if (le32_to_cpu(resp->id) == sensor_id)
  				for (i = 0; i < count; i++)
  					scmi_parse_sensor_readings(&readings[i],
  								   &resp->readings[i]);
  			else
  				ret = -EPROTO;
  		}
  	} else {
  		sensor->flags = cpu_to_le32(0);
  		ret = ph->xops->do_xfer(ph, t);
  		if (!ret) {
  			int i;
  			struct scmi_sensor_reading_resp *resp_readings;
  
  			resp_readings = t->rx.buf;
  			for (i = 0; i < count; i++)
  				scmi_parse_sensor_readings(&readings[i],
  							   &resp_readings[i]);
  		}
  	}
  
  	ph->xops->xfer_put(ph, t);
  	return ret;
  }
  
  static const struct scmi_sensor_info *
  scmi_sensor_info_get(const struct scmi_protocol_handle *ph, u32 sensor_id)
  {
  	struct sensors_info *si = ph->get_priv(ph);
  
  	return si->sensors + sensor_id;
  }
  
  static int scmi_sensor_count_get(const struct scmi_protocol_handle *ph)
  {
  	struct sensors_info *si = ph->get_priv(ph);
  
  	return si->num_sensors;
  }
  
  static const struct scmi_sensor_proto_ops sensor_proto_ops = {
  	.count_get = scmi_sensor_count_get,
  	.info_get = scmi_sensor_info_get,
  	.trip_point_config = scmi_sensor_trip_point_config,
  	.reading_get = scmi_sensor_reading_get,
  	.reading_get_timestamped = scmi_sensor_reading_get_timestamped,
  	.config_get = scmi_sensor_config_get,
  	.config_set = scmi_sensor_config_set,
  };
  
  static int scmi_sensor_set_notify_enabled(const struct scmi_protocol_handle *ph,
  					  u8 evt_id, u32 src_id, bool enable)
  {
  	int ret;
  
  	switch (evt_id) {
  	case SCMI_EVENT_SENSOR_TRIP_POINT_EVENT:
  		ret = scmi_sensor_trip_point_notify(ph, src_id, enable);
  		break;
  	case SCMI_EVENT_SENSOR_UPDATE:
  		ret = scmi_sensor_continuous_update_notify(ph, src_id, enable);
  		break;
  	default:
  		ret = -EINVAL;
  		break;
  	}
  
  	if (ret)
  		pr_debug("FAIL_ENABLED - evt[%X] dom[%d] - ret:%d\n",
  			 evt_id, src_id, ret);
  
  	return ret;
  }
  
  static void *
  scmi_sensor_fill_custom_report(const struct scmi_protocol_handle *ph,
  			       u8 evt_id, ktime_t timestamp,
  			       const void *payld, size_t payld_sz,
  			       void *report, u32 *src_id)
  {
  	void *rep = NULL;
  
  	switch (evt_id) {
  	case SCMI_EVENT_SENSOR_TRIP_POINT_EVENT:
  	{
  		const struct scmi_sensor_trip_notify_payld *p = payld;
  		struct scmi_sensor_trip_point_report *r = report;
  
  		if (sizeof(*p) != payld_sz)
  			break;
  
  		r->timestamp = timestamp;
  		r->agent_id = le32_to_cpu(p->agent_id);
  		r->sensor_id = le32_to_cpu(p->sensor_id);
  		r->trip_point_desc = le32_to_cpu(p->trip_point_desc);
  		*src_id = r->sensor_id;
  		rep = r;
  		break;
  	}
  	case SCMI_EVENT_SENSOR_UPDATE:
  	{
  		int i;
  		struct scmi_sensor_info *s;
  		const struct scmi_sensor_update_notify_payld *p = payld;
  		struct scmi_sensor_update_report *r = report;
  		struct sensors_info *sinfo = ph->get_priv(ph);
  
  		/* payld_sz is variable for this event */
  		r->sensor_id = le32_to_cpu(p->sensor_id);
  		if (r->sensor_id >= sinfo->num_sensors)
  			break;
  		r->timestamp = timestamp;
  		r->agent_id = le32_to_cpu(p->agent_id);
  		s = &sinfo->sensors[r->sensor_id];
  		/*
  		 * The generated report r (@struct scmi_sensor_update_report)
  		 * was pre-allocated to contain up to SCMI_MAX_NUM_SENSOR_AXIS
  		 * readings: here it is filled with the effective @num_axis
  		 * readings defined for this sensor or 1 for scalar sensors.
  		 */
  		r->readings_count = s->num_axis ?: 1;
  		for (i = 0; i < r->readings_count; i++)
  			scmi_parse_sensor_readings(&r->readings[i],
  						   &p->readings[i]);
  		*src_id = r->sensor_id;
  		rep = r;
  		break;
  	}
  	default:
  		break;
  	}
  
  	return rep;
  }
  
  static int scmi_sensor_get_num_sources(const struct scmi_protocol_handle *ph)
  {
  	struct sensors_info *si = ph->get_priv(ph);
  
  	return si->num_sensors;
  }
  
  static const struct scmi_event sensor_events[] = {
  	{
  		.id = SCMI_EVENT_SENSOR_TRIP_POINT_EVENT,
  		.max_payld_sz = sizeof(struct scmi_sensor_trip_notify_payld),
  		.max_report_sz = sizeof(struct scmi_sensor_trip_point_report),
  	},
  	{
  		.id = SCMI_EVENT_SENSOR_UPDATE,
  		.max_payld_sz =
  			sizeof(struct scmi_sensor_update_notify_payld) +
  			 SCMI_MAX_NUM_SENSOR_AXIS *
  			 sizeof(struct scmi_sensor_reading_resp),
  		.max_report_sz = sizeof(struct scmi_sensor_update_report) +
  				  SCMI_MAX_NUM_SENSOR_AXIS *
  				  sizeof(struct scmi_sensor_reading),
  	},
  };
  
  static const struct scmi_event_ops sensor_event_ops = {
  	.get_num_sources = scmi_sensor_get_num_sources,
  	.set_notify_enabled = scmi_sensor_set_notify_enabled,
  	.fill_custom_report = scmi_sensor_fill_custom_report,
  };
  
  static const struct scmi_protocol_events sensor_protocol_events = {
  	.queue_sz = SCMI_PROTO_QUEUE_SZ,
  	.ops = &sensor_event_ops,
  	.evts = sensor_events,
  	.num_events = ARRAY_SIZE(sensor_events),
  };
  
  static int scmi_sensors_protocol_init(const struct scmi_protocol_handle *ph)
  {
  	u32 version;
  	int ret;
  	struct sensors_info *sinfo;
  
  	ph->xops->version_get(ph, &version);
  
  	dev_dbg(ph->dev, "Sensor Version %d.%d\n",
  		PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version));
  
  	sinfo = devm_kzalloc(ph->dev, sizeof(*sinfo), GFP_KERNEL);
  	if (!sinfo)
  		return -ENOMEM;
  	sinfo->version = version;
  
  	ret = scmi_sensor_attributes_get(ph, sinfo);
  	if (ret)
  		return ret;
  	sinfo->sensors = devm_kcalloc(ph->dev, sinfo->num_sensors,
  				      sizeof(*sinfo->sensors), GFP_KERNEL);
  	if (!sinfo->sensors)
  		return -ENOMEM;
  
  	ret = scmi_sensor_description_get(ph, sinfo);
  	if (ret)
  		return ret;
  
  	return ph->set_priv(ph, sinfo);
  }
  
  static const struct scmi_protocol scmi_sensors = {
  	.id = SCMI_PROTOCOL_SENSOR,
  	.owner = THIS_MODULE,
  	.instance_init = &scmi_sensors_protocol_init,
  	.ops = &sensor_proto_ops,
  	.events = &sensor_protocol_events,
  };
  
  DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(sensors, scmi_sensors)
  