Loading...
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
15enum 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
25enum 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
36enum 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
42enum sys_t_message_clock_subtype {
43 MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,
44};
45
46enum 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
78struct sys_t_policy_node {
79 uuid_t uuid;
80 bool do_len;
81 unsigned long ts_interval;
82 unsigned long clocksync_interval;
83};
84
85struct sys_t_output {
86 struct sys_t_policy_node node;
87 unsigned long ts_jiffies;
88 unsigned long clocksync_jiffies;
89};
90
91static 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
98static 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
113static void sys_t_output_close(struct stm_output *output)
114{
115 kfree(output->pdrv_private);
116}
117
118static 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
126static ssize_t
127sys_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
141CONFIGFS_ATTR(sys_t_policy_, uuid);
142
143static 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
151static ssize_t
152sys_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
166CONFIGFS_ATTR(sys_t_policy_, do_len);
167
168static 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
176static ssize_t
177sys_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
197CONFIGFS_ATTR(sys_t_policy_, ts_interval);
198
199static 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
207static ssize_t
208sys_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
228CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);
229
230static 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
238static 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
250static 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
263static ssize_t
264sys_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
287static 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 /* We require an existing policy node to proceed */
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 * STP framing rules for SyS-T frames:
314 * * the first packet of the SyS-T frame is timestamped;
315 * * the last packet is a FLAG.
316 */
317 /* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */
318 /* HEADER */
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 /* GUID */
325 sz = stm_data_write(data, m, c, false, op->node.uuid.b, UUID_SIZE);
326 if (sz <= 0)
327 return sz;
328
329 /* [LENGTH] */
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 /* [TIMESTAMP] */
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 /* DATA */
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
356static 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
367static int sys_t_stm_init(void)
368{
369 return stm_register_protocol(&sys_t_pdrv);
370}
371
372static void sys_t_stm_exit(void)
373{
374 stm_unregister_protocol(&sys_t_pdrv);
375}
376
377module_init(sys_t_stm_init);
378module_exit(sys_t_stm_exit);
379
380MODULE_LICENSE("GPL v2");
381MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
382MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
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
15enum 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 MIPI_SYST_TYPE_SBD,
24};
25
26enum sys_t_message_severity {
27 MIPI_SYST_SEVERITY_MAX = 0,
28 MIPI_SYST_SEVERITY_FATAL,
29 MIPI_SYST_SEVERITY_ERROR,
30 MIPI_SYST_SEVERITY_WARNING,
31 MIPI_SYST_SEVERITY_INFO,
32 MIPI_SYST_SEVERITY_USER1,
33 MIPI_SYST_SEVERITY_USER2,
34 MIPI_SYST_SEVERITY_DEBUG,
35};
36
37enum sys_t_message_build_subtype {
38 MIPI_SYST_BUILD_ID_COMPACT32 = 0,
39 MIPI_SYST_BUILD_ID_COMPACT64,
40 MIPI_SYST_BUILD_ID_LONG,
41};
42
43enum sys_t_message_clock_subtype {
44 MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,
45};
46
47enum sys_t_message_string_subtype {
48 MIPI_SYST_STRING_GENERIC = 1,
49 MIPI_SYST_STRING_FUNCTIONENTER,
50 MIPI_SYST_STRING_FUNCTIONEXIT,
51 MIPI_SYST_STRING_INVALIDPARAM = 5,
52 MIPI_SYST_STRING_ASSERT = 7,
53 MIPI_SYST_STRING_PRINTF_32 = 11,
54 MIPI_SYST_STRING_PRINTF_64 = 12,
55};
56
57/**
58 * enum sys_t_message_sbd_subtype - SyS-T SBD message subtypes
59 * @MIPI_SYST_SBD_ID32: SBD message with 32-bit message ID
60 * @MIPI_SYST_SBD_ID64: SBD message with 64-bit message ID
61 *
62 * Structured Binary Data messages can send information of arbitrary length,
63 * together with ID's that describe BLOB's content and layout.
64 */
65enum sys_t_message_sbd_subtype {
66 MIPI_SYST_SBD_ID32 = 0,
67 MIPI_SYST_SBD_ID64 = 1,
68};
69
70#define MIPI_SYST_TYPE(t) ((u32)(MIPI_SYST_TYPE_ ## t))
71#define MIPI_SYST_SEVERITY(s) ((u32)(MIPI_SYST_SEVERITY_ ## s) << 4)
72#define MIPI_SYST_OPT_LOC BIT(8)
73#define MIPI_SYST_OPT_LEN BIT(9)
74#define MIPI_SYST_OPT_CHK BIT(10)
75#define MIPI_SYST_OPT_TS BIT(11)
76#define MIPI_SYST_UNIT(u) ((u32)(u) << 12)
77#define MIPI_SYST_ORIGIN(o) ((u32)(o) << 16)
78#define MIPI_SYST_OPT_GUID BIT(23)
79#define MIPI_SYST_SUBTYPE(s) ((u32)(MIPI_SYST_ ## s) << 24)
80#define MIPI_SYST_UNITLARGE(u) (MIPI_SYST_UNIT(u & 0xf) | \
81 MIPI_SYST_ORIGIN(u >> 4))
82#define MIPI_SYST_TYPES(t, s) (MIPI_SYST_TYPE(t) | \
83 MIPI_SYST_SUBTYPE(t ## _ ## s))
84
85#define DATA_HEADER (MIPI_SYST_TYPES(STRING, GENERIC) | \
86 MIPI_SYST_SEVERITY(INFO) | \
87 MIPI_SYST_OPT_GUID)
88
89#define CLOCK_SYNC_HEADER (MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC) | \
90 MIPI_SYST_SEVERITY(MAX))
91
92/*
93 * SyS-T and ftrace headers are compatible to an extent that ftrace event ID
94 * and args can be treated as SyS-T SBD message with 64-bit ID and arguments
95 * BLOB right behind the header without modification. Bits [16:63] coming
96 * together with message ID from ftrace aren't used by SBD and must be zeroed.
97 *
98 * 0 15 16 23 24 31 32 39 40 63
99 * ftrace: <event_id> <flags> <preempt> <-pid-> <----> <args>
100 * SBD: <------- msg_id ------------------------------> <BLOB>
101 */
102#define SBD_HEADER (MIPI_SYST_TYPES(SBD, ID64) | \
103 MIPI_SYST_SEVERITY(INFO) | \
104 MIPI_SYST_OPT_GUID)
105
106struct sys_t_policy_node {
107 uuid_t uuid;
108 bool do_len;
109 unsigned long ts_interval;
110 unsigned long clocksync_interval;
111};
112
113struct sys_t_output {
114 struct sys_t_policy_node node;
115 unsigned long ts_jiffies;
116 unsigned long clocksync_jiffies;
117};
118
119static void sys_t_policy_node_init(void *priv)
120{
121 struct sys_t_policy_node *pn = priv;
122
123 uuid_gen(&pn->uuid);
124}
125
126static int sys_t_output_open(void *priv, struct stm_output *output)
127{
128 struct sys_t_policy_node *pn = priv;
129 struct sys_t_output *opriv;
130
131 opriv = kzalloc(sizeof(*opriv), GFP_ATOMIC);
132 if (!opriv)
133 return -ENOMEM;
134
135 memcpy(&opriv->node, pn, sizeof(opriv->node));
136 output->pdrv_private = opriv;
137
138 return 0;
139}
140
141static void sys_t_output_close(struct stm_output *output)
142{
143 kfree(output->pdrv_private);
144}
145
146static ssize_t sys_t_policy_uuid_show(struct config_item *item,
147 char *page)
148{
149 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
150
151 return sprintf(page, "%pU\n", &pn->uuid);
152}
153
154static ssize_t
155sys_t_policy_uuid_store(struct config_item *item, const char *page,
156 size_t count)
157{
158 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
159 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
160 int ret;
161
162 mutex_lock(mutexp);
163 ret = uuid_parse(page, &pn->uuid);
164 mutex_unlock(mutexp);
165
166 return ret < 0 ? ret : count;
167}
168
169CONFIGFS_ATTR(sys_t_policy_, uuid);
170
171static ssize_t sys_t_policy_do_len_show(struct config_item *item,
172 char *page)
173{
174 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
175
176 return sprintf(page, "%d\n", pn->do_len);
177}
178
179static ssize_t
180sys_t_policy_do_len_store(struct config_item *item, const char *page,
181 size_t count)
182{
183 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
184 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
185 int ret;
186
187 mutex_lock(mutexp);
188 ret = kstrtobool(page, &pn->do_len);
189 mutex_unlock(mutexp);
190
191 return ret ? ret : count;
192}
193
194CONFIGFS_ATTR(sys_t_policy_, do_len);
195
196static ssize_t sys_t_policy_ts_interval_show(struct config_item *item,
197 char *page)
198{
199 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
200
201 return sprintf(page, "%u\n", jiffies_to_msecs(pn->ts_interval));
202}
203
204static ssize_t
205sys_t_policy_ts_interval_store(struct config_item *item, const char *page,
206 size_t count)
207{
208 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
209 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
210 unsigned int ms;
211 int ret;
212
213 mutex_lock(mutexp);
214 ret = kstrtouint(page, 10, &ms);
215 mutex_unlock(mutexp);
216
217 if (!ret) {
218 pn->ts_interval = msecs_to_jiffies(ms);
219 return count;
220 }
221
222 return ret;
223}
224
225CONFIGFS_ATTR(sys_t_policy_, ts_interval);
226
227static ssize_t sys_t_policy_clocksync_interval_show(struct config_item *item,
228 char *page)
229{
230 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
231
232 return sprintf(page, "%u\n", jiffies_to_msecs(pn->clocksync_interval));
233}
234
235static ssize_t
236sys_t_policy_clocksync_interval_store(struct config_item *item,
237 const char *page, size_t count)
238{
239 struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
240 struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
241 unsigned int ms;
242 int ret;
243
244 mutex_lock(mutexp);
245 ret = kstrtouint(page, 10, &ms);
246 mutex_unlock(mutexp);
247
248 if (!ret) {
249 pn->clocksync_interval = msecs_to_jiffies(ms);
250 return count;
251 }
252
253 return ret;
254}
255
256CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);
257
258static struct configfs_attribute *sys_t_policy_attrs[] = {
259 &sys_t_policy_attr_uuid,
260 &sys_t_policy_attr_do_len,
261 &sys_t_policy_attr_ts_interval,
262 &sys_t_policy_attr_clocksync_interval,
263 NULL,
264};
265
266static inline bool sys_t_need_ts(struct sys_t_output *op)
267{
268 if (op->node.ts_interval &&
269 time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {
270 op->ts_jiffies = jiffies;
271
272 return true;
273 }
274
275 return false;
276}
277
278static bool sys_t_need_clock_sync(struct sys_t_output *op)
279{
280 if (op->node.clocksync_interval &&
281 time_after(jiffies,
282 op->clocksync_jiffies + op->node.clocksync_interval)) {
283 op->clocksync_jiffies = jiffies;
284
285 return true;
286 }
287
288 return false;
289}
290
291static ssize_t
292sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c)
293{
294 u32 header = CLOCK_SYNC_HEADER;
295 const unsigned char nil = 0;
296 u64 payload[2]; /* Clock value and frequency */
297 ssize_t sz;
298
299 sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
300 4, (u8 *)&header);
301 if (sz <= 0)
302 return sz;
303
304 payload[0] = ktime_get_real_ns();
305 payload[1] = NSEC_PER_SEC;
306 sz = stm_data_write(data, m, c, false, &payload, sizeof(payload));
307 if (sz <= 0)
308 return sz;
309
310 data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);
311
312 return sizeof(header) + sizeof(payload);
313}
314
315static inline u32 sys_t_header(struct stm_source_data *source)
316{
317 if (source && source->type == STM_FTRACE)
318 return SBD_HEADER;
319 return DATA_HEADER;
320}
321
322static ssize_t sys_t_write_data(struct stm_data *data,
323 struct stm_source_data *source,
324 unsigned int master, unsigned int channel,
325 bool ts_first, const void *buf, size_t count)
326{
327 ssize_t sz;
328 const unsigned char nil = 0;
329
330 /*
331 * Ftrace is zero-copy compatible with SyS-T SBD, but requires
332 * special handling of first 64 bits. Trim and send them separately
333 * to avoid damage on original ftrace buffer.
334 */
335 if (source && source->type == STM_FTRACE) {
336 u64 compat_ftrace_header;
337 ssize_t header_sz;
338 ssize_t buf_sz;
339
340 if (count < sizeof(compat_ftrace_header))
341 return -EINVAL;
342
343 /* SBD only makes use of low 16 bits (event ID) from ftrace event */
344 compat_ftrace_header = *(u64 *)buf & 0xffff;
345 header_sz = stm_data_write(data, master, channel, false,
346 &compat_ftrace_header,
347 sizeof(compat_ftrace_header));
348 if (header_sz != sizeof(compat_ftrace_header))
349 return header_sz;
350
351 buf_sz = stm_data_write(data, master, channel, false,
352 buf + header_sz, count - header_sz);
353 if (buf_sz != count - header_sz)
354 return buf_sz;
355 sz = header_sz + buf_sz;
356 } else {
357 sz = stm_data_write(data, master, channel, false, buf, count);
358 }
359
360 if (sz <= 0)
361 return sz;
362
363 data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil);
364
365 return sz;
366}
367
368static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output,
369 unsigned int chan, const char *buf, size_t count,
370 struct stm_source_data *source)
371{
372 struct sys_t_output *op = output->pdrv_private;
373 unsigned int c = output->channel + chan;
374 unsigned int m = output->master;
375 u32 header = sys_t_header(source);
376 u8 uuid[UUID_SIZE];
377 ssize_t sz;
378
379 /* We require an existing policy node to proceed */
380 if (!op)
381 return -EINVAL;
382
383 if (sys_t_need_clock_sync(op)) {
384 sz = sys_t_clock_sync(data, m, c);
385 if (sz <= 0)
386 return sz;
387 }
388
389 if (op->node.do_len)
390 header |= MIPI_SYST_OPT_LEN;
391 if (sys_t_need_ts(op))
392 header |= MIPI_SYST_OPT_TS;
393
394 /*
395 * STP framing rules for SyS-T frames:
396 * * the first packet of the SyS-T frame is timestamped;
397 * * the last packet is a FLAG.
398 */
399 /* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */
400 /* HEADER */
401 sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
402 4, (u8 *)&header);
403 if (sz <= 0)
404 return sz;
405
406 /* GUID */
407 export_uuid(uuid, &op->node.uuid);
408 sz = stm_data_write(data, m, c, false, uuid, sizeof(op->node.uuid));
409 if (sz <= 0)
410 return sz;
411
412 /* [LENGTH] */
413 if (op->node.do_len) {
414 u16 length = count;
415
416 sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,
417 (u8 *)&length);
418 if (sz <= 0)
419 return sz;
420 }
421
422 /* [TIMESTAMP] */
423 if (header & MIPI_SYST_OPT_TS) {
424 u64 ts = ktime_get_real_ns();
425
426 sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));
427 if (sz <= 0)
428 return sz;
429 }
430
431 /* DATA */
432 return sys_t_write_data(data, source, m, c, false, buf, count);
433}
434
435static const struct stm_protocol_driver sys_t_pdrv = {
436 .owner = THIS_MODULE,
437 .name = "p_sys-t",
438 .priv_sz = sizeof(struct sys_t_policy_node),
439 .write = sys_t_write,
440 .policy_attr = sys_t_policy_attrs,
441 .policy_node_init = sys_t_policy_node_init,
442 .output_open = sys_t_output_open,
443 .output_close = sys_t_output_close,
444};
445
446static int sys_t_stm_init(void)
447{
448 return stm_register_protocol(&sys_t_pdrv);
449}
450
451static void sys_t_stm_exit(void)
452{
453 stm_unregister_protocol(&sys_t_pdrv);
454}
455
456module_init(sys_t_stm_init);
457module_exit(sys_t_stm_exit);
458
459MODULE_LICENSE("GPL v2");
460MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
461MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");