[mirror_ubuntu-hirsute-kernel.git] / drivers / hwtracing / stm / p_sys-t.c

// SPDX-License-Identifier: GPL-2.0
/*
 * MIPI SyS-T framing protocol for STM devices.
 * Copyright (c) 2018, Intel Corporation.
 */

#include <linux/configfs.h>
#include <linux/module.h>
#include <linux/device.h>
#include <linux/slab.h>
#include <linux/uuid.h>
#include <linux/stm.h>
#include "stm.h"

enum sys_t_message_type {
	MIPI_SYST_TYPE_BUILD	= 0,
	MIPI_SYST_TYPE_SHORT32,
	MIPI_SYST_TYPE_STRING,
	MIPI_SYST_TYPE_CATALOG,
	MIPI_SYST_TYPE_RAW	= 6,
	MIPI_SYST_TYPE_SHORT64,
	MIPI_SYST_TYPE_CLOCK,
};

enum sys_t_message_severity {
	MIPI_SYST_SEVERITY_MAX	= 0,
	MIPI_SYST_SEVERITY_FATAL,
	MIPI_SYST_SEVERITY_ERROR,
	MIPI_SYST_SEVERITY_WARNING,
	MIPI_SYST_SEVERITY_INFO,
	MIPI_SYST_SEVERITY_USER1,
	MIPI_SYST_SEVERITY_USER2,
	MIPI_SYST_SEVERITY_DEBUG,
};

enum sys_t_message_build_subtype {
	MIPI_SYST_BUILD_ID_COMPACT32 = 0,
	MIPI_SYST_BUILD_ID_COMPACT64,
	MIPI_SYST_BUILD_ID_LONG,
};

enum sys_t_message_clock_subtype {
	MIPI_SYST_CLOCK_TRANSPORT_SYNC = 1,
};

enum sys_t_message_string_subtype {
	MIPI_SYST_STRING_GENERIC	= 1,
	MIPI_SYST_STRING_FUNCTIONENTER,
	MIPI_SYST_STRING_FUNCTIONEXIT,
	MIPI_SYST_STRING_INVALIDPARAM	= 5,
	MIPI_SYST_STRING_ASSERT		= 7,
	MIPI_SYST_STRING_PRINTF_32	= 11,
	MIPI_SYST_STRING_PRINTF_64	= 12,
};

#define MIPI_SYST_TYPE(t)		((u32)(MIPI_SYST_TYPE_ ## t))
#define MIPI_SYST_SEVERITY(s)		((u32)(MIPI_SYST_SEVERITY_ ## s) << 4)
#define MIPI_SYST_OPT_LOC		BIT(8)
#define MIPI_SYST_OPT_LEN		BIT(9)
#define MIPI_SYST_OPT_CHK		BIT(10)
#define MIPI_SYST_OPT_TS		BIT(11)
#define MIPI_SYST_UNIT(u)		((u32)(u) << 12)
#define MIPI_SYST_ORIGIN(o)		((u32)(o) << 16)
#define MIPI_SYST_OPT_GUID		BIT(23)
#define MIPI_SYST_SUBTYPE(s)		((u32)(MIPI_SYST_ ## s) << 24)
#define MIPI_SYST_UNITLARGE(u)		(MIPI_SYST_UNIT(u & 0xf) | \
					 MIPI_SYST_ORIGIN(u >> 4))
#define MIPI_SYST_TYPES(t, s)		(MIPI_SYST_TYPE(t) | \
					 MIPI_SYST_SUBTYPE(t ## _ ## s))

#define DATA_HEADER	(MIPI_SYST_TYPES(STRING, GENERIC)	| \
			 MIPI_SYST_SEVERITY(INFO)		| \
			 MIPI_SYST_OPT_GUID)

#define CLOCK_SYNC_HEADER	(MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC)	| \
				 MIPI_SYST_SEVERITY(MAX))

struct sys_t_policy_node {
	uuid_t		uuid;
	bool		do_len;
	unsigned long	ts_interval;
	unsigned long	clocksync_interval;
};

struct sys_t_output {
	struct sys_t_policy_node	node;
	unsigned long	ts_jiffies;
	unsigned long	clocksync_jiffies;
};

static void sys_t_policy_node_init(void *priv)
{
	struct sys_t_policy_node *pn = priv;

	generate_random_uuid(pn->uuid.b);
}

static int sys_t_output_open(void *priv, struct stm_output *output)
{
	struct sys_t_policy_node *pn = priv;
	struct sys_t_output *opriv;

	opriv = kzalloc(sizeof(*opriv), GFP_ATOMIC);
	if (!opriv)
		return -ENOMEM;

	memcpy(&opriv->node, pn, sizeof(opriv->node));
	output->pdrv_private = opriv;

	return 0;
}

