Commit 7cab5377 authored by Cristian Marussi's avatar Cristian Marussi Committed by Sudeep Holla
Browse files

firmware: arm_scmi: Use common iterators in the sensor protocol 

Make SCMI sensor protocol use the common iterator protocol helpers
for issuing the multi-part commands.

Link: https://lore.kernel.org/r/20220330150551.2573938-15-cristian.marussi@arm.com


Signed-off-by: default avatarCristian Marussi <cristian.marussi@arm.com>
Signed-off-by: default avatarSudeep Holla <sudeep.holla@arm.com>
parent 36b6ea0f

drivers/firmware/arm_scmi/sensors.c

+297 −285
Original line number Diff line number Diff line
@@ -64,6 +64,10 @@ struct scmi_msg_resp_attrs { 
	__le32 max_range_high;

};



struct scmi_msg_sensor_description {

	__le32 desc_index;

};



struct scmi_msg_resp_sensor_description {

	__le16 num_returned;

	__le16 num_remaining;
@@ -233,235 +237,243 @@ static int scmi_sensor_attributes_get(const struct scmi_protocol_handle *ph, 
}



static inline void scmi_parse_range_attrs(struct scmi_range_attrs *out,

					  struct scmi_msg_resp_attrs *in)

					  const 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;

struct scmi_sens_ipriv {

	void *priv;

	struct device *dev;

};



	buf = ti->rx.buf;

