~hc/RK356X_SDK_RELEASE.git

..	..	@@ -2,16 +2,31 @@
2	2	/*
3	3	* System Control and Management Interface (SCMI) Sensor Protocol
4	4	*
5		- * Copyright (C) 2018 ARM Ltd.
	5	+ * Copyright (C) 2018-2020 ARM Ltd.
6	6	*/
7	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	+
8	14	#include "common.h"
	15	+#include "notify.h"
	16	+
	17	+#define SCMI_MAX_NUM_SENSOR_AXIS 63
	18	+#define SCMIv2_SENSOR_PROTOCOL 0x10000
9	19
10	20	enum scmi_sensor_protocol_cmd {
11	21	SENSOR_DESCRIPTION_GET = 0x3,
12	22	SENSOR_TRIP_POINT_NOTIFY = 0x4,
13	23	SENSOR_TRIP_POINT_CONFIG = 0x5,
14	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,
15	30	};
16	31
17	32	struct scmi_msg_resp_sensor_attributes {
..	..	@@ -23,29 +38,106 @@
23	38	__le32 reg_size;
24	39	};
25	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	+
26	66	struct scmi_msg_resp_sensor_description {
27	67	__le16 num_returned;
28	68	__le16 num_remaining;
29		- struct {
	69	+ struct scmi_sensor_descriptor {
30	70	__le32 id;
31	71	__le32 attributes_low;
32		-#define SUPPORTS_ASYNC_READ(x) ((x) & BIT(31))
33		-#define NUM_TRIP_POINTS(x) ((x) & 0xff)
	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))
34	75	__le32 attributes_high;
35		-#define SENSOR_TYPE(x) ((x) & 0xff)
36		-#define SENSOR_SCALE(x) (((x) >> 11) & 0x1f)
37		-#define SENSOR_SCALE_SIGN BIT(4)
38		-#define SENSOR_SCALE_EXTEND GENMASK(7, 5)
39		-#define SENSOR_UPDATE_SCALE(x) (((x) >> 22) & 0x1f)
40		-#define SENSOR_UPDATE_BASE(x) (((x) >> 27) & 0x1f)
41		- u8 name[SCMI_MAX_STR_SIZE];
42		- } desc[0];
	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[];