static void sys_t_output_close(struct stm_output *output)
{
	kfree(output->pdrv_private);
}

static ssize_t sys_t_policy_uuid_show(struct config_item *item,
				      char *page)
{
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);

	return sprintf(page, "%pU\n", &pn->uuid);
}

static ssize_t
sys_t_policy_uuid_store(struct config_item *item, const char *page,
			size_t count)
{
	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
	int ret;

	mutex_lock(mutexp);
	ret = uuid_parse(page, &pn->uuid);
	mutex_unlock(mutexp);

	return ret < 0 ? ret : count;
}

CONFIGFS_ATTR(sys_t_policy_, uuid);

static ssize_t sys_t_policy_do_len_show(struct config_item *item,
				      char *page)
{
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);

	return sprintf(page, "%d\n", pn->do_len);
}

static ssize_t
sys_t_policy_do_len_store(struct config_item *item, const char *page,
			size_t count)
{
	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
	int ret;

	mutex_lock(mutexp);
	ret = kstrtobool(page, &pn->do_len);
	mutex_unlock(mutexp);

	return ret ? ret : count;
}

CONFIGFS_ATTR(sys_t_policy_, do_len);

static ssize_t sys_t_policy_ts_interval_show(struct config_item *item,
					     char *page)
{
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);

	return sprintf(page, "%u\n", jiffies_to_msecs(pn->ts_interval));
}

static ssize_t
sys_t_policy_ts_interval_store(struct config_item *item, const char *page,
			       size_t count)
{
	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
	unsigned int ms;
	int ret;

	mutex_lock(mutexp);
	ret = kstrtouint(page, 10, &ms);
	mutex_unlock(mutexp);

	if (!ret) {
		pn->ts_interval = msecs_to_jiffies(ms);
		return count;
	}

	return ret;
}

CONFIGFS_ATTR(sys_t_policy_, ts_interval);

static ssize_t sys_t_policy_clocksync_interval_show(struct config_item *item,
						    char *page)
{
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);

	return sprintf(page, "%u\n", jiffies_to_msecs(pn->clocksync_interval));
}

static ssize_t
sys_t_policy_clocksync_interval_store(struct config_item *item,
				      const char *page, size_t count)
{
	struct mutex *mutexp = &item->ci_group->cg_subsys->su_mutex;
	struct sys_t_policy_node *pn = to_pdrv_policy_node(item);
	unsigned int ms;
	int ret;

	mutex_lock(mutexp);
	ret = kstrtouint(page, 10, &ms);
	mutex_unlock(mutexp);

	if (!ret) {
		pn->clocksync_interval = msecs_to_jiffies(ms);
		return count;
	}

	return ret;
}

CONFIGFS_ATTR(sys_t_policy_, clocksync_interval);

static struct configfs_attribute *sys_t_policy_attrs[] = {
	&sys_t_policy_attr_uuid,
	&sys_t_policy_attr_do_len,
	&sys_t_policy_attr_ts_interval,
	&sys_t_policy_attr_clocksync_interval,
	NULL,
};

static inline bool sys_t_need_ts(struct sys_t_output *op)
{
	if (op->node.ts_interval &&
	    time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {
		op->ts_jiffies = jiffies;

		return true;
	}

	return false;
}

static bool sys_t_need_clock_sync(struct sys_t_output *op)
{
	if (op->node.clocksync_interval &&
	    time_after(jiffies,
		       op->clocksync_jiffies + op->node.clocksync_interval)) {
		op->clocksync_jiffies = jiffies;

		return true;
	}

	return false;
}

static ssize_t
sys_t_clock_sync(struct stm_data *data, unsigned int m, unsigned int c)
{
	u32 header = CLOCK_SYNC_HEADER;
	const unsigned char nil = 0;
	u64 payload[2]; /* Clock value and frequency */
	ssize_t sz;

	sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
			  4, (u8 *)&header);
	if (sz <= 0)
		return sz;

	payload[0] = ktime_get_real_ns();
	payload[1] = NSEC_PER_SEC;
	sz = stm_data_write(data, m, c, false, &payload, sizeof(payload));
	if (sz <= 0)
		return sz;

	data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);

	return sizeof(header) + sizeof(payload);
}

