94 lines
2.4 KiB
C
94 lines
2.4 KiB
C
// 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) 2021-2022 Qualcomm Innovation Center, Inc. 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 __nocfi 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 v2");
|
|
MODULE_DESCRIPTION("MIPI Open System Trace STM framing protocol driver");
|