43	87	};
44	88
45		-struct scmi_msg_sensor_trip_point_notify {
	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 {
46	138	__le32 id;
47	139	__le32 event_control;
48		-#define SENSOR_TP_NOTIFY_ALL BIT(0)
	140	+#define SENSOR_NOTIFY_ALL BIT(0)
49	141	};
50	142
51	143	struct scmi_msg_set_sensor_trip_point {
..	..	@@ -61,10 +153,45 @@
61	153	__le32 value_high;
62	154	};
63	155
	156	+struct scmi_msg_sensor_config_set {
	157	+ __le32 id;
	158	+ __le32 sensor_config;
	159	+};
	160	+
64	161	struct scmi_msg_sensor_reading_get {
65	162	__le32 id;
66	163	__le32 flags;
67	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[];
68	195	};
69	196
70	197	struct sensors_info {
..	..	@@ -76,21 +203,21 @@
76	203	struct scmi_sensor_info *sensors;
77	204	};
78	205
79		-static int scmi_sensor_attributes_get(const struct scmi_handle *handle,
	206	+static int scmi_sensor_attributes_get(const struct scmi_protocol_handle *ph,
80	207	struct sensors_info *si)
81	208	{
82	209	int ret;
83	210	struct scmi_xfer *t;
84	211	struct scmi_msg_resp_sensor_attributes *attr;
85	212
86		- ret = scmi_xfer_get_init(handle, PROTOCOL_ATTRIBUTES,
87		- SCMI_PROTOCOL_SENSOR, 0, sizeof(*attr), &t);
	213	+ ret = ph->xops->xfer_get_init(ph, PROTOCOL_ATTRIBUTES,
	214	+ 0, sizeof(*attr), &t);
88	215	if (ret)
89	216	return ret;
90	217
91	218	attr = t->rx.buf;
92	219
93		- ret = scmi_do_xfer(handle, t);
	220	+ ret = ph->xops->do_xfer(ph, t);
94	221	if (!ret) {
95	222	si->num_sensors = le16_to_cpu(attr->num_sensors);
96	223	si->max_requests = attr->max_requests;
..	..	@@ -99,11 +226,199 @@
99	226	si->reg_size = le32_to_cpu(attr->reg_size);
100	227	}
101	228
102		- scmi_xfer_put(handle, t);
	229	+ ph->xops->xfer_put(ph, t);
103	230	return ret;
104	231	}
105	232
106		-static int scmi_sensor_description_get(const struct scmi_handle *handle,
	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,
107	422	struct sensors_info *si)
108	423	{
109	424	int ret, cnt;
..	..	@@ -112,18 +427,20 @@
112	427	struct scmi_xfer *t;
113	428	struct scmi_msg_resp_sensor_description *buf;
114	429
115		- ret = scmi_xfer_get_init(handle, SENSOR_DESCRIPTION_GET,
116		- SCMI_PROTOCOL_SENSOR, sizeof(__le32), 0, &t);
	430	+ ret = ph->xops->xfer_get_init(ph, SENSOR_DESCRIPTION_GET,
	431	+ sizeof(__le32), 0, &t);
117	432	if (ret)
118	433	return ret;
119	434
120	435	buf = t->rx.buf;
121	436
122	437	do {
	438	+ struct scmi_sensor_descriptor *sdesc;
	439	+
123	440	/* Set the number of sensors to be skipped/already read */
124	441	put_unaligned_le32(desc_index, t->tx.buf);
125	442
126		- ret = scmi_do_xfer(handle, t);
	443	+ ret = ph->xops->do_xfer(ph, t);
127	444	if (ret)
128	445	break;
129	446
..	..	@@ -131,50 +448,128 @@
131	448	num_remaining = le16_to_cpu(buf->num_remaining);
132	449
133	450	if (desc_index + num_returned > si->num_sensors) {
134		- dev_err(handle->dev, "No. of sensors can't exceed %d",
	451	+ dev_err(ph->dev, "No. of sensors can't exceed %d",
135	452	si->num_sensors);
136	453	break;
137	454	}
138	455
	456	+ sdesc = &buf->desc[0];
139	457	for (cnt = 0; cnt < num_returned; cnt++) {
140	458	u32 attrh, attrl;
141	459	struct scmi_sensor_info *s;
	460	+ size_t dsize = SCMI_MSG_RESP_SENS_DESCR_BASE_SZ;
142	461
143		- attrl = le32_to_cpu(buf->desc[cnt].attributes_low);
144		- attrh = le32_to_cpu(buf->desc[cnt].attributes_high);
145	462	s = &si->sensors[desc_index + cnt];
146		- s->id = le32_to_cpu(buf->desc[cnt].id);
147		- s->type = SENSOR_TYPE(attrh);
148		- s->scale = SENSOR_SCALE(attrh);
149		- /* Sign extend to a full s8 */
150		- if (s->scale & SENSOR_SCALE_SIGN)
151		- s->scale \|= SENSOR_SCALE_EXTEND;
	463	+ s->id = le32_to_cpu(sdesc->id);
	464	+
	465	+ attrl = le32_to_cpu(sdesc->attributes_low);
	466	+ /* common bitfields parsing */
152	467	s->async = SUPPORTS_ASYNC_READ(attrl);
153	468	s->num_trip_points = NUM_TRIP_POINTS(attrl);
154		- strlcpy(s->name, buf->desc[cnt].name, SCMI_MAX_STR_SIZE);
	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);
155	547	}
156	548
157	549	desc_index += num_returned;
	550	+
	551	+ ph->xops->reset_rx_to_maxsz(ph, t);
158	552	/*
159	553	* check for both returned and remaining to avoid infinite
160	554	* loop due to buggy firmware
161	555	*/
162	556	} while (num_returned && num_remaining);
163	557
164		- scmi_xfer_put(handle, t);
	558	+out:
	559	+ ph->xops->xfer_put(ph, t);
165	560	return ret;
166	561	}
167	562
168		-static int scmi_sensor_trip_point_notify(const struct scmi_handle *handle,
169		- u32 sensor_id, bool enable)
	563	+static inline int
	564	+scmi_sensor_request_notify(const struct scmi_protocol_handle *ph, u32 sensor_id,
	565	+ u8 message_id, bool enable)
170	566	{
171	567	int ret;
172		- u32 evt_cntl = enable ? SENSOR_TP_NOTIFY_ALL : 0;
	568	+ u32 evt_cntl = enable ? SENSOR_NOTIFY_ALL : 0;
173	569	struct scmi_xfer *t;
174		- struct scmi_msg_sensor_trip_point_notify *cfg;
	570	+ struct scmi_msg_sensor_request_notify *cfg;
175	571
176		- ret = scmi_xfer_get_init(handle, SENSOR_TRIP_POINT_NOTIFY,
177		- SCMI_PROTOCOL_SENSOR, sizeof(*cfg), 0, &t);
	572	+ ret = ph->xops->xfer_get_init(ph, message_id, sizeof(*cfg), 0, &t);
178	573	if (ret)
179	574	return ret;
180	575
..	..	@@ -182,23 +577,40 @@
182	577	cfg->id = cpu_to_le32(sensor_id);
183	578	cfg->event_control = cpu_to_le32(evt_cntl);
184	579
185		- ret = scmi_do_xfer(handle, t);
	580	+ ret = ph->xops->do_xfer(ph, t);
186	581
187		- scmi_xfer_put(handle, t);
	582	+ ph->xops->xfer_put(ph, t);
188	583	return ret;
189	584	}
190	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	+
191	594	static int
192		-scmi_sensor_trip_point_config(const struct scmi_handle *handle, u32 sensor_id,
193		- u8 trip_id, u64 trip_value)
	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)
194	606	{
195	607	int ret;
196	608	u32 evt_cntl = SENSOR_TP_BOTH;
197	609	struct scmi_xfer *t;
198	610	struct scmi_msg_set_sensor_trip_point *trip;
199	611
200		- ret = scmi_xfer_get_init(handle, SENSOR_TRIP_POINT_CONFIG,
201		- SCMI_PROTOCOL_SENSOR, sizeof(*trip), 0, &t);
	612	+ ret = ph->xops->xfer_get_init(ph, SENSOR_TRIP_POINT_CONFIG,
	613	+ sizeof(*trip), 0, &t);
202	614	if (ret)
203	615	return ret;
204	616
..	..	@@ -208,98 +620,383 @@
208	620	trip->value_low = cpu_to_le32(trip_value & 0xffffffff);
209	621	trip->value_high = cpu_to_le32(trip_value >> 32);
210	622
211		- ret = scmi_do_xfer(handle, t);
	623	+ ret = ph->xops->do_xfer(ph, t);
212	624
213		- scmi_xfer_put(handle, t);
	625	+ ph->xops->xfer_put(ph, t);
214	626	return ret;
215	627	}
216	628
217		-static int scmi_sensor_reading_get(const struct scmi_handle *handle,
	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,
218	698	u32 sensor_id, u64 *value)
219	699	{
220	700	int ret;
221	701	struct scmi_xfer *t;
222	702	struct scmi_msg_sensor_reading_get *sensor;
223		- struct sensors_info *si = handle->sensor_priv;
	703	+ struct sensors_info *si = ph->get_priv(ph);
224	704	struct scmi_sensor_info *s = si->sensors + sensor_id;
225	705
226		- ret = scmi_xfer_get_init(handle, SENSOR_READING_GET,
227		- SCMI_PROTOCOL_SENSOR, sizeof(*sensor),
228		- sizeof(u64), &t);
	706	+ ret = ph->xops->xfer_get_init(ph, SENSOR_READING_GET,
	707	+ sizeof(*sensor), 0, &t);
229	708	if (ret)
230	709	return ret;
231	710
232	711	sensor = t->tx.buf;
233	712	sensor->id = cpu_to_le32(sensor_id);
234		-
235	713	if (s->async) {
236	714	sensor->flags = cpu_to_le32(SENSOR_READ_ASYNC);
237		- ret = scmi_do_xfer_with_response(handle, t);
238		- if (!ret)
239		- value = get_unaligned_le64((void )
240		- ((__le32 *)t->rx.buf + 1));
	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	+ }
241	726	} else {
242	727	sensor->flags = cpu_to_le32(0);
243		- ret = scmi_do_xfer(handle, t);
	728	+ ret = ph->xops->do_xfer(ph, t);
244	729	if (!ret)
245	730	*value = get_unaligned_le64(t->rx.buf);
246	731	}
247	732
248		- scmi_xfer_put(handle, t);
	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);
