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 (MIPI_SYST_TYPES(STRING, GENERIC) | \ |
72 | MIPI_SYST_SEVERITY(INFO) | \ |
73 | MIPI_SYST_OPT_GUID) |
74 | |
75 | #define (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 | |
91 | static void sys_t_policy_node_init(void *priv) |
92 | { |
93 | struct sys_t_policy_node *pn = priv; |
94 | |
95 | uuid_gen(u: &pn->uuid); |
96 | } |
97 | |
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(size: 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 | |
113 | static void sys_t_output_close(struct stm_output *output) |
114 | { |
115 | kfree(objp: output->pdrv_private); |
116 | } |
117 | |
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(buf: page, fmt: "%pU\n" , &pn->uuid); |
124 | } |
125 | |
126 | static ssize_t |
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(uuid: page, u: &pn->uuid); |
136 | mutex_unlock(lock: mutexp); |
137 | |
138 | return ret < 0 ? ret : count; |
139 | } |
140 | |
141 | CONFIGFS_ATTR(sys_t_policy_, uuid); |
142 | |
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(buf: page, fmt: "%d\n" , pn->do_len); |
149 | } |
150 | |
151 | static ssize_t |
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(s: page, res: &pn->do_len); |
161 | mutex_unlock(lock: mutexp); |
162 | |
163 | return ret ? ret : count; |
164 | } |
165 | |
166 | CONFIGFS_ATTR(sys_t_policy_, do_len); |
167 | |
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(buf: page, fmt: "%u\n" , jiffies_to_msecs(j: pn->ts_interval)); |
174 | } |
175 | |
176 | static ssize_t |
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(s: page, base: 10, res: &ms); |
187 | mutex_unlock(lock: mutexp); |
188 | |
189 | if (!ret) { |
190 | pn->ts_interval = msecs_to_jiffies(m: ms); |
191 | return count; |
192 | } |
193 | |
194 | return ret; |
195 | } |
196 | |
197 | CONFIGFS_ATTR(sys_t_policy_, ts_interval); |
198 | |
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(buf: page, fmt: "%u\n" , jiffies_to_msecs(j: pn->clocksync_interval)); |
205 | } |
206 | |
207 | static ssize_t |
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(s: page, base: 10, res: &ms); |
218 | mutex_unlock(lock: mutexp); |
219 | |
220 | if (!ret) { |
221 | pn->clocksync_interval = msecs_to_jiffies(m: 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 | |
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 | |
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 |
264 | sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c) |
265 | { |
266 | u32 = 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, ts_first: false, buf: &payload, count: 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 | |
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 = 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(dst: uuid, src: &op->node.uuid); |
327 | sz = stm_data_write(data, m, c, ts_first: false, buf: uuid, count: 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, ts_first: false, buf: &ts, count: sizeof(ts)); |
346 | if (sz <= 0) |
347 | return sz; |
348 | } |
349 | |
350 | /* DATA */ |
351 | sz = stm_data_write(data, m, c, ts_first: 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 | |
369 | static int sys_t_stm_init(void) |
370 | { |
371 | return stm_register_protocol(pdrv: &sys_t_pdrv); |
372 | } |
373 | |
374 | static void sys_t_stm_exit(void) |
375 | { |
376 | stm_unregister_protocol(pdrv: &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 | |