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.comSigned-off-by: Cristian Marussi <cristian.marussi@arm.com>
Signed-off-by: Sudeep Holla <sudeep.holla@arm.com>

drivers/firmware/arm_scmi/sensors.c

@@ -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;
-
-	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;
+	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,
+	};
+
+	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);
+
+	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;
-
-		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);
+	st->priv = ((u8 *)adesc + dsize);
 
-	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,29 +557,34 @@ static int scmi_sensor_description_get(const struct scmi_protocol_handle *ph,
 			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;
+	if (s->num_axis > 0)
+		ret = scmi_sensor_axis_description(ph, s);
 
-		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);
+	st->priv = ((u8 *)sdesc + dsize);
 
-out:
-	ph->xops->xfer_put(ph, t);
 	return ret;
 }
 
+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,
+	};
+
+	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);
+
+	return ph->hops->iter_response_run(iter);
+}
+
 static inline int
 scmi_sensor_request_notify(const struct scmi_protocol_handle *ph, u32 sensor_id,
 			   u8 message_id, bool enable)