Add MIPI OST protocol support for stm to format the traces.
Framework copied from drivers/hwtracing/stm.p-sys-t.c as of
commit d69d5e83110f ("stm class: Add MIPI SyS-T protocol
support").
Signed-off-by: Tingwei Zhang <quic_tingweiz@xxxxxxxxxxx>
Signed-off-by: Mao Jinlong <quic_jinlmao@xxxxxxxxxxx>
---
drivers/hwtracing/stm/Kconfig | 14 +++++
drivers/hwtracing/stm/Makefile | 2 +
drivers/hwtracing/stm/p_ost.c | 95 ++++++++++++++++++++++++++++++++++
3 files changed, 111 insertions(+)
create mode 100644 drivers/hwtracing/stm/p_ost.c
diff --git a/drivers/hwtracing/stm/Kconfig b/drivers/hwtracing/stm/Kconfig
index aad594fe79cc..0b52901125f7 100644
--- a/drivers/hwtracing/stm/Kconfig
+++ b/drivers/hwtracing/stm/Kconfig
@@ -41,6 +41,20 @@ config STM_PROTO_SYS_T
If you don't know what this is, say N.
+config STM_PROTO_OST
+ tristate "MIPI OST STM framing protocol driver"
+ default CONFIG_STM
+ help
+ This is an implementation of MIPI OST protocol to be used
+ over the STP transport. In addition to the data payload, it
+ also carries additional metadata for entity, better
+ means of trace source identification, etc.
+
+ The receiving side must be able to decode this protocol in
+ addition to the MIPI STP, in order to extract the data.
+
+ If you don't know what this is, say N.
+
config STM_DUMMY
tristate "Dummy STM driver"
help
diff --git a/drivers/hwtracing/stm/Makefile b/drivers/hwtracing/stm/Makefile
index 1692fcd29277..715fc721891e 100644
--- a/drivers/hwtracing/stm/Makefile
+++ b/drivers/hwtracing/stm/Makefile
@@ -5,9 +5,11 @@ stm_core-y := core.o policy.o
obj-$(CONFIG_STM_PROTO_BASIC) += stm_p_basic.o
obj-$(CONFIG_STM_PROTO_SYS_T) += stm_p_sys-t.o
+obj-$(CONFIG_STM_PROTO_OST) += stm_p_ost.o
stm_p_basic-y := p_basic.o
stm_p_sys-t-y := p_sys-t.o
+stm_p_ost-y := p_ost.o
obj-$(CONFIG_STM_DUMMY) += dummy_stm.o
diff --git a/drivers/hwtracing/stm/p_ost.c b/drivers/hwtracing/stm/p_ost.c
new file mode 100644
index 000000000000..2ca1a3fda57f
--- /dev/null
+++ b/drivers/hwtracing/stm/p_ost.c
@@ -0,0 +1,95 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copied from drivers/hwtracing/stm.p-sys-t.c as of commit d69d5e83110f
+ * ("stm class: Add MIPI SyS-T protocol support").
+ *
+ * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
+ * Copyright (c) 2020 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2018, Intel Corporation.
+ *
+ * MIPI OST framing protocol for STM devices.
+ */
+
+#include <linux/configfs.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+#include <linux/sched/clock.h>
+#include "stm.h"
+
+#define OST_TOKEN_STARTSIMPLE (0x10)
+#define OST_VERSION_MIPI1 (0x10 << 8)
+#define OST_ENTITY_FTRACE (0x01 << 16)
+#define OST_CONTROL_PROTOCOL (0x0 << 24)
+
+#define DATA_HEADER (OST_TOKEN_STARTSIMPLE | OST_VERSION_MIPI1 | \
+ OST_ENTITY_FTRACE | OST_CONTROL_PROTOCOL)
+
+#define STM_MAKE_VERSION(ma, mi) ((ma << 8) | mi)
+#define STM_HEADER_MAGIC (0x5953)
+
+static ssize_t notrace ost_write(struct stm_data *data,
+ struct stm_output *output, unsigned int chan,
+ const char *buf, size_t count)
+{
+ unsigned int c = output->channel + chan;
+ unsigned int m = output->master;
+ const unsigned char nil = 0;
+ u32 header = DATA_HEADER;
+ u8 trc_hdr[24];
+ ssize_t sz;
+
+ /*
+ * STP framing rules for OST frames:
+ * * the first packet of the OST frame is marked;
+ * * the last packet is a FLAG.
+ */
+ /* Message layout: HEADER / DATA / TAIL */
+ /* HEADER */
+
+ sz = data->packet(data, m, c, STP_PACKET_DATA, STP_PACKET_MARKED,
+ 4, (u8 *)&header);
+ if (sz <= 0)
+ return sz;
+ *(uint16_t *)(trc_hdr) = STM_MAKE_VERSION(0, 3);
+ *(uint16_t *)(trc_hdr + 2) = STM_HEADER_MAGIC;
+ *(uint32_t *)(trc_hdr + 4) = raw_smp_processor_id();
+ *(uint64_t *)(trc_hdr + 8) = sched_clock();
+ *(uint64_t *)(trc_hdr + 16) = task_tgid_nr(get_current());
+ sz = stm_data_write(data, m, c, false, trc_hdr, sizeof(trc_hdr));
+ if (sz <= 0)
+ return sz;
+
+ /* DATA */
+ sz = stm_data_write(data, m, c, false, buf, count);
+
+ /* TAIL */
+ if (sz > 0)
+ data->packet(data, m, c, STP_PACKET_FLAG,
+ STP_PACKET_TIMESTAMPED, 0, &nil);
+
+ return sz;
+}
+
+static const struct stm_protocol_driver ost_pdrv = {
+ .owner = THIS_MODULE,
+ .name = "p_ost",
+ .write = ost_write,
+};
+
+static int ost_stm_init(void)
+{
+ return stm_register_protocol(&ost_pdrv);
+}
+
+static void ost_stm_exit(void)
+{
+ stm_unregister_protocol(&ost_pdrv);
+}
+
+module_init(ost_stm_init);
+module_exit(ost_stm_exit);
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("MIPI Open System Trace STM framing protocol driver");
