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