1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * MIPI SyS-T framing protocol for STM devices.
4  * Copyright (c) 2018, Intel Corporation.
5  */
6 
7 #include <linux/configfs.h>
8 #include <linux/module.h>
9 #include <linux/device.h>
10 #include <linux/slab.h>
11 #include <linux/uuid.h>
12 #include <linux/stm.h>
13 #include "stm.h"
14 
15 enum sys_t_message_type {
16 	MIPI_SYST_TYPE_BUILD	= 0,
17 	MIPI_SYST_TYPE_SHORT32,
18 	MIPI_SYST_TYPE_STRING,
19 	MIPI_SYST_TYPE_CATALOG,
20 	MIPI_SYST_TYPE_RAW	= 6,
21 	MIPI_SYST_TYPE_SHORT64,
22 	MIPI_SYST_TYPE_CLOCK,
23 };
24 
25 enum sys_t_message_severity {
26 	MIPI_SYST_SEVERITY_MAX	= 0,
27 	MIPI_SYST_SEVERITY_FATAL,
28 	MIPI_SYST_SEVERITY_ERROR,
29 	MIPI_SYST_SEVERITY_WARNING,
30 	MIPI_SYST_SEVERITY_INFO,
31 	MIPI_SYST_SEVERITY_USER1,
32 	MIPI_SYST_SEVERITY_USER2,
33 	MIPI_SYST_SEVERITY_DEBUG,
34 };
35 
36 enum sys_t_message_build_subtype {
37 	MIPI_SYST_BUILD_ID_COMPACT32 = 0,
38 	MIPI_SYST_BUILD_ID_COMPACT64,
39 	MIPI_SYST_BUILD_ID_LONG,
40 };
41 
42 enum sys_t_message_clock_subtype {
43 	MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,
44 };
45 
46 enum sys_t_message_string_subtype {
47 	MIPI_SYST_STRING_GENERIC	= 1,
48 	MIPI_SYST_STRING_FUNCTIONENTER,
49 	MIPI_SYST_STRING_FUNCTIONEXIT,
50 	MIPI_SYST_STRING_INVALIDPARAM	= 5,
51 	MIPI_SYST_STRING_ASSERT		= 7,
52 	MIPI_SYST_STRING_PRINTF_32	= 11,
53 	MIPI_SYST_STRING_PRINTF_64	= 12,
54 };
55 
56 #define MIPI_SYST_TYPE(t)		((u32)(MIPI_SYST_TYPE_ ## t))
57 #define MIPI_SYST_SEVERITY(s)		((u32)(MIPI_SYST_SEVERITY_ ## s) << 4)
58 #define MIPI_SYST_OPT_LOC		BIT(8)
59 #define MIPI_SYST_OPT_LEN		BIT(9)
60 #define MIPI_SYST_OPT_CHK		BIT(10)
61 #define MIPI_SYST_OPT_TS		BIT(11)
62 #define MIPI_SYST_UNIT(u)		((u32)(u) << 12)
63 #define MIPI_SYST_ORIGIN(o)		((u32)(o) << 16)
64 #define MIPI_SYST_OPT_GUID		BIT(23)
65 #define MIPI_SYST_SUBTYPE(s)		((u32)(MIPI_SYST_ ## s) << 24)
66 #define MIPI_SYST_UNITLARGE(u)		(MIPI_SYST_UNIT(u & 0xf) | \
67 					 MIPI_SYST_ORIGIN(u >> 4))
68 #define MIPI_SYST_TYPES(t, s)		(MIPI_SYST_TYPE(t) | \
69 					 MIPI_SYST_SUBTYPE(t ## _ ## s))
70 
71 #define DATA_HEADER	(MIPI_SYST_TYPES(STRING, GENERIC)	| \
72 			 MIPI_SYST_SEVERITY(INFO)		| \
73 			 MIPI_SYST_OPT_GUID)
74 
75 #define CLOCK_SYNC_HEADER	(MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC)	| \
76 				 MIPI_SYST_SEVERITY(MAX))
77 
78 struct sys_t_policy_node {
79 	uuid_t		uuid;
80 	bool		do_len;
81 	unsigned long	ts_interval;
82 	unsigned long	clocksync_interval;
83 };
84 
85 struct sys_t_output {
86 	struct sys_t_policy_node	node;
87 	unsigned long	ts_jiffies;
88 	unsigned long	clocksync_jiffies;
89 };
90 
sys_t_policy_node_init(void * priv)91 static void sys_t_policy_node_init(void *priv)
92 {
93 	struct sys_t_policy_node *pn = priv;
94 
95 	uuid_gen(&pn->uuid);
96 }
97 
sys_t_output_open(void * priv,struct stm_output * output)98 static int sys_t_output_open(void *priv, struct stm_output *output)
99 {
100 	struct sys_t_policy_node *pn = priv;
101 	struct sys_t_output *opriv;
102 
103 	opriv = kzalloc(sizeof(*opriv), GFP_ATOMIC);
104 	if (!opriv)
105 		return -ENOMEM;
106 
107 	memcpy(&opriv->node, pn, sizeof(opriv->node));
108 	output->pdrv_private = opriv;
109 
110 	return 0;
111 }
112 
sys_t_output_close(struct stm_output * output)113 static void sys_t_output_close(struct stm_output *output)
114 {
115 	kfree(output->pdrv_private);
116 }
117 
sys_t_policy_uuid_show(struct config_item * item,char * page)118 static ssize_t sys_t_policy_uuid_show(struct config_item *item,
119 				      char *page)
120 {
121 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
122 
123 	return sprintf(page, "%pU\n", &pn->uuid);
124 }
125 
126 static ssize_t
sys_t_policy_uuid_store(struct config_item * item,const char * page,size_t count)127 sys_t_policy_uuid_store(struct config_item *item, const char *page,
128 			size_t count)
129 {
130 	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
131 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
132 	int ret;
133 
134 	mutex_lock(mutexp);
135 	ret = uuid_parse(page, &pn->uuid);
136 	mutex_unlock(mutexp);
137 
138 	return ret < 0 ? ret : count;
139 }
140 
141 CONFIGFS_ATTR(sys_t_policy_, uuid);
142 
sys_t_policy_do_len_show(struct config_item * item,char * page)143 static ssize_t sys_t_policy_do_len_show(struct config_item *item,
144 				      char *page)
145 {
146 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
147 
148 	return sprintf(page, "%d\n", pn->do_len);
149 }
150 
151 static ssize_t
sys_t_policy_do_len_store(struct config_item * item,const char * page,size_t count)152 sys_t_policy_do_len_store(struct config_item *item, const char *page,
153 			size_t count)
154 {
155 	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
156 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
157 	int ret;
158 
159 	mutex_lock(mutexp);
160 	ret = kstrtobool(page, &pn->do_len);
161 	mutex_unlock(mutexp);
162 
163 	return ret ? ret : count;
164 }
165 
166 CONFIGFS_ATTR(sys_t_policy_, do_len);
167 
sys_t_policy_ts_interval_show(struct config_item * item,char * page)168 static ssize_t sys_t_policy_ts_interval_show(struct config_item *item,
169 					     char *page)
170 {
171 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
172 
173 	return sprintf(page, "%u\n", jiffies_to_msecs(pn->ts_interval));
174 }
175 
176 static ssize_t
sys_t_policy_ts_interval_store(struct config_item * item,const char * page,size_t count)177 sys_t_policy_ts_interval_store(struct config_item *item, const char *page,
178 			       size_t count)
179 {
180 	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
181 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
182 	unsigned int ms;
183 	int ret;
184 
185 	mutex_lock(mutexp);
186 	ret = kstrtouint(page, 10, &ms);
187 	mutex_unlock(mutexp);
188 
189 	if (!ret) {
190 		pn->ts_interval = msecs_to_jiffies(ms);
191 		return count;
192 	}
193 
194 	return ret;
195 }
196 
197 CONFIGFS_ATTR(sys_t_policy_, ts_interval);
198 
sys_t_policy_clocksync_interval_show(struct config_item * item,char * page)199 static ssize_t sys_t_policy_clocksync_interval_show(struct config_item *item,
200 						    char *page)
201 {
202 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
203 
204 	return sprintf(page, "%u\n", jiffies_to_msecs(pn->clocksync_interval));
205 }
206 
207 static ssize_t
sys_t_policy_clocksync_interval_store(struct config_item * item,const char * page,size_t count)208 sys_t_policy_clocksync_interval_store(struct config_item *item,
209 				      const char *page, size_t count)
210 {
211 	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
212 	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
213 	unsigned int ms;
214 	int ret;
215 
216 	mutex_lock(mutexp);
217 	ret = kstrtouint(page, 10, &ms);
218 	mutex_unlock(mutexp);
219 
220 	if (!ret) {
221 		pn->clocksync_interval = msecs_to_jiffies(ms);
222 		return count;
223 	}
224 
225 	return ret;
226 }
227 
228 CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);
229 
230 static struct configfs_attribute *sys_t_policy_attrs[] = {
231 	&sys_t_policy_attr_uuid,
232 	&sys_t_policy_attr_do_len,
233 	&sys_t_policy_attr_ts_interval,
234 	&sys_t_policy_attr_clocksync_interval,
235 	NULL,
236 };
237 
sys_t_need_ts(struct sys_t_output * op)238 static inline bool sys_t_need_ts(struct sys_t_output *op)
239 {
240 	if (op->node.ts_interval &&
241 	    time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {
242 		op->ts_jiffies = jiffies;
243 
244 		return true;
245 	}
246 
247 	return false;
248 }
249 
sys_t_need_clock_sync(struct sys_t_output * op)250 static bool sys_t_need_clock_sync(struct sys_t_output *op)
251 {
252 	if (op->node.clocksync_interval &&
253 	    time_after(jiffies,
254 		       op->clocksync_jiffies + op->node.clocksync_interval)) {
255 		op->clocksync_jiffies = jiffies;
256 
257 		return true;
258 	}
259 
260 	return false;
261 }
262 
263 static ssize_t
sys_t_clock_sync(struct stm_data * data,unsigned int m,unsigned int c)264 sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c)
265 {
266 	u32 header = CLOCK_SYNC_HEADER;
267 	const unsigned char nil = 0;
268 	u64 payload[2]; /* Clock value and frequency */
269 	ssize_t sz;
270 
271 	sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
272 			  4, (u8 *)&header);
273 	if (sz <= 0)
274 		return sz;
275 
276 	payload[0] = ktime_get_real_ns();
277 	payload[1] = NSEC_PER_SEC;
278 	sz = stm_data_write(data, m, c, false, &payload, sizeof(payload));
279 	if (sz <= 0)
280 		return sz;
281 
282 	data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
283 
284 	return sizeof(header) + sizeof(payload);
285 }
286 
sys_t_write(struct stm_data * data,struct stm_output * output,unsigned int chan,const char * buf,size_t count)287 static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output,
288 			   unsigned int chan, const char *buf, size_t count)
289 {
290 	struct sys_t_output *op = output->pdrv_private;
291 	unsigned int c = output->channel + chan;
292 	unsigned int m = output->master;
293 	const unsigned char nil = 0;
294 	u32 header = DATA_HEADER;
295 	u8 uuid[UUID_SIZE];
296 	ssize_t sz;
297 
298 	/* We require an existing policy node to proceed */
299 	if (!op)
300 		return -EINVAL;
301 
302 	if (sys_t_need_clock_sync(op)) {
303 		sz = sys_t_clock_sync(data, m, c);
304 		if (sz <= 0)
305 			return sz;
306 	}
307 
308 	if (op->node.do_len)
309 		header |= MIPI_SYST_OPT_LEN;
310 	if (sys_t_need_ts(op))
311 		header |= MIPI_SYST_OPT_TS;
312 
313 	/*
314 	 * STP framing rules for SyS-T frames:
315 	 *   * the first packet of the SyS-T frame is timestamped;
316 	 *   * the last packet is a FLAG.
317 	 */
318 	/* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */
319 	/* HEADER */
320 	sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
321 			  4, (u8 *)&header);
322 	if (sz <= 0)
323 		return sz;
324 
325 	/* GUID */
326 	export_uuid(uuid, &op->node.uuid);
327 	sz = stm_data_write(data, m, c, false, uuid, sizeof(op->node.uuid));
328 	if (sz <= 0)
329 		return sz;
330 
331 	/* [LENGTH] */
332 	if (op->node.do_len) {
333 		u16 length = count;
334 
335 		sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,
336 				  (u8 *)&length);
337 		if (sz <= 0)
338 			return sz;
339 	}
340 
341 	/* [TIMESTAMP] */
342 	if (header & MIPI_SYST_OPT_TS) {
343 		u64 ts = ktime_get_real_ns();
344 
345 		sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));
346 		if (sz <= 0)
347 			return sz;
348 	}
349 
350 	/* DATA */
351 	sz = stm_data_write(data, m, c, false, buf, count);
352 	if (sz > 0)
353 		data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
354 
355 	return sz;
356 }
357 
358 static const struct stm_protocol_driver sys_t_pdrv = {
359 	.owner			= THIS_MODULE,
360 	.name			= "p_sys-t",
361 	.priv_sz		= sizeof(struct sys_t_policy_node),
362 	.write			= sys_t_write,
363 	.policy_attr		= sys_t_policy_attrs,
364 	.policy_node_init	= sys_t_policy_node_init,
365 	.output_open		= sys_t_output_open,
366 	.output_close		= sys_t_output_close,
367 };
368 
sys_t_stm_init(void)369 static int sys_t_stm_init(void)
370 {
371 	return stm_register_protocol(&sys_t_pdrv);
372 }
373 
sys_t_stm_exit(void)374 static void sys_t_stm_exit(void)
375 {
376 	stm_unregister_protocol(&sys_t_pdrv);
377 }
378 
379 module_init(sys_t_stm_init);
380 module_exit(sys_t_stm_exit);
381 
382 MODULE_LICENSE("GPL v2");
383 MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
384 MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
385