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