	do {

		u32 flags;

static void iter_intervals_prepare_message(void *message,

					   unsigned int desc_index,

					   const void *p)

{

	struct scmi_msg_sensor_list_update_intervals *msg = message;

	const struct scmi_sensor_info *s;



		msg = ti->tx.buf;

	s = ((const struct scmi_sens_ipriv *)p)->priv;

	/* 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;

static int iter_intervals_update_state(struct scmi_iterator_state *st,

				       const void *response, void *p)

{

	u32 flags;

	struct scmi_sensor_info *s = ((struct scmi_sens_ipriv *)p)->priv;

	struct device *dev = ((struct scmi_sens_ipriv *)p)->dev;

	const struct scmi_msg_resp_sensor_list_update_intervals *r = response;



		flags = le32_to_cpu(buf->num_intervals_flags);

		num_returned = NUM_INTERVALS_RETURNED(flags);

		num_remaining = NUM_INTERVALS_REMAINING(flags);

	flags = le32_to_cpu(r->num_intervals_flags);

	st->num_returned = NUM_INTERVALS_RETURNED(flags);

	st->num_remaining = NUM_INTERVALS_REMAINING(flags);



	/*

	 * Max intervals is not declared previously anywhere so we

		 * assume it's returned+remaining.

	 * assume it's returned+remaining on first call.

	 */

		if (!s->intervals.count) {

	if (!st->max_resources) {

		s->intervals.segmented = SEGMENTED_INTVL_FORMAT(flags);

			s->intervals.count = num_returned + num_remaining;

		s->intervals.count = st->num_returned + st->num_remaining;

		/* segmented intervals are reported in one triplet */

		if (s->intervals.segmented &&

			    (num_remaining || num_returned != 3)) {

				dev_err(ph->dev,

		    (st->num_remaining || st->num_returned != 3)) {

			dev_err(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;

			return -EINVAL;

		}

		/* Direct allocation when exceeding pre-allocated */

		if (s->intervals.count >= SCMI_MAX_PREALLOC_POOL) {

			s->intervals.desc =

					devm_kcalloc(ph->dev,

				devm_kcalloc(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;

				return -ENOMEM;

			}

		}

		} 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;



		st->max_resources = s->intervals.count;

	}



		for (cnt = 0; cnt < num_returned; cnt++)

			s->intervals.desc[desc_index + cnt] =

					le32_to_cpu(buf->intervals[cnt]);

	return 0;

}



		desc_index += num_returned;

static int

iter_intervals_process_response(const struct scmi_protocol_handle *ph,

				const void *response,

				struct scmi_iterator_state *st, void *p)

{

	const struct scmi_msg_resp_sensor_list_update_intervals *r = response;

	struct scmi_sensor_info *s = ((struct scmi_sens_ipriv *)p)->priv;



		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);

	s->intervals.desc[st->desc_index + st->loop_idx] =

		le32_to_cpu(r->intervals[st->loop_idx]);



	ph->xops->xfer_put(ph, ti);

	return ret;

	return 0;

}



static int scmi_sensor_axis_description(const struct scmi_protocol_handle *ph,

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 *te;

	struct scmi_msg_resp_sensor_axis_description *buf;

	struct scmi_msg_sensor_axis_description_get *msg;

	void *iter;

	struct scmi_msg_sensor_list_update_intervals *msg;

	struct scmi_iterator_ops ops = {

		.prepare_message = iter_intervals_prepare_message,

		.update_state = iter_intervals_update_state,

		.process_response = iter_intervals_process_response,

	};

	struct scmi_sens_ipriv upriv = {

		.priv = s,

		.dev = ph->dev,

	};



	s->axis = devm_kcalloc(ph->dev, s->num_axis,

			       sizeof(*s->axis), GFP_KERNEL);

	if (!s->axis)

		return -ENOMEM;

	iter = ph->hops->iter_response_init(ph, &ops, s->intervals.count,

					    SENSOR_LIST_UPDATE_INTERVALS,

					    sizeof(*msg), &upriv);

	if (IS_ERR(iter))

		return PTR_ERR(iter);



	ret = ph->xops->xfer_get_init(ph, SENSOR_AXIS_DESCRIPTION_GET,

				      sizeof(*msg), 0, &te);

	if (ret)

		return ret;

	return ph->hops->iter_response_run(iter);

}



	buf = te->rx.buf;

	do {

		u32 flags;

		struct scmi_axis_descriptor *adesc;

static void iter_axes_desc_prepare_message(void *message,

					   const unsigned int desc_index,

					   const void *priv)

{

	struct scmi_msg_sensor_axis_description_get *msg = message;

	const struct scmi_sensor_info *s = priv;



		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;

static int

iter_axes_desc_update_state(struct scmi_iterator_state *st,

			    const void *response, void *priv)

{

	u32 flags;

	const struct scmi_msg_resp_sensor_axis_description *r = response;



		flags = le32_to_cpu(buf->num_axis_flags);

		num_returned = NUM_AXIS_RETURNED(flags);

		num_remaining = NUM_AXIS_REMAINING(flags);

	flags = le32_to_cpu(r->num_axis_flags);

	st->num_returned = NUM_AXIS_RETURNED(flags);

	st->num_remaining = NUM_AXIS_REMAINING(flags);

	st->priv = (void *)&r->desc[0];



		if (desc_index + num_returned > s->num_axis) {

			dev_err(ph->dev, "No. of axis can't exceed %d\n",

				s->num_axis);

			break;

	return 0;

}



		adesc = &buf->desc[0];

		for (cnt = 0; cnt < num_returned; cnt++) {

static int

iter_axes_desc_process_response(const struct scmi_protocol_handle *ph,

				const void *response,

				struct scmi_iterator_state *st, void *priv)

{

	u32 attrh, attrl;

	struct scmi_sensor_axis_info *a;

	size_t dsize = SCMI_MSG_RESP_AXIS_DESCR_BASE_SZ;

	struct scmi_sensor_info *s = priv;

	const struct scmi_axis_descriptor *adesc = st->priv;



	attrl = le32_to_cpu(adesc->attributes_low);



			a = &s->axis[desc_index + cnt];



	a = &s->axis[st->desc_index + st->loop_idx];

	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);

	strscpy(a->name, adesc->name, SCMI_MAX_STR_SIZE);



	if (a->extended_attrs) {

				unsigned int ares =

					le32_to_cpu(adesc->resolution);

		unsigned int ares = le32_to_cpu(adesc->resolution);



		a->resolution = SENSOR_RES(ares);

				a->exponent =

					S32_EXT(SENSOR_RES_EXP(ares));

		a->exponent = S32_EXT(SENSOR_RES_EXP(ares));

		dsize += sizeof(adesc->resolution);



				scmi_parse_range_attrs(&a->attrs,

						       &adesc->attrs);

		scmi_parse_range_attrs(&a->attrs, &adesc->attrs);

		dsize += sizeof(adesc->attrs);

	}



			adesc = (typeof(adesc))((u8 *)adesc + dsize);

		}



		desc_index += num_returned;

	st->priv = ((u8 *)adesc + dsize);



		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;

	return 0;

}



static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph,

				       struct sensors_info *si)

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 *t;

	struct scmi_msg_resp_sensor_description *buf;

	void *iter;

	struct scmi_msg_sensor_axis_description_get *msg;

	struct scmi_iterator_ops ops = {

		.prepare_message = iter_axes_desc_prepare_message,

		.update_state = iter_axes_desc_update_state,

		.process_response = iter_axes_desc_process_response,

	};



	ret = ph->xops->xfer_get_init(ph, SENSOR_DESCRIPTION_GET,

				      sizeof(__le32), 0, &t);

	if (ret)

		return ret;

	s->axis = devm_kcalloc(ph->dev, s->num_axis,

			       sizeof(*s->axis), GFP_KERNEL);

	if (!s->axis)

		return -ENOMEM;



	buf = t->rx.buf;

	iter = ph->hops->iter_response_init(ph, &ops, s->num_axis,

					    SENSOR_AXIS_DESCRIPTION_GET,

					    sizeof(*msg), s);

	if (IS_ERR(iter))

		return PTR_ERR(iter);



	do {

		struct scmi_sensor_descriptor *sdesc;

	return ph->hops->iter_response_run(iter);

}



		/* Set the number of sensors to be skipped/already read */

		put_unaligned_le32(desc_index, t->tx.buf);

static void iter_sens_descr_prepare_message(void *message,

					    unsigned int desc_index,

					    const void *priv)

{

	struct scmi_msg_sensor_description *msg = message;



		ret = ph->xops->do_xfer(ph, t);

		if (ret)

			break;

	msg->desc_index = cpu_to_le32(desc_index);

}



		num_returned = le16_to_cpu(buf->num_returned);

		num_remaining = le16_to_cpu(buf->num_remaining);

static int iter_sens_descr_update_state(struct scmi_iterator_state *st,

					const void *response, void *priv)

{

	const struct scmi_msg_resp_sensor_description *r = response;



		if (desc_index + num_returned > si->num_sensors) {

			dev_err(ph->dev, "No. of sensors can't exceed %d",

				si->num_sensors);

			break;

	st->num_returned = le16_to_cpu(r->num_returned);

	st->num_remaining = le16_to_cpu(r->num_remaining);

	st->priv = (void *)&r->desc[0];



	return 0;

}



		sdesc = &buf->desc[0];

		for (cnt = 0; cnt < num_returned; cnt++) {

static int

iter_sens_descr_process_response(const struct scmi_protocol_handle *ph,

				 const void *response,

				 struct scmi_iterator_state *st, void *priv)



{

	int ret = 0;

	u32 attrh, attrl;

			struct scmi_sensor_info *s;

	size_t dsize = SCMI_MSG_RESP_SENS_DESCR_BASE_SZ;

	struct scmi_sensor_info *s;

	struct sensors_info *si = priv;

	const struct scmi_sensor_descriptor *sdesc = st->priv;



			s = &si->sensors[desc_index + cnt];

	s = &si->sensors[st->desc_index + st->loop_idx];

	s->id = le32_to_cpu(sdesc->id);



	attrl = le32_to_cpu(sdesc->attributes_low);
@@ -476,10 +488,8 @@ static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph, 
	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);

		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 */
@@ -495,8 +505,7 @@ static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph, 
		 * SCMIv3.0 to be used as the common exposed

		 * descriptor, accessible via common macros.

		 */

				s->intervals.desc[0] =

					(SENSOR_UPDATE_BASE(attrh) << 5) |

		s->intervals.desc[0] = (SENSOR_UPDATE_BASE(attrh) << 5) |

					SENSOR_UPDATE_SCALE(attrh);

	} else {

		/*
@@ -519,7 +528,7 @@ static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph, 
			    SUPPORTS_AXIS(attrh) ?

			    SENSOR_AXIS_NUMBER(attrh) : 0,

			    SCMI_MAX_NUM_SENSOR_AXIS);

			strlcpy(s->name, sdesc->name, SCMI_MAX_STR_SIZE);

	strscpy(s->name, sdesc->name, SCMI_MAX_STR_SIZE);



	/*

	 * If supported overwrite short name with the extended
@@ -528,21 +537,19 @@ static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph, 
	 */

	if (PROTOCOL_REV_MAJOR(si->version) >= 0x3 &&

	    SUPPORTS_EXTENDED_NAMES(attrl))

				ph->hops->extended_name_get(ph, SENSOR_NAME_GET,

							    s->id, s->name,

							    SCMI_MAX_STR_SIZE);

		ph->hops->extended_name_get(ph, SENSOR_NAME_GET, s->id,

					    s->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);

			unsigned int sres = le32_to_cpu(sdesc->resolution);



			s->resolution = SENSOR_RES(sres);

					s->exponent =

						S32_EXT(SENSOR_RES_EXP(sres));

			s->exponent = S32_EXT(SENSOR_RES_EXP(sres));

			dsize += sizeof(sdesc->resolution);



			scmi_parse_range_attrs(&s->scalar_attrs,
@@ -550,27 +557,32 @@ static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph, 
			dsize += sizeof(sdesc->scalar_attrs);

		}

	}

			if (s->num_axis > 0) {



	if (s->num_axis > 0)

		ret = scmi_sensor_axis_description(ph, s);

				if (ret)

					goto out;

			}



			sdesc = (typeof(sdesc))((u8 *)sdesc + dsize);

	st->priv = ((u8 *)sdesc + dsize);



	return ret;

}



		desc_index += num_returned;

static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph,

				       struct sensors_info *si)

{

	void *iter;

	struct scmi_iterator_ops ops = {

		.prepare_message = iter_sens_descr_prepare_message,

		.update_state = iter_sens_descr_update_state,

		.process_response = iter_sens_descr_process_response,

	};



		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);

	iter = ph->hops->iter_response_init(ph, &ops, si->num_sensors,

					    SENSOR_DESCRIPTION_GET,

					    sizeof(__le32), si);

	if (IS_ERR(iter))

		return PTR_ERR(iter);



out:

	ph->xops->xfer_put(ph, t);

	return ret;

	return ph->hops->iter_response_run(iter);

}



static inline int