From a5f7a5bb3b7f28ba7e4fa246775b29a0e5537255 Mon Sep 17 00:00:00 2001 From: Jinke Han Date: Tue, 8 Sep 2026 15:37:42 +0800 Subject: x86/kprobes: Fix crash when probing CS CALL instructions When using eBPF to probe CS CALL instructions within a function, a crash can be triggered. The eBPF tool probes offset 257 of the __hrtimer_run_queues() function: <__hrtimer_run_queues+249>: nopl 0x0(%rax,%rax,1) <__hrtimer_run_queues+254>: mov %r14,%rdi <__hrtimer_run_queues+257>: cs call <__x86_indirect_thunk_r12> <__hrtimer_run_queues+263>: mov %eax,%r12d <__hrtimer_run_queues+266>: xchg %ax,%ax <__hrtimer_run_queues+268>: mov %r13,%rdi Which triggers this crash: BUG: unable to handle page fault for address: 00000000000f41c9 #PF: supervisor write access in kernel mode #PF: error_code(0x0002) - not-present page PGD 0 P4D 0 Oops: 0002 [#1] SMP NOPTI CPU: 1 PID: 0 Comm: swapper/1 Kdump: loaded Tainted: P RIP: 0010:__hrtimer_run_queues+0x106/0x230 Note that __hrtimer_run_queues+0x106 is __hrtimer_run_queues+262, which is at the 6th byte of the above CS CALL instruction. Since the CS CALL instruction occupies 6 bytes, the exception occurred in the middle of that call instruction. The root cause is that when using eBPF tools to probe in the middle of a function, a kprobe with INT3 is used as the underlying implementation. During single-step emulation of the original CALL instruction, int3_emulate_call() assumes that the probed CALL instruction is 5 bytes long. However, the actual CS-prefixed CALL instruction occupies 6 bytes, so it constructs an incorrect exception return address. When the CPU returns from the kprobe handler, the next instruction to be executed is at the address of the last byte of that CS CALL instruction. Coincidentally, starting from that address, the CPU fetches and decodes a completely different instruction, which ultimately triggers a kernel crash. Fix the issue by using the actual instruction length obtained from the instruction decoder when constructing the exception return address, rather than relying on the hardcoded CALL_INSN_SIZE macro. [ mingo: Refined the changelog ] Fixes: 6256e668b7af ("x86/kprobes: Use int3 instead of debug trap for single-step") Suggested-by: Masami Hiramatsu (Google) Signed-off-by: Jinke Han Signed-off-by: Ingo Molnar Reviewed-by: Masami Hiramatsu (Google) Acked-by: Yafang Shao Acked-by: Borislav Petkov Cc: Peter Zijlstra Link: https://patch.msgid.link/20260908073742.GA10517@didi-ThinkCentre-M920t-N000 --- drivers/hwtracing/stm/Kconfig | 82 +++ drivers/hwtracing/stm/Makefile | 20 + drivers/hwtracing/stm/console.c | 73 ++ drivers/hwtracing/stm/core.c | 1374 +++++++++++++++++++++++++++++++++++++ drivers/hwtracing/stm/dummy_stm.c | 125 ++++ drivers/hwtracing/stm/ftrace.c | 87 +++ drivers/hwtracing/stm/heartbeat.c | 119 ++++ drivers/hwtracing/stm/p_basic.c | 49 ++ drivers/hwtracing/stm/p_sys-t.c | 461 +++++++++++++ drivers/hwtracing/stm/policy.c | 570 +++++++++++++++ drivers/hwtracing/stm/stm.h | 117 ++++ 11 files changed, 3077 insertions(+) create mode 100644 drivers/hwtracing/stm/Kconfig create mode 100644 drivers/hwtracing/stm/Makefile create mode 100644 drivers/hwtracing/stm/console.c create mode 100644 drivers/hwtracing/stm/core.c create mode 100644 drivers/hwtracing/stm/dummy_stm.c create mode 100644 drivers/hwtracing/stm/ftrace.c create mode 100644 drivers/hwtracing/stm/heartbeat.c create mode 100644 drivers/hwtracing/stm/p_basic.c create mode 100644 drivers/hwtracing/stm/p_sys-t.c create mode 100644 drivers/hwtracing/stm/policy.c create mode 100644 drivers/hwtracing/stm/stm.h (limited to 'drivers/hwtracing/stm') diff --git a/drivers/hwtracing/stm/Kconfig b/drivers/hwtracing/stm/Kconfig new file mode 100644 index 000000000..cd7f0b0f3 --- /dev/null +++ b/drivers/hwtracing/stm/Kconfig @@ -0,0 +1,82 @@ +# SPDX-License-Identifier: GPL-2.0-only +config STM + tristate "System Trace Module devices" + select CONFIGFS_FS + help + A System Trace Module (STM) is a device exporting data in System + Trace Protocol (STP) format as defined by MIPI STP standards. + Examples of such devices are Intel(R) Trace Hub and Coresight STM. + + Say Y here to enable System Trace Module device support. + +if STM + +config STM_PROTO_BASIC + tristate "Basic STM framing protocol driver" + default STM + help + This is a simple framing protocol for sending data over STM + devices. This was the protocol that the STM framework used + exclusively until the MIPI SyS-T support was added. Use this + driver for compatibility with your existing STM setup. + + The receiving side only needs to be able to decode the MIPI + STP protocol in order to extract the data. + + If you want to be able to use the basic protocol or want the + backwards compatibility for your existing setup, say Y. + +config STM_PROTO_SYS_T + tristate "MIPI SyS-T STM framing protocol driver" + default STM + help + This is an implementation of MIPI SyS-T protocol to be used + over the STP transport. In addition to the data payload, it + also carries additional metadata for time correlation, 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 + This is a simple dummy device that pretends to be an stm device + and discards your data. Use for stm class testing. + + If you don't know what this is, say N. + +config STM_SOURCE_CONSOLE + tristate "Kernel console over STM devices" + help + This is a kernel space trace source that sends kernel log + messages to trace hosts over STM devices. + + If you want to send kernel console messages over STM devices, + say Y. + +config STM_SOURCE_HEARTBEAT + tristate "Heartbeat over STM devices" + help + This is a kernel space trace source that sends periodic + heartbeat messages to trace hosts over STM devices. It is + also useful for testing stm class drivers and the stm class + framework itself. + + If you want to send heartbeat messages over STM devices, + say Y. + +config STM_SOURCE_FTRACE + tristate "Copy the output from kernel Ftrace to STM engine" + depends on TRACING + help + This option can be used to copy the output from kernel Ftrace + to STM engine. Enabling this option will introduce a slight + timing effect. + + If you want to send kernel Ftrace messages over STM devices, + say Y. + +endif diff --git a/drivers/hwtracing/stm/Makefile b/drivers/hwtracing/stm/Makefile new file mode 100644 index 000000000..1692fcd29 --- /dev/null +++ b/drivers/hwtracing/stm/Makefile @@ -0,0 +1,20 @@ +# SPDX-License-Identifier: GPL-2.0 +obj-$(CONFIG_STM) += stm_core.o + +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 + +stm_p_basic-y := p_basic.o +stm_p_sys-t-y := p_sys-t.o + +obj-$(CONFIG_STM_DUMMY) += dummy_stm.o + +obj-$(CONFIG_STM_SOURCE_CONSOLE) += stm_console.o +obj-$(CONFIG_STM_SOURCE_HEARTBEAT) += stm_heartbeat.o +obj-$(CONFIG_STM_SOURCE_FTRACE) += stm_ftrace.o + +stm_console-y := console.o +stm_heartbeat-y := heartbeat.o +stm_ftrace-y := ftrace.o diff --git a/drivers/hwtracing/stm/console.c b/drivers/hwtracing/stm/console.c new file mode 100644 index 000000000..097a00ac4 --- /dev/null +++ b/drivers/hwtracing/stm/console.c @@ -0,0 +1,73 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Simple kernel console driver for STM devices + * Copyright (c) 2014, Intel Corporation. + * + * STM console will send kernel messages over STM devices to a trace host. + */ + +#include +#include +#include +#include +#include + +static int stm_console_link(struct stm_source_data *data); +static void stm_console_unlink(struct stm_source_data *data); + +static struct stm_console { + struct stm_source_data data; + struct console console; +} stm_console = { + .data = { + .name = "console", + .nr_chans = 1, + .type = STM_USER, + .link = stm_console_link, + .unlink = stm_console_unlink, + }, +}; + +static void +stm_console_write(struct console *con, const char *buf, unsigned len) +{ + struct stm_console *sc = container_of(con, struct stm_console, console); + + stm_source_write(&sc->data, 0, buf, len); +} + +static int stm_console_link(struct stm_source_data *data) +{ + struct stm_console *sc = container_of(data, struct stm_console, data); + + strcpy(sc->console.name, "stm_console"); + sc->console.write = stm_console_write; + sc->console.flags = CON_ENABLED | CON_PRINTBUFFER; + register_console(&sc->console); + + return 0; +} + +static void stm_console_unlink(struct stm_source_data *data) +{ + struct stm_console *sc = container_of(data, struct stm_console, data); + + unregister_console(&sc->console); +} + +static int stm_console_init(void) +{ + return stm_source_register_device(NULL, &stm_console.data); +} + +static void stm_console_exit(void) +{ + stm_source_unregister_device(&stm_console.data); +} + +module_init(stm_console_init); +module_exit(stm_console_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("stm_console driver"); +MODULE_AUTHOR("Alexander Shishkin "); diff --git a/drivers/hwtracing/stm/core.c b/drivers/hwtracing/stm/core.c new file mode 100644 index 000000000..f48c6a8a0 --- /dev/null +++ b/drivers/hwtracing/stm/core.c @@ -0,0 +1,1374 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * System Trace Module (STM) infrastructure + * Copyright (c) 2014, Intel Corporation. + * + * STM class implements generic infrastructure for System Trace Module devices + * as defined in MIPI STPv2 specification. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "stm.h" + +#include + +static unsigned int stm_core_up; + +/* + * The SRCU here makes sure that STM device doesn't disappear from under a + * stm_source_write() caller, which may want to have as little overhead as + * possible. + */ +static struct srcu_struct stm_source_srcu; + +static ssize_t masters_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct stm_device *stm = to_stm_device(dev); + int ret; + + ret = sprintf(buf, "%u %u\n", stm->data->sw_start, stm->data->sw_end); + + return ret; +} + +static DEVICE_ATTR_RO(masters); + +static ssize_t channels_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct stm_device *stm = to_stm_device(dev); + int ret; + + ret = sprintf(buf, "%u\n", stm->data->sw_nchannels); + + return ret; +} + +static DEVICE_ATTR_RO(channels); + +static ssize_t hw_override_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct stm_device *stm = to_stm_device(dev); + int ret; + + ret = sprintf(buf, "%u\n", stm->data->hw_override); + + return ret; +} + +static DEVICE_ATTR_RO(hw_override); + +static struct attribute *stm_attrs[] = { + &dev_attr_masters.attr, + &dev_attr_channels.attr, + &dev_attr_hw_override.attr, + NULL, +}; + +ATTRIBUTE_GROUPS(stm); + +static struct class stm_class = { + .name = "stm", + .dev_groups = stm_groups, +}; + +/** + * stm_find_device() - find stm device by name + * @buf: character buffer containing the name + * + * This is called when either policy gets assigned to an stm device or an + * stm_source device gets linked to an stm device. + * + * This grabs device's reference (get_device()) and module reference, both + * of which the calling path needs to make sure to drop with stm_put_device(). + * + * Return: stm device pointer or null if lookup failed. + */ +struct stm_device *stm_find_device(const char *buf) +{ + struct stm_device *stm; + struct device *dev; + + if (!stm_core_up) + return NULL; + + dev = class_find_device_by_name(&stm_class, buf); + if (!dev) + return NULL; + + stm = to_stm_device(dev); + if (!try_module_get(stm->owner)) { + /* matches class_find_device() above */ + put_device(dev); + return NULL; + } + + return stm; +} + +/** + * stm_put_device() - drop references on the stm device + * @stm: stm device, previously acquired by stm_find_device() + * + * This drops the module reference and device reference taken by + * stm_find_device() or stm_char_open(). + */ +void stm_put_device(struct stm_device *stm) +{ + module_put(stm->owner); + put_device(&stm->dev); +} + +/* + * Internally we only care about software-writable masters here, that is the + * ones in the range [stm_data->sw_start..stm_data..sw_end], however we need + * original master numbers to be visible externally, since they are the ones + * that will appear in the STP stream. Thus, the internal bookkeeping uses + * $master - stm_data->sw_start to reference master descriptors and such. + */ + +#define __stm_master(_s, _m) \ + ((_s)->masters[(_m) - (_s)->data->sw_start]) + +static inline struct stp_master * +stm_master(struct stm_device *stm, unsigned int idx) +{ + if (idx < stm->data->sw_start || idx > stm->data->sw_end) + return NULL; + + return __stm_master(stm, idx); +} + +static int stp_master_alloc(struct stm_device *stm, unsigned int idx) +{ + struct stp_master *master; + + master = kzalloc_flex(*master, chan_map, + BITS_TO_LONGS(stm->data->sw_nchannels), + GFP_ATOMIC); + if (!master) + return -ENOMEM; + + master->nr_free = stm->data->sw_nchannels; + __stm_master(stm, idx) = master; + + return 0; +} + +static void stp_master_free(struct stm_device *stm, unsigned int idx) +{ + struct stp_master *master = stm_master(stm, idx); + + if (!master) + return; + + __stm_master(stm, idx) = NULL; + kfree(master); +} + +static void stm_output_claim(struct stm_device *stm, struct stm_output *output) +{ + struct stp_master *master = stm_master(stm, output->master); + + lockdep_assert_held(&stm->mc_lock); + lockdep_assert_held(&output->lock); + + if (WARN_ON_ONCE(master->nr_free < output->nr_chans)) + return; + + bitmap_allocate_region(&master->chan_map[0], output->channel, + ilog2(output->nr_chans)); + + master->nr_free -= output->nr_chans; +} + +static void +stm_output_disclaim(struct stm_device *stm, struct stm_output *output) +{ + struct stp_master *master = stm_master(stm, output->master); + + lockdep_assert_held(&stm->mc_lock); + lockdep_assert_held(&output->lock); + + bitmap_release_region(&master->chan_map[0], output->channel, + ilog2(output->nr_chans)); + + master->nr_free += output->nr_chans; + output->nr_chans = 0; +} + +/* + * This is like bitmap_find_free_region(), except it can ignore @start bits + * at the beginning. + */ +static int find_free_channels(unsigned long *bitmap, unsigned int start, + unsigned int end, unsigned int width) +{ + unsigned int pos; + int i; + + for (pos = start; pos < end + 1; pos = ALIGN(pos, width)) { + pos = find_next_zero_bit(bitmap, end + 1, pos); + if (pos + width > end + 1) + break; + + if (pos & (width - 1)) + continue; + + for (i = 1; i < width && !test_bit(pos + i, bitmap); i++) + ; + if (i == width) + return pos; + + /* step over [pos..pos+i) to continue search */ + pos += i; + } + + return -1; +} + +static int +stm_find_master_chan(struct stm_device *stm, unsigned int width, + unsigned int *mstart, unsigned int mend, + unsigned int *cstart, unsigned int cend) +{ + struct stp_master *master; + unsigned int midx; + int pos, err; + + for (midx = *mstart; midx <= mend; midx++) { + if (!stm_master(stm, midx)) { + err = stp_master_alloc(stm, midx); + if (err) + return err; + } + + master = stm_master(stm, midx); + + if (!master->nr_free) + continue; + + pos = find_free_channels(master->chan_map, *cstart, cend, + width); + if (pos < 0) + continue; + + *mstart = midx; + *cstart = pos; + return 0; + } + + return -ENOSPC; +} + +static int stm_output_assign(struct stm_device *stm, unsigned int width, + struct stp_policy_node *policy_node, + struct stm_output *output) +{ + unsigned int midx, cidx, mend, cend; + int ret = -EINVAL; + + if (width > stm->data->sw_nchannels) + return -EINVAL; + + /* We no longer accept policy_node==NULL here */ + if (WARN_ON_ONCE(!policy_node)) + return -EINVAL; + + /* + * Also, the caller holds reference to policy_node, so it won't + * disappear on us. + */ + stp_policy_node_get_ranges(policy_node, &midx, &mend, &cidx, &cend); + + spin_lock(&stm->mc_lock); + spin_lock(&output->lock); + /* output is already assigned -- shouldn't happen */ + if (WARN_ON_ONCE(output->nr_chans)) + goto unlock; + + ret = stm_find_master_chan(stm, width, &midx, mend, &cidx, cend); + if (ret < 0) + goto unlock; + + output->master = midx; + output->channel = cidx; + output->nr_chans = width; + if (stm->pdrv->output_open) { + void *priv = stp_policy_node_priv(policy_node); + + if (WARN_ON_ONCE(!priv)) + goto unlock; + + /* configfs subsys mutex is held by the caller */ + ret = stm->pdrv->output_open(priv, output); + if (ret) + goto unlock; + } + + stm_output_claim(stm, output); + dev_dbg(&stm->dev, "assigned %u:%u (+%u)\n", midx, cidx, width); + + ret = 0; +unlock: + if (ret) + output->nr_chans = 0; + + spin_unlock(&output->lock); + spin_unlock(&stm->mc_lock); + + return ret; +} + +static void stm_output_free(struct stm_device *stm, struct stm_output *output) +{ + spin_lock(&stm->mc_lock); + spin_lock(&output->lock); + if (output->nr_chans) + stm_output_disclaim(stm, output); + if (stm->pdrv && stm->pdrv->output_close) + stm->pdrv->output_close(output); + spin_unlock(&output->lock); + spin_unlock(&stm->mc_lock); +} + +static void stm_output_init(struct stm_output *output) +{ + spin_lock_init(&output->lock); +} + +static int major_match(struct device *dev, const void *data) +{ + unsigned int major = *(unsigned int *)data; + + return MAJOR(dev->devt) == major; +} + +/* + * Framing protocol management + * Modules can implement STM protocol drivers and (un-)register them + * with the STM class framework. + */ +static struct list_head stm_pdrv_head; +static struct mutex stm_pdrv_mutex; + +struct stm_pdrv_entry { + struct list_head entry; + const struct stm_protocol_driver *pdrv; + const struct config_item_type *node_type; +}; + +static const struct stm_pdrv_entry * +__stm_lookup_protocol(const char *name) +{ + struct stm_pdrv_entry *pe; + + /* + * If no name is given (NULL or ""), fall back to "p_basic". + */ + if (!name || !*name) + name = "p_basic"; + + list_for_each_entry(pe, &stm_pdrv_head, entry) { + if (!strcmp(name, pe->pdrv->name)) + return pe; + } + + return NULL; +} + +int stm_register_protocol(const struct stm_protocol_driver *pdrv) +{ + struct stm_pdrv_entry *pe = NULL; + int ret = -ENOMEM; + + mutex_lock(&stm_pdrv_mutex); + + if (__stm_lookup_protocol(pdrv->name)) { + ret = -EEXIST; + goto unlock; + } + + pe = kzalloc_obj(*pe); + if (!pe) + goto unlock; + + if (pdrv->policy_attr) { + pe->node_type = get_policy_node_type(pdrv->policy_attr); + if (!pe->node_type) + goto unlock; + } + + list_add_tail(&pe->entry, &stm_pdrv_head); + pe->pdrv = pdrv; + + ret = 0; +unlock: + mutex_unlock(&stm_pdrv_mutex); + + if (ret) + kfree(pe); + + return ret; +} +EXPORT_SYMBOL_GPL(stm_register_protocol); + +void stm_unregister_protocol(const struct stm_protocol_driver *pdrv) +{ + struct stm_pdrv_entry *pe, *iter; + + mutex_lock(&stm_pdrv_mutex); + + list_for_each_entry_safe(pe, iter, &stm_pdrv_head, entry) { + if (pe->pdrv == pdrv) { + list_del(&pe->entry); + + if (pe->node_type) { + kfree(pe->node_type->ct_attrs); + kfree(pe->node_type); + } + kfree(pe); + break; + } + } + + mutex_unlock(&stm_pdrv_mutex); +} +EXPORT_SYMBOL_GPL(stm_unregister_protocol); + +static bool stm_get_protocol(const struct stm_protocol_driver *pdrv) +{ + return try_module_get(pdrv->owner); +} + +void stm_put_protocol(const struct stm_protocol_driver *pdrv) +{ + module_put(pdrv->owner); +} + +int stm_lookup_protocol(const char *name, + const struct stm_protocol_driver **pdrv, + const struct config_item_type **node_type) +{ + const struct stm_pdrv_entry *pe; + + mutex_lock(&stm_pdrv_mutex); + + pe = __stm_lookup_protocol(name); + if (pe && pe->pdrv && stm_get_protocol(pe->pdrv)) { + *pdrv = pe->pdrv; + *node_type = pe->node_type; + } + + mutex_unlock(&stm_pdrv_mutex); + + return pe ? 0 : -ENOENT; +} + +static int stm_char_open(struct inode *inode, struct file *file) +{ + struct stm_file *stmf; + struct device *dev; + unsigned int major = imajor(inode); + int err = -ENOMEM; + + dev = class_find_device(&stm_class, NULL, &major, major_match); + if (!dev) + return -ENODEV; + + stmf = kzalloc_obj(*stmf); + if (!stmf) + goto err_put_device; + + err = -ENODEV; + stm_output_init(&stmf->output); + stmf->stm = to_stm_device(dev); + + if (!try_module_get(stmf->stm->owner)) + goto err_free; + + file->private_data = stmf; + + return nonseekable_open(inode, file); + +err_free: + kfree(stmf); +err_put_device: + /* matches class_find_device() above */ + put_device(dev); + + return err; +} + +static int stm_char_release(struct inode *inode, struct file *file) +{ + struct stm_file *stmf = file->private_data; + struct stm_device *stm = stmf->stm; + + if (stm->data->unlink) + stm->data->unlink(stm->data, stmf->output.master, + stmf->output.channel); + + stm_output_free(stm, &stmf->output); + + /* + * matches the stm_char_open()'s + * class_find_device() + try_module_get() + */ + stm_put_device(stm); + kfree(stmf); + + return 0; +} + +static int +stm_assign_first_policy(struct stm_device *stm, struct stm_output *output, + char **ids, unsigned int width) +{ + struct stp_policy_node *pn; + int err, n; + + /* + * On success, stp_policy_node_lookup() will return holding the + * configfs subsystem mutex, which is then released in + * stp_policy_node_put(). This allows the pdrv->output_open() in + * stm_output_assign() to serialize against the attribute accessors. + */ + for (n = 0, pn = NULL; ids[n] && !pn; n++) + pn = stp_policy_node_lookup(stm, ids[n]); + + if (!pn) + return -EINVAL; + + err = stm_output_assign(stm, width, pn, output); + + stp_policy_node_put(pn); + + return err; +} + +/** + * stm_data_write() - send the given payload as data packets + * @data: stm driver's data + * @m: STP master + * @c: STP channel + * @ts_first: timestamp the first packet + * @buf: data payload buffer + * @count: data payload size + */ +ssize_t notrace stm_data_write(struct stm_data *data, unsigned int m, + unsigned int c, bool ts_first, const void *buf, + size_t count) +{ + unsigned int flags = ts_first ? STP_PACKET_TIMESTAMPED : 0; + ssize_t sz; + size_t pos; + + for (pos = 0, sz = 0; pos < count; pos += sz) { + sz = min_t(unsigned int, count - pos, 8); + sz = data->packet(data, m, c, STP_PACKET_DATA, flags, sz, + &((u8 *)buf)[pos]); + if (sz <= 0) + break; + + if (ts_first) { + flags = 0; + ts_first = false; + } + } + + return sz < 0 ? sz : pos; +} +EXPORT_SYMBOL_GPL(stm_data_write); + +static ssize_t notrace +stm_write(struct stm_device *stm, struct stm_output *output, + unsigned int chan, const char *buf, size_t count, struct stm_source_data *source) +{ + int err; + + /* stm->pdrv is serialized against policy_mutex */ + if (!stm->pdrv) + return -ENODEV; + + err = stm->pdrv->write(stm->data, output, chan, buf, count, source); + if (err < 0) + return err; + + return err; +} + +static ssize_t stm_char_write(struct file *file, const char __user *buf, + size_t count, loff_t *ppos) +{ + struct stm_file *stmf = file->private_data; + struct stm_device *stm = stmf->stm; + char *kbuf; + int err; + + if (count + 1 > PAGE_SIZE) + count = PAGE_SIZE - 1; + + /* + * If no m/c have been assigned to this writer up to this + * point, try to use the task name and "default" policy entries. + */ + if (!stmf->output.nr_chans) { + char comm[sizeof(current->comm)]; + char *ids[] = { comm, "default", NULL }; + + get_task_comm(comm, current); + + err = stm_assign_first_policy(stmf->stm, &stmf->output, ids, 1); + /* + * EBUSY means that somebody else just assigned this + * output, which is just fine for write() + */ + if (err) + return err; + } + + kbuf = kmalloc(count + 1, GFP_KERNEL); + if (!kbuf) + return -ENOMEM; + + err = copy_from_user(kbuf, buf, count); + if (err) { + kfree(kbuf); + return -EFAULT; + } + + pm_runtime_get_sync(&stm->dev); + + count = stm_write(stm, &stmf->output, 0, kbuf, count, NULL); + + pm_runtime_mark_last_busy(&stm->dev); + pm_runtime_put_autosuspend(&stm->dev); + kfree(kbuf); + + return count; +} + +static int stm_mmap_mapped(unsigned long start, unsigned long end, pgoff_t pgoff, + const struct file *file, void **vm_private_data) +{ + struct stm_file *stmf = file->private_data; + struct stm_device *stm = stmf->stm; + + pm_runtime_get_sync(&stm->dev); + return 0; +} + +static void stm_mmap_open(struct vm_area_struct *vma) +{ + struct stm_file *stmf = vma->vm_file->private_data; + struct stm_device *stm = stmf->stm; + + pm_runtime_get(&stm->dev); +} + +static void stm_mmap_close(struct vm_area_struct *vma) +{ + struct stm_file *stmf = vma->vm_file->private_data; + struct stm_device *stm = stmf->stm; + + pm_runtime_mark_last_busy(&stm->dev); + pm_runtime_put_autosuspend(&stm->dev); +} + +static const struct vm_operations_struct stm_mmap_vmops = { + .mapped = stm_mmap_mapped, + .open = stm_mmap_open, + .close = stm_mmap_close, +}; + +static int stm_char_mmap_prepare(struct vm_area_desc *desc) +{ + struct file *file = desc->file; + struct stm_file *stmf = file->private_data; + struct stm_device *stm = stmf->stm; + unsigned long size, phys; + + if (!stm->data->mmio_addr) + return -EOPNOTSUPP; + + if (desc->pgoff) + return -EINVAL; + + size = vma_desc_size(desc); + + if (stmf->output.nr_chans * stm->data->sw_mmiosz != size) + return -EINVAL; + + phys = stm->data->mmio_addr(stm->data, stmf->output.master, + stmf->output.channel, + stmf->output.nr_chans); + + if (!phys) + return -EINVAL; + + desc->page_prot = pgprot_noncached(desc->page_prot); + vma_desc_set_flags(desc, VMA_IO_BIT, VMA_DONTEXPAND_BIT, + VMA_DONTDUMP_BIT); + desc->vm_ops = &stm_mmap_vmops; + + mmap_action_simple_ioremap(desc, phys, size); + return 0; +} + +static int stm_char_policy_set_ioctl(struct stm_file *stmf, void __user *arg) +{ + struct stm_device *stm = stmf->stm; + struct stp_policy_id *id; + char *ids[] = { NULL, NULL }; + int ret = -EINVAL, wlimit = 1; + u32 size; + + if (stmf->output.nr_chans) + return -EBUSY; + + if (copy_from_user(&size, arg, sizeof(size))) + return -EFAULT; + + if (size < sizeof(*id) || size >= PATH_MAX + sizeof(*id)) + return -EINVAL; + + /* + * size + 1 to make sure the .id string at the bottom is terminated, + * which is also why memdup_user() is not useful here + */ + id = kzalloc(size + 1, GFP_KERNEL); + if (!id) + return -ENOMEM; + + if (copy_from_user(id, arg, size)) { + ret = -EFAULT; + goto err_free; + } + + if (id->__reserved_0 || id->__reserved_1) + goto err_free; + + if (stm->data->sw_mmiosz) + wlimit = PAGE_SIZE / stm->data->sw_mmiosz; + + if (id->width < 1 || id->width > wlimit) + goto err_free; + + ids[0] = id->id; + ret = stm_assign_first_policy(stmf->stm, &stmf->output, ids, + id->width); + if (ret) + goto err_free; + + if (stm->data->link) + ret = stm->data->link(stm->data, stmf->output.master, + stmf->output.channel); + + if (ret) + stm_output_free(stmf->stm, &stmf->output); + +err_free: + kfree(id); + + return ret; +} + +static int stm_char_policy_get_ioctl(struct stm_file *stmf, void __user *arg) +{ + struct stp_policy_id id = { + .size = sizeof(id), + .master = stmf->output.master, + .channel = stmf->output.channel, + .width = stmf->output.nr_chans, + .__reserved_0 = 0, + .__reserved_1 = 0, + }; + + return copy_to_user(arg, &id, id.size) ? -EFAULT : 0; +} + +static long +stm_char_ioctl(struct file *file, unsigned int cmd, unsigned long arg) +{ + struct stm_file *stmf = file->private_data; + struct stm_data *stm_data = stmf->stm->data; + int err = -ENOTTY; + u64 options; + + switch (cmd) { + case STP_POLICY_ID_SET: + err = stm_char_policy_set_ioctl(stmf, (void __user *)arg); + if (err) + return err; + + return stm_char_policy_get_ioctl(stmf, (void __user *)arg); + + case STP_POLICY_ID_GET: + return stm_char_policy_get_ioctl(stmf, (void __user *)arg); + + case STP_SET_OPTIONS: + if (copy_from_user(&options, (u64 __user *)arg, sizeof(u64))) + return -EFAULT; + + if (stm_data->set_options) + err = stm_data->set_options(stm_data, + stmf->output.master, + stmf->output.channel, + stmf->output.nr_chans, + options); + + break; + default: + break; + } + + return err; +} + +static const struct file_operations stm_fops = { + .open = stm_char_open, + .release = stm_char_release, + .write = stm_char_write, + .mmap_prepare = stm_char_mmap_prepare, + .unlocked_ioctl = stm_char_ioctl, + .compat_ioctl = compat_ptr_ioctl, +}; + +static void stm_device_release(struct device *dev) +{ + struct stm_device *stm = to_stm_device(dev); + + vfree(stm); +} + +int stm_register_device(struct device *parent, struct stm_data *stm_data, + struct module *owner) +{ + struct stm_device *stm; + unsigned int nmasters; + int err = -ENOMEM; + + if (!stm_core_up) + return -EPROBE_DEFER; + + if (!stm_data->packet || !stm_data->sw_nchannels) + return -EINVAL; + + nmasters = stm_data->sw_end - stm_data->sw_start + 1; + stm = vzalloc(sizeof(*stm) + nmasters * sizeof(void *)); + if (!stm) + return -ENOMEM; + + stm->major = register_chrdev(0, stm_data->name, &stm_fops); + if (stm->major < 0) { + err = stm->major; + vfree(stm); + return err; + } + + device_initialize(&stm->dev); + stm->dev.devt = MKDEV(stm->major, 0); + stm->dev.class = &stm_class; + stm->dev.parent = parent; + stm->dev.release = stm_device_release; + + mutex_init(&stm->link_mutex); + spin_lock_init(&stm->link_lock); + INIT_LIST_HEAD(&stm->link_list); + + /* initialize the object before it is accessible via sysfs */ + spin_lock_init(&stm->mc_lock); + mutex_init(&stm->policy_mutex); + stm->sw_nmasters = nmasters; + stm->owner = owner; + stm->data = stm_data; + stm_data->stm = stm; + + err = kobject_set_name(&stm->dev.kobj, "%s", stm_data->name); + if (err) + goto err_device; + + err = device_add(&stm->dev); + if (err) + goto err_device; + + /* + * Use delayed autosuspend to avoid bouncing back and forth + * on recurring character device writes, with the initial + * delay time of 2 seconds. + */ + pm_runtime_no_callbacks(&stm->dev); + pm_runtime_use_autosuspend(&stm->dev); + pm_runtime_set_autosuspend_delay(&stm->dev, 2000); + pm_runtime_set_suspended(&stm->dev); + pm_runtime_enable(&stm->dev); + + return 0; + +err_device: + unregister_chrdev(stm->major, stm_data->name); + + /* calls stm_device_release() */ + put_device(&stm->dev); + + return err; +} +EXPORT_SYMBOL_GPL(stm_register_device); + +static int __stm_source_link_drop(struct stm_source_device *src, + struct stm_device *stm); + +void stm_unregister_device(struct stm_data *stm_data) +{ + struct stm_device *stm = stm_data->stm; + struct stm_source_device *src, *iter; + int i, ret; + + pm_runtime_dont_use_autosuspend(&stm->dev); + pm_runtime_disable(&stm->dev); + + mutex_lock(&stm->link_mutex); + list_for_each_entry_safe(src, iter, &stm->link_list, link_entry) { + ret = __stm_source_link_drop(src, stm); + /* + * src <-> stm link must not change under the same + * stm::link_mutex, so complain loudly if it has; + * also in this situation ret!=0 means this src is + * not connected to this stm and it should be otherwise + * safe to proceed with the tear-down of stm. + */ + WARN_ON_ONCE(ret); + } + mutex_unlock(&stm->link_mutex); + + synchronize_srcu(&stm_source_srcu); + + unregister_chrdev(stm->major, stm_data->name); + + mutex_lock(&stm->policy_mutex); + if (stm->policy) + stp_policy_unbind(stm->policy); + mutex_unlock(&stm->policy_mutex); + + for (i = stm->data->sw_start; i <= stm->data->sw_end; i++) + stp_master_free(stm, i); + + device_unregister(&stm->dev); + stm_data->stm = NULL; +} +EXPORT_SYMBOL_GPL(stm_unregister_device); + +/* + * stm::link_list access serialization uses a spinlock and a mutex; holding + * either of them guarantees that the list is stable; modification requires + * holding both of them. + * + * Lock ordering is as follows: + * stm::link_mutex + * stm::link_lock + * src::link_lock + */ + +/** + * stm_source_link_add() - connect an stm_source device to an stm device + * @src: stm_source device + * @stm: stm device + * + * This function establishes a link from stm_source to an stm device so that + * the former can send out trace data to the latter. + * + * Return: 0 on success, -errno otherwise. + */ +static int stm_source_link_add(struct stm_source_device *src, + struct stm_device *stm) +{ + char *ids[] = { NULL, "default", NULL }; + int err = -ENOMEM; + + mutex_lock(&stm->link_mutex); + spin_lock(&stm->link_lock); + spin_lock(&src->link_lock); + + /* src->link is dereferenced under stm_source_srcu but not the list */ + rcu_assign_pointer(src->link, stm); + list_add_tail(&src->link_entry, &stm->link_list); + + spin_unlock(&src->link_lock); + spin_unlock(&stm->link_lock); + mutex_unlock(&stm->link_mutex); + + ids[0] = kstrdup(src->data->name, GFP_KERNEL); + if (!ids[0]) + goto fail_detach; + + err = stm_assign_first_policy(stm, &src->output, ids, + src->data->nr_chans); + kfree(ids[0]); + + if (err) + goto fail_detach; + + /* this is to notify the STM device that a new link has been made */ + if (stm->data->link) + err = stm->data->link(stm->data, src->output.master, + src->output.channel); + + if (err) + goto fail_free_output; + + /* this is to let the source carry out all necessary preparations */ + if (src->data->link) + src->data->link(src->data); + + return 0; + +fail_free_output: + stm_output_free(stm, &src->output); + +fail_detach: + mutex_lock(&stm->link_mutex); + spin_lock(&stm->link_lock); + spin_lock(&src->link_lock); + + rcu_assign_pointer(src->link, NULL); + list_del_init(&src->link_entry); + + spin_unlock(&src->link_lock); + spin_unlock(&stm->link_lock); + mutex_unlock(&stm->link_mutex); + + return err; +} + +/** + * __stm_source_link_drop() - detach stm_source from an stm device + * @src: stm_source device + * @stm: stm device + * + * If @stm is @src::link, disconnect them from one another and put the + * reference on the @stm device. + * + * Caller must hold stm::link_mutex. + */ +static int __stm_source_link_drop(struct stm_source_device *src, + struct stm_device *stm) +{ + struct stm_device *link; + int ret = 0; + + lockdep_assert_held(&stm->link_mutex); + + /* for stm::link_list modification, we hold both mutex and spinlock */ + spin_lock(&stm->link_lock); + spin_lock(&src->link_lock); + link = srcu_dereference_check(src->link, &stm_source_srcu, 1); + + /* + * The linked device may have changed since we last looked, because + * we weren't holding the src::link_lock back then; if this is the + * case, tell the caller to retry. + */ + if (link != stm) { + ret = -EAGAIN; + goto unlock; + } + + stm_output_free(link, &src->output); + list_del_init(&src->link_entry); + pm_runtime_mark_last_busy(&link->dev); + pm_runtime_put_autosuspend(&link->dev); + /* matches stm_find_device() from stm_source_link_store() */ + stm_put_device(link); + rcu_assign_pointer(src->link, NULL); + +unlock: + spin_unlock(&src->link_lock); + spin_unlock(&stm->link_lock); + + /* + * Call the unlink callbacks for both source and stm, when we know + * that we have actually performed the unlinking. + */ + if (!ret) { + if (src->data->unlink) + src->data->unlink(src->data); + + if (stm->data->unlink) + stm->data->unlink(stm->data, src->output.master, + src->output.channel); + } + + return ret; +} + +/** + * stm_source_link_drop() - detach stm_source from its stm device + * @src: stm_source device + * + * Unlinking means disconnecting from source's STM device; after this + * writes will be unsuccessful until it is linked to a new STM device. + * + * This will happen on "stm_source_link" sysfs attribute write to undo + * the existing link (if any), or on linked STM device's de-registration. + */ +static void stm_source_link_drop(struct stm_source_device *src) +{ + struct stm_device *stm; + int idx, ret; + +retry: + idx = srcu_read_lock(&stm_source_srcu); + /* + * The stm device will be valid for the duration of this + * read section, but the link may change before we grab + * the src::link_lock in __stm_source_link_drop(). + */ + stm = srcu_dereference(src->link, &stm_source_srcu); + + ret = 0; + if (stm) { + mutex_lock(&stm->link_mutex); + ret = __stm_source_link_drop(src, stm); + mutex_unlock(&stm->link_mutex); + } + + srcu_read_unlock(&stm_source_srcu, idx); + + /* if it did change, retry */ + if (ret == -EAGAIN) + goto retry; +} + +static ssize_t stm_source_link_show(struct device *dev, + struct device_attribute *attr, + char *buf) +{ + struct stm_source_device *src = to_stm_source_device(dev); + struct stm_device *stm; + int idx, ret; + + idx = srcu_read_lock(&stm_source_srcu); + stm = srcu_dereference(src->link, &stm_source_srcu); + ret = sprintf(buf, "%s\n", + stm ? dev_name(&stm->dev) : ""); + srcu_read_unlock(&stm_source_srcu, idx); + + return ret; +} + +static ssize_t stm_source_link_store(struct device *dev, + struct device_attribute *attr, + const char *buf, size_t count) +{ + struct stm_source_device *src = to_stm_source_device(dev); + struct stm_device *link; + int err; + + stm_source_link_drop(src); + + link = stm_find_device(buf); + if (!link) + return -EINVAL; + + pm_runtime_get(&link->dev); + + err = stm_source_link_add(src, link); + if (err) { + pm_runtime_put_autosuspend(&link->dev); + /* matches the stm_find_device() above */ + stm_put_device(link); + } + + return err ? : count; +} + +static DEVICE_ATTR_RW(stm_source_link); + +static struct attribute *stm_source_attrs[] = { + &dev_attr_stm_source_link.attr, + NULL, +}; + +ATTRIBUTE_GROUPS(stm_source); + +static struct class stm_source_class = { + .name = "stm_source", + .dev_groups = stm_source_groups, +}; + +static void stm_source_device_release(struct device *dev) +{ + struct stm_source_device *src = to_stm_source_device(dev); + + kfree(src); +} + +/** + * stm_source_register_device() - register an stm_source device + * @parent: parent device + * @data: device description structure + * + * This will create a device of stm_source class that can write + * data to an stm device once linked. + * + * Return: 0 on success, -errno otherwise. + */ +int stm_source_register_device(struct device *parent, + struct stm_source_data *data) +{ + struct stm_source_device *src; + int err; + + if (!stm_core_up) + return -EPROBE_DEFER; + + src = kzalloc_obj(*src); + if (!src) + return -ENOMEM; + + device_initialize(&src->dev); + src->dev.class = &stm_source_class; + src->dev.parent = parent; + src->dev.release = stm_source_device_release; + + err = kobject_set_name(&src->dev.kobj, "%s", data->name); + if (err) + goto err; + + pm_runtime_no_callbacks(&src->dev); + pm_runtime_forbid(&src->dev); + + err = device_add(&src->dev); + if (err) + goto err; + + stm_output_init(&src->output); + spin_lock_init(&src->link_lock); + INIT_LIST_HEAD(&src->link_entry); + src->data = data; + data->src = src; + + return 0; + +err: + put_device(&src->dev); + + return err; +} +EXPORT_SYMBOL_GPL(stm_source_register_device); + +/** + * stm_source_unregister_device() - unregister an stm_source device + * @data: device description that was used to register the device + * + * This will remove a previously created stm_source device from the system. + */ +void stm_source_unregister_device(struct stm_source_data *data) +{ + struct stm_source_device *src = data->src; + + stm_source_link_drop(src); + + device_unregister(&src->dev); +} +EXPORT_SYMBOL_GPL(stm_source_unregister_device); + +int notrace stm_source_write(struct stm_source_data *data, + unsigned int chan, + const char *buf, size_t count) +{ + struct stm_source_device *src = data->src; + struct stm_device *stm; + int idx; + + if (!src->output.nr_chans) + return -ENODEV; + + if (chan >= src->output.nr_chans) + return -EINVAL; + + idx = srcu_read_lock(&stm_source_srcu); + + stm = srcu_dereference(src->link, &stm_source_srcu); + if (stm) + count = stm_write(stm, &src->output, chan, buf, count, data); + else + count = -ENODEV; + + srcu_read_unlock(&stm_source_srcu, idx); + + return count; +} +EXPORT_SYMBOL_GPL(stm_source_write); + +static int __init stm_core_init(void) +{ + int err; + + err = class_register(&stm_class); + if (err) + return err; + + err = class_register(&stm_source_class); + if (err) + goto err_stm; + + err = stp_configfs_init(); + if (err) + goto err_src; + + init_srcu_struct(&stm_source_srcu); + INIT_LIST_HEAD(&stm_pdrv_head); + mutex_init(&stm_pdrv_mutex); + + /* + * So as to not confuse existing users with a requirement + * to load yet another module, do it here. + */ + if (IS_ENABLED(CONFIG_STM_PROTO_BASIC)) + (void)request_module_nowait("stm_p_basic"); + stm_core_up++; + + return 0; + +err_src: + class_unregister(&stm_source_class); +err_stm: + class_unregister(&stm_class); + + return err; +} + +module_init(stm_core_init); + +static void __exit stm_core_exit(void) +{ + cleanup_srcu_struct(&stm_source_srcu); + class_unregister(&stm_source_class); + class_unregister(&stm_class); + stp_configfs_exit(); +} + +module_exit(stm_core_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("System Trace Module device class"); +MODULE_AUTHOR("Alexander Shishkin "); diff --git a/drivers/hwtracing/stm/dummy_stm.c b/drivers/hwtracing/stm/dummy_stm.c new file mode 100644 index 000000000..38528ffdc --- /dev/null +++ b/drivers/hwtracing/stm/dummy_stm.c @@ -0,0 +1,125 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * A dummy STM device for stm/stm_source class testing. + * Copyright (c) 2014, Intel Corporation. + * + * STM class implements generic infrastructure for System Trace Module devices + * as defined in MIPI STPv2 specification. + */ + +#undef DEBUG +#include +#include +#include +#include +#include + +static ssize_t notrace +dummy_stm_packet(struct stm_data *stm_data, unsigned int master, + unsigned int channel, unsigned int packet, unsigned int flags, + unsigned int size, const unsigned char *payload) +{ +#ifdef DEBUG + u64 pl = 0; + + if (payload) + pl = *(u64 *)payload; + + if (size < 8) + pl &= (1ull << (size * 8)) - 1; + trace_printk("[%u:%u] [pkt: %x/%x] (%llx)\n", master, channel, + packet, size, pl); +#endif + return size; +} + +#define DUMMY_STM_MAX 32 + +static struct stm_data dummy_stm[DUMMY_STM_MAX]; + +static int nr_dummies = 4; + +module_param(nr_dummies, int, 0400); + +static unsigned int fail_mode; + +module_param(fail_mode, int, 0600); + +static unsigned int master_min; + +module_param(master_min, int, 0400); + +static unsigned int master_max = STP_MASTER_MAX; + +module_param(master_max, int, 0400); + +static unsigned int nr_channels = STP_CHANNEL_MAX; + +module_param(nr_channels, int, 0400); + +static int dummy_stm_link(struct stm_data *data, unsigned int master, + unsigned int channel) +{ + if (fail_mode && (channel & fail_mode)) + return -EINVAL; + + return 0; +} + +static int dummy_stm_init(void) +{ + int i, ret = -ENOMEM; + + if (nr_dummies < 0 || nr_dummies > DUMMY_STM_MAX) + return -EINVAL; + + if (master_min > master_max || + master_max > STP_MASTER_MAX || + nr_channels > STP_CHANNEL_MAX) + return -EINVAL; + + for (i = 0; i < nr_dummies; i++) { + dummy_stm[i].name = kasprintf(GFP_KERNEL, "dummy_stm.%d", i); + if (!dummy_stm[i].name) + goto fail_unregister; + + dummy_stm[i].sw_start = master_min; + dummy_stm[i].sw_end = master_max; + dummy_stm[i].sw_nchannels = nr_channels; + dummy_stm[i].packet = dummy_stm_packet; + dummy_stm[i].link = dummy_stm_link; + + ret = stm_register_device(NULL, &dummy_stm[i], THIS_MODULE); + if (ret) + goto fail_free; + } + + return 0; + +fail_unregister: + for (i--; i >= 0; i--) { + stm_unregister_device(&dummy_stm[i]); +fail_free: + kfree(dummy_stm[i].name); + } + + return ret; + +} + +static void dummy_stm_exit(void) +{ + int i; + + for (i = 0; i < nr_dummies; i++) { + stm_unregister_device(&dummy_stm[i]); + kfree(dummy_stm[i].name); + } +} + +module_init(dummy_stm_init); +module_exit(dummy_stm_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("dummy_stm device"); +MODULE_AUTHOR("Alexander Shishkin "); diff --git a/drivers/hwtracing/stm/ftrace.c b/drivers/hwtracing/stm/ftrace.c new file mode 100644 index 000000000..a7cea7ea0 --- /dev/null +++ b/drivers/hwtracing/stm/ftrace.c @@ -0,0 +1,87 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Simple kernel driver to link kernel Ftrace and an STM device + * Copyright (c) 2016, Linaro Ltd. + * + * STM Ftrace will be registered as a trace_export. + */ + +#include +#include +#include + +#define STM_FTRACE_NR_CHANNELS 1 +#define STM_FTRACE_CHAN 0 + +static int stm_ftrace_link(struct stm_source_data *data); +static void stm_ftrace_unlink(struct stm_source_data *data); + +static struct stm_ftrace { + struct stm_source_data data; + struct trace_export ftrace; +} stm_ftrace = { + .data = { + .name = "ftrace", + .nr_chans = STM_FTRACE_NR_CHANNELS, + .type = STM_FTRACE, + .link = stm_ftrace_link, + .unlink = stm_ftrace_unlink, + }, +}; + +/** + * stm_ftrace_write() - write data to STM via 'stm_ftrace' source + * @buf: buffer containing the data packet + * @len: length of the data packet + */ +static void notrace +stm_ftrace_write(struct trace_export *export, const void *buf, unsigned int len) +{ + struct stm_ftrace *stm = container_of(export, struct stm_ftrace, ftrace); + /* This is called from trace system with preemption disabled */ + unsigned int cpu = smp_processor_id(); + + stm_source_write(&stm->data, STM_FTRACE_CHAN + cpu, buf, len); +} + +static int stm_ftrace_link(struct stm_source_data *data) +{ + struct stm_ftrace *sf = container_of(data, struct stm_ftrace, data); + + sf->ftrace.write = stm_ftrace_write; + sf->ftrace.flags = TRACE_EXPORT_FUNCTION | TRACE_EXPORT_EVENT + | TRACE_EXPORT_MARKER; + + return register_ftrace_export(&sf->ftrace); +} + +static void stm_ftrace_unlink(struct stm_source_data *data) +{ + struct stm_ftrace *sf = container_of(data, struct stm_ftrace, data); + + unregister_ftrace_export(&sf->ftrace); +} + +static int __init stm_ftrace_init(void) +{ + int ret; + + stm_ftrace.data.nr_chans = roundup_pow_of_two(num_possible_cpus()); + ret = stm_source_register_device(NULL, &stm_ftrace.data); + if (ret) + pr_err("Failed to register stm_source - ftrace.\n"); + + return ret; +} + +static void __exit stm_ftrace_exit(void) +{ + stm_source_unregister_device(&stm_ftrace.data); +} + +module_init(stm_ftrace_init); +module_exit(stm_ftrace_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("stm_ftrace driver"); +MODULE_AUTHOR("Chunyan Zhang "); diff --git a/drivers/hwtracing/stm/heartbeat.c b/drivers/hwtracing/stm/heartbeat.c new file mode 100644 index 000000000..495eb1dc8 --- /dev/null +++ b/drivers/hwtracing/stm/heartbeat.c @@ -0,0 +1,119 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Simple heartbeat STM source driver + * Copyright (c) 2016, Intel Corporation. + * + * Heartbeat STM source will send repetitive messages over STM devices to a + * trace host. + */ + +#include +#include +#include +#include +#include + +#define STM_HEARTBEAT_MAX 32 + +static int nr_devs = 4; +static int interval_ms = 10; + +module_param(nr_devs, int, 0400); +module_param(interval_ms, int, 0600); + +static struct stm_heartbeat { + struct stm_source_data data; + struct hrtimer hrtimer; + unsigned int active; +} stm_heartbeat[STM_HEARTBEAT_MAX]; + +static const char str[] = "heartbeat stm source driver is here to serve you"; + +static enum hrtimer_restart stm_heartbeat_hrtimer_handler(struct hrtimer *hr) +{ + struct stm_heartbeat *heartbeat = container_of(hr, struct stm_heartbeat, + hrtimer); + + stm_source_write(&heartbeat->data, 0, str, sizeof str); + if (heartbeat->active) + hrtimer_forward_now(hr, ms_to_ktime(interval_ms)); + + return heartbeat->active ? HRTIMER_RESTART : HRTIMER_NORESTART; +} + +static int stm_heartbeat_link(struct stm_source_data *data) +{ + struct stm_heartbeat *heartbeat = + container_of(data, struct stm_heartbeat, data); + + heartbeat->active = 1; + hrtimer_start(&heartbeat->hrtimer, ms_to_ktime(interval_ms), + HRTIMER_MODE_ABS); + + return 0; +} + +static void stm_heartbeat_unlink(struct stm_source_data *data) +{ + struct stm_heartbeat *heartbeat = + container_of(data, struct stm_heartbeat, data); + + heartbeat->active = 0; + hrtimer_cancel(&heartbeat->hrtimer); +} + +static int stm_heartbeat_init(void) +{ + int i, ret; + + if (nr_devs < 0 || nr_devs > STM_HEARTBEAT_MAX) + return -EINVAL; + + for (i = 0; i < nr_devs; i++) { + stm_heartbeat[i].data.name = + kasprintf(GFP_KERNEL, "heartbeat.%d", i); + if (!stm_heartbeat[i].data.name) { + ret = -ENOMEM; + goto fail_unregister; + } + + stm_heartbeat[i].data.nr_chans = 1; + stm_heartbeat[i].data.type = STM_USER; + stm_heartbeat[i].data.link = stm_heartbeat_link; + stm_heartbeat[i].data.unlink = stm_heartbeat_unlink; + hrtimer_setup(&stm_heartbeat[i].hrtimer, stm_heartbeat_hrtimer_handler, + CLOCK_MONOTONIC, HRTIMER_MODE_ABS); + + ret = stm_source_register_device(NULL, &stm_heartbeat[i].data); + if (ret) + goto fail_free; + } + + return 0; + +fail_unregister: + for (i--; i >= 0; i--) { + stm_source_unregister_device(&stm_heartbeat[i].data); +fail_free: + kfree(stm_heartbeat[i].data.name); + } + + return ret; +} + +static void stm_heartbeat_exit(void) +{ + int i; + + for (i = 0; i < nr_devs; i++) { + stm_source_unregister_device(&stm_heartbeat[i].data); + kfree(stm_heartbeat[i].data.name); + } +} + +module_init(stm_heartbeat_init); +module_exit(stm_heartbeat_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("stm_heartbeat driver"); +MODULE_AUTHOR("Alexander Shishkin "); diff --git a/drivers/hwtracing/stm/p_basic.c b/drivers/hwtracing/stm/p_basic.c new file mode 100644 index 000000000..5525c975c --- /dev/null +++ b/drivers/hwtracing/stm/p_basic.c @@ -0,0 +1,49 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Basic framing protocol for STM devices. + * Copyright (c) 2018, Intel Corporation. + */ + +#include +#include +#include +#include "stm.h" + +static ssize_t basic_write(struct stm_data *data, struct stm_output *output, + unsigned int chan, const char *buf, size_t count, + struct stm_source_data *source) +{ + unsigned int c = output->channel + chan; + unsigned int m = output->master; + const unsigned char nil = 0; + ssize_t sz; + + sz = stm_data_write(data, m, c, true, buf, count); + if (sz > 0) + data->packet(data, m, c, STP_PACKET_FLAG, 0, 0, &nil); + + return sz; +} + +static const struct stm_protocol_driver basic_pdrv = { + .owner = THIS_MODULE, + .name = "p_basic", + .write = basic_write, +}; + +static int basic_stm_init(void) +{ + return stm_register_protocol(&basic_pdrv); +} + +static void basic_stm_exit(void) +{ + stm_unregister_protocol(&basic_pdrv); +} + +module_init(basic_stm_init); +module_exit(basic_stm_exit); + +MODULE_LICENSE("GPL v2"); +MODULE_DESCRIPTION("Basic STM framing protocol driver"); +MODULE_AUTHOR("Alexander Shishkin "); diff --git a/drivers/hwtracing/stm/p_sys-t.c b/drivers/hwtracing/stm/p_sys-t.c new file mode 100644 index 000000000..bcbbc4d92 --- /dev/null +++ b/drivers/hwtracing/stm/p_sys-t.c @@ -0,0 +1,461 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * MIPI SyS-T framing protocol for STM devices. + * Copyright (c) 2018, Intel Corporation. + */ + +#include +#include +#include +#include +#include +#include +#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, + MIPI_SYST_TYPE_SBD, +}; + +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, +}; + +/** + * enum sys_t_message_sbd_subtype - SyS-T SBD message subtypes + * @MIPI_SYST_SBD_ID32: SBD message with 32-bit message ID + * @MIPI_SYST_SBD_ID64: SBD message with 64-bit message ID + * + * Structured Binary Data messages can send information of arbitrary length, + * together with ID's that describe BLOB's content and layout. + */ +enum sys_t_message_sbd_subtype { + MIPI_SYST_SBD_ID32 = 0, + MIPI_SYST_SBD_ID64 = 1, +}; + +#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)) + +/* + * SyS-T and ftrace headers are compatible to an extent that ftrace event ID + * and args can be treated as SyS-T SBD message with 64-bit ID and arguments + * BLOB right behind the header without modification. Bits [16:63] coming + * together with message ID from ftrace aren't used by SBD and must be zeroed. + * + * 0 15 16 23 24 31 32 39 40 63 + * ftrace: <-pid-> <----> + * SBD: <------- msg_id ------------------------------> + */ +#define SBD_HEADER (MIPI_SYST_TYPES(SBD, ID64) | \ + MIPI_SYST_SEVERITY(INFO) | \ + MIPI_SYST_OPT_GUID) + +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; + + uuid_gen(&pn->uuid); +} + +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_obj(*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 inline u32 sys_t_header(struct stm_source_data *source) +{ + if (source && source->type == STM_FTRACE) + return SBD_HEADER; + return DATA_HEADER; +} + +static ssize_t sys_t_write_data(struct stm_data *data, + struct stm_source_data *source, + unsigned int master, unsigned int channel, + bool ts_first, const void *buf, size_t count) +{ + ssize_t sz; + const unsigned char nil = 0; + + /* + * Ftrace is zero-copy compatible with SyS-T SBD, but requires + * special handling of first 64 bits. Trim and send them separately + * to avoid damage on original ftrace buffer. + */ + if (source && source->type == STM_FTRACE) { + u64 compat_ftrace_header; + ssize_t header_sz; + ssize_t buf_sz; + + if (count < sizeof(compat_ftrace_header)) + return -EINVAL; + + /* SBD only makes use of low 16 bits (event ID) from ftrace event */ + compat_ftrace_header = *(u64 *)buf & 0xffff; + header_sz = stm_data_write(data, master, channel, false, + &compat_ftrace_header, + sizeof(compat_ftrace_header)); + if (header_sz != sizeof(compat_ftrace_header)) + return header_sz; + + buf_sz = stm_data_write(data, master, channel, false, + buf + header_sz, count - header_sz); + if (buf_sz != count - header_sz) + return buf_sz; + sz = header_sz + buf_sz; + } else { + sz = stm_data_write(data, master, channel, false, buf, count); + } + + if (sz <= 0) + return sz; + + data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil); + + return sz; +} + +static ssize_t sys_t_write(struct stm_data *data, struct stm_output *output, + unsigned int chan, const char *buf, size_t count, + struct stm_source_data *source) +{ + struct sys_t_output *op = output->pdrv_private; + unsigned int c = output->channel + chan; + unsigned int m = output->master; + u32 header = sys_t_header(source); + u8 uuid[UUID_SIZE]; + 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 */ + export_uuid(uuid, &op->node.uuid); + sz = stm_data_write(data, m, c, false, uuid, sizeof(op->node.uuid)); + 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 */ + return sys_t_write_data(data, source, m, c, false, buf, count); +} + +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 "); diff --git a/drivers/hwtracing/stm/policy.c b/drivers/hwtracing/stm/policy.c new file mode 100644 index 000000000..fabaf4b14 --- /dev/null +++ b/drivers/hwtracing/stm/policy.c @@ -0,0 +1,570 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * System Trace Module (STM) master/channel allocation policy management + * Copyright (c) 2014, Intel Corporation. + * + * A master/channel allocation policy allows mapping string identifiers to + * master and channel ranges, where allocation can be done. + */ + +#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt + +#include +#include +#include +#include +#include +#include +#include "stm.h" + +/* + * STP Master/Channel allocation policy configfs layout. + */ + +struct stp_policy { + struct config_group group; + struct stm_device *stm; +}; + +struct stp_policy_node { + struct config_group group; + struct stp_policy *policy; + unsigned int first_master; + unsigned int last_master; + unsigned int first_channel; + unsigned int last_channel; + /* this is the one that's exposed to the attributes */ + unsigned char priv[]; +}; + +void *stp_policy_node_priv(struct stp_policy_node *pn) +{ + if (!pn) + return NULL; + + return pn->priv; +} + +static struct configfs_subsystem stp_policy_subsys; + +void stp_policy_node_get_ranges(struct stp_policy_node *policy_node, + unsigned int *mstart, unsigned int *mend, + unsigned int *cstart, unsigned int *cend) +{ + *mstart = policy_node->first_master; + *mend = policy_node->last_master; + *cstart = policy_node->first_channel; + *cend = policy_node->last_channel; +} + +static inline struct stp_policy *to_stp_policy(struct config_item *item) +{ + return item ? + container_of(to_config_group(item), struct stp_policy, group) : + NULL; +} + +static inline struct stp_policy_node * +to_stp_policy_node(struct config_item *item) +{ + return item ? + container_of(to_config_group(item), struct stp_policy_node, + group) : + NULL; +} + +void *to_pdrv_policy_node(struct config_item *item) +{ + struct stp_policy_node *node = to_stp_policy_node(item); + + return stp_policy_node_priv(node); +} +EXPORT_SYMBOL_GPL(to_pdrv_policy_node); + +static ssize_t +stp_policy_node_masters_show(struct config_item *item, char *page) +{ + struct stp_policy_node *policy_node = to_stp_policy_node(item); + ssize_t count; + + count = sprintf(page, "%u %u\n", policy_node->first_master, + policy_node->last_master); + + return count; +} + +static ssize_t +stp_policy_node_masters_store(struct config_item *item, const char *page, + size_t count) +{ + struct stp_policy_node *policy_node = to_stp_policy_node(item); + unsigned int first, last; + struct stm_device *stm; + char *p = (char *)page; + ssize_t ret = -ENODEV; + + if (sscanf(p, "%u %u", &first, &last) != 2) + return -EINVAL; + + mutex_lock(&stp_policy_subsys.su_mutex); + stm = policy_node->policy->stm; + if (!stm) + goto unlock; + + /* must be within [sw_start..sw_end], which is an inclusive range */ + if (first > last || first < stm->data->sw_start || + last > stm->data->sw_end) { + ret = -ERANGE; + goto unlock; + } + + ret = count; + policy_node->first_master = first; + policy_node->last_master = last; + +unlock: + mutex_unlock(&stp_policy_subsys.su_mutex); + + return ret; +} + +static ssize_t +stp_policy_node_channels_show(struct config_item *item, char *page) +{ + struct stp_policy_node *policy_node = to_stp_policy_node(item); + ssize_t count; + + count = sprintf(page, "%u %u\n", policy_node->first_channel, + policy_node->last_channel); + + return count; +} + +static ssize_t +stp_policy_node_channels_store(struct config_item *item, const char *page, + size_t count) +{ + struct stp_policy_node *policy_node = to_stp_policy_node(item); + unsigned int first, last; + struct stm_device *stm; + char *p = (char *)page; + ssize_t ret = -ENODEV; + + if (sscanf(p, "%u %u", &first, &last) != 2) + return -EINVAL; + + mutex_lock(&stp_policy_subsys.su_mutex); + stm = policy_node->policy->stm; + if (!stm) + goto unlock; + + if (first > INT_MAX || last > INT_MAX || first > last || + last >= stm->data->sw_nchannels) { + ret = -ERANGE; + goto unlock; + } + + ret = count; + policy_node->first_channel = first; + policy_node->last_channel = last; + +unlock: + mutex_unlock(&stp_policy_subsys.su_mutex); + + return ret; +} + +static void stp_policy_node_release(struct config_item *item) +{ + struct stp_policy_node *node = to_stp_policy_node(item); + + kfree(node); +} + +static struct configfs_item_operations stp_policy_node_item_ops = { + .release = stp_policy_node_release, +}; + +CONFIGFS_ATTR(stp_policy_node_, masters); +CONFIGFS_ATTR(stp_policy_node_, channels); + +static struct configfs_attribute *stp_policy_node_attrs[] = { + &stp_policy_node_attr_masters, + &stp_policy_node_attr_channels, + NULL, +}; + +static const struct config_item_type stp_policy_type; +static const struct config_item_type stp_policy_node_type; + +const struct config_item_type * +get_policy_node_type(struct configfs_attribute **attrs) +{ + struct config_item_type *type; + struct configfs_attribute **merged; + + type = kmemdup(&stp_policy_node_type, sizeof(stp_policy_node_type), + GFP_KERNEL); + if (!type) + return NULL; + + merged = memcat_p(stp_policy_node_attrs, attrs); + if (!merged) { + kfree(type); + return NULL; + } + + type->ct_attrs = merged; + + return type; +} + +static struct config_group * +stp_policy_node_make(struct config_group *group, const char *name) +{ + const struct config_item_type *type = &stp_policy_node_type; + struct stp_policy_node *policy_node, *parent_node; + const struct stm_protocol_driver *pdrv; + struct stp_policy *policy; + + if (group->cg_item.ci_type == &stp_policy_type) { + policy = container_of(group, struct stp_policy, group); + } else { + parent_node = container_of(group, struct stp_policy_node, + group); + policy = parent_node->policy; + } + + if (!policy->stm) + return ERR_PTR(-ENODEV); + + pdrv = policy->stm->pdrv; + policy_node = + kzalloc(offsetof(struct stp_policy_node, priv[pdrv->priv_sz]), + GFP_KERNEL); + if (!policy_node) + return ERR_PTR(-ENOMEM); + + if (pdrv->policy_node_init) + pdrv->policy_node_init((void *)policy_node->priv); + + if (policy->stm->pdrv_node_type) + type = policy->stm->pdrv_node_type; + + config_group_init_type_name(&policy_node->group, name, type); + + policy_node->policy = policy; + + /* default values for the attributes */ + policy_node->first_master = policy->stm->data->sw_start; + policy_node->last_master = policy->stm->data->sw_end; + policy_node->first_channel = 0; + policy_node->last_channel = policy->stm->data->sw_nchannels - 1; + + return &policy_node->group; +} + +static void +stp_policy_node_drop(struct config_group *group, struct config_item *item) +{ + config_item_put(item); +} + +static struct configfs_group_operations stp_policy_node_group_ops = { + .make_group = stp_policy_node_make, + .drop_item = stp_policy_node_drop, +}; + +static const struct config_item_type stp_policy_node_type = { + .ct_item_ops = &stp_policy_node_item_ops, + .ct_group_ops = &stp_policy_node_group_ops, + .ct_attrs = stp_policy_node_attrs, + .ct_owner = THIS_MODULE, +}; + +/* + * Root group: policies. + */ +static ssize_t stp_policy_device_show(struct config_item *item, + char *page) +{ + struct stp_policy *policy = to_stp_policy(item); + ssize_t count; + + count = sprintf(page, "%s\n", + (policy && policy->stm) ? + policy->stm->data->name : + ""); + + return count; +} + +CONFIGFS_ATTR_RO(stp_policy_, device); + +static ssize_t stp_policy_protocol_show(struct config_item *item, + char *page) +{ + struct stp_policy *policy = to_stp_policy(item); + ssize_t count; + + count = sprintf(page, "%s\n", + (policy && policy->stm) ? + policy->stm->pdrv->name : + ""); + + return count; +} + +CONFIGFS_ATTR_RO(stp_policy_, protocol); + +static struct configfs_attribute *stp_policy_attrs[] = { + &stp_policy_attr_device, + &stp_policy_attr_protocol, + NULL, +}; + +void stp_policy_unbind(struct stp_policy *policy) +{ + struct stm_device *stm = policy->stm; + + /* + * stp_policy_release() will not call here if the policy is already + * unbound; other users should not either, as no link exists between + * this policy and anything else in that case + */ + if (WARN_ON_ONCE(!policy->stm)) + return; + + lockdep_assert_held(&stm->policy_mutex); + + stm->policy = NULL; + policy->stm = NULL; + + /* + * Drop the reference on the protocol driver and lose the link. + */ + stm_put_protocol(stm->pdrv); + stm->pdrv = NULL; + stm_put_device(stm); +} + +static void stp_policy_release(struct config_item *item) +{ + struct stp_policy *policy = to_stp_policy(item); + struct stm_device *stm = policy->stm; + + /* a policy *can* be unbound and still exist in configfs tree */ + if (!stm) + return; + + mutex_lock(&stm->policy_mutex); + stp_policy_unbind(policy); + mutex_unlock(&stm->policy_mutex); + + kfree(policy); +} + +static struct configfs_item_operations stp_policy_item_ops = { + .release = stp_policy_release, +}; + +static struct configfs_group_operations stp_policy_group_ops = { + .make_group = stp_policy_node_make, +}; + +static const struct config_item_type stp_policy_type = { + .ct_item_ops = &stp_policy_item_ops, + .ct_group_ops = &stp_policy_group_ops, + .ct_attrs = stp_policy_attrs, + .ct_owner = THIS_MODULE, +}; + +static struct config_group * +stp_policy_make(struct config_group *group, const char *name) +{ + const struct config_item_type *pdrv_node_type; + const struct stm_protocol_driver *pdrv; + char *devname, *proto, *p; + struct config_group *ret; + struct stm_device *stm; + int err; + + devname = kasprintf(GFP_KERNEL, "%s", name); + if (!devname) + return ERR_PTR(-ENOMEM); + + /* + * node must look like ., where + * is the name of an existing stm device; may + * contain dots; + * is an arbitrary string; may not contain dots + * :. + */ + p = strrchr(devname, '.'); + if (!p) { + kfree(devname); + return ERR_PTR(-EINVAL); + } + + *p = '\0'; + + /* + * look for ":": + * + no protocol suffix: fall back to whatever is available; + * + unknown protocol: fail the whole thing + */ + proto = strrchr(devname, ':'); + if (proto) + *proto++ = '\0'; + + stm = stm_find_device(devname); + if (!stm) { + kfree(devname); + return ERR_PTR(-ENODEV); + } + + err = stm_lookup_protocol(proto, &pdrv, &pdrv_node_type); + kfree(devname); + + if (err) { + stm_put_device(stm); + return ERR_PTR(-ENODEV); + } + + mutex_lock(&stm->policy_mutex); + if (stm->policy) { + ret = ERR_PTR(-EBUSY); + goto unlock_policy; + } + + stm->policy = kzalloc_obj(*stm->policy); + if (!stm->policy) { + ret = ERR_PTR(-ENOMEM); + goto unlock_policy; + } + + config_group_init_type_name(&stm->policy->group, name, + &stp_policy_type); + + stm->pdrv = pdrv; + stm->pdrv_node_type = pdrv_node_type; + stm->policy->stm = stm; + ret = &stm->policy->group; + +unlock_policy: + mutex_unlock(&stm->policy_mutex); + + if (IS_ERR(ret)) { + /* + * pdrv and stm->pdrv at this point can be quite different, + * and only one of them needs to be 'put' + */ + stm_put_protocol(pdrv); + stm_put_device(stm); + } + + return ret; +} + +static struct configfs_group_operations stp_policy_root_group_ops = { + .make_group = stp_policy_make, +}; + +static const struct config_item_type stp_policy_root_type = { + .ct_group_ops = &stp_policy_root_group_ops, + .ct_owner = THIS_MODULE, +}; + +static struct configfs_subsystem stp_policy_subsys = { + .su_group = { + .cg_item = { + .ci_namebuf = "stp-policy", + .ci_type = &stp_policy_root_type, + }, + }, +}; + +/* + * Lock the policy mutex from the outside + */ +static struct stp_policy_node * +__stp_policy_node_lookup(struct stp_policy *policy, char *s) +{ + struct stp_policy_node *policy_node, *ret = NULL; + struct list_head *head = &policy->group.cg_children; + struct config_item *item; + char *start, *end = s; + + if (list_empty(head)) + return NULL; + +next: + for (;;) { + start = strsep(&end, "/"); + if (!start) + break; + + if (!*start) + continue; + + list_for_each_entry(item, head, ci_entry) { + policy_node = to_stp_policy_node(item); + + if (!strcmp(start, + policy_node->group.cg_item.ci_name)) { + ret = policy_node; + + if (!end) + goto out; + + head = &policy_node->group.cg_children; + goto next; + } + } + break; + } + +out: + return ret; +} + + +struct stp_policy_node * +stp_policy_node_lookup(struct stm_device *stm, char *s) +{ + struct stp_policy_node *policy_node = NULL; + + mutex_lock(&stp_policy_subsys.su_mutex); + + mutex_lock(&stm->policy_mutex); + if (stm->policy) + policy_node = __stp_policy_node_lookup(stm->policy, s); + mutex_unlock(&stm->policy_mutex); + + if (policy_node) + config_item_get(&policy_node->group.cg_item); + else + mutex_unlock(&stp_policy_subsys.su_mutex); + + return policy_node; +} + +void stp_policy_node_put(struct stp_policy_node *policy_node) +{ + lockdep_assert_held(&stp_policy_subsys.su_mutex); + + mutex_unlock(&stp_policy_subsys.su_mutex); + config_item_put(&policy_node->group.cg_item); +} + +int __init stp_configfs_init(void) +{ + config_group_init(&stp_policy_subsys.su_group); + mutex_init(&stp_policy_subsys.su_mutex); + return configfs_register_subsystem(&stp_policy_subsys); +} + +void __exit stp_configfs_exit(void) +{ + configfs_unregister_subsystem(&stp_policy_subsys); +} diff --git a/drivers/hwtracing/stm/stm.h b/drivers/hwtracing/stm/stm.h new file mode 100644 index 000000000..85dda6e0d --- /dev/null +++ b/drivers/hwtracing/stm/stm.h @@ -0,0 +1,117 @@ +/* SPDX-License-Identifier: GPL-2.0 */ +/* + * System Trace Module (STM) infrastructure + * Copyright (c) 2014, Intel Corporation. + * + * STM class implements generic infrastructure for System Trace Module devices + * as defined in MIPI STPv2 specification. + */ + +#ifndef _STM_STM_H_ +#define _STM_STM_H_ + +#include + +struct stp_policy; +struct stp_policy_node; +struct stm_protocol_driver; + +int stp_configfs_init(void); +void stp_configfs_exit(void); + +void *stp_policy_node_priv(struct stp_policy_node *pn); + +struct stp_master { + unsigned int nr_free; + unsigned long chan_map[]; +}; + +struct stm_device { + struct device dev; + struct module *owner; + struct stp_policy *policy; + struct mutex policy_mutex; + int major; + unsigned int sw_nmasters; + struct stm_data *data; + struct mutex link_mutex; + spinlock_t link_lock; + struct list_head link_list; + /* framing protocol in use */ + const struct stm_protocol_driver *pdrv; + const struct config_item_type *pdrv_node_type; + /* master allocation */ + spinlock_t mc_lock; + struct stp_master *masters[]; +}; + +#define to_stm_device(_d) \ + container_of((_d), struct stm_device, dev) + +struct stp_policy_node * +stp_policy_node_lookup(struct stm_device *stm, char *s); +void stp_policy_node_put(struct stp_policy_node *policy_node); +void stp_policy_unbind(struct stp_policy *policy); + +void stp_policy_node_get_ranges(struct stp_policy_node *policy_node, + unsigned int *mstart, unsigned int *mend, + unsigned int *cstart, unsigned int *cend); + +const struct config_item_type * +get_policy_node_type(struct configfs_attribute **attrs); + +struct stm_output { + spinlock_t lock; + unsigned int master; + unsigned int channel; + unsigned int nr_chans; + void *pdrv_private; +}; + +struct stm_file { + struct stm_device *stm; + struct stm_output output; +}; + +struct stm_device *stm_find_device(const char *name); +void stm_put_device(struct stm_device *stm); + +struct stm_source_device { + struct device dev; + struct stm_source_data *data; + spinlock_t link_lock; + struct stm_device __rcu *link; + struct list_head link_entry; + /* one output per stm_source device */ + struct stm_output output; +}; + +#define to_stm_source_device(_d) \ + container_of((_d), struct stm_source_device, dev) + +void *to_pdrv_policy_node(struct config_item *item); + +struct stm_protocol_driver { + struct module *owner; + const char *name; + ssize_t (*write)(struct stm_data *data, + struct stm_output *output, unsigned int chan, + const char *buf, size_t count, struct stm_source_data *source); + void (*policy_node_init)(void *arg); + int (*output_open)(void *priv, struct stm_output *output); + void (*output_close)(struct stm_output *output); + ssize_t priv_sz; + struct configfs_attribute **policy_attr; +}; + +int stm_register_protocol(const struct stm_protocol_driver *pdrv); +void stm_unregister_protocol(const struct stm_protocol_driver *pdrv); +int stm_lookup_protocol(const char *name, + const struct stm_protocol_driver **pdrv, + const struct config_item_type **type); +void stm_put_protocol(const struct stm_protocol_driver *pdrv); +ssize_t stm_data_write(struct stm_data *data, unsigned int m, + unsigned int c, bool ts_first, const void *buf, + size_t count); + +#endif /* _STM_STM_H_ */ -- cgit v1.3.1