6 files changed, 244 insertions, 23 deletions

diff --git a/drivers/hwtracing/stm/Kconfig b/drivers/hwtracing/stm/Kconfig
index e0ac75395526..847a39b35307 100644
--- a/drivers/hwtracing/stm/Kconfig
+++ b/drivers/hwtracing/stm/Kconfig
@@ -28,4 +28,15 @@ config STM_SOURCE_CONSOLE
 	  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.
+
 endif
diff --git a/drivers/hwtracing/stm/Makefile b/drivers/hwtracing/stm/Makefile
index f9312c38dd7a..a9ce3d487e57 100644
--- a/drivers/hwtracing/stm/Makefile
+++ b/drivers/hwtracing/stm/Makefile
@@ -5,5 +5,7 @@ stm_core-y		:= core.o policy.o
 obj-$(CONFIG_STM_DUMMY)	+= dummy_stm.o
 
 obj-$(CONFIG_STM_SOURCE_CONSOLE)	+= stm_console.o
+obj-$(CONFIG_STM_SOURCE_HEARTBEAT)	+= stm_heartbeat.o
 
 stm_console-y		:= console.o
+stm_heartbeat-y		:= heartbeat.o
diff --git a/drivers/hwtracing/stm/core.c b/drivers/hwtracing/stm/core.c
index 5eb9c3bba216..127ff76b0808 100644
--- a/drivers/hwtracing/stm/core.c
+++ b/drivers/hwtracing/stm/core.c
@@ -68,9 +68,24 @@ static ssize_t channels_show(struct device *dev,
 
 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,
 };
 
@@ -380,14 +395,19 @@ err_free:
 static int stm_char_release(struct inode *inode, struct file *file)
 {
 	struct stm_file *stmf = file->private_data;
+	struct stm_device *stm = stmf->stm;
 
-	stm_output_free(stmf->stm, &stmf->output);
+	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(stmf->stm);
+	stm_put_device(stm);
 	kfree(stmf);
 
 	return 0;
@@ -408,8 +428,8 @@ static int stm_file_assign(struct stm_file *stmf, char *id, unsigned int width)
 	return ret;
 }
 
-static void stm_write(struct stm_data *data, unsigned int master,
-		      unsigned int channel, const char *buf, size_t count)
+static ssize_t stm_write(struct stm_data *data, unsigned int master,
+			  unsigned int channel, const char *buf, size_t count)
 {
 	unsigned int flags = STP_PACKET_TIMESTAMPED;
 	const unsigned char *p = buf, nil = 0;
@@ -421,9 +441,14 @@ static void stm_write(struct stm_data *data, unsigned int master,
 		sz = data->packet(data, master, channel, STP_PACKET_DATA, flags,
 				  sz, p);
 		flags = 0;
+
+		if (sz < 0)
+			break;
 	}
 
 	data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil);
+
+	return pos;
 }
 
 static ssize_t stm_char_write(struct file *file, const char __user *buf,
@@ -461,8 +486,8 @@ static ssize_t stm_char_write(struct file *file, const char __user *buf,
 		return -EFAULT;
 	}
 
-	stm_write(stm->data, stmf->output.master, stmf->output.channel, kbuf,
-		  count);
+	count = stm_write(stm->data, stmf->output.master, stmf->output.channel,
+			  kbuf, count);
 
 	kfree(kbuf);
 
@@ -542,8 +567,6 @@ static int stm_char_policy_set_ioctl(struct stm_file *stmf, void __user *arg)
 	if (ret)
 		goto err_free;
 
-	ret = 0;
-
 	if (stm->data->link)
 		ret = stm->data->link(stm->data, stmf->output.master,
 				      stmf->output.channel);
@@ -869,8 +892,18 @@ unlock:
 	spin_unlock(&src->link_lock);
 	spin_unlock(&stm->link_lock);
 
-	if (!ret && src->data->unlink)
-		src->data->unlink(src->data);
+	/*
+	 * 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;
 }
@@ -1058,9 +1091,9 @@ int stm_source_write(struct stm_source_data *data, unsigned int chan,
 
 	stm = srcu_dereference(src->link, &stm_source_srcu);
 	if (stm)
-		stm_write(stm->data, src->output.master,
-			  src->output.channel + chan,
-			  buf, count);
+		count = stm_write(stm->data, src->output.master,
+				  src->output.channel + chan,
+				  buf, count);
 	else
 		count = -ENODEV;
 
diff --git a/drivers/hwtracing/stm/dummy_stm.c b/drivers/hwtracing/stm/dummy_stm.c
index 3709bef0b21f..a86612d989f9 100644
--- a/drivers/hwtracing/stm/dummy_stm.c
+++ b/drivers/hwtracing/stm/dummy_stm.c
@@ -40,22 +40,71 @@ dummy_stm_packet(struct stm_data *stm_data, unsigned int master,
 	return size;
 }
 
-static struct stm_data dummy_stm = {
-	.name		= "dummy_stm",
-	.sw_start	= 0x0000,
-	.sw_end		= 0xffff,
-	.sw_nchannels	= 0xffff,
-	.packet		= dummy_stm_packet,
-};
+#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 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)
 {
-	return stm_register_device(NULL, &dummy_stm, THIS_MODULE);
+	int i, ret = -ENOMEM;
+
+	if (nr_dummies < 0 || nr_dummies > DUMMY_STM_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		= 0x0000;
+		dummy_stm[i].sw_end		= 0xffff;
+		dummy_stm[i].sw_nchannels	= 0xffff;
+		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)
 {
-	stm_unregister_device(&dummy_stm);
+	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);
diff --git a/drivers/hwtracing/stm/heartbeat.c b/drivers/hwtracing/stm/heartbeat.c
new file mode 100644
index 000000000000..3da7b673aab2
--- /dev/null
+++ b/drivers/hwtracing/stm/heartbeat.c
@@ -0,0 +1,126 @@
+/*
+ * Simple heartbeat STM source driver
+ * Copyright (c) 2016, Intel Corporation.
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms and conditions of the GNU General Public License,
+ * version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
+ * more details.
+ *
+ * Heartbeat STM source will send repetitive messages over STM devices to a
+ * trace host.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/hrtimer.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+
+#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 = -ENOMEM;
+
+	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)
+			goto fail_unregister;
+
+		stm_heartbeat[i].data.nr_chans	= 1;
+		stm_heartbeat[i].data.link		= stm_heartbeat_link;
+		stm_heartbeat[i].data.unlink	= stm_heartbeat_unlink;
+		hrtimer_init(&stm_heartbeat[i].hrtimer, CLOCK_MONOTONIC,
+			     HRTIMER_MODE_ABS);
+		stm_heartbeat[i].hrtimer.function =
+			stm_heartbeat_hrtimer_handler;
+
+		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 <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/stm/policy.c b/drivers/hwtracing/stm/policy.c
index 1db189657b2b..1c061cb9bff0 100644
--- a/drivers/hwtracing/stm/policy.c
+++ b/drivers/hwtracing/stm/policy.c
@@ -342,7 +342,7 @@ stp_policies_make(struct config_group *group, const char *name)
 		return ERR_PTR(-EINVAL);
 	}
 
-	*p++ = '\0';
+	*p = '\0';
 
 	stm = stm_find_device(devname);
 	kfree(devname);