xref: /openbmc/linux/drivers/hwtracing/stm/core.c (revision 7b3bb0e7)
17bd1d409SAlexander Shishkin /*
27bd1d409SAlexander Shishkin  * System Trace Module (STM) infrastructure
37bd1d409SAlexander Shishkin  * Copyright (c) 2014, Intel Corporation.
47bd1d409SAlexander Shishkin  *
57bd1d409SAlexander Shishkin  * This program is free software; you can redistribute it and/or modify it
67bd1d409SAlexander Shishkin  * under the terms and conditions of the GNU General Public License,
77bd1d409SAlexander Shishkin  * version 2, as published by the Free Software Foundation.
87bd1d409SAlexander Shishkin  *
97bd1d409SAlexander Shishkin  * This program is distributed in the hope it will be useful, but WITHOUT
107bd1d409SAlexander Shishkin  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
117bd1d409SAlexander Shishkin  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
127bd1d409SAlexander Shishkin  * more details.
137bd1d409SAlexander Shishkin  *
147bd1d409SAlexander Shishkin  * STM class implements generic infrastructure for  System Trace Module devices
157bd1d409SAlexander Shishkin  * as defined in MIPI STPv2 specification.
167bd1d409SAlexander Shishkin  */
177bd1d409SAlexander Shishkin 
187bd1d409SAlexander Shishkin #include <linux/uaccess.h>
197bd1d409SAlexander Shishkin #include <linux/kernel.h>
207bd1d409SAlexander Shishkin #include <linux/module.h>
217bd1d409SAlexander Shishkin #include <linux/device.h>
227bd1d409SAlexander Shishkin #include <linux/compat.h>
237bd1d409SAlexander Shishkin #include <linux/kdev_t.h>
247bd1d409SAlexander Shishkin #include <linux/srcu.h>
257bd1d409SAlexander Shishkin #include <linux/slab.h>
267bd1d409SAlexander Shishkin #include <linux/stm.h>
277bd1d409SAlexander Shishkin #include <linux/fs.h>
287bd1d409SAlexander Shishkin #include <linux/mm.h>
297bd1d409SAlexander Shishkin #include "stm.h"
307bd1d409SAlexander Shishkin 
317bd1d409SAlexander Shishkin #include <uapi/linux/stm.h>
327bd1d409SAlexander Shishkin 
337bd1d409SAlexander Shishkin static unsigned int stm_core_up;
347bd1d409SAlexander Shishkin 
357bd1d409SAlexander Shishkin /*
367bd1d409SAlexander Shishkin  * The SRCU here makes sure that STM device doesn't disappear from under a
377bd1d409SAlexander Shishkin  * stm_source_write() caller, which may want to have as little overhead as
387bd1d409SAlexander Shishkin  * possible.
397bd1d409SAlexander Shishkin  */
407bd1d409SAlexander Shishkin static struct srcu_struct stm_source_srcu;
417bd1d409SAlexander Shishkin 
427bd1d409SAlexander Shishkin static ssize_t masters_show(struct device *dev,
437bd1d409SAlexander Shishkin 			    struct device_attribute *attr,
447bd1d409SAlexander Shishkin 			    char *buf)
457bd1d409SAlexander Shishkin {
467bd1d409SAlexander Shishkin 	struct stm_device *stm = to_stm_device(dev);
477bd1d409SAlexander Shishkin 	int ret;
487bd1d409SAlexander Shishkin 
497bd1d409SAlexander Shishkin 	ret = sprintf(buf, "%u %u\n", stm->data->sw_start, stm->data->sw_end);
507bd1d409SAlexander Shishkin 
517bd1d409SAlexander Shishkin 	return ret;
527bd1d409SAlexander Shishkin }
537bd1d409SAlexander Shishkin 
547bd1d409SAlexander Shishkin static DEVICE_ATTR_RO(masters);
557bd1d409SAlexander Shishkin 
567bd1d409SAlexander Shishkin static ssize_t channels_show(struct device *dev,
577bd1d409SAlexander Shishkin 			     struct device_attribute *attr,
587bd1d409SAlexander Shishkin 			     char *buf)
597bd1d409SAlexander Shishkin {
607bd1d409SAlexander Shishkin 	struct stm_device *stm = to_stm_device(dev);
617bd1d409SAlexander Shishkin 	int ret;
627bd1d409SAlexander Shishkin 
637bd1d409SAlexander Shishkin 	ret = sprintf(buf, "%u\n", stm->data->sw_nchannels);
647bd1d409SAlexander Shishkin 
657bd1d409SAlexander Shishkin 	return ret;
667bd1d409SAlexander Shishkin }
677bd1d409SAlexander Shishkin 
687bd1d409SAlexander Shishkin static DEVICE_ATTR_RO(channels);
697bd1d409SAlexander Shishkin 
707bd1d409SAlexander Shishkin static struct attribute *stm_attrs[] = {
717bd1d409SAlexander Shishkin 	&dev_attr_masters.attr,
727bd1d409SAlexander Shishkin 	&dev_attr_channels.attr,
737bd1d409SAlexander Shishkin 	NULL,
747bd1d409SAlexander Shishkin };
757bd1d409SAlexander Shishkin 
767bd1d409SAlexander Shishkin ATTRIBUTE_GROUPS(stm);
777bd1d409SAlexander Shishkin 
787bd1d409SAlexander Shishkin static struct class stm_class = {
797bd1d409SAlexander Shishkin 	.name		= "stm",
807bd1d409SAlexander Shishkin 	.dev_groups	= stm_groups,
817bd1d409SAlexander Shishkin };
827bd1d409SAlexander Shishkin 
837bd1d409SAlexander Shishkin static int stm_dev_match(struct device *dev, const void *data)
847bd1d409SAlexander Shishkin {
857bd1d409SAlexander Shishkin 	const char *name = data;
867bd1d409SAlexander Shishkin 
877bd1d409SAlexander Shishkin 	return sysfs_streq(name, dev_name(dev));
887bd1d409SAlexander Shishkin }
897bd1d409SAlexander Shishkin 
907bd1d409SAlexander Shishkin /**
917bd1d409SAlexander Shishkin  * stm_find_device() - find stm device by name
927bd1d409SAlexander Shishkin  * @buf:	character buffer containing the name
937bd1d409SAlexander Shishkin  *
947bd1d409SAlexander Shishkin  * This is called when either policy gets assigned to an stm device or an
957bd1d409SAlexander Shishkin  * stm_source device gets linked to an stm device.
967bd1d409SAlexander Shishkin  *
977bd1d409SAlexander Shishkin  * This grabs device's reference (get_device()) and module reference, both
987bd1d409SAlexander Shishkin  * of which the calling path needs to make sure to drop with stm_put_device().
997bd1d409SAlexander Shishkin  *
1007bd1d409SAlexander Shishkin  * Return:	stm device pointer or null if lookup failed.
1017bd1d409SAlexander Shishkin  */
1027bd1d409SAlexander Shishkin struct stm_device *stm_find_device(const char *buf)
1037bd1d409SAlexander Shishkin {
1047bd1d409SAlexander Shishkin 	struct stm_device *stm;
1057bd1d409SAlexander Shishkin 	struct device *dev;
1067bd1d409SAlexander Shishkin 
1077bd1d409SAlexander Shishkin 	if (!stm_core_up)
1087bd1d409SAlexander Shishkin 		return NULL;
1097bd1d409SAlexander Shishkin 
1107bd1d409SAlexander Shishkin 	dev = class_find_device(&stm_class, NULL, buf, stm_dev_match);
1117bd1d409SAlexander Shishkin 	if (!dev)
1127bd1d409SAlexander Shishkin 		return NULL;
1137bd1d409SAlexander Shishkin 
1147bd1d409SAlexander Shishkin 	stm = to_stm_device(dev);
1157bd1d409SAlexander Shishkin 	if (!try_module_get(stm->owner)) {
1167bd1d409SAlexander Shishkin 		put_device(dev);
1177bd1d409SAlexander Shishkin 		return NULL;
1187bd1d409SAlexander Shishkin 	}
1197bd1d409SAlexander Shishkin 
1207bd1d409SAlexander Shishkin 	return stm;
1217bd1d409SAlexander Shishkin }
1227bd1d409SAlexander Shishkin 
1237bd1d409SAlexander Shishkin /**
1247bd1d409SAlexander Shishkin  * stm_put_device() - drop references on the stm device
1257bd1d409SAlexander Shishkin  * @stm:	stm device, previously acquired by stm_find_device()
1267bd1d409SAlexander Shishkin  *
1277bd1d409SAlexander Shishkin  * This drops the module reference and device reference taken by
1287bd1d409SAlexander Shishkin  * stm_find_device().
1297bd1d409SAlexander Shishkin  */
1307bd1d409SAlexander Shishkin void stm_put_device(struct stm_device *stm)
1317bd1d409SAlexander Shishkin {
1327bd1d409SAlexander Shishkin 	module_put(stm->owner);
1337bd1d409SAlexander Shishkin 	put_device(&stm->dev);
1347bd1d409SAlexander Shishkin }
1357bd1d409SAlexander Shishkin 
1367bd1d409SAlexander Shishkin /*
1377bd1d409SAlexander Shishkin  * Internally we only care about software-writable masters here, that is the
1387bd1d409SAlexander Shishkin  * ones in the range [stm_data->sw_start..stm_data..sw_end], however we need
1397bd1d409SAlexander Shishkin  * original master numbers to be visible externally, since they are the ones
1407bd1d409SAlexander Shishkin  * that will appear in the STP stream. Thus, the internal bookkeeping uses
1417bd1d409SAlexander Shishkin  * $master - stm_data->sw_start to reference master descriptors and such.
1427bd1d409SAlexander Shishkin  */
1437bd1d409SAlexander Shishkin 
1447bd1d409SAlexander Shishkin #define __stm_master(_s, _m)				\
1457bd1d409SAlexander Shishkin 	((_s)->masters[(_m) - (_s)->data->sw_start])
1467bd1d409SAlexander Shishkin 
1477bd1d409SAlexander Shishkin static inline struct stp_master *
1487bd1d409SAlexander Shishkin stm_master(struct stm_device *stm, unsigned int idx)
1497bd1d409SAlexander Shishkin {
1507bd1d409SAlexander Shishkin 	if (idx < stm->data->sw_start || idx > stm->data->sw_end)
1517bd1d409SAlexander Shishkin 		return NULL;
1527bd1d409SAlexander Shishkin 
1537bd1d409SAlexander Shishkin 	return __stm_master(stm, idx);
1547bd1d409SAlexander Shishkin }
1557bd1d409SAlexander Shishkin 
1567bd1d409SAlexander Shishkin static int stp_master_alloc(struct stm_device *stm, unsigned int idx)
1577bd1d409SAlexander Shishkin {
1587bd1d409SAlexander Shishkin 	struct stp_master *master;
1597bd1d409SAlexander Shishkin 	size_t size;
1607bd1d409SAlexander Shishkin 
1617bd1d409SAlexander Shishkin 	size = ALIGN(stm->data->sw_nchannels, 8) / 8;
1627bd1d409SAlexander Shishkin 	size += sizeof(struct stp_master);
1637bd1d409SAlexander Shishkin 	master = kzalloc(size, GFP_ATOMIC);
1647bd1d409SAlexander Shishkin 	if (!master)
1657bd1d409SAlexander Shishkin 		return -ENOMEM;
1667bd1d409SAlexander Shishkin 
1677bd1d409SAlexander Shishkin 	master->nr_free = stm->data->sw_nchannels;
1687bd1d409SAlexander Shishkin 	__stm_master(stm, idx) = master;
1697bd1d409SAlexander Shishkin 
1707bd1d409SAlexander Shishkin 	return 0;
1717bd1d409SAlexander Shishkin }
1727bd1d409SAlexander Shishkin 
1737bd1d409SAlexander Shishkin static void stp_master_free(struct stm_device *stm, unsigned int idx)
1747bd1d409SAlexander Shishkin {
1757bd1d409SAlexander Shishkin 	struct stp_master *master = stm_master(stm, idx);
1767bd1d409SAlexander Shishkin 
1777bd1d409SAlexander Shishkin 	if (!master)
1787bd1d409SAlexander Shishkin 		return;
1797bd1d409SAlexander Shishkin 
1807bd1d409SAlexander Shishkin 	__stm_master(stm, idx) = NULL;
1817bd1d409SAlexander Shishkin 	kfree(master);
1827bd1d409SAlexander Shishkin }
1837bd1d409SAlexander Shishkin 
1847bd1d409SAlexander Shishkin static void stm_output_claim(struct stm_device *stm, struct stm_output *output)
1857bd1d409SAlexander Shishkin {
1867bd1d409SAlexander Shishkin 	struct stp_master *master = stm_master(stm, output->master);
1877bd1d409SAlexander Shishkin 
1887bd1d409SAlexander Shishkin 	if (WARN_ON_ONCE(master->nr_free < output->nr_chans))
1897bd1d409SAlexander Shishkin 		return;
1907bd1d409SAlexander Shishkin 
1917bd1d409SAlexander Shishkin 	bitmap_allocate_region(&master->chan_map[0], output->channel,
1927bd1d409SAlexander Shishkin 			       ilog2(output->nr_chans));
1937bd1d409SAlexander Shishkin 
1947bd1d409SAlexander Shishkin 	master->nr_free -= output->nr_chans;
1957bd1d409SAlexander Shishkin }
1967bd1d409SAlexander Shishkin 
1977bd1d409SAlexander Shishkin static void
1987bd1d409SAlexander Shishkin stm_output_disclaim(struct stm_device *stm, struct stm_output *output)
1997bd1d409SAlexander Shishkin {
2007bd1d409SAlexander Shishkin 	struct stp_master *master = stm_master(stm, output->master);
2017bd1d409SAlexander Shishkin 
2027bd1d409SAlexander Shishkin 	bitmap_release_region(&master->chan_map[0], output->channel,
2037bd1d409SAlexander Shishkin 			      ilog2(output->nr_chans));
2047bd1d409SAlexander Shishkin 
2057bd1d409SAlexander Shishkin 	output->nr_chans = 0;
2067bd1d409SAlexander Shishkin 	master->nr_free += output->nr_chans;
2077bd1d409SAlexander Shishkin }
2087bd1d409SAlexander Shishkin 
2097bd1d409SAlexander Shishkin /*
2107bd1d409SAlexander Shishkin  * This is like bitmap_find_free_region(), except it can ignore @start bits
2117bd1d409SAlexander Shishkin  * at the beginning.
2127bd1d409SAlexander Shishkin  */
2137bd1d409SAlexander Shishkin static int find_free_channels(unsigned long *bitmap, unsigned int start,
2147bd1d409SAlexander Shishkin 			      unsigned int end, unsigned int width)
2157bd1d409SAlexander Shishkin {
2167bd1d409SAlexander Shishkin 	unsigned int pos;
2177bd1d409SAlexander Shishkin 	int i;
2187bd1d409SAlexander Shishkin 
2197bd1d409SAlexander Shishkin 	for (pos = start; pos < end + 1; pos = ALIGN(pos, width)) {
2207bd1d409SAlexander Shishkin 		pos = find_next_zero_bit(bitmap, end + 1, pos);
2217bd1d409SAlexander Shishkin 		if (pos + width > end + 1)
2227bd1d409SAlexander Shishkin 			break;
2237bd1d409SAlexander Shishkin 
2247bd1d409SAlexander Shishkin 		if (pos & (width - 1))
2257bd1d409SAlexander Shishkin 			continue;
2267bd1d409SAlexander Shishkin 
2277bd1d409SAlexander Shishkin 		for (i = 1; i < width && !test_bit(pos + i, bitmap); i++)
2287bd1d409SAlexander Shishkin 			;
2297bd1d409SAlexander Shishkin 		if (i == width)
2307bd1d409SAlexander Shishkin 			return pos;
2317bd1d409SAlexander Shishkin 	}
2327bd1d409SAlexander Shishkin 
2337bd1d409SAlexander Shishkin 	return -1;
2347bd1d409SAlexander Shishkin }
2357bd1d409SAlexander Shishkin 
2367bd1d409SAlexander Shishkin static unsigned int
2377bd1d409SAlexander Shishkin stm_find_master_chan(struct stm_device *stm, unsigned int width,
2387bd1d409SAlexander Shishkin 		     unsigned int *mstart, unsigned int mend,
2397bd1d409SAlexander Shishkin 		     unsigned int *cstart, unsigned int cend)
2407bd1d409SAlexander Shishkin {
2417bd1d409SAlexander Shishkin 	struct stp_master *master;
2427bd1d409SAlexander Shishkin 	unsigned int midx;
2437bd1d409SAlexander Shishkin 	int pos, err;
2447bd1d409SAlexander Shishkin 
2457bd1d409SAlexander Shishkin 	for (midx = *mstart; midx <= mend; midx++) {
2467bd1d409SAlexander Shishkin 		if (!stm_master(stm, midx)) {
2477bd1d409SAlexander Shishkin 			err = stp_master_alloc(stm, midx);
2487bd1d409SAlexander Shishkin 			if (err)
2497bd1d409SAlexander Shishkin 				return err;
2507bd1d409SAlexander Shishkin 		}
2517bd1d409SAlexander Shishkin 
2527bd1d409SAlexander Shishkin 		master = stm_master(stm, midx);
2537bd1d409SAlexander Shishkin 
2547bd1d409SAlexander Shishkin 		if (!master->nr_free)
2557bd1d409SAlexander Shishkin 			continue;
2567bd1d409SAlexander Shishkin 
2577bd1d409SAlexander Shishkin 		pos = find_free_channels(master->chan_map, *cstart, cend,
2587bd1d409SAlexander Shishkin 					 width);
2597bd1d409SAlexander Shishkin 		if (pos < 0)
2607bd1d409SAlexander Shishkin 			continue;
2617bd1d409SAlexander Shishkin 
2627bd1d409SAlexander Shishkin 		*mstart = midx;
2637bd1d409SAlexander Shishkin 		*cstart = pos;
2647bd1d409SAlexander Shishkin 		return 0;
2657bd1d409SAlexander Shishkin 	}
2667bd1d409SAlexander Shishkin 
2677bd1d409SAlexander Shishkin 	return -ENOSPC;
2687bd1d409SAlexander Shishkin }
2697bd1d409SAlexander Shishkin 
2707bd1d409SAlexander Shishkin static int stm_output_assign(struct stm_device *stm, unsigned int width,
2717bd1d409SAlexander Shishkin 			     struct stp_policy_node *policy_node,
2727bd1d409SAlexander Shishkin 			     struct stm_output *output)
2737bd1d409SAlexander Shishkin {
2747bd1d409SAlexander Shishkin 	unsigned int midx, cidx, mend, cend;
2757bd1d409SAlexander Shishkin 	int ret = -EINVAL;
2767bd1d409SAlexander Shishkin 
2777bd1d409SAlexander Shishkin 	if (width > stm->data->sw_nchannels)
2787bd1d409SAlexander Shishkin 		return -EINVAL;
2797bd1d409SAlexander Shishkin 
2807bd1d409SAlexander Shishkin 	if (policy_node) {
2817bd1d409SAlexander Shishkin 		stp_policy_node_get_ranges(policy_node,
2827bd1d409SAlexander Shishkin 					   &midx, &mend, &cidx, &cend);
2837bd1d409SAlexander Shishkin 	} else {
2847bd1d409SAlexander Shishkin 		midx = stm->data->sw_start;
2857bd1d409SAlexander Shishkin 		cidx = 0;
2867bd1d409SAlexander Shishkin 		mend = stm->data->sw_end;
2877bd1d409SAlexander Shishkin 		cend = stm->data->sw_nchannels - 1;
2887bd1d409SAlexander Shishkin 	}
2897bd1d409SAlexander Shishkin 
2907bd1d409SAlexander Shishkin 	spin_lock(&stm->mc_lock);
2917bd1d409SAlexander Shishkin 	/* output is already assigned -- shouldn't happen */
2927bd1d409SAlexander Shishkin 	if (WARN_ON_ONCE(output->nr_chans))
2937bd1d409SAlexander Shishkin 		goto unlock;
2947bd1d409SAlexander Shishkin 
2957bd1d409SAlexander Shishkin 	ret = stm_find_master_chan(stm, width, &midx, mend, &cidx, cend);
2967bd1d409SAlexander Shishkin 	if (ret)
2977bd1d409SAlexander Shishkin 		goto unlock;
2987bd1d409SAlexander Shishkin 
2997bd1d409SAlexander Shishkin 	output->master = midx;
3007bd1d409SAlexander Shishkin 	output->channel = cidx;
3017bd1d409SAlexander Shishkin 	output->nr_chans = width;
3027bd1d409SAlexander Shishkin 	stm_output_claim(stm, output);
3037bd1d409SAlexander Shishkin 	dev_dbg(&stm->dev, "assigned %u:%u (+%u)\n", midx, cidx, width);
3047bd1d409SAlexander Shishkin 
3057bd1d409SAlexander Shishkin 	ret = 0;
3067bd1d409SAlexander Shishkin unlock:
3077bd1d409SAlexander Shishkin 	spin_unlock(&stm->mc_lock);
3087bd1d409SAlexander Shishkin 
3097bd1d409SAlexander Shishkin 	return ret;
3107bd1d409SAlexander Shishkin }
3117bd1d409SAlexander Shishkin 
3127bd1d409SAlexander Shishkin static void stm_output_free(struct stm_device *stm, struct stm_output *output)
3137bd1d409SAlexander Shishkin {
3147bd1d409SAlexander Shishkin 	spin_lock(&stm->mc_lock);
3157bd1d409SAlexander Shishkin 	if (output->nr_chans)
3167bd1d409SAlexander Shishkin 		stm_output_disclaim(stm, output);
3177bd1d409SAlexander Shishkin 	spin_unlock(&stm->mc_lock);
3187bd1d409SAlexander Shishkin }
3197bd1d409SAlexander Shishkin 
3207bd1d409SAlexander Shishkin static int major_match(struct device *dev, const void *data)
3217bd1d409SAlexander Shishkin {
3227bd1d409SAlexander Shishkin 	unsigned int major = *(unsigned int *)data;
3237bd1d409SAlexander Shishkin 
3247bd1d409SAlexander Shishkin 	return MAJOR(dev->devt) == major;
3257bd1d409SAlexander Shishkin }
3267bd1d409SAlexander Shishkin 
3277bd1d409SAlexander Shishkin static int stm_char_open(struct inode *inode, struct file *file)
3287bd1d409SAlexander Shishkin {
3297bd1d409SAlexander Shishkin 	struct stm_file *stmf;
3307bd1d409SAlexander Shishkin 	struct device *dev;
3317bd1d409SAlexander Shishkin 	unsigned int major = imajor(inode);
3327bd1d409SAlexander Shishkin 	int err = -ENODEV;
3337bd1d409SAlexander Shishkin 
3347bd1d409SAlexander Shishkin 	dev = class_find_device(&stm_class, NULL, &major, major_match);
3357bd1d409SAlexander Shishkin 	if (!dev)
3367bd1d409SAlexander Shishkin 		return -ENODEV;
3377bd1d409SAlexander Shishkin 
3387bd1d409SAlexander Shishkin 	stmf = kzalloc(sizeof(*stmf), GFP_KERNEL);
3397bd1d409SAlexander Shishkin 	if (!stmf)
3407bd1d409SAlexander Shishkin 		return -ENOMEM;
3417bd1d409SAlexander Shishkin 
3427bd1d409SAlexander Shishkin 	stmf->stm = to_stm_device(dev);
3437bd1d409SAlexander Shishkin 
3447bd1d409SAlexander Shishkin 	if (!try_module_get(stmf->stm->owner))
3457bd1d409SAlexander Shishkin 		goto err_free;
3467bd1d409SAlexander Shishkin 
3477bd1d409SAlexander Shishkin 	file->private_data = stmf;
3487bd1d409SAlexander Shishkin 
3497bd1d409SAlexander Shishkin 	return nonseekable_open(inode, file);
3507bd1d409SAlexander Shishkin 
3517bd1d409SAlexander Shishkin err_free:
3527bd1d409SAlexander Shishkin 	kfree(stmf);
3537bd1d409SAlexander Shishkin 
3547bd1d409SAlexander Shishkin 	return err;
3557bd1d409SAlexander Shishkin }
3567bd1d409SAlexander Shishkin 
3577bd1d409SAlexander Shishkin static int stm_char_release(struct inode *inode, struct file *file)
3587bd1d409SAlexander Shishkin {
3597bd1d409SAlexander Shishkin 	struct stm_file *stmf = file->private_data;
3607bd1d409SAlexander Shishkin 
3617bd1d409SAlexander Shishkin 	stm_output_free(stmf->stm, &stmf->output);
3627bd1d409SAlexander Shishkin 	stm_put_device(stmf->stm);
3637bd1d409SAlexander Shishkin 	kfree(stmf);
3647bd1d409SAlexander Shishkin 
3657bd1d409SAlexander Shishkin 	return 0;
3667bd1d409SAlexander Shishkin }
3677bd1d409SAlexander Shishkin 
3687bd1d409SAlexander Shishkin static int stm_file_assign(struct stm_file *stmf, char *id, unsigned int width)
3697bd1d409SAlexander Shishkin {
3707bd1d409SAlexander Shishkin 	struct stm_device *stm = stmf->stm;
3717bd1d409SAlexander Shishkin 	int ret;
3727bd1d409SAlexander Shishkin 
3737bd1d409SAlexander Shishkin 	stmf->policy_node = stp_policy_node_lookup(stm, id);
3747bd1d409SAlexander Shishkin 
3757bd1d409SAlexander Shishkin 	ret = stm_output_assign(stm, width, stmf->policy_node, &stmf->output);
3767bd1d409SAlexander Shishkin 
3777bd1d409SAlexander Shishkin 	if (stmf->policy_node)
3787bd1d409SAlexander Shishkin 		stp_policy_node_put(stmf->policy_node);
3797bd1d409SAlexander Shishkin 
3807bd1d409SAlexander Shishkin 	return ret;
3817bd1d409SAlexander Shishkin }
3827bd1d409SAlexander Shishkin 
3837bd1d409SAlexander Shishkin static void stm_write(struct stm_data *data, unsigned int master,
3847bd1d409SAlexander Shishkin 		      unsigned int channel, const char *buf, size_t count)
3857bd1d409SAlexander Shishkin {
3867bd1d409SAlexander Shishkin 	unsigned int flags = STP_PACKET_TIMESTAMPED;
3877bd1d409SAlexander Shishkin 	const unsigned char *p = buf, nil = 0;
3887bd1d409SAlexander Shishkin 	size_t pos;
3897bd1d409SAlexander Shishkin 	ssize_t sz;
3907bd1d409SAlexander Shishkin 
3917bd1d409SAlexander Shishkin 	for (pos = 0, p = buf; count > pos; pos += sz, p += sz) {
3927bd1d409SAlexander Shishkin 		sz = min_t(unsigned int, count - pos, 8);
3937bd1d409SAlexander Shishkin 		sz = data->packet(data, master, channel, STP_PACKET_DATA, flags,
3947bd1d409SAlexander Shishkin 				  sz, p);
3957bd1d409SAlexander Shishkin 		flags = 0;
3967bd1d409SAlexander Shishkin 	}
3977bd1d409SAlexander Shishkin 
3987bd1d409SAlexander Shishkin 	data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil);
3997bd1d409SAlexander Shishkin }
4007bd1d409SAlexander Shishkin 
4017bd1d409SAlexander Shishkin static ssize_t stm_char_write(struct file *file, const char __user *buf,
4027bd1d409SAlexander Shishkin 			      size_t count, loff_t *ppos)
4037bd1d409SAlexander Shishkin {
4047bd1d409SAlexander Shishkin 	struct stm_file *stmf = file->private_data;
4057bd1d409SAlexander Shishkin 	struct stm_device *stm = stmf->stm;
4067bd1d409SAlexander Shishkin 	char *kbuf;
4077bd1d409SAlexander Shishkin 	int err;
4087bd1d409SAlexander Shishkin 
4097bd1d409SAlexander Shishkin 	/*
4107bd1d409SAlexander Shishkin 	 * if no m/c have been assigned to this writer up to this
4117bd1d409SAlexander Shishkin 	 * point, use "default" policy entry
4127bd1d409SAlexander Shishkin 	 */
4137bd1d409SAlexander Shishkin 	if (!stmf->output.nr_chans) {
4147bd1d409SAlexander Shishkin 		err = stm_file_assign(stmf, "default", 1);
4157bd1d409SAlexander Shishkin 		/*
4167bd1d409SAlexander Shishkin 		 * EBUSY means that somebody else just assigned this
4177bd1d409SAlexander Shishkin 		 * output, which is just fine for write()
4187bd1d409SAlexander Shishkin 		 */
4197bd1d409SAlexander Shishkin 		if (err && err != -EBUSY)
4207bd1d409SAlexander Shishkin 			return err;
4217bd1d409SAlexander Shishkin 	}
4227bd1d409SAlexander Shishkin 
4237bd1d409SAlexander Shishkin 	kbuf = kmalloc(count + 1, GFP_KERNEL);
4247bd1d409SAlexander Shishkin 	if (!kbuf)
4257bd1d409SAlexander Shishkin 		return -ENOMEM;
4267bd1d409SAlexander Shishkin 
4277bd1d409SAlexander Shishkin 	err = copy_from_user(kbuf, buf, count);
4287bd1d409SAlexander Shishkin 	if (err) {
4297bd1d409SAlexander Shishkin 		kfree(kbuf);
4307bd1d409SAlexander Shishkin 		return -EFAULT;
4317bd1d409SAlexander Shishkin 	}
4327bd1d409SAlexander Shishkin 
4337bd1d409SAlexander Shishkin 	stm_write(stm->data, stmf->output.master, stmf->output.channel, kbuf,
4347bd1d409SAlexander Shishkin 		  count);
4357bd1d409SAlexander Shishkin 
4367bd1d409SAlexander Shishkin 	kfree(kbuf);
4377bd1d409SAlexander Shishkin 
4387bd1d409SAlexander Shishkin 	return count;
4397bd1d409SAlexander Shishkin }
4407bd1d409SAlexander Shishkin 
4417bd1d409SAlexander Shishkin static int stm_char_mmap(struct file *file, struct vm_area_struct *vma)
4427bd1d409SAlexander Shishkin {
4437bd1d409SAlexander Shishkin 	struct stm_file *stmf = file->private_data;
4447bd1d409SAlexander Shishkin 	struct stm_device *stm = stmf->stm;
4457bd1d409SAlexander Shishkin 	unsigned long size, phys;
4467bd1d409SAlexander Shishkin 
4477bd1d409SAlexander Shishkin 	if (!stm->data->mmio_addr)
4487bd1d409SAlexander Shishkin 		return -EOPNOTSUPP;
4497bd1d409SAlexander Shishkin 
4507bd1d409SAlexander Shishkin 	if (vma->vm_pgoff)
4517bd1d409SAlexander Shishkin 		return -EINVAL;
4527bd1d409SAlexander Shishkin 
4537bd1d409SAlexander Shishkin 	size = vma->vm_end - vma->vm_start;
4547bd1d409SAlexander Shishkin 
4557bd1d409SAlexander Shishkin 	if (stmf->output.nr_chans * stm->data->sw_mmiosz != size)
4567bd1d409SAlexander Shishkin 		return -EINVAL;
4577bd1d409SAlexander Shishkin 
4587bd1d409SAlexander Shishkin 	phys = stm->data->mmio_addr(stm->data, stmf->output.master,
4597bd1d409SAlexander Shishkin 				    stmf->output.channel,
4607bd1d409SAlexander Shishkin 				    stmf->output.nr_chans);
4617bd1d409SAlexander Shishkin 
4627bd1d409SAlexander Shishkin 	if (!phys)
4637bd1d409SAlexander Shishkin 		return -EINVAL;
4647bd1d409SAlexander Shishkin 
4657bd1d409SAlexander Shishkin 	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
4667bd1d409SAlexander Shishkin 	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
4677bd1d409SAlexander Shishkin 	vm_iomap_memory(vma, phys, size);
4687bd1d409SAlexander Shishkin 
4697bd1d409SAlexander Shishkin 	return 0;
4707bd1d409SAlexander Shishkin }
4717bd1d409SAlexander Shishkin 
4727bd1d409SAlexander Shishkin static int stm_char_policy_set_ioctl(struct stm_file *stmf, void __user *arg)
4737bd1d409SAlexander Shishkin {
4747bd1d409SAlexander Shishkin 	struct stm_device *stm = stmf->stm;
4757bd1d409SAlexander Shishkin 	struct stp_policy_id *id;
4767bd1d409SAlexander Shishkin 	int ret = -EINVAL;
4777bd1d409SAlexander Shishkin 	u32 size;
4787bd1d409SAlexander Shishkin 
4797bd1d409SAlexander Shishkin 	if (stmf->output.nr_chans)
4807bd1d409SAlexander Shishkin 		return -EBUSY;
4817bd1d409SAlexander Shishkin 
4827bd1d409SAlexander Shishkin 	if (copy_from_user(&size, arg, sizeof(size)))
4837bd1d409SAlexander Shishkin 		return -EFAULT;
4847bd1d409SAlexander Shishkin 
4857bd1d409SAlexander Shishkin 	if (size >= PATH_MAX + sizeof(*id))
4867bd1d409SAlexander Shishkin 		return -EINVAL;
4877bd1d409SAlexander Shishkin 
4887bd1d409SAlexander Shishkin 	/*
4897bd1d409SAlexander Shishkin 	 * size + 1 to make sure the .id string at the bottom is terminated,
4907bd1d409SAlexander Shishkin 	 * which is also why memdup_user() is not useful here
4917bd1d409SAlexander Shishkin 	 */
4927bd1d409SAlexander Shishkin 	id = kzalloc(size + 1, GFP_KERNEL);
4937bd1d409SAlexander Shishkin 	if (!id)
4947bd1d409SAlexander Shishkin 		return -ENOMEM;
4957bd1d409SAlexander Shishkin 
4967bd1d409SAlexander Shishkin 	if (copy_from_user(id, arg, size)) {
4977bd1d409SAlexander Shishkin 		ret = -EFAULT;
4987bd1d409SAlexander Shishkin 		goto err_free;
4997bd1d409SAlexander Shishkin 	}
5007bd1d409SAlexander Shishkin 
5017bd1d409SAlexander Shishkin 	if (id->__reserved_0 || id->__reserved_1)
5027bd1d409SAlexander Shishkin 		goto err_free;
5037bd1d409SAlexander Shishkin 
5047bd1d409SAlexander Shishkin 	if (id->width < 1 ||
5057bd1d409SAlexander Shishkin 	    id->width > PAGE_SIZE / stm->data->sw_mmiosz)
5067bd1d409SAlexander Shishkin 		goto err_free;
5077bd1d409SAlexander Shishkin 
5087bd1d409SAlexander Shishkin 	ret = stm_file_assign(stmf, id->id, id->width);
5097bd1d409SAlexander Shishkin 	if (ret)
5107bd1d409SAlexander Shishkin 		goto err_free;
5117bd1d409SAlexander Shishkin 
5127bd1d409SAlexander Shishkin 	ret = 0;
5137bd1d409SAlexander Shishkin 
5147bd1d409SAlexander Shishkin 	if (stm->data->link)
5157bd1d409SAlexander Shishkin 		ret = stm->data->link(stm->data, stmf->output.master,
5167bd1d409SAlexander Shishkin 				      stmf->output.channel);
5177bd1d409SAlexander Shishkin 
5187bd1d409SAlexander Shishkin 	if (ret) {
5197bd1d409SAlexander Shishkin 		stm_output_free(stmf->stm, &stmf->output);
5207bd1d409SAlexander Shishkin 		stm_put_device(stmf->stm);
5217bd1d409SAlexander Shishkin 	}
5227bd1d409SAlexander Shishkin 
5237bd1d409SAlexander Shishkin err_free:
5247bd1d409SAlexander Shishkin 	kfree(id);
5257bd1d409SAlexander Shishkin 
5267bd1d409SAlexander Shishkin 	return ret;
5277bd1d409SAlexander Shishkin }
5287bd1d409SAlexander Shishkin 
5297bd1d409SAlexander Shishkin static int stm_char_policy_get_ioctl(struct stm_file *stmf, void __user *arg)
5307bd1d409SAlexander Shishkin {
5317bd1d409SAlexander Shishkin 	struct stp_policy_id id = {
5327bd1d409SAlexander Shishkin 		.size		= sizeof(id),
5337bd1d409SAlexander Shishkin 		.master		= stmf->output.master,
5347bd1d409SAlexander Shishkin 		.channel	= stmf->output.channel,
5357bd1d409SAlexander Shishkin 		.width		= stmf->output.nr_chans,
5367bd1d409SAlexander Shishkin 		.__reserved_0	= 0,
5377bd1d409SAlexander Shishkin 		.__reserved_1	= 0,
5387bd1d409SAlexander Shishkin 	};
5397bd1d409SAlexander Shishkin 
5407bd1d409SAlexander Shishkin 	return copy_to_user(arg, &id, id.size) ? -EFAULT : 0;
5417bd1d409SAlexander Shishkin }
5427bd1d409SAlexander Shishkin 
5437bd1d409SAlexander Shishkin static long
5447bd1d409SAlexander Shishkin stm_char_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
5457bd1d409SAlexander Shishkin {
5467bd1d409SAlexander Shishkin 	struct stm_file *stmf = file->private_data;
5477bd1d409SAlexander Shishkin 	struct stm_data *stm_data = stmf->stm->data;
5487bd1d409SAlexander Shishkin 	int err = -ENOTTY;
5497bd1d409SAlexander Shishkin 	u64 options;
5507bd1d409SAlexander Shishkin 
5517bd1d409SAlexander Shishkin 	switch (cmd) {
5527bd1d409SAlexander Shishkin 	case STP_POLICY_ID_SET:
5537bd1d409SAlexander Shishkin 		err = stm_char_policy_set_ioctl(stmf, (void __user *)arg);
5547bd1d409SAlexander Shishkin 		if (err)
5557bd1d409SAlexander Shishkin 			return err;
5567bd1d409SAlexander Shishkin 
5577bd1d409SAlexander Shishkin 		return stm_char_policy_get_ioctl(stmf, (void __user *)arg);
5587bd1d409SAlexander Shishkin 
5597bd1d409SAlexander Shishkin 	case STP_POLICY_ID_GET:
5607bd1d409SAlexander Shishkin 		return stm_char_policy_get_ioctl(stmf, (void __user *)arg);
5617bd1d409SAlexander Shishkin 
5627bd1d409SAlexander Shishkin 	case STP_SET_OPTIONS:
5637bd1d409SAlexander Shishkin 		if (copy_from_user(&options, (u64 __user *)arg, sizeof(u64)))
5647bd1d409SAlexander Shishkin 			return -EFAULT;
5657bd1d409SAlexander Shishkin 
5667bd1d409SAlexander Shishkin 		if (stm_data->set_options)
5677bd1d409SAlexander Shishkin 			err = stm_data->set_options(stm_data,
5687bd1d409SAlexander Shishkin 						    stmf->output.master,
5697bd1d409SAlexander Shishkin 						    stmf->output.channel,
5707bd1d409SAlexander Shishkin 						    stmf->output.nr_chans,
5717bd1d409SAlexander Shishkin 						    options);
5727bd1d409SAlexander Shishkin 
5737bd1d409SAlexander Shishkin 		break;
5747bd1d409SAlexander Shishkin 	default:
5757bd1d409SAlexander Shishkin 		break;
5767bd1d409SAlexander Shishkin 	}
5777bd1d409SAlexander Shishkin 
5787bd1d409SAlexander Shishkin 	return err;
5797bd1d409SAlexander Shishkin }
5807bd1d409SAlexander Shishkin 
5817bd1d409SAlexander Shishkin #ifdef CONFIG_COMPAT
5827bd1d409SAlexander Shishkin static long
5837bd1d409SAlexander Shishkin stm_char_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
5847bd1d409SAlexander Shishkin {
5857bd1d409SAlexander Shishkin 	return stm_char_ioctl(file, cmd, (unsigned long)compat_ptr(arg));
5867bd1d409SAlexander Shishkin }
5877bd1d409SAlexander Shishkin #else
5887bd1d409SAlexander Shishkin #define stm_char_compat_ioctl	NULL
5897bd1d409SAlexander Shishkin #endif
5907bd1d409SAlexander Shishkin 
5917bd1d409SAlexander Shishkin static const struct file_operations stm_fops = {
5927bd1d409SAlexander Shishkin 	.open		= stm_char_open,
5937bd1d409SAlexander Shishkin 	.release	= stm_char_release,
5947bd1d409SAlexander Shishkin 	.write		= stm_char_write,
5957bd1d409SAlexander Shishkin 	.mmap		= stm_char_mmap,
5967bd1d409SAlexander Shishkin 	.unlocked_ioctl	= stm_char_ioctl,
5977bd1d409SAlexander Shishkin 	.compat_ioctl	= stm_char_compat_ioctl,
5987bd1d409SAlexander Shishkin 	.llseek		= no_llseek,
5997bd1d409SAlexander Shishkin };
6007bd1d409SAlexander Shishkin 
6017bd1d409SAlexander Shishkin static void stm_device_release(struct device *dev)
6027bd1d409SAlexander Shishkin {
6037bd1d409SAlexander Shishkin 	struct stm_device *stm = to_stm_device(dev);
6047bd1d409SAlexander Shishkin 
6057bd1d409SAlexander Shishkin 	kfree(stm);
6067bd1d409SAlexander Shishkin }
6077bd1d409SAlexander Shishkin 
6087bd1d409SAlexander Shishkin int stm_register_device(struct device *parent, struct stm_data *stm_data,
6097bd1d409SAlexander Shishkin 			struct module *owner)
6107bd1d409SAlexander Shishkin {
6117bd1d409SAlexander Shishkin 	struct stm_device *stm;
6127bd1d409SAlexander Shishkin 	unsigned int nmasters;
6137bd1d409SAlexander Shishkin 	int err = -ENOMEM;
6147bd1d409SAlexander Shishkin 
6157bd1d409SAlexander Shishkin 	if (!stm_core_up)
6167bd1d409SAlexander Shishkin 		return -EPROBE_DEFER;
6177bd1d409SAlexander Shishkin 
6187bd1d409SAlexander Shishkin 	if (!stm_data->packet || !stm_data->sw_nchannels)
6197bd1d409SAlexander Shishkin 		return -EINVAL;
6207bd1d409SAlexander Shishkin 
6217b3bb0e7SChunyan Zhang 	nmasters = stm_data->sw_end - stm_data->sw_start + 1;
6227bd1d409SAlexander Shishkin 	stm = kzalloc(sizeof(*stm) + nmasters * sizeof(void *), GFP_KERNEL);
6237bd1d409SAlexander Shishkin 	if (!stm)
6247bd1d409SAlexander Shishkin 		return -ENOMEM;
6257bd1d409SAlexander Shishkin 
6267bd1d409SAlexander Shishkin 	stm->major = register_chrdev(0, stm_data->name, &stm_fops);
6277bd1d409SAlexander Shishkin 	if (stm->major < 0)
6287bd1d409SAlexander Shishkin 		goto err_free;
6297bd1d409SAlexander Shishkin 
6307bd1d409SAlexander Shishkin 	device_initialize(&stm->dev);
6317bd1d409SAlexander Shishkin 	stm->dev.devt = MKDEV(stm->major, 0);
6327bd1d409SAlexander Shishkin 	stm->dev.class = &stm_class;
6337bd1d409SAlexander Shishkin 	stm->dev.parent = parent;
6347bd1d409SAlexander Shishkin 	stm->dev.release = stm_device_release;
6357bd1d409SAlexander Shishkin 
6367bd1d409SAlexander Shishkin 	err = kobject_set_name(&stm->dev.kobj, "%s", stm_data->name);
6377bd1d409SAlexander Shishkin 	if (err)
6387bd1d409SAlexander Shishkin 		goto err_device;
6397bd1d409SAlexander Shishkin 
6407bd1d409SAlexander Shishkin 	err = device_add(&stm->dev);
6417bd1d409SAlexander Shishkin 	if (err)
6427bd1d409SAlexander Shishkin 		goto err_device;
6437bd1d409SAlexander Shishkin 
644c74f7e82SAlexander Shishkin 	mutex_init(&stm->link_mutex);
6457bd1d409SAlexander Shishkin 	spin_lock_init(&stm->link_lock);
6467bd1d409SAlexander Shishkin 	INIT_LIST_HEAD(&stm->link_list);
6477bd1d409SAlexander Shishkin 
6487bd1d409SAlexander Shishkin 	spin_lock_init(&stm->mc_lock);
6497bd1d409SAlexander Shishkin 	mutex_init(&stm->policy_mutex);
6507bd1d409SAlexander Shishkin 	stm->sw_nmasters = nmasters;
6517bd1d409SAlexander Shishkin 	stm->owner = owner;
6527bd1d409SAlexander Shishkin 	stm->data = stm_data;
6537bd1d409SAlexander Shishkin 	stm_data->stm = stm;
6547bd1d409SAlexander Shishkin 
6557bd1d409SAlexander Shishkin 	return 0;
6567bd1d409SAlexander Shishkin 
6577bd1d409SAlexander Shishkin err_device:
6587bd1d409SAlexander Shishkin 	put_device(&stm->dev);
6597bd1d409SAlexander Shishkin err_free:
6607bd1d409SAlexander Shishkin 	kfree(stm);
6617bd1d409SAlexander Shishkin 
6627bd1d409SAlexander Shishkin 	return err;
6637bd1d409SAlexander Shishkin }
6647bd1d409SAlexander Shishkin EXPORT_SYMBOL_GPL(stm_register_device);
6657bd1d409SAlexander Shishkin 
6667bd1d409SAlexander Shishkin static void __stm_source_link_drop(struct stm_source_device *src,
6677bd1d409SAlexander Shishkin 				   struct stm_device *stm);
6687bd1d409SAlexander Shishkin 
6697bd1d409SAlexander Shishkin void stm_unregister_device(struct stm_data *stm_data)
6707bd1d409SAlexander Shishkin {
6717bd1d409SAlexander Shishkin 	struct stm_device *stm = stm_data->stm;
6727bd1d409SAlexander Shishkin 	struct stm_source_device *src, *iter;
6737bd1d409SAlexander Shishkin 	int i;
6747bd1d409SAlexander Shishkin 
675c74f7e82SAlexander Shishkin 	mutex_lock(&stm->link_mutex);
6767bd1d409SAlexander Shishkin 	list_for_each_entry_safe(src, iter, &stm->link_list, link_entry) {
6777bd1d409SAlexander Shishkin 		__stm_source_link_drop(src, stm);
6787bd1d409SAlexander Shishkin 	}
679c74f7e82SAlexander Shishkin 	mutex_unlock(&stm->link_mutex);
6807bd1d409SAlexander Shishkin 
6817bd1d409SAlexander Shishkin 	synchronize_srcu(&stm_source_srcu);
6827bd1d409SAlexander Shishkin 
6837bd1d409SAlexander Shishkin 	unregister_chrdev(stm->major, stm_data->name);
6847bd1d409SAlexander Shishkin 
6857bd1d409SAlexander Shishkin 	mutex_lock(&stm->policy_mutex);
6867bd1d409SAlexander Shishkin 	if (stm->policy)
6877bd1d409SAlexander Shishkin 		stp_policy_unbind(stm->policy);
6887bd1d409SAlexander Shishkin 	mutex_unlock(&stm->policy_mutex);
6897bd1d409SAlexander Shishkin 
6907bd1d409SAlexander Shishkin 	for (i = 0; i < stm->sw_nmasters; i++)
6917bd1d409SAlexander Shishkin 		stp_master_free(stm, i);
6927bd1d409SAlexander Shishkin 
6937bd1d409SAlexander Shishkin 	device_unregister(&stm->dev);
6947bd1d409SAlexander Shishkin 	stm_data->stm = NULL;
6957bd1d409SAlexander Shishkin }
6967bd1d409SAlexander Shishkin EXPORT_SYMBOL_GPL(stm_unregister_device);
6977bd1d409SAlexander Shishkin 
698c74f7e82SAlexander Shishkin /*
699c74f7e82SAlexander Shishkin  * stm::link_list access serialization uses a spinlock and a mutex; holding
700c74f7e82SAlexander Shishkin  * either of them guarantees that the list is stable; modification requires
701c74f7e82SAlexander Shishkin  * holding both of them.
702c74f7e82SAlexander Shishkin  *
703c74f7e82SAlexander Shishkin  * Lock ordering is as follows:
704c74f7e82SAlexander Shishkin  *   stm::link_mutex
705c74f7e82SAlexander Shishkin  *     stm::link_lock
706c74f7e82SAlexander Shishkin  *       src::link_lock
707c74f7e82SAlexander Shishkin  */
708c74f7e82SAlexander Shishkin 
7097bd1d409SAlexander Shishkin /**
7107bd1d409SAlexander Shishkin  * stm_source_link_add() - connect an stm_source device to an stm device
7117bd1d409SAlexander Shishkin  * @src:	stm_source device
7127bd1d409SAlexander Shishkin  * @stm:	stm device
7137bd1d409SAlexander Shishkin  *
7147bd1d409SAlexander Shishkin  * This function establishes a link from stm_source to an stm device so that
7157bd1d409SAlexander Shishkin  * the former can send out trace data to the latter.
7167bd1d409SAlexander Shishkin  *
7177bd1d409SAlexander Shishkin  * Return:	0 on success, -errno otherwise.
7187bd1d409SAlexander Shishkin  */
7197bd1d409SAlexander Shishkin static int stm_source_link_add(struct stm_source_device *src,
7207bd1d409SAlexander Shishkin 			       struct stm_device *stm)
7217bd1d409SAlexander Shishkin {
7227bd1d409SAlexander Shishkin 	char *id;
7237bd1d409SAlexander Shishkin 	int err;
7247bd1d409SAlexander Shishkin 
725c74f7e82SAlexander Shishkin 	mutex_lock(&stm->link_mutex);
7267bd1d409SAlexander Shishkin 	spin_lock(&stm->link_lock);
7277bd1d409SAlexander Shishkin 	spin_lock(&src->link_lock);
7287bd1d409SAlexander Shishkin 
7297bd1d409SAlexander Shishkin 	/* src->link is dereferenced under stm_source_srcu but not the list */
7307bd1d409SAlexander Shishkin 	rcu_assign_pointer(src->link, stm);
7317bd1d409SAlexander Shishkin 	list_add_tail(&src->link_entry, &stm->link_list);
7327bd1d409SAlexander Shishkin 
7337bd1d409SAlexander Shishkin 	spin_unlock(&src->link_lock);
7347bd1d409SAlexander Shishkin 	spin_unlock(&stm->link_lock);
735c74f7e82SAlexander Shishkin 	mutex_unlock(&stm->link_mutex);
7367bd1d409SAlexander Shishkin 
7377bd1d409SAlexander Shishkin 	id = kstrdup(src->data->name, GFP_KERNEL);
7387bd1d409SAlexander Shishkin 	if (id) {
7397bd1d409SAlexander Shishkin 		src->policy_node =
7407bd1d409SAlexander Shishkin 			stp_policy_node_lookup(stm, id);
7417bd1d409SAlexander Shishkin 
7427bd1d409SAlexander Shishkin 		kfree(id);
7437bd1d409SAlexander Shishkin 	}
7447bd1d409SAlexander Shishkin 
7457bd1d409SAlexander Shishkin 	err = stm_output_assign(stm, src->data->nr_chans,
7467bd1d409SAlexander Shishkin 				src->policy_node, &src->output);
7477bd1d409SAlexander Shishkin 
7487bd1d409SAlexander Shishkin 	if (src->policy_node)
7497bd1d409SAlexander Shishkin 		stp_policy_node_put(src->policy_node);
7507bd1d409SAlexander Shishkin 
7517bd1d409SAlexander Shishkin 	if (err)
7527bd1d409SAlexander Shishkin 		goto fail_detach;
7537bd1d409SAlexander Shishkin 
7547bd1d409SAlexander Shishkin 	/* this is to notify the STM device that a new link has been made */
7557bd1d409SAlexander Shishkin 	if (stm->data->link)
7567bd1d409SAlexander Shishkin 		err = stm->data->link(stm->data, src->output.master,
7577bd1d409SAlexander Shishkin 				      src->output.channel);
7587bd1d409SAlexander Shishkin 
7597bd1d409SAlexander Shishkin 	if (err)
7607bd1d409SAlexander Shishkin 		goto fail_free_output;
7617bd1d409SAlexander Shishkin 
7627bd1d409SAlexander Shishkin 	/* this is to let the source carry out all necessary preparations */
7637bd1d409SAlexander Shishkin 	if (src->data->link)
7647bd1d409SAlexander Shishkin 		src->data->link(src->data);
7657bd1d409SAlexander Shishkin 
7667bd1d409SAlexander Shishkin 	return 0;
7677bd1d409SAlexander Shishkin 
7687bd1d409SAlexander Shishkin fail_free_output:
7697bd1d409SAlexander Shishkin 	stm_output_free(stm, &src->output);
7707bd1d409SAlexander Shishkin 	stm_put_device(stm);
7717bd1d409SAlexander Shishkin 
7727bd1d409SAlexander Shishkin fail_detach:
773c74f7e82SAlexander Shishkin 	mutex_lock(&stm->link_mutex);
7747bd1d409SAlexander Shishkin 	spin_lock(&stm->link_lock);
7757bd1d409SAlexander Shishkin 	spin_lock(&src->link_lock);
7767bd1d409SAlexander Shishkin 
7777bd1d409SAlexander Shishkin 	rcu_assign_pointer(src->link, NULL);
7787bd1d409SAlexander Shishkin 	list_del_init(&src->link_entry);
7797bd1d409SAlexander Shishkin 
7807bd1d409SAlexander Shishkin 	spin_unlock(&src->link_lock);
7817bd1d409SAlexander Shishkin 	spin_unlock(&stm->link_lock);
782c74f7e82SAlexander Shishkin 	mutex_unlock(&stm->link_mutex);
7837bd1d409SAlexander Shishkin 
7847bd1d409SAlexander Shishkin 	return err;
7857bd1d409SAlexander Shishkin }
7867bd1d409SAlexander Shishkin 
7877bd1d409SAlexander Shishkin /**
7887bd1d409SAlexander Shishkin  * __stm_source_link_drop() - detach stm_source from an stm device
7897bd1d409SAlexander Shishkin  * @src:	stm_source device
7907bd1d409SAlexander Shishkin  * @stm:	stm device
7917bd1d409SAlexander Shishkin  *
7927bd1d409SAlexander Shishkin  * If @stm is @src::link, disconnect them from one another and put the
7937bd1d409SAlexander Shishkin  * reference on the @stm device.
7947bd1d409SAlexander Shishkin  *
795c74f7e82SAlexander Shishkin  * Caller must hold stm::link_mutex.
7967bd1d409SAlexander Shishkin  */
7977bd1d409SAlexander Shishkin static void __stm_source_link_drop(struct stm_source_device *src,
7987bd1d409SAlexander Shishkin 				   struct stm_device *stm)
7997bd1d409SAlexander Shishkin {
8000df771deSAlexander Shishkin 	struct stm_device *link;
8010df771deSAlexander Shishkin 
802c74f7e82SAlexander Shishkin 	lockdep_assert_held(&stm->link_mutex);
803c74f7e82SAlexander Shishkin 
804c74f7e82SAlexander Shishkin 	if (src->data->unlink)
805c74f7e82SAlexander Shishkin 		src->data->unlink(src->data);
806c74f7e82SAlexander Shishkin 
807c74f7e82SAlexander Shishkin 	/* for stm::link_list modification, we hold both mutex and spinlock */
808c74f7e82SAlexander Shishkin 	spin_lock(&stm->link_lock);
8097bd1d409SAlexander Shishkin 	spin_lock(&src->link_lock);
8100df771deSAlexander Shishkin 	link = srcu_dereference_check(src->link, &stm_source_srcu, 1);
8110df771deSAlexander Shishkin 	if (WARN_ON_ONCE(link != stm)) {
8127bd1d409SAlexander Shishkin 		spin_unlock(&src->link_lock);
8137bd1d409SAlexander Shishkin 		return;
8147bd1d409SAlexander Shishkin 	}
8157bd1d409SAlexander Shishkin 
8160df771deSAlexander Shishkin 	stm_output_free(link, &src->output);
8177bd1d409SAlexander Shishkin 	list_del_init(&src->link_entry);
8187bd1d409SAlexander Shishkin 	/* matches stm_find_device() from stm_source_link_store() */
8190df771deSAlexander Shishkin 	stm_put_device(link);
8207bd1d409SAlexander Shishkin 	rcu_assign_pointer(src->link, NULL);
8217bd1d409SAlexander Shishkin 
8227bd1d409SAlexander Shishkin 	spin_unlock(&src->link_lock);
823c74f7e82SAlexander Shishkin 	spin_unlock(&stm->link_lock);
8247bd1d409SAlexander Shishkin }
8257bd1d409SAlexander Shishkin 
8267bd1d409SAlexander Shishkin /**
8277bd1d409SAlexander Shishkin  * stm_source_link_drop() - detach stm_source from its stm device
8287bd1d409SAlexander Shishkin  * @src:	stm_source device
8297bd1d409SAlexander Shishkin  *
8307bd1d409SAlexander Shishkin  * Unlinking means disconnecting from source's STM device; after this
8317bd1d409SAlexander Shishkin  * writes will be unsuccessful until it is linked to a new STM device.
8327bd1d409SAlexander Shishkin  *
8337bd1d409SAlexander Shishkin  * This will happen on "stm_source_link" sysfs attribute write to undo
8347bd1d409SAlexander Shishkin  * the existing link (if any), or on linked STM device's de-registration.
8357bd1d409SAlexander Shishkin  */
8367bd1d409SAlexander Shishkin static void stm_source_link_drop(struct stm_source_device *src)
8377bd1d409SAlexander Shishkin {
8387bd1d409SAlexander Shishkin 	struct stm_device *stm;
8397bd1d409SAlexander Shishkin 	int idx;
8407bd1d409SAlexander Shishkin 
8417bd1d409SAlexander Shishkin 	idx = srcu_read_lock(&stm_source_srcu);
8427bd1d409SAlexander Shishkin 	stm = srcu_dereference(src->link, &stm_source_srcu);
8437bd1d409SAlexander Shishkin 
8447bd1d409SAlexander Shishkin 	if (stm) {
845c74f7e82SAlexander Shishkin 		mutex_lock(&stm->link_mutex);
8467bd1d409SAlexander Shishkin 		__stm_source_link_drop(src, stm);
847c74f7e82SAlexander Shishkin 		mutex_unlock(&stm->link_mutex);
8487bd1d409SAlexander Shishkin 	}
8497bd1d409SAlexander Shishkin 
8507bd1d409SAlexander Shishkin 	srcu_read_unlock(&stm_source_srcu, idx);
8517bd1d409SAlexander Shishkin }
8527bd1d409SAlexander Shishkin 
8537bd1d409SAlexander Shishkin static ssize_t stm_source_link_show(struct device *dev,
8547bd1d409SAlexander Shishkin 				    struct device_attribute *attr,
8557bd1d409SAlexander Shishkin 				    char *buf)
8567bd1d409SAlexander Shishkin {
8577bd1d409SAlexander Shishkin 	struct stm_source_device *src = to_stm_source_device(dev);
8587bd1d409SAlexander Shishkin 	struct stm_device *stm;
8597bd1d409SAlexander Shishkin 	int idx, ret;
8607bd1d409SAlexander Shishkin 
8617bd1d409SAlexander Shishkin 	idx = srcu_read_lock(&stm_source_srcu);
8627bd1d409SAlexander Shishkin 	stm = srcu_dereference(src->link, &stm_source_srcu);
8637bd1d409SAlexander Shishkin 	ret = sprintf(buf, "%s\n",
8647bd1d409SAlexander Shishkin 		      stm ? dev_name(&stm->dev) : "<none>");
8657bd1d409SAlexander Shishkin 	srcu_read_unlock(&stm_source_srcu, idx);
8667bd1d409SAlexander Shishkin 
8677bd1d409SAlexander Shishkin 	return ret;
8687bd1d409SAlexander Shishkin }
8697bd1d409SAlexander Shishkin 
8707bd1d409SAlexander Shishkin static ssize_t stm_source_link_store(struct device *dev,
8717bd1d409SAlexander Shishkin 				     struct device_attribute *attr,
8727bd1d409SAlexander Shishkin 				     const char *buf, size_t count)
8737bd1d409SAlexander Shishkin {
8747bd1d409SAlexander Shishkin 	struct stm_source_device *src = to_stm_source_device(dev);
8757bd1d409SAlexander Shishkin 	struct stm_device *link;
8767bd1d409SAlexander Shishkin 	int err;
8777bd1d409SAlexander Shishkin 
8787bd1d409SAlexander Shishkin 	stm_source_link_drop(src);
8797bd1d409SAlexander Shishkin 
8807bd1d409SAlexander Shishkin 	link = stm_find_device(buf);
8817bd1d409SAlexander Shishkin 	if (!link)
8827bd1d409SAlexander Shishkin 		return -EINVAL;
8837bd1d409SAlexander Shishkin 
8847bd1d409SAlexander Shishkin 	err = stm_source_link_add(src, link);
8857bd1d409SAlexander Shishkin 	if (err)
8867bd1d409SAlexander Shishkin 		stm_put_device(link);
8877bd1d409SAlexander Shishkin 
8887bd1d409SAlexander Shishkin 	return err ? : count;
8897bd1d409SAlexander Shishkin }
8907bd1d409SAlexander Shishkin 
8917bd1d409SAlexander Shishkin static DEVICE_ATTR_RW(stm_source_link);
8927bd1d409SAlexander Shishkin 
8937bd1d409SAlexander Shishkin static struct attribute *stm_source_attrs[] = {
8947bd1d409SAlexander Shishkin 	&dev_attr_stm_source_link.attr,
8957bd1d409SAlexander Shishkin 	NULL,
8967bd1d409SAlexander Shishkin };
8977bd1d409SAlexander Shishkin 
8987bd1d409SAlexander Shishkin ATTRIBUTE_GROUPS(stm_source);
8997bd1d409SAlexander Shishkin 
9007bd1d409SAlexander Shishkin static struct class stm_source_class = {
9017bd1d409SAlexander Shishkin 	.name		= "stm_source",
9027bd1d409SAlexander Shishkin 	.dev_groups	= stm_source_groups,
9037bd1d409SAlexander Shishkin };
9047bd1d409SAlexander Shishkin 
9057bd1d409SAlexander Shishkin static void stm_source_device_release(struct device *dev)
9067bd1d409SAlexander Shishkin {
9077bd1d409SAlexander Shishkin 	struct stm_source_device *src = to_stm_source_device(dev);
9087bd1d409SAlexander Shishkin 
9097bd1d409SAlexander Shishkin 	kfree(src);
9107bd1d409SAlexander Shishkin }
9117bd1d409SAlexander Shishkin 
9127bd1d409SAlexander Shishkin /**
9137bd1d409SAlexander Shishkin  * stm_source_register_device() - register an stm_source device
9147bd1d409SAlexander Shishkin  * @parent:	parent device
9157bd1d409SAlexander Shishkin  * @data:	device description structure
9167bd1d409SAlexander Shishkin  *
9177bd1d409SAlexander Shishkin  * This will create a device of stm_source class that can write
9187bd1d409SAlexander Shishkin  * data to an stm device once linked.
9197bd1d409SAlexander Shishkin  *
9207bd1d409SAlexander Shishkin  * Return:	0 on success, -errno otherwise.
9217bd1d409SAlexander Shishkin  */
9227bd1d409SAlexander Shishkin int stm_source_register_device(struct device *parent,
9237bd1d409SAlexander Shishkin 			       struct stm_source_data *data)
9247bd1d409SAlexander Shishkin {
9257bd1d409SAlexander Shishkin 	struct stm_source_device *src;
9267bd1d409SAlexander Shishkin 	int err;
9277bd1d409SAlexander Shishkin 
9287bd1d409SAlexander Shishkin 	if (!stm_core_up)
9297bd1d409SAlexander Shishkin 		return -EPROBE_DEFER;
9307bd1d409SAlexander Shishkin 
9317bd1d409SAlexander Shishkin 	src = kzalloc(sizeof(*src), GFP_KERNEL);
9327bd1d409SAlexander Shishkin 	if (!src)
9337bd1d409SAlexander Shishkin 		return -ENOMEM;
9347bd1d409SAlexander Shishkin 
9357bd1d409SAlexander Shishkin 	device_initialize(&src->dev);
9367bd1d409SAlexander Shishkin 	src->dev.class = &stm_source_class;
9377bd1d409SAlexander Shishkin 	src->dev.parent = parent;
9387bd1d409SAlexander Shishkin 	src->dev.release = stm_source_device_release;
9397bd1d409SAlexander Shishkin 
9407bd1d409SAlexander Shishkin 	err = kobject_set_name(&src->dev.kobj, "%s", data->name);
9417bd1d409SAlexander Shishkin 	if (err)
9427bd1d409SAlexander Shishkin 		goto err;
9437bd1d409SAlexander Shishkin 
9447bd1d409SAlexander Shishkin 	err = device_add(&src->dev);
9457bd1d409SAlexander Shishkin 	if (err)
9467bd1d409SAlexander Shishkin 		goto err;
9477bd1d409SAlexander Shishkin 
9487bd1d409SAlexander Shishkin 	spin_lock_init(&src->link_lock);
9497bd1d409SAlexander Shishkin 	INIT_LIST_HEAD(&src->link_entry);
9507bd1d409SAlexander Shishkin 	src->data = data;
9517bd1d409SAlexander Shishkin 	data->src = src;
9527bd1d409SAlexander Shishkin 
9537bd1d409SAlexander Shishkin 	return 0;
9547bd1d409SAlexander Shishkin 
9557bd1d409SAlexander Shishkin err:
9567bd1d409SAlexander Shishkin 	put_device(&src->dev);
9577bd1d409SAlexander Shishkin 	kfree(src);
9587bd1d409SAlexander Shishkin 
9597bd1d409SAlexander Shishkin 	return err;
9607bd1d409SAlexander Shishkin }
9617bd1d409SAlexander Shishkin EXPORT_SYMBOL_GPL(stm_source_register_device);
9627bd1d409SAlexander Shishkin 
9637bd1d409SAlexander Shishkin /**
9647bd1d409SAlexander Shishkin  * stm_source_unregister_device() - unregister an stm_source device
9657bd1d409SAlexander Shishkin  * @data:	device description that was used to register the device
9667bd1d409SAlexander Shishkin  *
9677bd1d409SAlexander Shishkin  * This will remove a previously created stm_source device from the system.
9687bd1d409SAlexander Shishkin  */
9697bd1d409SAlexander Shishkin void stm_source_unregister_device(struct stm_source_data *data)
9707bd1d409SAlexander Shishkin {
9717bd1d409SAlexander Shishkin 	struct stm_source_device *src = data->src;
9727bd1d409SAlexander Shishkin 
9737bd1d409SAlexander Shishkin 	stm_source_link_drop(src);
9747bd1d409SAlexander Shishkin 
9757bd1d409SAlexander Shishkin 	device_destroy(&stm_source_class, src->dev.devt);
9767bd1d409SAlexander Shishkin }
9777bd1d409SAlexander Shishkin EXPORT_SYMBOL_GPL(stm_source_unregister_device);
9787bd1d409SAlexander Shishkin 
9797bd1d409SAlexander Shishkin int stm_source_write(struct stm_source_data *data, unsigned int chan,
9807bd1d409SAlexander Shishkin 		     const char *buf, size_t count)
9817bd1d409SAlexander Shishkin {
9827bd1d409SAlexander Shishkin 	struct stm_source_device *src = data->src;
9837bd1d409SAlexander Shishkin 	struct stm_device *stm;
9847bd1d409SAlexander Shishkin 	int idx;
9857bd1d409SAlexander Shishkin 
9867bd1d409SAlexander Shishkin 	if (!src->output.nr_chans)
9877bd1d409SAlexander Shishkin 		return -ENODEV;
9887bd1d409SAlexander Shishkin 
9897bd1d409SAlexander Shishkin 	if (chan >= src->output.nr_chans)
9907bd1d409SAlexander Shishkin 		return -EINVAL;
9917bd1d409SAlexander Shishkin 
9927bd1d409SAlexander Shishkin 	idx = srcu_read_lock(&stm_source_srcu);
9937bd1d409SAlexander Shishkin 
9947bd1d409SAlexander Shishkin 	stm = srcu_dereference(src->link, &stm_source_srcu);
9957bd1d409SAlexander Shishkin 	if (stm)
9967bd1d409SAlexander Shishkin 		stm_write(stm->data, src->output.master,
9977bd1d409SAlexander Shishkin 			  src->output.channel + chan,
9987bd1d409SAlexander Shishkin 			  buf, count);
9997bd1d409SAlexander Shishkin 	else
10007bd1d409SAlexander Shishkin 		count = -ENODEV;
10017bd1d409SAlexander Shishkin 
10027bd1d409SAlexander Shishkin 	srcu_read_unlock(&stm_source_srcu, idx);
10037bd1d409SAlexander Shishkin 
10047bd1d409SAlexander Shishkin 	return count;
10057bd1d409SAlexander Shishkin }
10067bd1d409SAlexander Shishkin EXPORT_SYMBOL_GPL(stm_source_write);
10077bd1d409SAlexander Shishkin 
10087bd1d409SAlexander Shishkin static int __init stm_core_init(void)
10097bd1d409SAlexander Shishkin {
10107bd1d409SAlexander Shishkin 	int err;
10117bd1d409SAlexander Shishkin 
10127bd1d409SAlexander Shishkin 	err = class_register(&stm_class);
10137bd1d409SAlexander Shishkin 	if (err)
10147bd1d409SAlexander Shishkin 		return err;
10157bd1d409SAlexander Shishkin 
10167bd1d409SAlexander Shishkin 	err = class_register(&stm_source_class);
10177bd1d409SAlexander Shishkin 	if (err)
10187bd1d409SAlexander Shishkin 		goto err_stm;
10197bd1d409SAlexander Shishkin 
10207bd1d409SAlexander Shishkin 	err = stp_configfs_init();
10217bd1d409SAlexander Shishkin 	if (err)
10227bd1d409SAlexander Shishkin 		goto err_src;
10237bd1d409SAlexander Shishkin 
10247bd1d409SAlexander Shishkin 	init_srcu_struct(&stm_source_srcu);
10257bd1d409SAlexander Shishkin 
10267bd1d409SAlexander Shishkin 	stm_core_up++;
10277bd1d409SAlexander Shishkin 
10287bd1d409SAlexander Shishkin 	return 0;
10297bd1d409SAlexander Shishkin 
10307bd1d409SAlexander Shishkin err_src:
10317bd1d409SAlexander Shishkin 	class_unregister(&stm_source_class);
10327bd1d409SAlexander Shishkin err_stm:
10337bd1d409SAlexander Shishkin 	class_unregister(&stm_class);
10347bd1d409SAlexander Shishkin 
10357bd1d409SAlexander Shishkin 	return err;
10367bd1d409SAlexander Shishkin }
10377bd1d409SAlexander Shishkin 
10387bd1d409SAlexander Shishkin module_init(stm_core_init);
10397bd1d409SAlexander Shishkin 
10407bd1d409SAlexander Shishkin static void __exit stm_core_exit(void)
10417bd1d409SAlexander Shishkin {
10427bd1d409SAlexander Shishkin 	cleanup_srcu_struct(&stm_source_srcu);
10437bd1d409SAlexander Shishkin 	class_unregister(&stm_source_class);
10447bd1d409SAlexander Shishkin 	class_unregister(&stm_class);
10457bd1d409SAlexander Shishkin 	stp_configfs_exit();
10467bd1d409SAlexander Shishkin }
10477bd1d409SAlexander Shishkin 
10487bd1d409SAlexander Shishkin module_exit(stm_core_exit);
10497bd1d409SAlexander Shishkin 
10507bd1d409SAlexander Shishkin MODULE_LICENSE("GPL v2");
10517bd1d409SAlexander Shishkin MODULE_DESCRIPTION("System Trace Module device class");
10527bd1d409SAlexander Shishkin MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
1053