249	812	return ret;
250	813	}
251	814
252	815	static const struct scmi_sensor_info *
253		-scmi_sensor_info_get(const struct scmi_handle *handle, u32 sensor_id)
	816	+scmi_sensor_info_get(const struct scmi_protocol_handle *ph, u32 sensor_id)
254	817	{
255		- struct sensors_info *si = handle->sensor_priv;
	818	+ struct sensors_info *si = ph->get_priv(ph);
256	819
257	820	return si->sensors + sensor_id;
258	821	}
259	822
260		-static int scmi_sensor_count_get(const struct scmi_handle *handle)
	823	+static int scmi_sensor_count_get(const struct scmi_protocol_handle *ph)
261	824	{
262		- struct sensors_info *si = handle->sensor_priv;
	825	+ struct sensors_info *si = ph->get_priv(ph);
263	826
264	827	return si->num_sensors;
265	828	}
266	829
267		-static struct scmi_sensor_ops sensor_ops = {
	830	+static const struct scmi_sensor_proto_ops sensor_proto_ops = {
268	831	.count_get = scmi_sensor_count_get,
269	832	.info_get = scmi_sensor_info_get,
270		- .trip_point_notify = scmi_sensor_trip_point_notify,
271	833	.trip_point_config = scmi_sensor_trip_point_config,
272	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,
273	838	};
274	839
275		-static int scmi_sensors_protocol_init(struct scmi_handle *handle)
	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)
