This source file includes following definitions.
- sys_t_policy_node_init
- sys_t_output_open
- sys_t_output_close
- sys_t_policy_uuid_show
- sys_t_policy_uuid_store
- sys_t_policy_do_len_show
- sys_t_policy_do_len_store
- sys_t_policy_ts_interval_show
- sys_t_policy_ts_interval_store
- sys_t_policy_clocksync_interval_show
- sys_t_policy_clocksync_interval_store
- sys_t_need_ts
- sys_t_need_clock_sync
- sys_t_clock_sync
- sys_t_write
- sys_t_stm_init
- sys_t_stm_exit
1
2
3
4
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
91 static void sys_t_policy_node_init(void *priv)
92 {
93 struct sys_t_policy_node *pn = priv;
94
95 generate_random_uuid(pn->uuid.b);
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(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(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(page, "%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(page, &pn->uuid);
136 mutex_unlock(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(page, "%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(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
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
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
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
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
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 header = CLOCK_SYNC_HEADER;
267 const unsigned char nil = 0;
268 u64 payload[2];
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
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 ssize_t sz;
296
297
298 if (!op)
299 return -EINVAL;
300
301 if (sys_t_need_clock_sync(op)) {
302 sz = sys_t_clock_sync(data, m, c);
303 if (sz <= 0)
304 return sz;
305 }
306
307 if (op->node.do_len)
308 header |= MIPI_SYST_OPT_LEN;
309 if (sys_t_need_ts(op))
310 header |= MIPI_SYST_OPT_TS;
311
312
313
314
315
316
317
318
319 sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
320 4, (u8 *)&header);
321 if (sz <= 0)
322 return sz;
323
324
325 sz = stm_data_write(data, m, c, false, op->node.uuid.b, UUID_SIZE);
326 if (sz <= 0)
327 return sz;
328
329
330 if (op->node.do_len) {
331 u16 length = count;
332
333 sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,
334 (u8 *)&length);
335 if (sz <= 0)
336 return sz;
337 }
338
339
340 if (header & MIPI_SYST_OPT_TS) {
341 u64 ts = ktime_get_real_ns();
342
343 sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));
344 if (sz <= 0)
345 return sz;
346 }
347
348
349 sz = stm_data_write(data, m, c, false, buf, count);
350 if (sz > 0)
351 data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
352
353 return sz;
354 }
355
356 static const struct stm_protocol_driver sys_t_pdrv = {
357 .owner = THIS_MODULE,
358 .name = "p_sys-t",
359 .priv_sz = sizeof(struct sys_t_policy_node),
360 .write = sys_t_write,
361 .policy_attr = sys_t_policy_attrs,
362 .policy_node_init = sys_t_policy_node_init,
363 .output_open = sys_t_output_open,
364 .output_close = sys_t_output_close,
365 };
366
367 static int sys_t_stm_init(void)
368 {
369 return stm_register_protocol(&sys_t_pdrv);
370 }
371
372 static void sys_t_stm_exit(void)
373 {
374 stm_unregister_protocol(&sys_t_pdrv);
375 }
376
377 module_init(sys_t_stm_init);
378 module_exit(sys_t_stm_exit);
379
380 MODULE_LICENSE("GPL v2");
381 MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
382 MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");