static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output,
			   unsigned int chan, const char *buf, size_t count)
{
	struct sys_t_output *op = output->pdrv_private;
	unsigned int c = output->channel + chan;
	unsigned int m = output->master;
	const unsigned char nil = 0;
	u32 header = DATA_HEADER;
	ssize_t sz;

	/* We require an existing policy node to proceed */
	if (!op)
		return -EINVAL;

	if (sys_t_need_clock_sync(op)) {
		sz = sys_t_clock_sync(data, m, c);
		if (sz <= 0)
			return sz;
	}

	if (op->node.do_len)
		header |= MIPI_SYST_OPT_LEN;
	if (sys_t_need_ts(op))
		header |= MIPI_SYST_OPT_TS;

	/*
	 * STP framing rules for SyS-T frames:
	 *   * the first packet of the SyS-T frame is timestamped;
	 *   * the last packet is a FLAG.
	 */
	/* Message layout: HEADER / GUID / [LENGTH /][TIMESTAMP /] DATA */
	/* HEADER */
	sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_TIMESTAMPED,
			  4, (u8 *)&header);
	if (sz <= 0)
		return sz;

	/* GUID */
	sz = stm_data_write(data, m, c, false, op->node.uuid.b, UUID_SIZE);
	if (sz <= 0)
		return sz;

	/* [LENGTH] */
	if (op->node.do_len) {
		u16 length = count;

		sz = data->packet(data, m, c, STP_PACKET_DATA, 0, 2,
				  (u8 *)&length);
		if (sz <= 0)
			return sz;
	}

	/* [TIMESTAMP] */
	if (header & MIPI_SYST_OPT_TS) {
		u64 ts = ktime_get_real_ns();

		sz = stm_data_write(data, m, c, false, &ts, sizeof(ts));
		if (sz <= 0)
			return sz;
	}

	/* DATA */
	sz = stm_data_write(data, m, c, false, buf, count);
	if (sz > 0)
		data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil);

	return sz;
}

static const struct stm_protocol_driver sys_t_pdrv = {
	.owner			= THIS_MODULE,
	.name			= "p_sys-t",
	.priv_sz		= sizeof(struct sys_t_policy_node),
	.write			= sys_t_write,
	.policy_attr		= sys_t_policy_attrs,
	.policy_node_init	= sys_t_policy_node_init,
	.output_open		= sys_t_output_open,
	.output_close		= sys_t_output_close,
};

static int sys_t_stm_init(void)
{
	return stm_register_protocol(&sys_t_pdrv);
}

static void sys_t_stm_exit(void)
{
	stm_unregister_protocol(&sys_t_pdrv);
}

module_init(sys_t_stm_init);
module_exit(sys_t_stm_exit);

MODULE_LICENSE("GPL v2");
MODULE_DESCRIPTION("MIPI SyS-T STM framing protocol driver");
MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
Commit	Line	Data
d69d5e83 AS	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
39f10239 AS	75	#define CLOCK_SYNC_HEADER (MIPI_SYST_TYPES(CLOCK, TRANSPORT_SYNC) \| \
	76	MIPI_SYST_SEVERITY(MAX))
	77
d69d5e83 AS	78	struct sys_t_policy_node {
	79	uuid_t uuid;
	80	bool do_len;
	81	unsigned long ts_interval;
39f10239	82	unsigned long clocksync_interval;
d69d5e83 AS	83	};
	84
	85	struct sys_t_output {
	86	struct sys_t_policy_node node;
	87	unsigned long ts_jiffies;
39f10239	88	unsigned long clocksync_jiffies;
d69d5e83 AS	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
39f10239 AS	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
d69d5e83 AS	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,
39f10239	234	&sys_t_policy_attr_clocksync_interval,
d69d5e83 AS	235	NULL,
	236	};
	237
	238	static inline bool sys_t_need_ts(struct sys_t_output *op)
	239	{
	240	if (op->node.ts_interval &&
283f87c0	241	time_after(jiffies, op->ts_jiffies + op->node.ts_interval)) {
d69d5e83 AS	242	op->ts_jiffies = jiffies;
	243
	244	return true;
	245	}
	246
	247	return false;
	248	}
	249
39f10239 AS	250	static bool sys_t_need_clock_sync(struct sys_t_output *op)
	251	{
	252	if (op->node.clocksync_interval &&
283f87c0 AS	253	time_after(jiffies,
283f87c0 AS	254	op->clocksync_jiffies + op->node.clocksync_interval)) {
39f10239 AS	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]; /* 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
d69d5e83 AS	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	/* We require an existing policy node to proceed */
	298	if (!op)
	299	return -EINVAL;
	300
39f10239 AS	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
d69d5e83 AS	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
	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>");