276	964	{
277	965	u32 version;
	966	+ int ret;
278	967	struct sensors_info *sinfo;
279	968
280		- scmi_version_get(handle, SCMI_PROTOCOL_SENSOR, &version);
	969	+ ph->xops->version_get(ph, &version);
281	970
282		- dev_dbg(handle->dev, "Sensor Version %d.%d\n",
	971	+ dev_dbg(ph->dev, "Sensor Version %d.%d\n",
283	972	PROTOCOL_REV_MAJOR(version), PROTOCOL_REV_MINOR(version));
284	973
285		- sinfo = devm_kzalloc(handle->dev, sizeof(*sinfo), GFP_KERNEL);
	974	+ sinfo = devm_kzalloc(ph->dev, sizeof(*sinfo), GFP_KERNEL);
286	975	if (!sinfo)
287	976	return -ENOMEM;
	977	+ sinfo->version = version;
288	978
289		- scmi_sensor_attributes_get(handle, sinfo);
290		-
291		- sinfo->sensors = devm_kcalloc(handle->dev, sinfo->num_sensors,
	979	+ ret = scmi_sensor_attributes_get(ph, sinfo);
	980	+ if (ret)
	981	+ return ret;
	982	+ sinfo->sensors = devm_kcalloc(ph->dev, sinfo->num_sensors,
292	983	sizeof(*sinfo->sensors), GFP_KERNEL);
293	984	if (!sinfo->sensors)
294	985	return -ENOMEM;
295	986
296		- scmi_sensor_description_get(handle, sinfo);
	987	+ ret = scmi_sensor_description_get(ph, sinfo);
	988	+ if (ret)
	989	+ return ret;
297	990
298		- sinfo->version = version;
299		- handle->sensor_ops = &sensor_ops;
300		- handle->sensor_priv = sinfo;
301		-
302		- return 0;
	991	+ return ph->set_priv(ph, sinfo);
303	992	}
304	993
305		-DEFINE_SCMI_PROTOCOL_REGISTER_UNREGISTER(SCMI_PROTOCOL_SENSOR, sensors)
	994	+static const struct scmi_protocol scmi_sensors = {
	995	+ .id = SCMI_PROTOCOL_SENSOR,
	996	+ .owner = THIS_MODULE,
	997	+ .init_instance = &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)