Linux-libre 4.4-gnupck-4.4-gnu

25 files changed, 6016 insertions, 51 deletions

diff --git a/drivers/hwtracing/coresight/coresight-etm3x.c b/drivers/hwtracing/coresight/coresight-etm3x.c
index bf2476ed9..d630b7ece 100644
--- a/drivers/hwtracing/coresight/coresight-etm3x.c
+++ b/drivers/hwtracing/coresight/coresight-etm3x.c
@@ -191,7 +191,8 @@ static void etm_set_prog(struct etm_drvdata *drvdata)
 	isb();
 	if (coresight_timeout_etm(drvdata, ETMSR, ETMSR_PROG_BIT, 1)) {
 		dev_err(drvdata->dev,
-			"timeout observed when probing at offset %#x\n", ETMSR);
+			"%s: timeout observed when probing at offset %#x\n",
+			__func__, ETMSR);
 	}
 }
 
@@ -209,7 +210,8 @@ static void etm_clr_prog(struct etm_drvdata *drvdata)
 	isb();
 	if (coresight_timeout_etm(drvdata, ETMSR, ETMSR_PROG_BIT, 0)) {
 		dev_err(drvdata->dev,
-			"timeout observed when probing at offset %#x\n", ETMSR);
+			"%s: timeout observed when probing at offset %#x\n",
+			__func__, ETMSR);
 	}
 }
 
@@ -313,14 +315,6 @@ static void etm_enable_hw(void *info)
 	dev_dbg(drvdata->dev, "cpu: %d enable smp call done\n", drvdata->cpu);
 }
 
-static int etm_trace_id_simple(struct etm_drvdata *drvdata)
-{
-	if (!drvdata->enable)
-		return drvdata->traceid;
-
-	return (etm_readl(drvdata, ETMTRACEIDR) & ETM_TRACEID_MASK);
-}
-
 static int etm_trace_id(struct coresight_device *csdev)
 {
 	struct etm_drvdata *drvdata = dev_get_drvdata(csdev->dev.parent);
@@ -1506,44 +1500,17 @@ static ssize_t timestamp_event_store(struct device *dev,
 }
 static DEVICE_ATTR_RW(timestamp_event);
 
-static ssize_t status_show(struct device *dev,
-			   struct device_attribute *attr, char *buf)
+static ssize_t cpu_show(struct device *dev,
+			struct device_attribute *attr, char *buf)
 {
-	int ret;
-	unsigned long flags;
+	int val;
 	struct etm_drvdata *drvdata = dev_get_drvdata(dev->parent);
 
-	pm_runtime_get_sync(drvdata->dev);
-	spin_lock_irqsave(&drvdata->spinlock, flags);
-
-	CS_UNLOCK(drvdata->base);
-	ret = sprintf(buf,
-		      "ETMCCR: 0x%08x\n"
-		      "ETMCCER: 0x%08x\n"
-		      "ETMSCR: 0x%08x\n"
-		      "ETMIDR: 0x%08x\n"
-		      "ETMCR: 0x%08x\n"
-		      "ETMTRACEIDR: 0x%08x\n"
-		      "Enable event: 0x%08x\n"
-		      "Enable start/stop: 0x%08x\n"
-		      "Enable control: CR1 0x%08x CR2 0x%08x\n"
-		      "CPU affinity: %d\n",
-		      drvdata->etmccr, drvdata->etmccer,
-		      etm_readl(drvdata, ETMSCR), etm_readl(drvdata, ETMIDR),
-		      etm_readl(drvdata, ETMCR), etm_trace_id_simple(drvdata),
-		      etm_readl(drvdata, ETMTEEVR),
-		      etm_readl(drvdata, ETMTSSCR),
-		      etm_readl(drvdata, ETMTECR1),
-		      etm_readl(drvdata, ETMTECR2),
-		      drvdata->cpu);
-	CS_LOCK(drvdata->base);
-
-	spin_unlock_irqrestore(&drvdata->spinlock, flags);
-	pm_runtime_put(drvdata->dev);
+	val = drvdata->cpu;
+	return scnprintf(buf, PAGE_SIZE, "%d\n", val);
 
-	return ret;
 }
-static DEVICE_ATTR_RO(status);
+static DEVICE_ATTR_RO(cpu);
 
 static ssize_t traceid_show(struct device *dev,
 			    struct device_attribute *attr, char *buf)
@@ -1619,11 +1586,61 @@ static struct attribute *coresight_etm_attrs[] = {
 	&dev_attr_ctxid_mask.attr,
 	&dev_attr_sync_freq.attr,
 	&dev_attr_timestamp_event.attr,
-	&dev_attr_status.attr,
 	&dev_attr_traceid.attr,
+	&dev_attr_cpu.attr,
+	NULL,
+};
+
+#define coresight_simple_func(name, offset)                             \
+static ssize_t name##_show(struct device *_dev,                         \
+			   struct device_attribute *attr, char *buf)    \
+{                                                                       \
+	struct etm_drvdata *drvdata = dev_get_drvdata(_dev->parent);    \
+	return scnprintf(buf, PAGE_SIZE, "0x%x\n",                      \
+			 readl_relaxed(drvdata->base + offset));        \
+}                                                                       \
+DEVICE_ATTR_RO(name)
+
+coresight_simple_func(etmccr, ETMCCR);
+coresight_simple_func(etmccer, ETMCCER);
+coresight_simple_func(etmscr, ETMSCR);
+coresight_simple_func(etmidr, ETMIDR);
+coresight_simple_func(etmcr, ETMCR);
+coresight_simple_func(etmtraceidr, ETMTRACEIDR);
+coresight_simple_func(etmteevr, ETMTEEVR);
+coresight_simple_func(etmtssvr, ETMTSSCR);
+coresight_simple_func(etmtecr1, ETMTECR1);
+coresight_simple_func(etmtecr2, ETMTECR2);
+
+static struct attribute *coresight_etm_mgmt_attrs[] = {
+	&dev_attr_etmccr.attr,
+	&dev_attr_etmccer.attr,
+	&dev_attr_etmscr.attr,
+	&dev_attr_etmidr.attr,
+	&dev_attr_etmcr.attr,
+	&dev_attr_etmtraceidr.attr,
+	&dev_attr_etmteevr.attr,
+	&dev_attr_etmtssvr.attr,
+	&dev_attr_etmtecr1.attr,
+	&dev_attr_etmtecr2.attr,
+	NULL,
+};
+
+static const struct attribute_group coresight_etm_group = {
+	.attrs = coresight_etm_attrs,
+};
+
+
+static const struct attribute_group coresight_etm_mgmt_group = {
+	.attrs = coresight_etm_mgmt_attrs,
+	.name = "mgmt",
+};
+
+static const struct attribute_group *coresight_etm_groups[] = {
+	&coresight_etm_group,
+	&coresight_etm_mgmt_group,
 	NULL,
 };
-ATTRIBUTE_GROUPS(coresight_etm);
 
 static int etm_cpu_callback(struct notifier_block *nfb, unsigned long action,
 			    void *hcpu)
diff --git a/drivers/hwtracing/coresight/coresight-etm4x.c b/drivers/hwtracing/coresight/coresight-etm4x.c
index 254a81a4e..a6707642b 100644
--- a/drivers/hwtracing/coresight/coresight-etm4x.c
+++ b/drivers/hwtracing/coresight/coresight-etm4x.c
@@ -136,7 +136,9 @@ static void etm4_enable_hw(void *info)
 		writel_relaxed(drvdata->cntr_val[i],
 			       drvdata->base + TRCCNTVRn(i));
 	}
-	for (i = 0; i < drvdata->nr_resource; i++)
+
+	/* Resource selector pair 0 is always implemented and reserved */
+	for (i = 2; i < drvdata->nr_resource * 2; i++)
 		writel_relaxed(drvdata->res_ctrl[i],
 			       drvdata->base + TRCRSCTLRn(i));
 
@@ -489,8 +491,9 @@ static ssize_t reset_store(struct device *dev,
 		drvdata->cntr_val[i] = 0x0;
 	}
 
-	drvdata->res_idx = 0x0;
-	for (i = 0; i < drvdata->nr_resource; i++)
+	/* Resource selector pair 0 is always implemented and reserved */
+	drvdata->res_idx = 0x2;
+	for (i = 2; i < drvdata->nr_resource * 2; i++)
 		drvdata->res_ctrl[i] = 0x0;
 
 	for (i = 0; i < drvdata->nr_ss_cmp; i++) {
@@ -1732,7 +1735,7 @@ static ssize_t res_idx_store(struct device *dev,
 	if (kstrtoul(buf, 16, &val))
 		return -EINVAL;
 	/* Resource selector pair 0 is always implemented and reserved */
-	if ((val == 0) || (val >= drvdata->nr_resource))
+	if (val < 2 || val >= drvdata->nr_resource * 2)
 		return -EINVAL;
 
 	/*
@@ -2416,8 +2419,13 @@ static void etm4_init_arch_data(void *info)
 	drvdata->nr_addr_cmp = BMVAL(etmidr4, 0, 3);
 	/* NUMPC, bits[15:12] number of PE comparator inputs for tracing */
 	drvdata->nr_pe_cmp = BMVAL(etmidr4, 12, 15);
-	/* NUMRSPAIR, bits[19:16] the number of resource pairs for tracing */
-	drvdata->nr_resource = BMVAL(etmidr4, 16, 19);
+	/*
+	 * NUMRSPAIR, bits[19:16]
+	 * The number of resource pairs conveyed by the HW starts at 0, i.e a
+	 * value of 0x0 indicate 1 resource pair, 0x1 indicate two and so on.
+	 * As such add 1 to the value of NUMRSPAIR for a better representation.
+	 */
+	drvdata->nr_resource = BMVAL(etmidr4, 16, 19) + 1;
 	/*
 	 * NUMSSCC, bits[23:20] the number of single-shot
 	 * comparator control for tracing
@@ -2504,6 +2512,8 @@ static void etm4_init_default_data(struct etmv4_drvdata *drvdata)
 		drvdata->cntr_val[i] = 0x0;
 	}
 
+	/* Resource selector pair 0 is always implemented and reserved */
+	drvdata->res_idx = 0x2;
 	for (i = 2; i < drvdata->nr_resource * 2; i++)
 		drvdata->res_ctrl[i] = 0x0;
 
diff --git a/drivers/hwtracing/coresight/coresight.c b/drivers/hwtracing/coresight/coresight.c
index 894531d31..e25492137 100644
--- a/drivers/hwtracing/coresight/coresight.c
+++ b/drivers/hwtracing/coresight/coresight.c
@@ -240,6 +240,11 @@ static int coresight_enable_path(struct list_head *path)
 	int ret = 0;
 	struct coresight_device *cd;
 
+	/*
+	 * At this point we have a full @path, from source to sink.  The
+	 * sink is the first entry and the source the last one.  Go through
+	 * all the components and enable them one by one.
+	 */
 	list_for_each_entry(cd, path, path_link) {
 		if (cd == list_first_entry(path, struct coresight_device,
 					   path_link)) {
diff --git a/drivers/hwtracing/intel_th/Kconfig b/drivers/hwtracing/intel_th/Kconfig
new file mode 100644
index 000000000..b7a9073d9
--- /dev/null
+++ b/drivers/hwtracing/intel_th/Kconfig
@@ -0,0 +1,72 @@
+config INTEL_TH
+	tristate "Intel(R) Trace Hub controller"
+	help
+	  Intel(R) Trace Hub (TH) is a set of hardware blocks (subdevices) that
+	  produce, switch and output trace data from multiple hardware and
+	  software sources over several types of trace output ports encoded
+	  in System Trace Protocol (MIPI STPv2) and is intended to perform
+	  full system debugging.
+
+	  This option enables intel_th bus and common code used by TH
+	  subdevices to interact with each other and hardware and for
+	  platform glue layers to drive Intel TH devices.
+
+	  Say Y here to enable Intel(R) Trace Hub controller support.
+
+if INTEL_TH
+
+config INTEL_TH_PCI
+	tristate "Intel(R) Trace Hub PCI controller"
+	depends on PCI
+	help
+	  Intel(R) Trace Hub may exist as a PCI device. This option enables
+	  support glue layer for PCI-based Intel TH.
+
+	  Say Y here to enable PCI Intel TH support.
+
+config INTEL_TH_GTH
+	tristate "Intel(R) Trace Hub Global Trace Hub"
+	help
+	  Global Trace Hub (GTH) is the central component of the
+	  Intel TH infrastructure and acts as a switch for source
+	  and output devices. This driver is required for other
+	  Intel TH subdevices to initialize.
+
+	  Say Y here to enable GTH subdevice of Intel(R) Trace Hub.
+
+config INTEL_TH_STH
+	tristate "Intel(R) Trace Hub Software Trace Hub support"
+	depends on STM
+	help
+	  Software Trace Hub (STH) enables trace data from software
+	  trace sources to be sent out via Intel(R) Trace Hub. It
+	  uses stm class device to interface with its sources.
+
+	  Say Y here to enable STH subdevice of Intel(R) Trace Hub.
+
+config INTEL_TH_MSU
+	tristate "Intel(R) Trace Hub Memory Storage Unit"
+	help
+	  Memory Storage Unit (MSU) trace output device enables
+	  storing STP traces to system memory. It supports single
+	  and multiblock modes of operation and provides read()
+	  and mmap() access to the collected data.
+
+	  Say Y here to enable MSU output device for Intel TH.
+
+config INTEL_TH_PTI
+	tristate "Intel(R) Trace Hub PTI output"
+	help
+	  Parallel Trace Interface unit (PTI) is a trace output device
+	  of Intel TH architecture that facilitates STP trace output via
+	  a PTI port.
+
+	  Say Y to enable PTI output of Intel TH data.
+
+config INTEL_TH_DEBUG
+	bool "Intel(R) Trace Hub debugging"
+	depends on DEBUG_FS
+	help
+	  Say Y here to enable debugging.
+
+endif
diff --git a/drivers/hwtracing/intel_th/Makefile b/drivers/hwtracing/intel_th/Makefile
new file mode 100644
index 000000000..81d42fe91
--- /dev/null
+++ b/drivers/hwtracing/intel_th/Makefile
@@ -0,0 +1,18 @@
+obj-$(CONFIG_INTEL_TH)		+= intel_th.o
+intel_th-y			:= core.o
+intel_th-$(CONFIG_INTEL_TH_DEBUG) += debug.o
+
+obj-$(CONFIG_INTEL_TH_PCI)	+= intel_th_pci.o
+intel_th_pci-y			:= pci.o
+
+obj-$(CONFIG_INTEL_TH_GTH)	+= intel_th_gth.o
+intel_th_gth-y			:= gth.o
+
+obj-$(CONFIG_INTEL_TH_STH)	+= intel_th_sth.o
+intel_th_sth-y			:= sth.o
+
+obj-$(CONFIG_INTEL_TH_MSU)	+= intel_th_msu.o
+intel_th_msu-y			:= msu.o
+
+obj-$(CONFIG_INTEL_TH_PTI)	+= intel_th_pti.o
+intel_th_pti-y			:= pti.o
diff --git a/drivers/hwtracing/intel_th/core.c b/drivers/hwtracing/intel_th/core.c
new file mode 100644
index 000000000..165d3001c
--- /dev/null
+++ b/drivers/hwtracing/intel_th/core.c
@@ -0,0 +1,692 @@
+/*
+ * Intel(R) Trace Hub driver core
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sysfs.h>
+#include <linux/kdev_t.h>
+#include <linux/debugfs.h>
+#include <linux/idr.h>
+#include <linux/pci.h>
+#include <linux/dma-mapping.h>
+
+#include "intel_th.h"
+#include "debug.h"
+
+static DEFINE_IDA(intel_th_ida);
+
+static int intel_th_match(struct device *dev, struct device_driver *driver)
+{
+	struct intel_th_driver *thdrv = to_intel_th_driver(driver);
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+
+	if (thdev->type == INTEL_TH_SWITCH &&
+	    (!thdrv->enable || !thdrv->disable))
+		return 0;
+
+	return !strcmp(thdev->name, driver->name);
+}
+
+static int intel_th_child_remove(struct device *dev, void *data)
+{
+	device_release_driver(dev);
+
+	return 0;
+}
+
+static int intel_th_probe(struct device *dev)
+{
+	struct intel_th_driver *thdrv = to_intel_th_driver(dev->driver);
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+	struct intel_th_driver *hubdrv;
+	struct intel_th_device *hub = NULL;
+	int ret;
+
+	if (thdev->type == INTEL_TH_SWITCH)
+		hub = thdev;
+	else if (dev->parent)
+		hub = to_intel_th_device(dev->parent);
+
+	if (!hub || !hub->dev.driver)
+		return -EPROBE_DEFER;
+
+	hubdrv = to_intel_th_driver(hub->dev.driver);
+
+	ret = thdrv->probe(to_intel_th_device(dev));
+	if (ret)
+		return ret;
+
+	if (thdev->type == INTEL_TH_OUTPUT &&
+	    !intel_th_output_assigned(thdev))
+		ret = hubdrv->assign(hub, thdev);
+
+	return ret;
+}
+
+static int intel_th_remove(struct device *dev)
+{
+	struct intel_th_driver *thdrv = to_intel_th_driver(dev->driver);
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+	struct intel_th_device *hub = to_intel_th_device(dev->parent);
+	int err;
+
+	if (thdev->type == INTEL_TH_SWITCH) {
+		err = device_for_each_child(dev, thdev, intel_th_child_remove);
+		if (err)
+			return err;
+	}
+
+	thdrv->remove(thdev);
+
+	if (intel_th_output_assigned(thdev)) {
+		struct intel_th_driver *hubdrv =
+			to_intel_th_driver(dev->parent->driver);
+
+		if (hub->dev.driver)
+			hubdrv->unassign(hub, thdev);
+	}
+
+	return 0;
+}
+
+static struct bus_type intel_th_bus = {
+	.name		= "intel_th",
+	.dev_attrs	= NULL,
+	.match		= intel_th_match,
+	.probe		= intel_th_probe,
+	.remove		= intel_th_remove,
+};
+
+static void intel_th_device_free(struct intel_th_device *thdev);
+
+static void intel_th_device_release(struct device *dev)
+{
+	intel_th_device_free(to_intel_th_device(dev));
+}
+
+static struct device_type intel_th_source_device_type = {
+	.name		= "intel_th_source_device",
+	.release	= intel_th_device_release,
+};
+
+static char *intel_th_output_devnode(struct device *dev, umode_t *mode,
+				     kuid_t *uid, kgid_t *gid)
+{
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+	char *node;
+
+	if (thdev->id >= 0)
+		node = kasprintf(GFP_KERNEL, "intel_th%d/%s%d", 0, thdev->name,
+				 thdev->id);
+	else
+		node = kasprintf(GFP_KERNEL, "intel_th%d/%s", 0, thdev->name);
+
+	return node;
+}
+
+static ssize_t port_show(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+
+	if (thdev->output.port >= 0)
+		return scnprintf(buf, PAGE_SIZE, "%u\n", thdev->output.port);
+
+	return scnprintf(buf, PAGE_SIZE, "unassigned\n");
+}
+
+static DEVICE_ATTR_RO(port);
+
+static int intel_th_output_activate(struct intel_th_device *thdev)
+{
+	struct intel_th_driver *thdrv = to_intel_th_driver(thdev->dev.driver);
+
+	if (thdrv->activate)
+		return thdrv->activate(thdev);
+
+	intel_th_trace_enable(thdev);
+
+	return 0;
+}
+
+static void intel_th_output_deactivate(struct intel_th_device *thdev)
+{
+	struct intel_th_driver *thdrv = to_intel_th_driver(thdev->dev.driver);
+
+	if (thdrv->deactivate)
+		thdrv->deactivate(thdev);
+	else
+		intel_th_trace_disable(thdev);
+}
+
+static ssize_t active_show(struct device *dev, struct device_attribute *attr,
+			   char *buf)
+{
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", thdev->output.active);
+}
+
+static ssize_t active_store(struct device *dev, struct device_attribute *attr,
+			    const char *buf, size_t size)
+{
+	struct intel_th_device *thdev = to_intel_th_device(dev);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (!!val != thdev->output.active) {
+		if (val)
+			ret = intel_th_output_activate(thdev);
+		else
+			intel_th_output_deactivate(thdev);
+	}
+
+	return ret ? ret : size;
+}
+
+static DEVICE_ATTR_RW(active);
+
+static struct attribute *intel_th_output_attrs[] = {
+	&dev_attr_port.attr,
+	&dev_attr_active.attr,
+	NULL,
+};
+
+ATTRIBUTE_GROUPS(intel_th_output);
+
+static struct device_type intel_th_output_device_type = {
+	.name		= "intel_th_output_device",
+	.groups		= intel_th_output_groups,
+	.release	= intel_th_device_release,
+	.devnode	= intel_th_output_devnode,
+};
+
+static struct device_type intel_th_switch_device_type = {
+	.name		= "intel_th_switch_device",
+	.release	= intel_th_device_release,
+};
+
+static struct device_type *intel_th_device_type[] = {
+	[INTEL_TH_SOURCE]	= &intel_th_source_device_type,
+	[INTEL_TH_OUTPUT]	= &intel_th_output_device_type,
+	[INTEL_TH_SWITCH]	= &intel_th_switch_device_type,
+};
+
+int intel_th_driver_register(struct intel_th_driver *thdrv)
+{
+	if (!thdrv->probe || !thdrv->remove)
+		return -EINVAL;
+
+	thdrv->driver.bus = &intel_th_bus;
+
+	return driver_register(&thdrv->driver);
+}
+EXPORT_SYMBOL_GPL(intel_th_driver_register);
+
+void intel_th_driver_unregister(struct intel_th_driver *thdrv)
+{
+	driver_unregister(&thdrv->driver);
+}
+EXPORT_SYMBOL_GPL(intel_th_driver_unregister);
+
+static struct intel_th_device *
+intel_th_device_alloc(struct intel_th *th, unsigned int type, const char *name,
+		      int id)
+{
+	struct device *parent;
+	struct intel_th_device *thdev;
+
+	if (type == INTEL_TH_SWITCH)
+		parent = th->dev;
+	else
+		parent = &th->hub->dev;
+
+	thdev = kzalloc(sizeof(*thdev) + strlen(name) + 1, GFP_KERNEL);
+	if (!thdev)
+		return NULL;
+
+	thdev->id = id;
+	thdev->type = type;
+
+	strcpy(thdev->name, name);
+	device_initialize(&thdev->dev);
+	thdev->dev.bus = &intel_th_bus;
+	thdev->dev.type = intel_th_device_type[type];
+	thdev->dev.parent = parent;
+	thdev->dev.dma_mask = parent->dma_mask;
+	thdev->dev.dma_parms = parent->dma_parms;
+	dma_set_coherent_mask(&thdev->dev, parent->coherent_dma_mask);
+	if (id >= 0)
+		dev_set_name(&thdev->dev, "%d-%s%d", th->id, name, id);
+	else
+		dev_set_name(&thdev->dev, "%d-%s", th->id, name);
+
+	return thdev;
+}
+
+static int intel_th_device_add_resources(struct intel_th_device *thdev,
+					 struct resource *res, int nres)
+{
+	struct resource *r;
+
+	r = kmemdup(res, sizeof(*res) * nres, GFP_KERNEL);
+	if (!r)
+		return -ENOMEM;
+
+	thdev->resource = r;
+	thdev->num_resources = nres;
+
+	return 0;
+}
+
+static void intel_th_device_remove(struct intel_th_device *thdev)
+{
+	device_del(&thdev->dev);
+	put_device(&thdev->dev);
+}
+
+static void intel_th_device_free(struct intel_th_device *thdev)
+{
+	kfree(thdev->resource);
+	kfree(thdev);
+}
+
+/*
+ * Intel(R) Trace Hub subdevices
+ */
+static struct intel_th_subdevice {
+	const char		*name;
+	struct resource		res[3];
+	unsigned		nres;
+	unsigned		type;
+	unsigned		otype;
+	int			id;
+} intel_th_subdevices[TH_SUBDEVICE_MAX] = {
+	{
+		.nres	= 1,
+		.res	= {
+			{
+				.start	= REG_GTH_OFFSET,
+				.end	= REG_GTH_OFFSET + REG_GTH_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+		},
+		.name	= "gth",
+		.type	= INTEL_TH_SWITCH,
+		.id	= -1,
+	},
+	{
+		.nres	= 2,
+		.res	= {
+			{
+				.start	= REG_MSU_OFFSET,
+				.end	= REG_MSU_OFFSET + REG_MSU_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+			{
+				.start	= BUF_MSU_OFFSET,
+				.end	= BUF_MSU_OFFSET + BUF_MSU_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+		},
+		.name	= "msc",
+		.id	= 0,
+		.type	= INTEL_TH_OUTPUT,
+		.otype	= GTH_MSU,
+	},
+	{
+		.nres	= 2,
+		.res	= {
+			{
+				.start	= REG_MSU_OFFSET,
+				.end	= REG_MSU_OFFSET + REG_MSU_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+			{
+				.start	= BUF_MSU_OFFSET,
+				.end	= BUF_MSU_OFFSET + BUF_MSU_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+		},
+		.name	= "msc",
+		.id	= 1,
+		.type	= INTEL_TH_OUTPUT,
+		.otype	= GTH_MSU,
+	},
+	{
+		.nres	= 2,
+		.res	= {
+			{
+				.start	= REG_STH_OFFSET,
+				.end	= REG_STH_OFFSET + REG_STH_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+			{
+				.start	= TH_MMIO_SW,
+				.end	= 0,
+				.flags	= IORESOURCE_MEM,
+			},
+		},
+		.id	= -1,
+		.name	= "sth",
+		.type	= INTEL_TH_SOURCE,
+	},
+	{
+		.nres	= 1,
+		.res	= {
+			{
+				.start	= REG_PTI_OFFSET,
+				.end	= REG_PTI_OFFSET + REG_PTI_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+		},
+		.id	= -1,
+		.name	= "pti",
+		.type	= INTEL_TH_OUTPUT,
+		.otype	= GTH_PTI,
+	},
+	{
+		.nres	= 1,
+		.res	= {
+			{
+				.start	= REG_DCIH_OFFSET,
+				.end	= REG_DCIH_OFFSET + REG_DCIH_LENGTH - 1,
+				.flags	= IORESOURCE_MEM,
+			},
+		},
+		.id	= -1,
+		.name	= "dcih",
+		.type	= INTEL_TH_OUTPUT,
+	},
+};
+
+static int intel_th_populate(struct intel_th *th, struct resource *devres,
+			     unsigned int ndevres, int irq)
+{
+	struct resource res[3];
+	unsigned int req = 0;
+	int i, err;
+
+	/* create devices for each intel_th_subdevice */
+	for (i = 0; i < ARRAY_SIZE(intel_th_subdevices); i++) {
+		struct intel_th_subdevice *subdev = &intel_th_subdevices[i];
+		struct intel_th_device *thdev;
+		int r;
+
+		thdev = intel_th_device_alloc(th, subdev->type, subdev->name,
+					      subdev->id);
+		if (!thdev) {
+			err = -ENOMEM;
+			goto kill_subdevs;
+		}
+
+		memcpy(res, subdev->res,
+		       sizeof(struct resource) * subdev->nres);
+
+		for (r = 0; r < subdev->nres; r++) {
+			int bar = TH_MMIO_CONFIG;
+
+			/*
+			 * Take .end == 0 to mean 'take the whole bar',
+			 * .start then tells us which bar it is. Default to
+			 * TH_MMIO_CONFIG.
+			 */
+			if (!res[r].end && res[r].flags == IORESOURCE_MEM) {
+				bar = res[r].start;
+				res[r].start = 0;
+				res[r].end = resource_size(&devres[bar]) - 1;
+			}
+
+			if (res[r].flags & IORESOURCE_MEM) {
+				res[r].start	+= devres[bar].start;
+				res[r].end	+= devres[bar].start;
+
+				dev_dbg(th->dev, "%s:%d @ %pR\n",
+					subdev->name, r, &res[r]);
+			} else if (res[r].flags & IORESOURCE_IRQ) {
+				res[r].start	= irq;
+			}
+		}
+
+		err = intel_th_device_add_resources(thdev, res, subdev->nres);
+		if (err) {
+			put_device(&thdev->dev);
+			goto kill_subdevs;
+		}
+
+		if (subdev->type == INTEL_TH_OUTPUT) {
+			thdev->dev.devt = MKDEV(th->major, i);
+			thdev->output.type = subdev->otype;
+			thdev->output.port = -1;
+		}
+
+		err = device_add(&thdev->dev);
+		if (err) {
+			put_device(&thdev->dev);
+			goto kill_subdevs;
+		}
+
+		/* need switch driver to be loaded to enumerate the rest */
+		if (subdev->type == INTEL_TH_SWITCH && !req) {
+			th->hub = thdev;
+			err = request_module("intel_th_%s", subdev->name);
+			if (!err)
+				req++;
+		}
+
+		th->thdev[i] = thdev;
+	}
+
+	return 0;
+
+kill_subdevs:
+	for (i-- ; i >= 0; i--)
+		intel_th_device_remove(th->thdev[i]);
+
+	return err;
+}
+
+static int match_devt(struct device *dev, void *data)
+{
+	dev_t devt = (dev_t)(unsigned long)data;
+
+	return dev->devt == devt;
+}
+
+static int intel_th_output_open(struct inode *inode, struct file *file)
+{
+	const struct file_operations *fops;
+	struct intel_th_driver *thdrv;
+	struct device *dev;
+	int err;
+
+	dev = bus_find_device(&intel_th_bus, NULL,
+			      (void *)(unsigned long)inode->i_rdev,
+			      match_devt);
+	if (!dev || !dev->driver)
+		return -ENODEV;
+
+	thdrv = to_intel_th_driver(dev->driver);
+	fops = fops_get(thdrv->fops);
+	if (!fops)
+		return -ENODEV;
+
+	replace_fops(file, fops);
+
+	file->private_data = to_intel_th_device(dev);
+
+	if (file->f_op->open) {
+		err = file->f_op->open(inode, file);
+		return err;
+	}
+
+	return 0;
+}
+
+static const struct file_operations intel_th_output_fops = {
+	.open	= intel_th_output_open,
+	.llseek	= noop_llseek,
+};
+
+/**
+ * intel_th_alloc() - allocate a new Intel TH device and its subdevices
+ * @dev:	parent device
+ * @devres:	parent's resources
+ * @ndevres:	number of resources
+ * @irq:	irq number
+ */
+struct intel_th *
+intel_th_alloc(struct device *dev, struct resource *devres,
+	       unsigned int ndevres, int irq)
+{
+	struct intel_th *th;
+	int err;
+
+	th = kzalloc(sizeof(*th), GFP_KERNEL);
+	if (!th)
+		return ERR_PTR(-ENOMEM);
+
+	th->id = ida_simple_get(&intel_th_ida, 0, 0, GFP_KERNEL);
+	if (th->id < 0) {
+		err = th->id;
+		goto err_alloc;
+	}
+
+	th->major = __register_chrdev(0, 0, TH_POSSIBLE_OUTPUTS,
+				      "intel_th/output", &intel_th_output_fops);
+	if (th->major < 0) {
+		err = th->major;
+		goto err_ida;
+	}
+	th->dev = dev;
+
+	err = intel_th_populate(th, devres, ndevres, irq);
+	if (err)
+		goto err_chrdev;
+
+	return th;
+
+err_chrdev:
+	__unregister_chrdev(th->major, 0, TH_POSSIBLE_OUTPUTS,
+			    "intel_th/output");
+
+err_ida:
+	ida_simple_remove(&intel_th_ida, th->id);
+
+err_alloc:
+	kfree(th);
+
+	return ERR_PTR(err);
+}
+EXPORT_SYMBOL_GPL(intel_th_alloc);
+
+void intel_th_free(struct intel_th *th)
+{
+	int i;
+
+	for (i = 0; i < TH_SUBDEVICE_MAX; i++)
+		if (th->thdev[i] != th->hub)
+			intel_th_device_remove(th->thdev[i]);
+
+	intel_th_device_remove(th->hub);
+
+	__unregister_chrdev(th->major, 0, TH_POSSIBLE_OUTPUTS,
+			    "intel_th/output");
+
+	ida_simple_remove(&intel_th_ida, th->id);
+
+	kfree(th);
+}
+EXPORT_SYMBOL_GPL(intel_th_free);
+
+/**
+ * intel_th_trace_enable() - enable tracing for an output device
+ * @thdev:	output device that requests tracing be enabled
+ */
+int intel_th_trace_enable(struct intel_th_device *thdev)
+{
+	struct intel_th_device *hub = to_intel_th_device(thdev->dev.parent);
+	struct intel_th_driver *hubdrv = to_intel_th_driver(hub->dev.driver);
+
+	if (WARN_ON_ONCE(hub->type != INTEL_TH_SWITCH))
+		return -EINVAL;
+
+	if (WARN_ON_ONCE(thdev->type != INTEL_TH_OUTPUT))
+		return -EINVAL;
+
+	hubdrv->enable(hub, &thdev->output);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(intel_th_trace_enable);
+
+/**
+ * intel_th_trace_disable() - disable tracing for an output device
+ * @thdev:	output device that requests tracing be disabled
+ */
+int intel_th_trace_disable(struct intel_th_device *thdev)
+{
+	struct intel_th_device *hub = to_intel_th_device(thdev->dev.parent);
+	struct intel_th_driver *hubdrv = to_intel_th_driver(hub->dev.driver);
+
+	WARN_ON_ONCE(hub->type != INTEL_TH_SWITCH);
+	if (WARN_ON_ONCE(thdev->type != INTEL_TH_OUTPUT))
+		return -EINVAL;
+
+	hubdrv->disable(hub, &thdev->output);
+
+	return 0;
+}
+EXPORT_SYMBOL_GPL(intel_th_trace_disable);
+
+int intel_th_set_output(struct intel_th_device *thdev,
+			unsigned int master)
+{
+	struct intel_th_device *hub = to_intel_th_device(thdev->dev.parent);
+	struct intel_th_driver *hubdrv = to_intel_th_driver(hub->dev.driver);
+
+	if (!hubdrv->set_output)
+		return -ENOTSUPP;
+
+	return hubdrv->set_output(hub, master);
+}
+EXPORT_SYMBOL_GPL(intel_th_set_output);
+
+static int __init intel_th_init(void)
+{
+	intel_th_debug_init();
+
+	return bus_register(&intel_th_bus);
+}
+subsys_initcall(intel_th_init);
+
+static void __exit intel_th_exit(void)
+{
+	intel_th_debug_done();
+
+	bus_unregister(&intel_th_bus);
+}
+module_exit(intel_th_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel(R) Trace Hub controller driver");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/intel_th/debug.c b/drivers/hwtracing/intel_th/debug.c
new file mode 100644
index 000000000..788a1f0a9
--- /dev/null
+++ b/drivers/hwtracing/intel_th/debug.c
@@ -0,0 +1,36 @@
+/*
+ * Intel(R) Trace Hub driver debugging
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/debugfs.h>
+
+#include "intel_th.h"
+#include "debug.h"
+
+struct dentry *intel_th_dbg;
+
+void intel_th_debug_init(void)
+{
+	intel_th_dbg = debugfs_create_dir("intel_th", NULL);
+	if (IS_ERR(intel_th_dbg))
+		intel_th_dbg = NULL;
+}
+
+void intel_th_debug_done(void)
+{
+	debugfs_remove(intel_th_dbg);
+	intel_th_dbg = NULL;
+}
diff --git a/drivers/hwtracing/intel_th/debug.h b/drivers/hwtracing/intel_th/debug.h
new file mode 100644
index 000000000..88311bad3
--- /dev/null
+++ b/drivers/hwtracing/intel_th/debug.h
@@ -0,0 +1,34 @@
+/*
+ * Intel(R) Trace Hub driver debugging
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#ifndef __INTEL_TH_DEBUG_H__
+#define __INTEL_TH_DEBUG_H__
+
+#ifdef CONFIG_INTEL_TH_DEBUG
+extern struct dentry *intel_th_dbg;
+
+void intel_th_debug_init(void);
+void intel_th_debug_done(void);
+#else
+static inline void intel_th_debug_init(void)
+{
+}
+
+static inline void intel_th_debug_done(void)
+{
+}
+#endif
+
+#endif /* __INTEL_TH_DEBUG_H__ */
diff --git a/drivers/hwtracing/intel_th/gth.c b/drivers/hwtracing/intel_th/gth.c
new file mode 100644
index 000000000..2dc5378cc
--- /dev/null
+++ b/drivers/hwtracing/intel_th/gth.c
@@ -0,0 +1,706 @@
+/*
+ * Intel(R) Trace Hub Global Trace Hub
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/bitmap.h>
+
+#include "intel_th.h"
+#include "gth.h"
+
+struct gth_device;
+
+/**
+ * struct gth_output - GTH view on an output port
+ * @gth:	backlink to the GTH device
+ * @output:	link to output device's output descriptor
+ * @index:	output port number
+ * @port_type:	one of GTH_* port type values
+ * @master:	bitmap of masters configured for this output
+ */
+struct gth_output {
+	struct gth_device	*gth;
+	struct intel_th_output	*output;
+	unsigned int		index;
+	unsigned int		port_type;
+	DECLARE_BITMAP(master, TH_CONFIGURABLE_MASTERS + 1);
+};
+
+/**
+ * struct gth_device - GTH device
+ * @dev:	driver core's device
+ * @base:	register window base address
+ * @output_group:	attributes describing output ports
+ * @master_group:	attributes describing master assignments
+ * @output:		output ports
+ * @master:		master/output port assignments
+ * @gth_lock:		serializes accesses to GTH bits
+ */
+struct gth_device {
+	struct device		*dev;
+	void __iomem		*base;
+
+	struct attribute_group	output_group;
+	struct attribute_group	master_group;
+	struct gth_output	output[TH_POSSIBLE_OUTPUTS];
+	signed char		master[TH_CONFIGURABLE_MASTERS + 1];
+	spinlock_t		gth_lock;
+};
+
+static void gth_output_set(struct gth_device *gth, int port,
+			   unsigned int config)
+{
+	unsigned long reg = port & 4 ? REG_GTH_GTHOPT1 : REG_GTH_GTHOPT0;
+	u32 val;
+	int shift = (port & 3) * 8;
+
+	val = ioread32(gth->base + reg);
+	val &= ~(0xff << shift);
+	val |= config << shift;
+	iowrite32(val, gth->base + reg);
+}
+
+static unsigned int gth_output_get(struct gth_device *gth, int port)
+{
+	unsigned long reg = port & 4 ? REG_GTH_GTHOPT1 : REG_GTH_GTHOPT0;
+	u32 val;
+	int shift = (port & 3) * 8;
+
+	val = ioread32(gth->base + reg);
+	val &= 0xff << shift;
+	val >>= shift;
+
+	return val;
+}
+
+static void gth_smcfreq_set(struct gth_device *gth, int port,
+			    unsigned int freq)
+{
+	unsigned long reg = REG_GTH_SMCR0 + ((port / 2) * 4);
+	int shift = (port & 1) * 16;
+	u32 val;
+
+	val = ioread32(gth->base + reg);
+	val &= ~(0xffff << shift);
+	val |= freq << shift;
+	iowrite32(val, gth->base + reg);
+}
+
+static unsigned int gth_smcfreq_get(struct gth_device *gth, int port)
+{
+	unsigned long reg = REG_GTH_SMCR0 + ((port / 2) * 4);
+	int shift = (port & 1) * 16;
+	u32 val;
+
+	val = ioread32(gth->base + reg);
+	val &= 0xffff << shift;
+	val >>= shift;
+
+	return val;
+}
+
+/*
+ * "masters" attribute group
+ */
+
+struct master_attribute {
+	struct device_attribute	attr;
+	struct gth_device	*gth;
+	unsigned int		master;
+};
+
+static void
+gth_master_set(struct gth_device *gth, unsigned int master, int port)
+{
+	unsigned int reg = REG_GTH_SWDEST0 + ((master >> 1) & ~3u);
+	unsigned int shift = (master & 0x7) * 4;
+	u32 val;
+
+	if (master >= 256) {
+		reg = REG_GTH_GSWTDEST;
+		shift = 0;
+	}
+
+	val = ioread32(gth->base + reg);
+	val &= ~(0xf << shift);
+	if (port >= 0)
+		val |= (0x8 | port) << shift;
+	iowrite32(val, gth->base + reg);
+}
+
+/*static int gth_master_get(struct gth_device *gth, unsigned int master)
+{
+	unsigned int reg = REG_GTH_SWDEST0 + ((master >> 1) & ~3u);
+	unsigned int shift = (master & 0x7) * 4;
+	u32 val;
+
+	if (master >= 256) {
+		reg = REG_GTH_GSWTDEST;
+		shift = 0;
+	}
+
+	val = ioread32(gth->base + reg);
+	val &= (0xf << shift);
+	val >>= shift;
+
+	return val ? val & 0x7 : -1;
+	}*/
+
+static ssize_t master_attr_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct master_attribute *ma =
+		container_of(attr, struct master_attribute, attr);
+	struct gth_device *gth = ma->gth;
+	size_t count;
+	int port;
+
+	spin_lock(&gth->gth_lock);
+	port = gth->master[ma->master];
+	spin_unlock(&gth->gth_lock);
+
+	if (port >= 0)
+		count = snprintf(buf, PAGE_SIZE, "%x\n", port);
+	else
+		count = snprintf(buf, PAGE_SIZE, "disabled\n");
+
+	return count;
+}
+
+static ssize_t master_attr_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct master_attribute *ma =
+		container_of(attr, struct master_attribute, attr);
+	struct gth_device *gth = ma->gth;
+	int old_port, port;
+
+	if (kstrtoint(buf, 10, &port) < 0)
+		return -EINVAL;
+
+	if (port >= TH_POSSIBLE_OUTPUTS || port < -1)
+		return -EINVAL;
+
+	spin_lock(&gth->gth_lock);
+
+	/* disconnect from the previous output port, if any */
+	old_port = gth->master[ma->master];
+	if (old_port >= 0) {
+		gth->master[ma->master] = -1;
+		clear_bit(ma->master, gth->output[old_port].master);
+		if (gth->output[old_port].output->active)
+			gth_master_set(gth, ma->master, -1);
+	}
+
+	/* connect to the new output port, if any */
+	if (port >= 0) {
+		/* check if there's a driver for this port */
+		if (!gth->output[port].output) {
+			count = -ENODEV;
+			goto unlock;
+		}
+
+		set_bit(ma->master, gth->output[port].master);
+
+		/* if the port is active, program this setting */
+		if (gth->output[port].output->active)
+			gth_master_set(gth, ma->master, port);
+	}
+
+	gth->master[ma->master] = port;
+
+unlock:
+	spin_unlock(&gth->gth_lock);
+
+	return count;
+}
+
+struct output_attribute {
+	struct device_attribute attr;
+	struct gth_device	*gth;
+	unsigned int		port;
+	unsigned int		parm;
+};
+
+#define OUTPUT_PARM(_name, _mask, _r, _w, _what)			\
+	[TH_OUTPUT_PARM(_name)] = { .name = __stringify(_name),		\
+				    .get = gth_ ## _what ## _get,	\
+				    .set = gth_ ## _what ## _set,	\
+				    .mask = (_mask),			\
+				    .readable = (_r),			\
+				    .writable = (_w) }
+
+static const struct output_parm {
+	const char	*name;
+	unsigned int	(*get)(struct gth_device *gth, int port);
+	void		(*set)(struct gth_device *gth, int port,
+			       unsigned int val);
+	unsigned int	mask;
+	unsigned int	readable : 1,
+			writable : 1;
+} output_parms[] = {
+	OUTPUT_PARM(port,	0x7,	1, 0, output),
+	OUTPUT_PARM(null,	BIT(3),	1, 1, output),
+	OUTPUT_PARM(drop,	BIT(4),	1, 1, output),
+	OUTPUT_PARM(reset,	BIT(5),	1, 0, output),
+	OUTPUT_PARM(flush,	BIT(7),	0, 1, output),
+	OUTPUT_PARM(smcfreq,	0xffff,	1, 1, smcfreq),
+};
+
+static void
+gth_output_parm_set(struct gth_device *gth, int port, unsigned int parm,
+		    unsigned int val)
+{
+	unsigned int config = output_parms[parm].get(gth, port);
+	unsigned int mask = output_parms[parm].mask;
+	unsigned int shift = __ffs(mask);
+
+	config &= ~mask;
+	config |= (val << shift) & mask;
+	output_parms[parm].set(gth, port, config);
+}
+
+static unsigned int
+gth_output_parm_get(struct gth_device *gth, int port, unsigned int parm)
+{
+	unsigned int config = output_parms[parm].get(gth, port);
+	unsigned int mask = output_parms[parm].mask;
+	unsigned int shift = __ffs(mask);
+
+	config &= mask;
+	config >>= shift;
+	return config;
+}
+
+/*
+ * Reset outputs and sources
+ */
+static int intel_th_gth_reset(struct gth_device *gth)
+{
+	u32 scratchpad;
+	int port, i;
+
+	scratchpad = ioread32(gth->base + REG_GTH_SCRPD0);
+	if (scratchpad & SCRPD_DEBUGGER_IN_USE)
+		return -EBUSY;
+
+	/* output ports */
+	for (port = 0; port < 8; port++) {
+		if (gth_output_parm_get(gth, port, TH_OUTPUT_PARM(port)) ==
+		    GTH_NONE)
+			continue;
+
+		gth_output_set(gth, port, 0);
+		gth_smcfreq_set(gth, port, 16);
+	}
+	/* disable overrides */
+	iowrite32(0, gth->base + REG_GTH_DESTOVR);
+
+	/* masters swdest_0~31 and gswdest */
+	for (i = 0; i < 33; i++)
+		iowrite32(0, gth->base + REG_GTH_SWDEST0 + i * 4);
+
+	/* sources */
+	iowrite32(0, gth->base + REG_GTH_SCR);
+	iowrite32(0xfc, gth->base + REG_GTH_SCR2);
+
+	return 0;
+}
+
+/*
+ * "outputs" attribute group
+ */
+
+static ssize_t output_attr_show(struct device *dev,
+				struct device_attribute *attr,
+				char *buf)
+{
+	struct output_attribute *oa =
+		container_of(attr, struct output_attribute, attr);
+	struct gth_device *gth = oa->gth;
+	size_t count;
+
+	spin_lock(&gth->gth_lock);
+	count = snprintf(buf, PAGE_SIZE, "%x\n",
+			 gth_output_parm_get(gth, oa->port, oa->parm));
+	spin_unlock(&gth->gth_lock);
+
+	return count;
+}
+
+static ssize_t output_attr_store(struct device *dev,
+				 struct device_attribute *attr,
+				 const char *buf, size_t count)
+{
+	struct output_attribute *oa =
+		container_of(attr, struct output_attribute, attr);
+	struct gth_device *gth = oa->gth;
+	unsigned int config;
+
+	if (kstrtouint(buf, 16, &config) < 0)
+		return -EINVAL;
+
+	spin_lock(&gth->gth_lock);
+	gth_output_parm_set(gth, oa->port, oa->parm, config);
+	spin_unlock(&gth->gth_lock);
+
+	return count;
+}
+
+static int intel_th_master_attributes(struct gth_device *gth)
+{
+	struct master_attribute *master_attrs;
+	struct attribute **attrs;
+	int i, nattrs = TH_CONFIGURABLE_MASTERS + 2;
+
+	attrs = devm_kcalloc(gth->dev, nattrs, sizeof(void *), GFP_KERNEL);
+	if (!attrs)
+		return -ENOMEM;
+
+	master_attrs = devm_kcalloc(gth->dev, nattrs,
+				    sizeof(struct master_attribute),
+				    GFP_KERNEL);
+	if (!master_attrs)
+		return -ENOMEM;
+
+	for (i = 0; i < TH_CONFIGURABLE_MASTERS + 1; i++) {
+		char *name;
+
+		name = devm_kasprintf(gth->dev, GFP_KERNEL, "%d%s", i,
+				      i == TH_CONFIGURABLE_MASTERS ? "+" : "");
+		if (!name)
+			return -ENOMEM;
+
+		master_attrs[i].attr.attr.name = name;
+		master_attrs[i].attr.attr.mode = S_IRUGO | S_IWUSR;
+		master_attrs[i].attr.show = master_attr_show;
+		master_attrs[i].attr.store = master_attr_store;
+
+		sysfs_attr_init(&master_attrs[i].attr.attr);
+		attrs[i] = &master_attrs[i].attr.attr;
+
+		master_attrs[i].gth = gth;
+		master_attrs[i].master = i;
+	}
+
+	gth->master_group.name	= "masters";
+	gth->master_group.attrs = attrs;
+
+	return sysfs_create_group(&gth->dev->kobj, &gth->master_group);
+}
+
+static int intel_th_output_attributes(struct gth_device *gth)
+{
+	struct output_attribute *out_attrs;
+	struct attribute **attrs;
+	int i, j, nouts = TH_POSSIBLE_OUTPUTS;
+	int nparms = ARRAY_SIZE(output_parms);
+	int nattrs = nouts * nparms + 1;
+
+	attrs = devm_kcalloc(gth->dev, nattrs, sizeof(void *), GFP_KERNEL);
+	if (!attrs)
+		return -ENOMEM;
+
+	out_attrs = devm_kcalloc(gth->dev, nattrs,
+				 sizeof(struct output_attribute),
+				 GFP_KERNEL);
+	if (!out_attrs)
+		return -ENOMEM;
+
+	for (i = 0; i < nouts; i++) {
+		for (j = 0; j < nparms; j++) {
+			unsigned int idx = i * nparms + j;
+			char *name;
+
+			name = devm_kasprintf(gth->dev, GFP_KERNEL, "%d_%s", i,
+					      output_parms[j].name);
+			if (!name)
+				return -ENOMEM;
+
+			out_attrs[idx].attr.attr.name = name;
+
+			if (output_parms[j].readable) {
+				out_attrs[idx].attr.attr.mode |= S_IRUGO;
+				out_attrs[idx].attr.show = output_attr_show;
+			}
+
+			if (output_parms[j].writable) {
+				out_attrs[idx].attr.attr.mode |= S_IWUSR;
+				out_attrs[idx].attr.store = output_attr_store;
+			}
+
+			sysfs_attr_init(&out_attrs[idx].attr.attr);
+			attrs[idx] = &out_attrs[idx].attr.attr;
+
+			out_attrs[idx].gth = gth;
+			out_attrs[idx].port = i;
+			out_attrs[idx].parm = j;
+		}
+	}
+
+	gth->output_group.name	= "outputs";
+	gth->output_group.attrs = attrs;
+
+	return sysfs_create_group(&gth->dev->kobj, &gth->output_group);
+}
+
+/**
+ * intel_th_gth_disable() - enable tracing to an output device
+ * @thdev:	GTH device
+ * @output:	output device's descriptor
+ *
+ * This will deconfigure all masters set to output to this device,
+ * disable tracing using force storeEn off signal and wait for the
+ * "pipeline empty" bit for corresponding output port.
+ */
+static void intel_th_gth_disable(struct intel_th_device *thdev,
+				 struct intel_th_output *output)
+{
+	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+	unsigned long count;
+	int master;
+	u32 reg;
+
+	spin_lock(&gth->gth_lock);
+	output->active = false;
+
+	for_each_set_bit(master, gth->output[output->port].master,
+			 TH_CONFIGURABLE_MASTERS) {
+		gth_master_set(gth, master, -1);
+	}
+	spin_unlock(&gth->gth_lock);
+
+	iowrite32(0, gth->base + REG_GTH_SCR);
+	iowrite32(0xfd, gth->base + REG_GTH_SCR2);
+
+	/* wait on pipeline empty for the given port */
+	for (reg = 0, count = GTH_PLE_WAITLOOP_DEPTH;
+	     count && !(reg & BIT(output->port)); count--) {
+		reg = ioread32(gth->base + REG_GTH_STAT);
+		cpu_relax();
+	}
+
+	/* clear force capture done for next captures */
+	iowrite32(0xfc, gth->base + REG_GTH_SCR2);
+
+	if (!count)
+		dev_dbg(&thdev->dev, "timeout waiting for GTH[%d] PLE\n",
+			output->port);
+}
+
+/**
+ * intel_th_gth_enable() - enable tracing to an output device
+ * @thdev:	GTH device
+ * @output:	output device's descriptor
+ *
+ * This will configure all masters set to output to this device and
+ * enable tracing using force storeEn signal.
+ */
+static void intel_th_gth_enable(struct intel_th_device *thdev,
+				struct intel_th_output *output)
+{
+	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+	u32 scr = 0xfc0000;
+	int master;
+
+	spin_lock(&gth->gth_lock);
+	for_each_set_bit(master, gth->output[output->port].master,
+			 TH_CONFIGURABLE_MASTERS + 1) {
+		gth_master_set(gth, master, output->port);
+	}
+
+	if (output->multiblock)
+		scr |= 0xff;
+
+	output->active = true;
+	spin_unlock(&gth->gth_lock);
+
+	iowrite32(scr, gth->base + REG_GTH_SCR);
+	iowrite32(0, gth->base + REG_GTH_SCR2);
+}
+
+/**
+ * intel_th_gth_assign() - assign output device to a GTH output port
+ * @thdev:	GTH device
+ * @othdev:	output device
+ *
+ * This will match a given output device parameters against present
+ * output ports on the GTH and fill out relevant bits in output device's
+ * descriptor.
+ *
+ * Return:	0 on success, -errno on error.
+ */
+static int intel_th_gth_assign(struct intel_th_device *thdev,
+			       struct intel_th_device *othdev)
+{
+	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+	int i, id;
+
+	if (othdev->type != INTEL_TH_OUTPUT)
+		return -EINVAL;
+
+	for (i = 0, id = 0; i < TH_POSSIBLE_OUTPUTS; i++) {
+		if (gth->output[i].port_type != othdev->output.type)
+			continue;
+
+		if (othdev->id == -1 || othdev->id == id)
+			goto found;
+
+		id++;
+	}
+
+	return -ENOENT;
+
+found:
+	spin_lock(&gth->gth_lock);
+	othdev->output.port = i;
+	othdev->output.active = false;
+	gth->output[i].output = &othdev->output;
+	spin_unlock(&gth->gth_lock);
+
+	return 0;
+}
+
+/**
+ * intel_th_gth_unassign() - deassociate an output device from its output port
+ * @thdev:	GTH device
+ * @othdev:	output device
+ */
+static void intel_th_gth_unassign(struct intel_th_device *thdev,
+				  struct intel_th_device *othdev)
+{
+	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+	int port = othdev->output.port;
+
+	spin_lock(&gth->gth_lock);
+	othdev->output.port = -1;
+	othdev->output.active = false;
+	gth->output[port].output = NULL;
+	spin_unlock(&gth->gth_lock);
+}
+
+static int
+intel_th_gth_set_output(struct intel_th_device *thdev, unsigned int master)
+{
+	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+	int port = 0; /* FIXME: make default output configurable */
+
+	/*
+	 * everything above TH_CONFIGURABLE_MASTERS is controlled by the
+	 * same register
+	 */
+	if (master > TH_CONFIGURABLE_MASTERS)
+		master = TH_CONFIGURABLE_MASTERS;
+
+	spin_lock(&gth->gth_lock);
+	if (gth->master[master] == -1) {
+		set_bit(master, gth->output[port].master);
+		gth->master[master] = port;
+	}
+	spin_unlock(&gth->gth_lock);
+
+	return 0;
+}
+
+static int intel_th_gth_probe(struct intel_th_device *thdev)
+{
+	struct device *dev = &thdev->dev;
+	struct gth_device *gth;
+	struct resource *res;
+	void __iomem *base;
+	int i, ret;
+
+	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!base)
+		return -ENOMEM;
+
+	gth = devm_kzalloc(dev, sizeof(*gth), GFP_KERNEL);
+	if (!gth)
+		return -ENOMEM;
+
+	gth->dev = dev;
+	gth->base = base;
+	spin_lock_init(&gth->gth_lock);
+
+	ret = intel_th_gth_reset(gth);
+	if (ret)
+		return ret;
+
+	for (i = 0; i < TH_CONFIGURABLE_MASTERS + 1; i++)
+		gth->master[i] = -1;
+
+	for (i = 0; i < TH_POSSIBLE_OUTPUTS; i++) {
+		gth->output[i].gth = gth;
+		gth->output[i].index = i;
+		gth->output[i].port_type =
+			gth_output_parm_get(gth, i, TH_OUTPUT_PARM(port));
+	}
+
+	if (intel_th_output_attributes(gth) ||
+	    intel_th_master_attributes(gth)) {
+		pr_warn("Can't initialize sysfs attributes\n");
+
+		if (gth->output_group.attrs)
+			sysfs_remove_group(&gth->dev->kobj, &gth->output_group);
+		return -ENOMEM;
+	}
+
+	dev_set_drvdata(dev, gth);
+
+	return 0;
+}
+
+static void intel_th_gth_remove(struct intel_th_device *thdev)
+{
+	struct gth_device *gth = dev_get_drvdata(&thdev->dev);
+
+	sysfs_remove_group(&gth->dev->kobj, &gth->output_group);
+	sysfs_remove_group(&gth->dev->kobj, &gth->master_group);
+}
+
+static struct intel_th_driver intel_th_gth_driver = {
+	.probe		= intel_th_gth_probe,
+	.remove		= intel_th_gth_remove,
+	.assign		= intel_th_gth_assign,
+	.unassign	= intel_th_gth_unassign,
+	.set_output	= intel_th_gth_set_output,
+	.enable		= intel_th_gth_enable,
+	.disable	= intel_th_gth_disable,
+	.driver	= {
+		.name	= "gth",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_driver(intel_th_gth_driver,
+	      intel_th_driver_register,
+	      intel_th_driver_unregister);
+
+MODULE_ALIAS("intel_th_switch");
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel(R) Trace Hub Global Trace Hub driver");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/intel_th/gth.h b/drivers/hwtracing/intel_th/gth.h
new file mode 100644
index 000000000..3b714b7a6
--- /dev/null
+++ b/drivers/hwtracing/intel_th/gth.h
@@ -0,0 +1,66 @@
+/*
+ * Intel(R) Trace Hub Global Trace Hub (GTH) data structures
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#ifndef __INTEL_TH_GTH_H__
+#define __INTEL_TH_GTH_H__
+
+/* Map output port parameter bits to symbolic names */
+#define TH_OUTPUT_PARM(name)			\
+	TH_OUTPUT_ ## name
+
+enum intel_th_output_parm {
+	/* output port type */
+	TH_OUTPUT_PARM(port),
+	/* generate NULL packet */
+	TH_OUTPUT_PARM(null),
+	/* packet drop */
+	TH_OUTPUT_PARM(drop),
+	/* port in reset state */
+	TH_OUTPUT_PARM(reset),
+	/* flush out data */
+	TH_OUTPUT_PARM(flush),
+	/* mainenance packet frequency */
+	TH_OUTPUT_PARM(smcfreq),
+};
+
+/*
+ * Register offsets
+ */
+enum {
+	REG_GTH_GTHOPT0		= 0x00, /* Output ports 0..3 config */
+	REG_GTH_GTHOPT1		= 0x04, /* Output ports 4..7 config */
+	REG_GTH_SWDEST0		= 0x08, /* Switching destination masters 0..7 */
+	REG_GTH_GSWTDEST	= 0x88, /* Global sw trace destination */
+	REG_GTH_SMCR0		= 0x9c, /* STP mainenance for ports 0/1 */
+	REG_GTH_SMCR1		= 0xa0, /* STP mainenance for ports 2/3 */
+	REG_GTH_SMCR2		= 0xa4, /* STP mainenance for ports 4/5 */
+	REG_GTH_SMCR3		= 0xa8, /* STP mainenance for ports 6/7 */
+	REG_GTH_SCR		= 0xc8, /* Source control (storeEn override) */
+	REG_GTH_STAT		= 0xd4, /* GTH status */
+	REG_GTH_SCR2		= 0xd8, /* Source control (force storeEn off) */
+	REG_GTH_DESTOVR		= 0xdc, /* Destination override */
+	REG_GTH_SCRPD0		= 0xe0, /* ScratchPad[0] */
+	REG_GTH_SCRPD1		= 0xe4, /* ScratchPad[1] */
+	REG_GTH_SCRPD2		= 0xe8, /* ScratchPad[2] */
+	REG_GTH_SCRPD3		= 0xec, /* ScratchPad[3] */
+};
+
+/* Externall debugger is using Intel TH */
+#define SCRPD_DEBUGGER_IN_USE	BIT(24)
+
+/* waiting for Pipeline Empty bit(s) to assert for GTH */
+#define GTH_PLE_WAITLOOP_DEPTH	10000
+
+#endif /* __INTEL_TH_GTH_H__ */
diff --git a/drivers/hwtracing/intel_th/intel_th.h b/drivers/hwtracing/intel_th/intel_th.h
new file mode 100644
index 000000000..57fd72b20
--- /dev/null
+++ b/drivers/hwtracing/intel_th/intel_th.h
@@ -0,0 +1,244 @@
+/*
+ * Intel(R) Trace Hub data structures
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#ifndef __INTEL_TH_H__
+#define __INTEL_TH_H__
+
+/* intel_th_device device types */
+enum {
+	/* Devices that generate trace data */
+	INTEL_TH_SOURCE = 0,
+	/* Output ports (MSC, PTI) */
+	INTEL_TH_OUTPUT,
+	/* Switch, the Global Trace Hub (GTH) */
+	INTEL_TH_SWITCH,
+};
+
+/**
+ * struct intel_th_output - descriptor INTEL_TH_OUTPUT type devices
+ * @port:	output port number, assigned by the switch
+ * @type:	GTH_{MSU,CTP,PTI}
+ * @multiblock:	true for multiblock output configuration
+ * @active:	true when this output is enabled
+ *
+ * Output port descriptor, used by switch driver to tell which output
+ * port this output device corresponds to. Filled in at output device's
+ * probe time by switch::assign(). Passed from output device driver to
+ * switch related code to enable/disable its port.
+ */
+struct intel_th_output {
+	int		port;
+	unsigned int	type;
+	bool		multiblock;
+	bool		active;
+};
+
+/**
+ * struct intel_th_device - device on the intel_th bus
+ * @dev:		device
+ * @resource:		array of resources available to this device
+ * @num_resources:	number of resources in @resource array
+ * @type:		INTEL_TH_{SOURCE,OUTPUT,SWITCH}
+ * @id:			device instance or -1
+ * @output:		output descriptor for INTEL_TH_OUTPUT devices
+ * @name:		device name to match the driver
+ */
+struct intel_th_device {
+	struct device	dev;
+	struct resource	*resource;
+	unsigned int	num_resources;
+	unsigned int	type;
+	int		id;
+
+	/* INTEL_TH_OUTPUT specific */
+	struct intel_th_output	output;
+
+	char		name[];
+};
+
+#define to_intel_th_device(_d)				\
+	container_of((_d), struct intel_th_device, dev)
+
+/**
+ * intel_th_device_get_resource() - obtain @num'th resource of type @type
+ * @thdev:	the device to search the resource for
+ * @type:	resource type
+ * @num:	number of the resource
+ */
+static inline struct resource *
+intel_th_device_get_resource(struct intel_th_device *thdev, unsigned int type,
+			     unsigned int num)
+{
+	int i;
+
+	for (i = 0; i < thdev->num_resources; i++)
+		if (resource_type(&thdev->resource[i]) == type && !num--)
+			return &thdev->resource[i];
+
+	return NULL;
+}
+
+/**
+ * intel_th_output_assigned() - if an output device is assigned to a switch port
+ * @thdev:	the output device
+ *
+ * Return:	true if the device is INTEL_TH_OUTPUT *and* is assigned a port
+ */
+static inline bool
+intel_th_output_assigned(struct intel_th_device *thdev)
+{
+	return thdev->type == INTEL_TH_OUTPUT &&
+		thdev->output.port >= 0;
+}
+
+/**
+ * struct intel_th_driver - driver for an intel_th_device device
+ * @driver:	generic driver
+ * @probe:	probe method
+ * @remove:	remove method
+ * @assign:	match a given output type device against available outputs
+ * @unassign:	deassociate an output type device from an output port
+ * @enable:	enable tracing for a given output device
+ * @disable:	disable tracing for a given output device
+ * @fops:	file operations for device nodes
+ *
+ * Callbacks @probe and @remove are required for all device types.
+ * Switch device driver needs to fill in @assign, @enable and @disable
+ * callbacks.
+ */
+struct intel_th_driver {
+	struct device_driver	driver;
+	int			(*probe)(struct intel_th_device *thdev);
+	void			(*remove)(struct intel_th_device *thdev);
+	/* switch (GTH) ops */
+	int			(*assign)(struct intel_th_device *thdev,
+					  struct intel_th_device *othdev);
+	void			(*unassign)(struct intel_th_device *thdev,
+					    struct intel_th_device *othdev);
+	void			(*enable)(struct intel_th_device *thdev,
+					  struct intel_th_output *output);
+	void			(*disable)(struct intel_th_device *thdev,
+					   struct intel_th_output *output);
+	/* output ops */
+	void			(*irq)(struct intel_th_device *thdev);
+	int			(*activate)(struct intel_th_device *thdev);
+	void			(*deactivate)(struct intel_th_device *thdev);
+	/* file_operations for those who want a device node */
+	const struct file_operations *fops;
+
+	/* source ops */
+	int			(*set_output)(struct intel_th_device *thdev,
+					      unsigned int master);
+};
+
+#define to_intel_th_driver(_d)					\
+	container_of((_d), struct intel_th_driver, driver)
+
+static inline struct intel_th_device *
+to_intel_th_hub(struct intel_th_device *thdev)
+{
+	struct device *parent = thdev->dev.parent;
+
+	if (!parent)
+		return NULL;
+
+	return to_intel_th_device(parent);
+}
+
+struct intel_th *
+intel_th_alloc(struct device *dev, struct resource *devres,
+	       unsigned int ndevres, int irq);
+void intel_th_free(struct intel_th *th);
+
+int intel_th_driver_register(struct intel_th_driver *thdrv);
+void intel_th_driver_unregister(struct intel_th_driver *thdrv);
+
+int intel_th_trace_enable(struct intel_th_device *thdev);
+int intel_th_trace_disable(struct intel_th_device *thdev);
+int intel_th_set_output(struct intel_th_device *thdev,
+			unsigned int master);
+
+enum {
+	TH_MMIO_CONFIG = 0,
+	TH_MMIO_SW = 2,
+	TH_MMIO_END,
+};
+
+#define TH_SUBDEVICE_MAX	6
+#define TH_POSSIBLE_OUTPUTS	8
+#define TH_CONFIGURABLE_MASTERS 256
+#define TH_MSC_MAX		2
+
+/**
+ * struct intel_th - Intel TH controller
+ * @dev:	driver core's device
+ * @thdev:	subdevices
+ * @hub:	"switch" subdevice (GTH)
+ * @id:		this Intel TH controller's device ID in the system
+ * @major:	device node major for output devices
+ */
+struct intel_th {
+	struct device		*dev;
+
+	struct intel_th_device	*thdev[TH_SUBDEVICE_MAX];
+	struct intel_th_device	*hub;
+
+	int			id;
+	int			major;
+#ifdef CONFIG_INTEL_TH_DEBUG
+	struct dentry		*dbg;
+#endif
+};
+
+/*
+ * Register windows
+ */
+enum {
+	/* Global Trace Hub (GTH) */
+	REG_GTH_OFFSET		= 0x0000,
+	REG_GTH_LENGTH		= 0x2000,
+
+	/* Software Trace Hub (STH) [0x4000..0x4fff] */
+	REG_STH_OFFSET		= 0x4000,
+	REG_STH_LENGTH		= 0x2000,
+
+	/* Memory Storage Unit (MSU) [0xa0000..0xa1fff] */
+	REG_MSU_OFFSET		= 0xa0000,
+	REG_MSU_LENGTH		= 0x02000,
+
+	/* Internal MSU trace buffer [0x80000..0x9ffff] */
+	BUF_MSU_OFFSET		= 0x80000,
+	BUF_MSU_LENGTH		= 0x20000,
+
+	/* PTI output == same window as GTH */
+	REG_PTI_OFFSET		= REG_GTH_OFFSET,
+	REG_PTI_LENGTH		= REG_GTH_LENGTH,
+
+	/* DCI Handler (DCIH) == some window as MSU */
+	REG_DCIH_OFFSET		= REG_MSU_OFFSET,
+	REG_DCIH_LENGTH		= REG_MSU_LENGTH,
+};
+
+/*
+ * GTH, output ports configuration
+ */
+enum {
+	GTH_NONE = 0,
+	GTH_MSU,	/* memory/usb */
+	GTH_CTP,	/* Common Trace Port */
+	GTH_PTI = 4,	/* MIPI-PTI */
+};
+
+#endif
diff --git a/drivers/hwtracing/intel_th/msu.c b/drivers/hwtracing/intel_th/msu.c
new file mode 100644
index 000000000..70ca27e45
--- /dev/null
+++ b/drivers/hwtracing/intel_th/msu.c
@@ -0,0 +1,1509 @@
+/*
+ * Intel(R) Trace Hub Memory Storage Unit
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/uaccess.h>
+#include <linux/sizes.h>
+#include <linux/printk.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/fs.h>
+#include <linux/io.h>
+#include <linux/dma-mapping.h>
+
+#include <asm/cacheflush.h>
+
+#include "intel_th.h"
+#include "msu.h"
+
+#define msc_dev(x) (&(x)->thdev->dev)
+
+/**
+ * struct msc_block - multiblock mode block descriptor
+ * @bdesc:	pointer to hardware descriptor (beginning of the block)
+ * @addr:	physical address of the block
+ */
+struct msc_block {
+	struct msc_block_desc	*bdesc;
+	dma_addr_t		addr;
+};
+
+/**
+ * struct msc_window - multiblock mode window descriptor
+ * @entry:	window list linkage (msc::win_list)
+ * @pgoff:	page offset into the buffer that this window starts at
+ * @nr_blocks:	number of blocks (pages) in this window
+ * @block:	array of block descriptors
+ */
+struct msc_window {
+	struct list_head	entry;
+	unsigned long		pgoff;
+	unsigned int		nr_blocks;
+	struct msc		*msc;
+	struct msc_block	block[0];
+};
+
+/**
+ * struct msc_iter - iterator for msc buffer
+ * @entry:		msc::iter_list linkage
+ * @msc:		pointer to the MSC device
+ * @start_win:		oldest window
+ * @win:		current window
+ * @offset:		current logical offset into the buffer
+ * @start_block:	oldest block in the window
+ * @block:		block number in the window
+ * @block_off:		offset into current block
+ * @wrap_count:		block wrapping handling
+ * @eof:		end of buffer reached
+ */
+struct msc_iter {
+	struct list_head	entry;
+	struct msc		*msc;
+	struct msc_window	*start_win;
+	struct msc_window	*win;
+	unsigned long		offset;
+	int			start_block;
+	int			block;
+	unsigned int		block_off;
+	unsigned int		wrap_count;
+	unsigned int		eof;
+};
+
+/**
+ * struct msc - MSC device representation
+ * @reg_base:		register window base address
+ * @thdev:		intel_th_device pointer
+ * @win_list:		list of windows in multiblock mode
+ * @nr_pages:		total number of pages allocated for this buffer
+ * @single_sz:		amount of data in single mode
+ * @single_wrap:	single mode wrap occurred
+ * @base:		buffer's base pointer
+ * @base_addr:		buffer's base address
+ * @user_count:		number of users of the buffer
+ * @mmap_count:		number of mappings
+ * @buf_mutex:		mutex to serialize access to buffer-related bits
+
+ * @enabled:		MSC is enabled
+ * @wrap:		wrapping is enabled
+ * @mode:		MSC operating mode
+ * @burst_len:		write burst length
+ * @index:		number of this MSC in the MSU
+ */
+struct msc {
+	void __iomem		*reg_base;
+	struct intel_th_device	*thdev;
+
+	struct list_head	win_list;
+	unsigned long		nr_pages;
+	unsigned long		single_sz;
+	unsigned int		single_wrap : 1;
+	void			*base;
+	dma_addr_t		base_addr;
+
+	/* <0: no buffer, 0: no users, >0: active users */
+	atomic_t		user_count;
+
+	atomic_t		mmap_count;
+	struct mutex		buf_mutex;
+
+	struct mutex		iter_mutex;
+	struct list_head	iter_list;
+
+	/* config */
+	unsigned int		enabled : 1,
+				wrap	: 1;
+	unsigned int		mode;
+	unsigned int		burst_len;
+	unsigned int		index;
+};
+
+static inline bool msc_block_is_empty(struct msc_block_desc *bdesc)
+{
+	/* header hasn't been written */
+	if (!bdesc->valid_dw)
+		return true;
+
+	/* valid_dw includes the header */
+	if (!msc_data_sz(bdesc))
+		return true;
+
+	return false;
+}
+
+/**
+ * msc_oldest_window() - locate the window with oldest data
+ * @msc:	MSC device
+ *
+ * This should only be used in multiblock mode. Caller should hold the
+ * msc::user_count reference.
+ *
+ * Return:	the oldest window with valid data
+ */
+static struct msc_window *msc_oldest_window(struct msc *msc)
+{
+	struct msc_window *win;
+	u32 reg = ioread32(msc->reg_base + REG_MSU_MSC0NWSA);
+	unsigned long win_addr = (unsigned long)reg << PAGE_SHIFT;
+	unsigned int found = 0;
+
+	if (list_empty(&msc->win_list))
+		return NULL;
+
+	/*
+	 * we might need a radix tree for this, depending on how
+	 * many windows a typical user would allocate; ideally it's
+	 * something like 2, in which case we're good
+	 */
+	list_for_each_entry(win, &msc->win_list, entry) {
+		if (win->block[0].addr == win_addr)
+			found++;
+
+		/* skip the empty ones */
+		if (msc_block_is_empty(win->block[0].bdesc))
+			continue;
+
+		if (found)
+			return win;
+	}
+
+	return list_entry(msc->win_list.next, struct msc_window, entry);
+}
+
+/**
+ * msc_win_oldest_block() - locate the oldest block in a given window
+ * @win:	window to look at
+ *
+ * Return:	index of the block with the oldest data
+ */
+static unsigned int msc_win_oldest_block(struct msc_window *win)
+{
+	unsigned int blk;
+	struct msc_block_desc *bdesc = win->block[0].bdesc;
+
+	/* without wrapping, first block is the oldest */
+	if (!msc_block_wrapped(bdesc))
+		return 0;
+
+	/*
+	 * with wrapping, last written block contains both the newest and the
+	 * oldest data for this window.
+	 */
+	for (blk = 0; blk < win->nr_blocks; blk++) {
+		bdesc = win->block[blk].bdesc;
+
+		if (msc_block_last_written(bdesc))
+			return blk;
+	}
+
+	return 0;
+}
+
+/**
+ * msc_is_last_win() - check if a window is the last one for a given MSC
+ * @win:	window
+ * Return:	true if @win is the last window in MSC's multiblock buffer
+ */
+static inline bool msc_is_last_win(struct msc_window *win)
+{
+	return win->entry.next == &win->msc->win_list;
+}
+
+/**
+ * msc_next_window() - return next window in the multiblock buffer
+ * @win:	current window
+ *
+ * Return:	window following the current one
+ */
+static struct msc_window *msc_next_window(struct msc_window *win)
+{
+	if (msc_is_last_win(win))
+		return list_entry(win->msc->win_list.next, struct msc_window,
+				  entry);
+
+	return list_entry(win->entry.next, struct msc_window, entry);
+}
+
+static struct msc_block_desc *msc_iter_bdesc(struct msc_iter *iter)
+{
+	return iter->win->block[iter->block].bdesc;
+}
+
+static void msc_iter_init(struct msc_iter *iter)
+{
+	memset(iter, 0, sizeof(*iter));
+	iter->start_block = -1;
+	iter->block = -1;
+}
+
+static struct msc_iter *msc_iter_install(struct msc *msc)
+{
+	struct msc_iter *iter;
+
+	iter = kzalloc(sizeof(*iter), GFP_KERNEL);
+	if (!iter)
+		return NULL;
+
+	msc_iter_init(iter);
+	iter->msc = msc;
+
+	mutex_lock(&msc->iter_mutex);
+	list_add_tail(&iter->entry, &msc->iter_list);
+	mutex_unlock(&msc->iter_mutex);
+
+	return iter;
+}
+
+static void msc_iter_remove(struct msc_iter *iter, struct msc *msc)
+{
+	mutex_lock(&msc->iter_mutex);
+	list_del(&iter->entry);
+	mutex_unlock(&msc->iter_mutex);
+
+	kfree(iter);
+}
+
+static void msc_iter_block_start(struct msc_iter *iter)
+{
+	if (iter->start_block != -1)
+		return;
+
+	iter->start_block = msc_win_oldest_block(iter->win);
+	iter->block = iter->start_block;
+	iter->wrap_count = 0;
+
+	/*
+	 * start with the block with oldest data; if data has wrapped
+	 * in this window, it should be in this block
+	 */
+	if (msc_block_wrapped(msc_iter_bdesc(iter)))
+		iter->wrap_count = 2;
+
+}
+
+static int msc_iter_win_start(struct msc_iter *iter, struct msc *msc)
+{
+	/* already started, nothing to do */
+	if (iter->start_win)
+		return 0;
+
+	iter->start_win = msc_oldest_window(msc);
+	if (!iter->start_win)
+		return -EINVAL;
+
+	iter->win = iter->start_win;
+	iter->start_block = -1;
+
+	msc_iter_block_start(iter);
+
+	return 0;
+}
+
+static int msc_iter_win_advance(struct msc_iter *iter)
+{
+	iter->win = msc_next_window(iter->win);
+	iter->start_block = -1;
+
+	if (iter->win == iter->start_win) {
+		iter->eof++;
+		return 1;
+	}
+
+	msc_iter_block_start(iter);
+
+	return 0;
+}
+
+static int msc_iter_block_advance(struct msc_iter *iter)
+{
+	iter->block_off = 0;
+
+	/* wrapping */
+	if (iter->wrap_count && iter->block == iter->start_block) {
+		iter->wrap_count--;
+		if (!iter->wrap_count)
+			/* copied newest data from the wrapped block */
+			return msc_iter_win_advance(iter);
+	}
+
+	/* no wrapping, check for last written block */
+	if (!iter->wrap_count && msc_block_last_written(msc_iter_bdesc(iter)))
+		/* copied newest data for the window */
+		return msc_iter_win_advance(iter);
+
+	/* block advance */
+	if (++iter->block == iter->win->nr_blocks)
+		iter->block = 0;
+
+	/* no wrapping, sanity check in case there is no last written block */
+	if (!iter->wrap_count && iter->block == iter->start_block)
+		return msc_iter_win_advance(iter);
+
+	return 0;
+}
+
+/**
+ * msc_buffer_iterate() - go through multiblock buffer's data
+ * @iter:	iterator structure
+ * @size:	amount of data to scan
+ * @data:	callback's private data
+ * @fn:		iterator callback
+ *
+ * This will start at the window which will be written to next (containing
+ * the oldest data) and work its way to the current window, calling @fn
+ * for each chunk of data as it goes.
+ *
+ * Caller should have msc::user_count reference to make sure the buffer
+ * doesn't disappear from under us.
+ *
+ * Return:	amount of data actually scanned.
+ */
+static ssize_t
+msc_buffer_iterate(struct msc_iter *iter, size_t size, void *data,
+		   unsigned long (*fn)(void *, void *, size_t))
+{
+	struct msc *msc = iter->msc;
+	size_t len = size;
+	unsigned int advance;
+
+	if (iter->eof)
+		return 0;
+
+	/* start with the oldest window */
+	if (msc_iter_win_start(iter, msc))
+		return 0;
+
+	do {
+		unsigned long data_bytes = msc_data_sz(msc_iter_bdesc(iter));
+		void *src = (void *)msc_iter_bdesc(iter) + MSC_BDESC;
+		size_t tocopy = data_bytes, copied = 0;
+		size_t remaining = 0;
+
+		advance = 1;
+
+		/*
+		 * If block wrapping happened, we need to visit the last block
+		 * twice, because it contains both the oldest and the newest
+		 * data in this window.
+		 *
+		 * First time (wrap_count==2), in the very beginning, to collect
+		 * the oldest data, which is in the range
+		 * (data_bytes..DATA_IN_PAGE).
+		 *
+		 * Second time (wrap_count==1), it's just like any other block,
+		 * containing data in the range of [MSC_BDESC..data_bytes].
+		 */
+		if (iter->block == iter->start_block && iter->wrap_count) {
+			tocopy = DATA_IN_PAGE - data_bytes;
+			src += data_bytes;
+		}
+
+		if (!tocopy)
+			goto next_block;
+
+		tocopy -= iter->block_off;
+		src += iter->block_off;
+
+		if (len < tocopy) {
+			tocopy = len;
+			advance = 0;
+		}
+
+		remaining = fn(data, src, tocopy);
+
+		if (remaining)
+			advance = 0;
+
+		copied = tocopy - remaining;
+		len -= copied;
+		iter->block_off += copied;
+		iter->offset += copied;
+
+		if (!advance)
+			break;
+
+next_block:
+		if (msc_iter_block_advance(iter))
+			break;
+
+	} while (len);
+
+	return size - len;
+}
+
+/**
+ * msc_buffer_clear_hw_header() - clear hw header for multiblock
+ * @msc:	MSC device
+ */
+static void msc_buffer_clear_hw_header(struct msc *msc)
+{
+	struct msc_window *win;
+
+	mutex_lock(&msc->buf_mutex);
+	list_for_each_entry(win, &msc->win_list, entry) {
+		unsigned int blk;
+		size_t hw_sz = sizeof(struct msc_block_desc) -
+			offsetof(struct msc_block_desc, hw_tag);
+
+		for (blk = 0; blk < win->nr_blocks; blk++) {
+			struct msc_block_desc *bdesc = win->block[blk].bdesc;
+
+			memset(&bdesc->hw_tag, 0, hw_sz);
+		}
+	}
+	mutex_unlock(&msc->buf_mutex);
+}
+
+/**
+ * msc_configure() - set up MSC hardware
+ * @msc:	the MSC device to configure
+ *
+ * Program storage mode, wrapping, burst length and trace buffer address
+ * into a given MSC. If msc::enabled is set, enable the trace, too.
+ */
+static int msc_configure(struct msc *msc)
+{
+	u32 reg;
+
+	if (msc->mode > MSC_MODE_MULTI)
+		return -ENOTSUPP;
+
+	if (msc->mode == MSC_MODE_MULTI)
+		msc_buffer_clear_hw_header(msc);
+
+	reg = msc->base_addr >> PAGE_SHIFT;
+	iowrite32(reg, msc->reg_base + REG_MSU_MSC0BAR);
+
+	if (msc->mode == MSC_MODE_SINGLE) {
+		reg = msc->nr_pages;
+		iowrite32(reg, msc->reg_base + REG_MSU_MSC0SIZE);
+	}
+
+	reg = ioread32(msc->reg_base + REG_MSU_MSC0CTL);
+	reg &= ~(MSC_MODE | MSC_WRAPEN | MSC_EN | MSC_RD_HDR_OVRD);
+
+	reg |= msc->mode << __ffs(MSC_MODE);
+	reg |= msc->burst_len << __ffs(MSC_LEN);
+	/*if (msc->mode == MSC_MODE_MULTI)
+	  reg |= MSC_RD_HDR_OVRD; */
+	if (msc->wrap)
+		reg |= MSC_WRAPEN;
+	if (msc->enabled)
+		reg |= MSC_EN;
+
+	iowrite32(reg, msc->reg_base + REG_MSU_MSC0CTL);
+
+	if (msc->enabled) {
+		msc->thdev->output.multiblock = msc->mode == MSC_MODE_MULTI;
+		intel_th_trace_enable(msc->thdev);
+	}
+
+	return 0;
+}
+
+/**
+ * msc_disable() - disable MSC hardware
+ * @msc:	MSC device to disable
+ *
+ * If @msc is enabled, disable tracing on the switch and then disable MSC
+ * storage.
+ */
+static void msc_disable(struct msc *msc)
+{
+	unsigned long count;
+	u32 reg;
+
+	if (!msc->enabled)
+		return;
+
+	intel_th_trace_disable(msc->thdev);
+
+	for (reg = 0, count = MSC_PLE_WAITLOOP_DEPTH;
+	     count && !(reg & MSCSTS_PLE); count--) {
+		reg = ioread32(msc->reg_base + REG_MSU_MSC0STS);
+		cpu_relax();
+	}
+
+	if (!count)
+		dev_dbg(msc_dev(msc), "timeout waiting for MSC0 PLE\n");
+
+	if (msc->mode == MSC_MODE_SINGLE) {
+		msc->single_wrap = !!(reg & MSCSTS_WRAPSTAT);
+
+		reg = ioread32(msc->reg_base + REG_MSU_MSC0MWP);
+		msc->single_sz = reg & ((msc->nr_pages << PAGE_SHIFT) - 1);
+		dev_dbg(msc_dev(msc), "MSCnMWP: %08x/%08lx, wrap: %d\n",
+			reg, msc->single_sz, msc->single_wrap);
+	}
+
+	reg = ioread32(msc->reg_base + REG_MSU_MSC0CTL);
+	reg &= ~MSC_EN;
+	iowrite32(reg, msc->reg_base + REG_MSU_MSC0CTL);
+	msc->enabled = 0;
+
+	iowrite32(0, msc->reg_base + REG_MSU_MSC0BAR);
+	iowrite32(0, msc->reg_base + REG_MSU_MSC0SIZE);
+
+	dev_dbg(msc_dev(msc), "MSCnNWSA: %08x\n",
+		ioread32(msc->reg_base + REG_MSU_MSC0NWSA));
+
+	reg = ioread32(msc->reg_base + REG_MSU_MSC0STS);
+	dev_dbg(msc_dev(msc), "MSCnSTS: %08x\n", reg);
+}
+
+static int intel_th_msc_activate(struct intel_th_device *thdev)
+{
+	struct msc *msc = dev_get_drvdata(&thdev->dev);
+	int ret = 0;
+
+	if (!atomic_inc_unless_negative(&msc->user_count))
+		return -ENODEV;
+
+	mutex_lock(&msc->iter_mutex);
+	if (!list_empty(&msc->iter_list))
+		ret = -EBUSY;
+	mutex_unlock(&msc->iter_mutex);
+
+	if (ret) {
+		atomic_dec(&msc->user_count);
+		return ret;
+	}
+
+	msc->enabled = 1;
+
+	return msc_configure(msc);
+}
+
+static void intel_th_msc_deactivate(struct intel_th_device *thdev)
+{
+	struct msc *msc = dev_get_drvdata(&thdev->dev);
+
+	msc_disable(msc);
+
+	atomic_dec(&msc->user_count);
+}
+
+/**
+ * msc_buffer_contig_alloc() - allocate a contiguous buffer for SINGLE mode
+ * @msc:	MSC device
+ * @size:	allocation size in bytes
+ *
+ * This modifies msc::base, which requires msc::buf_mutex to serialize, so the
+ * caller is expected to hold it.
+ *
+ * Return:	0 on success, -errno otherwise.
+ */
+static int msc_buffer_contig_alloc(struct msc *msc, unsigned long size)
+{
+	unsigned int order = get_order(size);
+	struct page *page;
+
+	if (!size)
+		return 0;
+
+	page = alloc_pages(GFP_KERNEL | __GFP_ZERO, order);
+	if (!page)
+		return -ENOMEM;
+
+	split_page(page, order);
+	msc->nr_pages = size >> PAGE_SHIFT;
+	msc->base = page_address(page);
+	msc->base_addr = page_to_phys(page);
+
+	return 0;
+}
+
+/**
+ * msc_buffer_contig_free() - free a contiguous buffer
+ * @msc:	MSC configured in SINGLE mode
+ */
+static void msc_buffer_contig_free(struct msc *msc)
+{
+	unsigned long off;
+
+	for (off = 0; off < msc->nr_pages << PAGE_SHIFT; off += PAGE_SIZE) {
+		struct page *page = virt_to_page(msc->base + off);
+
+		page->mapping = NULL;
+		__free_page(page);
+	}
+
+	msc->nr_pages = 0;
+}
+
+/**
+ * msc_buffer_contig_get_page() - find a page at a given offset
+ * @msc:	MSC configured in SINGLE mode
+ * @pgoff:	page offset
+ *
+ * Return:	page, if @pgoff is within the range, NULL otherwise.
+ */
+static struct page *msc_buffer_contig_get_page(struct msc *msc,
+					       unsigned long pgoff)
+{
+	if (pgoff >= msc->nr_pages)
+		return NULL;
+
+	return virt_to_page(msc->base + (pgoff << PAGE_SHIFT));
+}
+
+/**
+ * msc_buffer_win_alloc() - alloc a window for a multiblock mode
+ * @msc:	MSC device
+ * @nr_blocks:	number of pages in this window
+ *
+ * This modifies msc::win_list and msc::base, which requires msc::buf_mutex
+ * to serialize, so the caller is expected to hold it.
+ *
+ * Return:	0 on success, -errno otherwise.
+ */
+static int msc_buffer_win_alloc(struct msc *msc, unsigned int nr_blocks)
+{
+	struct msc_window *win;
+	unsigned long size = PAGE_SIZE;
+	int i, ret = -ENOMEM;
+
+	if (!nr_blocks)
+		return 0;
+
+	win = kzalloc(offsetof(struct msc_window, block[nr_blocks]),
+		      GFP_KERNEL);
+	if (!win)
+		return -ENOMEM;
+
+	if (!list_empty(&msc->win_list)) {
+		struct msc_window *prev = list_entry(msc->win_list.prev,
+						     struct msc_window, entry);
+
+		win->pgoff = prev->pgoff + prev->nr_blocks;
+	}
+
+	for (i = 0; i < nr_blocks; i++) {
+		win->block[i].bdesc = dma_alloc_coherent(msc_dev(msc), size,
+							 &win->block[i].addr,
+							 GFP_KERNEL);
+
+#ifdef CONFIG_X86
+		/* Set the page as uncached */
+		set_memory_uc((unsigned long)win->block[i].bdesc, 1);
+#endif
+
+		if (!win->block[i].bdesc)
+			goto err_nomem;
+	}
+
+	win->msc = msc;
+	win->nr_blocks = nr_blocks;
+
+	if (list_empty(&msc->win_list)) {
+		msc->base = win->block[0].bdesc;
+		msc->base_addr = win->block[0].addr;
+	}
+
+	list_add_tail(&win->entry, &msc->win_list);
+	msc->nr_pages += nr_blocks;
+
+	return 0;
+
+err_nomem:
+	for (i--; i >= 0; i--) {
+#ifdef CONFIG_X86
+		/* Reset the page to write-back before releasing */
+		set_memory_wb((unsigned long)win->block[i].bdesc, 1);
+#endif
+		dma_free_coherent(msc_dev(msc), size, win->block[i].bdesc,
+				  win->block[i].addr);
+	}
+	kfree(win);
+
+	return ret;
+}
+
+/**
+ * msc_buffer_win_free() - free a window from MSC's window list
+ * @msc:	MSC device
+ * @win:	window to free
+ *
+ * This modifies msc::win_list and msc::base, which requires msc::buf_mutex
+ * to serialize, so the caller is expected to hold it.
+ */
+static void msc_buffer_win_free(struct msc *msc, struct msc_window *win)
+{
+	int i;
+
+	msc->nr_pages -= win->nr_blocks;
+
+	list_del(&win->entry);
+	if (list_empty(&msc->win_list)) {
+		msc->base = NULL;
+		msc->base_addr = 0;
+	}
+
+	for (i = 0; i < win->nr_blocks; i++) {
+		struct page *page = virt_to_page(win->block[i].bdesc);
+
+		page->mapping = NULL;
+#ifdef CONFIG_X86
+		/* Reset the page to write-back before releasing */
+		set_memory_wb((unsigned long)win->block[i].bdesc, 1);
+#endif
+		dma_free_coherent(msc_dev(win->msc), PAGE_SIZE,
+				  win->block[i].bdesc, win->block[i].addr);
+	}
+
+	kfree(win);
+}
+
+/**
+ * msc_buffer_relink() - set up block descriptors for multiblock mode
+ * @msc:	MSC device
+ *
+ * This traverses msc::win_list, which requires msc::buf_mutex to serialize,
+ * so the caller is expected to hold it.
+ */
+static void msc_buffer_relink(struct msc *msc)
+{
+	struct msc_window *win, *next_win;
+
+	/* call with msc::mutex locked */
+	list_for_each_entry(win, &msc->win_list, entry) {
+		unsigned int blk;
+		u32 sw_tag = 0;
+
+		/*
+		 * Last window's next_win should point to the first window
+		 * and MSC_SW_TAG_LASTWIN should be set.
+		 */
+		if (msc_is_last_win(win)) {
+			sw_tag |= MSC_SW_TAG_LASTWIN;
+			next_win = list_entry(msc->win_list.next,
+					      struct msc_window, entry);
+		} else {
+			next_win = list_entry(win->entry.next,
+					      struct msc_window, entry);
+		}
+
+		for (blk = 0; blk < win->nr_blocks; blk++) {
+			struct msc_block_desc *bdesc = win->block[blk].bdesc;
+
+			memset(bdesc, 0, sizeof(*bdesc));
+
+			bdesc->next_win = next_win->block[0].addr >> PAGE_SHIFT;
+
+			/*
+			 * Similarly to last window, last block should point
+			 * to the first one.
+			 */
+			if (blk == win->nr_blocks - 1) {
+				sw_tag |= MSC_SW_TAG_LASTBLK;
+				bdesc->next_blk =
+					win->block[0].addr >> PAGE_SHIFT;
+			} else {
+				bdesc->next_blk =
+					win->block[blk + 1].addr >> PAGE_SHIFT;
+			}
+
+			bdesc->sw_tag = sw_tag;
+			bdesc->block_sz = PAGE_SIZE / 64;
+		}
+	}
+
+	/*
+	 * Make the above writes globally visible before tracing is
+	 * enabled to make sure hardware sees them coherently.
+	 */
+	wmb();
+}
+
+static void msc_buffer_multi_free(struct msc *msc)
+{
+	struct msc_window *win, *iter;
+
+	list_for_each_entry_safe(win, iter, &msc->win_list, entry)
+		msc_buffer_win_free(msc, win);
+}
+
+static int msc_buffer_multi_alloc(struct msc *msc, unsigned long *nr_pages,
+				  unsigned int nr_wins)
+{
+	int ret, i;
+
+	for (i = 0; i < nr_wins; i++) {
+		ret = msc_buffer_win_alloc(msc, nr_pages[i]);
+		if (ret) {
+			msc_buffer_multi_free(msc);
+			return ret;
+		}
+	}
+
+	msc_buffer_relink(msc);
+
+	return 0;
+}
+
+/**
+ * msc_buffer_free() - free buffers for MSC
+ * @msc:	MSC device
+ *
+ * Free MSC's storage buffers.
+ *
+ * This modifies msc::win_list and msc::base, which requires msc::buf_mutex to
+ * serialize, so the caller is expected to hold it.
+ */
+static void msc_buffer_free(struct msc *msc)
+{
+	if (msc->mode == MSC_MODE_SINGLE)
+		msc_buffer_contig_free(msc);
+	else if (msc->mode == MSC_MODE_MULTI)
+		msc_buffer_multi_free(msc);
+}
+
+/**
+ * msc_buffer_alloc() - allocate a buffer for MSC
+ * @msc:	MSC device
+ * @size:	allocation size in bytes
+ *
+ * Allocate a storage buffer for MSC, depending on the msc::mode, it will be
+ * either done via msc_buffer_contig_alloc() for SINGLE operation mode or
+ * msc_buffer_win_alloc() for multiblock operation. The latter allocates one
+ * window per invocation, so in multiblock mode this can be called multiple
+ * times for the same MSC to allocate multiple windows.
+ *
+ * This modifies msc::win_list and msc::base, which requires msc::buf_mutex
+ * to serialize, so the caller is expected to hold it.
+ *
+ * Return:	0 on success, -errno otherwise.
+ */
+static int msc_buffer_alloc(struct msc *msc, unsigned long *nr_pages,
+			    unsigned int nr_wins)
+{
+	int ret;
+
+	/* -1: buffer not allocated */
+	if (atomic_read(&msc->user_count) != -1)
+		return -EBUSY;
+
+	if (msc->mode == MSC_MODE_SINGLE) {
+		if (nr_wins != 1)
+			return -EINVAL;
+
+		ret = msc_buffer_contig_alloc(msc, nr_pages[0] << PAGE_SHIFT);
+	} else if (msc->mode == MSC_MODE_MULTI) {
+		ret = msc_buffer_multi_alloc(msc, nr_pages, nr_wins);
+	} else {
+		ret = -ENOTSUPP;
+	}
+
+	if (!ret) {
+		/* allocation should be visible before the counter goes to 0 */
+		smp_mb__before_atomic();
+
+		if (WARN_ON_ONCE(atomic_cmpxchg(&msc->user_count, -1, 0) != -1))
+			return -EINVAL;
+	}
+
+	return ret;
+}
+
+/**
+ * msc_buffer_unlocked_free_unless_used() - free a buffer unless it's in use
+ * @msc:	MSC device
+ *
+ * This will free MSC buffer unless it is in use or there is no allocated
+ * buffer.
+ * Caller needs to hold msc::buf_mutex.
+ *
+ * Return:	0 on successful deallocation or if there was no buffer to
+ *		deallocate, -EBUSY if there are active users.
+ */
+static int msc_buffer_unlocked_free_unless_used(struct msc *msc)
+{
+	int count, ret = 0;
+
+	count = atomic_cmpxchg(&msc->user_count, 0, -1);
+
+	/* > 0: buffer is allocated and has users */
+	if (count > 0)
+		ret = -EBUSY;
+	/* 0: buffer is allocated, no users */
+	else if (!count)
+		msc_buffer_free(msc);
+	/* < 0: no buffer, nothing to do */
+
+	return ret;
+}
+
+/**
+ * msc_buffer_free_unless_used() - free a buffer unless it's in use
+ * @msc:	MSC device
+ *
+ * This is a locked version of msc_buffer_unlocked_free_unless_used().
+ */
+static int msc_buffer_free_unless_used(struct msc *msc)
+{
+	int ret;
+
+	mutex_lock(&msc->buf_mutex);
+	ret = msc_buffer_unlocked_free_unless_used(msc);
+	mutex_unlock(&msc->buf_mutex);
+
+	return ret;
+}
+
+/**
+ * msc_buffer_get_page() - get MSC buffer page at a given offset
+ * @msc:	MSC device
+ * @pgoff:	page offset into the storage buffer
+ *
+ * This traverses msc::win_list, so holding msc::buf_mutex is expected from
+ * the caller.
+ *
+ * Return:	page if @pgoff corresponds to a valid buffer page or NULL.
+ */
+static struct page *msc_buffer_get_page(struct msc *msc, unsigned long pgoff)
+{
+	struct msc_window *win;
+
+	if (msc->mode == MSC_MODE_SINGLE)
+		return msc_buffer_contig_get_page(msc, pgoff);
+
+	list_for_each_entry(win, &msc->win_list, entry)
+		if (pgoff >= win->pgoff && pgoff < win->pgoff + win->nr_blocks)
+			goto found;
+
+	return NULL;
+
+found:
+	pgoff -= win->pgoff;
+	return virt_to_page(win->block[pgoff].bdesc);
+}
+
+/**
+ * struct msc_win_to_user_struct - data for copy_to_user() callback
+ * @buf:	userspace buffer to copy data to
+ * @offset:	running offset
+ */
+struct msc_win_to_user_struct {
+	char __user	*buf;
+	unsigned long	offset;
+};
+
+/**
+ * msc_win_to_user() - iterator for msc_buffer_iterate() to copy data to user
+ * @data:	callback's private data
+ * @src:	source buffer
+ * @len:	amount of data to copy from the source buffer
+ */
+static unsigned long msc_win_to_user(void *data, void *src, size_t len)
+{
+	struct msc_win_to_user_struct *u = data;
+	unsigned long ret;
+
+	ret = copy_to_user(u->buf + u->offset, src, len);
+	u->offset += len - ret;
+
+	return ret;
+}
+
+
+/*
+ * file operations' callbacks
+ */
+
+static int intel_th_msc_open(struct inode *inode, struct file *file)
+{
+	struct intel_th_device *thdev = file->private_data;
+	struct msc *msc = dev_get_drvdata(&thdev->dev);
+	struct msc_iter *iter;
+
+	if (!capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	iter = msc_iter_install(msc);
+	if (!iter)
+		return -ENOMEM;
+
+	file->private_data = iter;
+
+	return nonseekable_open(inode, file);
+}
+
+static int intel_th_msc_release(struct inode *inode, struct file *file)
+{
+	struct msc_iter *iter = file->private_data;
+	struct msc *msc = iter->msc;
+
+	msc_iter_remove(iter, msc);
+
+	return 0;
+}
+
+static ssize_t
+msc_single_to_user(struct msc *msc, char __user *buf, loff_t off, size_t len)
+{
+	unsigned long size = msc->nr_pages << PAGE_SHIFT, rem = len;
+	unsigned long start = off, tocopy = 0;
+
+	if (msc->single_wrap) {
+		start += msc->single_sz;
+		if (start < size) {
+			tocopy = min(rem, size - start);
+			if (copy_to_user(buf, msc->base + start, tocopy))
+				return -EFAULT;
+
+			buf += tocopy;
+			rem -= tocopy;
+			start += tocopy;
+		}
+
+		start &= size - 1;
+		if (rem) {
+			tocopy = min(rem, msc->single_sz - start);
+			if (copy_to_user(buf, msc->base + start, tocopy))
+				return -EFAULT;
+
+			rem -= tocopy;
+		}
+
+		return len - rem;
+	}
+
+	if (copy_to_user(buf, msc->base + start, rem))
+		return -EFAULT;
+
+	return len;
+}
+
+static ssize_t intel_th_msc_read(struct file *file, char __user *buf,
+				 size_t len, loff_t *ppos)
+{
+	struct msc_iter *iter = file->private_data;
+	struct msc *msc = iter->msc;
+	size_t size;
+	loff_t off = *ppos;
+	ssize_t ret = 0;
+
+	if (!atomic_inc_unless_negative(&msc->user_count))
+		return 0;
+
+	if (msc->enabled) {
+		ret = -EBUSY;
+		goto put_count;
+	}
+
+	if (msc->mode == MSC_MODE_SINGLE && !msc->single_wrap)
+		size = msc->single_sz;
+	else
+		size = msc->nr_pages << PAGE_SHIFT;
+
+	if (!size)
+		return 0;
+
+	if (off >= size) {
+		len = 0;
+		goto put_count;
+	}
+	if (off + len >= size)
+		len = size - off;
+
+	if (msc->mode == MSC_MODE_SINGLE) {
+		ret = msc_single_to_user(msc, buf, off, len);
+		if (ret >= 0)
+			*ppos += ret;
+	} else if (msc->mode == MSC_MODE_MULTI) {
+		struct msc_win_to_user_struct u = {
+			.buf	= buf,
+			.offset	= 0,
+		};
+
+		ret = msc_buffer_iterate(iter, len, &u, msc_win_to_user);
+		if (ret >= 0)
+			*ppos = iter->offset;
+	} else {
+		ret = -ENOTSUPP;
+	}
+
+put_count:
+	atomic_dec(&msc->user_count);
+
+	return ret;
+}
+
+/*
+ * vm operations callbacks (vm_ops)
+ */
+
+static void msc_mmap_open(struct vm_area_struct *vma)
+{
+	struct msc_iter *iter = vma->vm_file->private_data;
+	struct msc *msc = iter->msc;
+
+	atomic_inc(&msc->mmap_count);
+}
+
+static void msc_mmap_close(struct vm_area_struct *vma)
+{
+	struct msc_iter *iter = vma->vm_file->private_data;
+	struct msc *msc = iter->msc;
+	unsigned long pg;
+
+	if (!atomic_dec_and_mutex_lock(&msc->mmap_count, &msc->buf_mutex))
+		return;
+
+	/* drop page _counts */
+	for (pg = 0; pg < msc->nr_pages; pg++) {
+		struct page *page = msc_buffer_get_page(msc, pg);
+
+		if (WARN_ON_ONCE(!page))
+			continue;
+
+		if (page->mapping)
+			page->mapping = NULL;
+	}
+
+	/* last mapping -- drop user_count */
+	atomic_dec(&msc->user_count);
+	mutex_unlock(&msc->buf_mutex);
+}
+
+static int msc_mmap_fault(struct vm_area_struct *vma, struct vm_fault *vmf)
+{
+	struct msc_iter *iter = vma->vm_file->private_data;
+	struct msc *msc = iter->msc;
+
+	vmf->page = msc_buffer_get_page(msc, vmf->pgoff);
+	if (!vmf->page)
+		return VM_FAULT_SIGBUS;
+
+	get_page(vmf->page);
+	vmf->page->mapping = vma->vm_file->f_mapping;
+	vmf->page->index = vmf->pgoff;
+
+	return 0;
+}
+
+static const struct vm_operations_struct msc_mmap_ops = {
+	.open	= msc_mmap_open,
+	.close	= msc_mmap_close,
+	.fault	= msc_mmap_fault,
+};
+
+static int intel_th_msc_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	unsigned long size = vma->vm_end - vma->vm_start;
+	struct msc_iter *iter = vma->vm_file->private_data;
+	struct msc *msc = iter->msc;
+	int ret = -EINVAL;
+
+	if (!size || offset_in_page(size))
+		return -EINVAL;
+
+	if (vma->vm_pgoff)
+		return -EINVAL;
+
+	/* grab user_count once per mmap; drop in msc_mmap_close() */
+	if (!atomic_inc_unless_negative(&msc->user_count))
+		return -EINVAL;
+
+	if (msc->mode != MSC_MODE_SINGLE &&
+	    msc->mode != MSC_MODE_MULTI)
+		goto out;
+
+	if (size >> PAGE_SHIFT != msc->nr_pages)
+		goto out;
+
+	atomic_set(&msc->mmap_count, 1);
+	ret = 0;
+
+out:
+	if (ret)
+		atomic_dec(&msc->user_count);
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_flags |= VM_DONTEXPAND | VM_DONTCOPY;
+	vma->vm_ops = &msc_mmap_ops;
+	return ret;
+}
+
+static const struct file_operations intel_th_msc_fops = {
+	.open		= intel_th_msc_open,
+	.release	= intel_th_msc_release,
+	.read		= intel_th_msc_read,
+	.mmap		= intel_th_msc_mmap,
+	.llseek		= no_llseek,
+};
+
+static int intel_th_msc_init(struct msc *msc)
+{
+	atomic_set(&msc->user_count, -1);
+
+	msc->mode = MSC_MODE_MULTI;
+	mutex_init(&msc->buf_mutex);
+	INIT_LIST_HEAD(&msc->win_list);
+
+	mutex_init(&msc->iter_mutex);
+	INIT_LIST_HEAD(&msc->iter_list);
+
+	msc->burst_len =
+		(ioread32(msc->reg_base + REG_MSU_MSC0CTL) & MSC_LEN) >>
+		__ffs(MSC_LEN);
+
+	return 0;
+}
+
+static const char * const msc_mode[] = {
+	[MSC_MODE_SINGLE]	= "single",
+	[MSC_MODE_MULTI]	= "multi",
+	[MSC_MODE_EXI]		= "ExI",
+	[MSC_MODE_DEBUG]	= "debug",
+};
+
+static ssize_t
+wrap_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct msc *msc = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", msc->wrap);
+}
+
+static ssize_t
+wrap_store(struct device *dev, struct device_attribute *attr, const char *buf,
+	   size_t size)
+{
+	struct msc *msc = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	msc->wrap = !!val;
+
+	return size;
+}
+
+static DEVICE_ATTR_RW(wrap);
+
+static ssize_t
+mode_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct msc *msc = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%s\n", msc_mode[msc->mode]);
+}
+
+static ssize_t
+mode_store(struct device *dev, struct device_attribute *attr, const char *buf,
+	   size_t size)
+{
+	struct msc *msc = dev_get_drvdata(dev);
+	size_t len = size;
+	char *cp;
+	int i, ret;
+
+	if (!capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	cp = memchr(buf, '\n', len);
+	if (cp)
+		len = cp - buf;
+
+	for (i = 0; i < ARRAY_SIZE(msc_mode); i++)
+		if (!strncmp(msc_mode[i], buf, len))
+			goto found;
+
+	return -EINVAL;
+
+found:
+	mutex_lock(&msc->buf_mutex);
+	ret = msc_buffer_unlocked_free_unless_used(msc);
+	if (!ret)
+		msc->mode = i;
+	mutex_unlock(&msc->buf_mutex);
+
+	return ret ? ret : size;
+}
+
+static DEVICE_ATTR_RW(mode);
+
+static ssize_t
+nr_pages_show(struct device *dev, struct device_attribute *attr, char *buf)
+{
+	struct msc *msc = dev_get_drvdata(dev);
+	struct msc_window *win;
+	size_t count = 0;
+
+	mutex_lock(&msc->buf_mutex);
+
+	if (msc->mode == MSC_MODE_SINGLE)
+		count = scnprintf(buf, PAGE_SIZE, "%ld\n", msc->nr_pages);
+	else if (msc->mode == MSC_MODE_MULTI) {
+		list_for_each_entry(win, &msc->win_list, entry) {
+			count += scnprintf(buf + count, PAGE_SIZE - count,
+					   "%d%c", win->nr_blocks,
+					   msc_is_last_win(win) ? '\n' : ',');
+		}
+	} else {
+		count = scnprintf(buf, PAGE_SIZE, "unsupported\n");
+	}
+
+	mutex_unlock(&msc->buf_mutex);
+
+	return count;
+}
+
+static ssize_t
+nr_pages_store(struct device *dev, struct device_attribute *attr,
+	       const char *buf, size_t size)
+{
+	struct msc *msc = dev_get_drvdata(dev);
+	unsigned long val, *win = NULL, *rewin;
+	size_t len = size;
+	const char *p = buf;
+	char *end, *s;
+	int ret, nr_wins = 0;
+
+	if (!capable(CAP_SYS_RAWIO))
+		return -EPERM;
+
+	ret = msc_buffer_free_unless_used(msc);
+	if (ret)
+		return ret;
+
+	/* scan the comma-separated list of allocation sizes */
+	end = memchr(buf, '\n', len);
+	if (end)
+		len = end - buf;
+
+	do {
+		end = memchr(p, ',', len);
+		s = kstrndup(p, end ? end - p : len, GFP_KERNEL);
+		ret = kstrtoul(s, 10, &val);
+		kfree(s);
+
+		if (ret || !val)
+			goto free_win;
+
+		if (nr_wins && msc->mode == MSC_MODE_SINGLE) {
+			ret = -EINVAL;
+			goto free_win;
+		}
+
+		nr_wins++;
+		rewin = krealloc(win, sizeof(*win) * nr_wins, GFP_KERNEL);
+		if (!rewin) {
+			kfree(win);
+			return -ENOMEM;
+		}
+
+		win = rewin;
+		win[nr_wins - 1] = val;
+
+		if (!end)
+			break;
+
+		len -= end - p;
+		p = end + 1;
+	} while (len);
+
+	mutex_lock(&msc->buf_mutex);
+	ret = msc_buffer_alloc(msc, win, nr_wins);
+	mutex_unlock(&msc->buf_mutex);
+
+free_win:
+	kfree(win);
+
+	return ret ? ret : size;
+}
+
+static DEVICE_ATTR_RW(nr_pages);
+
+static struct attribute *msc_output_attrs[] = {
+	&dev_attr_wrap.attr,
+	&dev_attr_mode.attr,
+	&dev_attr_nr_pages.attr,
+	NULL,
+};
+
+static struct attribute_group msc_output_group = {
+	.attrs	= msc_output_attrs,
+};
+
+static int intel_th_msc_probe(struct intel_th_device *thdev)
+{
+	struct device *dev = &thdev->dev;
+	struct resource *res;
+	struct msc *msc;
+	void __iomem *base;
+	int err;
+
+	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!base)
+		return -ENOMEM;
+
+	msc = devm_kzalloc(dev, sizeof(*msc), GFP_KERNEL);
+	if (!msc)
+		return -ENOMEM;
+
+	msc->index = thdev->id;
+
+	msc->thdev = thdev;
+	msc->reg_base = base + msc->index * 0x100;
+
+	err = intel_th_msc_init(msc);
+	if (err)
+		return err;
+
+	err = sysfs_create_group(&dev->kobj, &msc_output_group);
+	if (err)
+		return err;
+
+	dev_set_drvdata(dev, msc);
+
+	return 0;
+}
+
+static void intel_th_msc_remove(struct intel_th_device *thdev)
+{
+	sysfs_remove_group(&thdev->dev.kobj, &msc_output_group);
+}
+
+static struct intel_th_driver intel_th_msc_driver = {
+	.probe	= intel_th_msc_probe,
+	.remove	= intel_th_msc_remove,
+	.activate	= intel_th_msc_activate,
+	.deactivate	= intel_th_msc_deactivate,
+	.fops	= &intel_th_msc_fops,
+	.driver	= {
+		.name	= "msc",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_driver(intel_th_msc_driver,
+	      intel_th_driver_register,
+	      intel_th_driver_unregister);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel(R) Trace Hub Memory Storage Unit driver");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/intel_th/msu.h b/drivers/hwtracing/intel_th/msu.h
new file mode 100644
index 000000000..9b710e4aa
--- /dev/null
+++ b/drivers/hwtracing/intel_th/msu.h
@@ -0,0 +1,116 @@
+/*
+ * Intel(R) Trace Hub Memory Storage Unit (MSU) data structures
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#ifndef __INTEL_TH_MSU_H__
+#define __INTEL_TH_MSU_H__
+
+enum {
+	REG_MSU_MSUPARAMS	= 0x0000,
+	REG_MSU_MSUSTS		= 0x0008,
+	REG_MSU_MSC0CTL		= 0x0100, /* MSC0 control */
+	REG_MSU_MSC0STS		= 0x0104, /* MSC0 status */
+	REG_MSU_MSC0BAR		= 0x0108, /* MSC0 output base address */
+	REG_MSU_MSC0SIZE	= 0x010c, /* MSC0 output size */
+	REG_MSU_MSC0MWP		= 0x0110, /* MSC0 write pointer */
+	REG_MSU_MSC0NWSA	= 0x011c, /* MSC0 next window start address */
+
+	REG_MSU_MSC1CTL		= 0x0200, /* MSC1 control */
+	REG_MSU_MSC1STS		= 0x0204, /* MSC1 status */
+	REG_MSU_MSC1BAR		= 0x0208, /* MSC1 output base address */
+	REG_MSU_MSC1SIZE	= 0x020c, /* MSC1 output size */
+	REG_MSU_MSC1MWP		= 0x0210, /* MSC1 write pointer */
+	REG_MSU_MSC1NWSA	= 0x021c, /* MSC1 next window start address */
+};
+
+/* MSUSTS bits */
+#define MSUSTS_MSU_INT	BIT(0)
+
+/* MSCnCTL bits */
+#define MSC_EN		BIT(0)
+#define MSC_WRAPEN	BIT(1)
+#define MSC_RD_HDR_OVRD	BIT(2)
+#define MSC_MODE	(BIT(4) | BIT(5))
+#define MSC_LEN		(BIT(8) | BIT(9) | BIT(10))
+
+/* MSC operating modes (MSC_MODE) */
+enum {
+	MSC_MODE_SINGLE	= 0,
+	MSC_MODE_MULTI,
+	MSC_MODE_EXI,
+	MSC_MODE_DEBUG,
+};
+
+/* MSCnSTS bits */
+#define MSCSTS_WRAPSTAT	BIT(1)	/* Wrap occurred */
+#define MSCSTS_PLE	BIT(2)	/* Pipeline Empty */
+
+/*
+ * Multiblock/multiwindow block descriptor
+ */
+struct msc_block_desc {
+	u32	sw_tag;
+	u32	block_sz;
+	u32	next_blk;
+	u32	next_win;
+	u32	res0[4];
+	u32	hw_tag;
+	u32	valid_dw;
+	u32	ts_low;
+	u32	ts_high;
+	u32	res1[4];
+} __packed;
+
+#define MSC_BDESC	sizeof(struct msc_block_desc)
+#define DATA_IN_PAGE	(PAGE_SIZE - MSC_BDESC)
+
+/* MSC multiblock sw tag bits */
+#define MSC_SW_TAG_LASTBLK	BIT(0)
+#define MSC_SW_TAG_LASTWIN	BIT(1)
+
+/* MSC multiblock hw tag bits */
+#define MSC_HW_TAG_TRIGGER	BIT(0)
+#define MSC_HW_TAG_BLOCKWRAP	BIT(1)
+#define MSC_HW_TAG_WINWRAP	BIT(2)
+#define MSC_HW_TAG_ENDBIT	BIT(3)
+
+static inline unsigned long msc_data_sz(struct msc_block_desc *bdesc)
+{
+	if (!bdesc->valid_dw)
+		return 0;
+
+	return bdesc->valid_dw * 4 - MSC_BDESC;
+}
+
+static inline bool msc_block_wrapped(struct msc_block_desc *bdesc)
+{
+	if (bdesc->hw_tag & MSC_HW_TAG_BLOCKWRAP)
+		return true;
+
+	return false;
+}
+
+static inline bool msc_block_last_written(struct msc_block_desc *bdesc)
+{
+	if ((bdesc->hw_tag & MSC_HW_TAG_ENDBIT) ||
+	    (msc_data_sz(bdesc) != DATA_IN_PAGE))
+		return true;
+
+	return false;
+}
+
+/* waiting for Pipeline Empty bit(s) to assert for MSC */
+#define MSC_PLE_WAITLOOP_DEPTH	10000
+
+#endif /* __INTEL_TH_MSU_H__ */
diff --git a/drivers/hwtracing/intel_th/pci.c b/drivers/hwtracing/intel_th/pci.c
new file mode 100644
index 000000000..641e87936
--- /dev/null
+++ b/drivers/hwtracing/intel_th/pci.c
@@ -0,0 +1,86 @@
+/*
+ * Intel(R) Trace Hub pci driver
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sysfs.h>
+#include <linux/pci.h>
+
+#include "intel_th.h"
+
+#define DRIVER_NAME "intel_th_pci"
+
+#define BAR_MASK (BIT(TH_MMIO_CONFIG) | BIT(TH_MMIO_SW))
+
+static int intel_th_pci_probe(struct pci_dev *pdev,
+			      const struct pci_device_id *id)
+{
+	struct intel_th *th;
+	int err;
+
+	err = pcim_enable_device(pdev);
+	if (err)
+		return err;
+
+	err = pcim_iomap_regions_request_all(pdev, BAR_MASK, DRIVER_NAME);
+	if (err)
+		return err;
+
+	th = intel_th_alloc(&pdev->dev, pdev->resource,
+			    DEVICE_COUNT_RESOURCE, pdev->irq);
+	if (IS_ERR(th))
+		return PTR_ERR(th);
+
+	pci_set_drvdata(pdev, th);
+
+	return 0;
+}
+
+static void intel_th_pci_remove(struct pci_dev *pdev)
+{
+	struct intel_th *th = pci_get_drvdata(pdev);
+
+	intel_th_free(th);
+}
+
+static const struct pci_device_id intel_th_pci_id_table[] = {
+	{
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0x9d26),
+		.driver_data = (kernel_ulong_t)0,
+	},
+	{
+		PCI_DEVICE(PCI_VENDOR_ID_INTEL, 0xa126),
+		.driver_data = (kernel_ulong_t)0,
+	},
+	{ 0 },
+};
+
+MODULE_DEVICE_TABLE(pci, intel_th_pci_id_table);
+
+static struct pci_driver intel_th_pci_driver = {
+	.name		= DRIVER_NAME,
+	.id_table	= intel_th_pci_id_table,
+	.probe		= intel_th_pci_probe,
+	.remove		= intel_th_pci_remove,
+};
+
+module_pci_driver(intel_th_pci_driver);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel(R) Trace Hub PCI controller driver");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>");
diff --git a/drivers/hwtracing/intel_th/pti.c b/drivers/hwtracing/intel_th/pti.c
new file mode 100644
index 000000000..57cbfdcc7
--- /dev/null
+++ b/drivers/hwtracing/intel_th/pti.c
@@ -0,0 +1,252 @@
+/*
+ * Intel(R) Trace Hub PTI output driver
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/sizes.h>
+#include <linux/printk.h>
+#include <linux/slab.h>
+#include <linux/mm.h>
+#include <linux/io.h>
+
+#include "intel_th.h"
+#include "pti.h"
+
+struct pti_device {
+	void __iomem		*base;
+	struct intel_th_device	*thdev;
+	unsigned int		mode;
+	unsigned int		freeclk;
+	unsigned int		clkdiv;
+	unsigned int		patgen;
+};
+
+/* map PTI widths to MODE settings of PTI_CTL register */
+static const unsigned int pti_mode[] = {
+	0, 4, 8, 0, 12, 0, 0, 0, 16, 0, 0, 0, 0, 0, 0, 0,
+};
+
+static int pti_width_mode(unsigned int width)
+{
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(pti_mode); i++)
+		if (pti_mode[i] == width)
+			return i;
+
+	return -EINVAL;
+}
+
+static ssize_t mode_show(struct device *dev, struct device_attribute *attr,
+			 char *buf)
+{
+	struct pti_device *pti = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", pti_mode[pti->mode]);
+}
+
+static ssize_t mode_store(struct device *dev, struct device_attribute *attr,
+			  const char *buf, size_t size)
+{
+	struct pti_device *pti = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	ret = pti_width_mode(val);
+	if (ret < 0)
+		return ret;
+
+	pti->mode = ret;
+
+	return size;
+}
+
+static DEVICE_ATTR_RW(mode);
+
+static ssize_t
+freerunning_clock_show(struct device *dev, struct device_attribute *attr,
+		       char *buf)
+{
+	struct pti_device *pti = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", pti->freeclk);
+}
+
+static ssize_t
+freerunning_clock_store(struct device *dev, struct device_attribute *attr,
+			const char *buf, size_t size)
+{
+	struct pti_device *pti = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	pti->freeclk = !!val;
+
+	return size;
+}
+
+static DEVICE_ATTR_RW(freerunning_clock);
+
+static ssize_t
+clock_divider_show(struct device *dev, struct device_attribute *attr,
+		   char *buf)
+{
+	struct pti_device *pti = dev_get_drvdata(dev);
+
+	return scnprintf(buf, PAGE_SIZE, "%d\n", 1u << pti->clkdiv);
+}
+
+static ssize_t
+clock_divider_store(struct device *dev, struct device_attribute *attr,
+		    const char *buf, size_t size)
+{
+	struct pti_device *pti = dev_get_drvdata(dev);
+	unsigned long val;
+	int ret;
+
+	ret = kstrtoul(buf, 10, &val);
+	if (ret)
+		return ret;
+
+	if (!is_power_of_2(val) || val > 8 || !val)
+		return -EINVAL;
+
+	pti->clkdiv = val;
+
+	return size;
+}
+
+static DEVICE_ATTR_RW(clock_divider);
+
+static struct attribute *pti_output_attrs[] = {
+	&dev_attr_mode.attr,
+	&dev_attr_freerunning_clock.attr,
+	&dev_attr_clock_divider.attr,
+	NULL,
+};
+
+static struct attribute_group pti_output_group = {
+	.attrs	= pti_output_attrs,
+};
+
+static int intel_th_pti_activate(struct intel_th_device *thdev)
+{
+	struct pti_device *pti = dev_get_drvdata(&thdev->dev);
+	u32 ctl = PTI_EN;
+
+	if (pti->patgen)
+		ctl |= pti->patgen << __ffs(PTI_PATGENMODE);
+	if (pti->freeclk)
+		ctl |= PTI_FCEN;
+	ctl |= pti->mode << __ffs(PTI_MODE);
+	ctl |= pti->clkdiv << __ffs(PTI_CLKDIV);
+
+	iowrite32(ctl, pti->base + REG_PTI_CTL);
+
+	intel_th_trace_enable(thdev);
+
+	return 0;
+}
+
+static void intel_th_pti_deactivate(struct intel_th_device *thdev)
+{
+	struct pti_device *pti = dev_get_drvdata(&thdev->dev);
+
+	intel_th_trace_disable(thdev);
+
+	iowrite32(0, pti->base + REG_PTI_CTL);
+}
+
+static void read_hw_config(struct pti_device *pti)
+{
+	u32 ctl = ioread32(pti->base + REG_PTI_CTL);
+
+	pti->mode	= (ctl & PTI_MODE) >> __ffs(PTI_MODE);
+	pti->clkdiv	= (ctl & PTI_CLKDIV) >> __ffs(PTI_CLKDIV);
+	pti->freeclk	= !!(ctl & PTI_FCEN);
+
+	if (!pti_mode[pti->mode])
+		pti->mode = pti_width_mode(4);
+	if (!pti->clkdiv)
+		pti->clkdiv = 1;
+}
+
+static int intel_th_pti_probe(struct intel_th_device *thdev)
+{
+	struct device *dev = &thdev->dev;
+	struct resource *res;
+	struct pti_device *pti;
+	void __iomem *base;
+	int ret;
+
+	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!base)
+		return -ENOMEM;
+
+	pti = devm_kzalloc(dev, sizeof(*pti), GFP_KERNEL);
+	if (!pti)
+		return -ENOMEM;
+
+	pti->thdev = thdev;
+	pti->base = base;
+
+	read_hw_config(pti);
+
+	ret = sysfs_create_group(&dev->kobj, &pti_output_group);
+	if (ret)
+		return ret;
+
+	dev_set_drvdata(dev, pti);
+
+	return 0;
+}
+
+static void intel_th_pti_remove(struct intel_th_device *thdev)
+{
+}
+
+static struct intel_th_driver intel_th_pti_driver = {
+	.probe	= intel_th_pti_probe,
+	.remove	= intel_th_pti_remove,
+	.activate	= intel_th_pti_activate,
+	.deactivate	= intel_th_pti_deactivate,
+	.driver	= {
+		.name	= "pti",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_driver(intel_th_pti_driver,
+	      intel_th_driver_register,
+	      intel_th_driver_unregister);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel(R) Trace Hub PTI output driver");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/intel_th/pti.h b/drivers/hwtracing/intel_th/pti.h
new file mode 100644
index 000000000..20883f562
--- /dev/null
+++ b/drivers/hwtracing/intel_th/pti.h
@@ -0,0 +1,29 @@
+/*
+ * Intel(R) Trace Hub PTI output data structures
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#ifndef __INTEL_TH_STH_H__
+#define __INTEL_TH_STH_H__
+
+enum {
+	REG_PTI_CTL	= 0x1c00,
+};
+
+#define PTI_EN		BIT(0)
+#define PTI_FCEN	BIT(1)
+#define PTI_MODE	0xf0
+#define PTI_CLKDIV	0x000f0000
+#define PTI_PATGENMODE	0x00f00000
+
+#endif /* __INTEL_TH_STH_H__ */
diff --git a/drivers/hwtracing/intel_th/sth.c b/drivers/hwtracing/intel_th/sth.c
new file mode 100644
index 000000000..56101c33e
--- /dev/null
+++ b/drivers/hwtracing/intel_th/sth.c
@@ -0,0 +1,259 @@
+/*
+ * Intel(R) Trace Hub Software Trace Hub support
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#define pr_fmt(fmt)	KBUILD_MODNAME ": " fmt
+
+#include <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/io.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+
+#include "intel_th.h"
+#include "sth.h"
+
+struct sth_device {
+	void __iomem	*base;
+	void __iomem	*channels;
+	phys_addr_t	channels_phys;
+	struct device	*dev;
+	struct stm_data	stm;
+	unsigned int	sw_nmasters;
+};
+
+static struct intel_th_channel __iomem *
+sth_channel(struct sth_device *sth, unsigned int master, unsigned int channel)
+{
+	struct intel_th_channel __iomem *sw_map = sth->channels;
+
+	return &sw_map[(master - sth->stm.sw_start) * sth->stm.sw_nchannels +
+		       channel];
+}
+
+static void sth_iowrite(void __iomem *dest, const unsigned char *payload,
+			unsigned int size)
+{
+	switch (size) {
+#ifdef CONFIG_64BIT
+	case 8:
+		writeq_relaxed(*(u64 *)payload, dest);
+		break;
+#endif
+	case 4:
+		writel_relaxed(*(u32 *)payload, dest);
+		break;
+	case 2:
+		writew_relaxed(*(u16 *)payload, dest);
+		break;
+	case 1:
+		writeb_relaxed(*(u8 *)payload, dest);
+		break;
+	default:
+		break;
+	}
+}
+
+static ssize_t sth_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)
+{
+	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
+	struct intel_th_channel __iomem *out =
+		sth_channel(sth, master, channel);
+	u64 __iomem *outp = &out->Dn;
+	unsigned long reg = REG_STH_TRIG;
+
+#ifndef CONFIG_64BIT
+	if (size > 4)
+		size = 4;
+#endif
+
+	size = rounddown_pow_of_two(size);
+
+	switch (packet) {
+	/* Global packets (GERR, XSYNC, TRIG) are sent with register writes */
+	case STP_PACKET_GERR:
+		reg += 4;
+	case STP_PACKET_XSYNC:
+		reg += 8;
+	case STP_PACKET_TRIG:
+		if (flags & STP_PACKET_TIMESTAMPED)
+			reg += 4;
+		iowrite8(*payload, sth->base + reg);
+		break;
+
+	case STP_PACKET_MERR:
+		sth_iowrite(&out->MERR, payload, size);
+		break;
+
+	case STP_PACKET_FLAG:
+		if (flags & STP_PACKET_TIMESTAMPED)
+			outp = (u64 __iomem *)&out->FLAG_TS;
+		else
+			outp = (u64 __iomem *)&out->FLAG;
+
+		size = 1;
+		sth_iowrite(outp, payload, size);
+		break;
+
+	case STP_PACKET_USER:
+		if (flags & STP_PACKET_TIMESTAMPED)
+			outp = &out->USER_TS;
+		else
+			outp = &out->USER;
+		sth_iowrite(outp, payload, size);
+		break;
+
+	case STP_PACKET_DATA:
+		outp = &out->Dn;
+
+		if (flags & STP_PACKET_TIMESTAMPED)
+			outp += 2;
+		if (flags & STP_PACKET_MARKED)
+			outp++;
+
+		sth_iowrite(outp, payload, size);
+		break;
+	}
+
+	return size;
+}
+
+static phys_addr_t
+sth_stm_mmio_addr(struct stm_data *stm_data, unsigned int master,
+		  unsigned int channel, unsigned int nr_chans)
+{
+	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
+	phys_addr_t addr;
+
+	master -= sth->stm.sw_start;
+	addr = sth->channels_phys + (master * sth->stm.sw_nchannels + channel) *
+		sizeof(struct intel_th_channel);
+
+	if (offset_in_page(addr) ||
+	    offset_in_page(nr_chans * sizeof(struct intel_th_channel)))
+		return 0;
+
+	return addr;
+}
+
+static int sth_stm_link(struct stm_data *stm_data, unsigned int master,
+			 unsigned int channel)
+{
+	struct sth_device *sth = container_of(stm_data, struct sth_device, stm);
+
+	intel_th_set_output(to_intel_th_device(sth->dev), master);
+
+	return 0;
+}
+
+static int intel_th_sw_init(struct sth_device *sth)
+{
+	u32 reg;
+
+	reg = ioread32(sth->base + REG_STH_STHCAP1);
+	sth->stm.sw_nchannels = reg & 0xff;
+
+	reg = ioread32(sth->base + REG_STH_STHCAP0);
+	sth->stm.sw_start = reg & 0xffff;
+	sth->stm.sw_end = reg >> 16;
+
+	sth->sw_nmasters = sth->stm.sw_end - sth->stm.sw_start;
+	dev_dbg(sth->dev, "sw_start: %x sw_end: %x masters: %x nchannels: %x\n",
+		sth->stm.sw_start, sth->stm.sw_end, sth->sw_nmasters,
+		sth->stm.sw_nchannels);
+
+	return 0;
+}
+
+static int intel_th_sth_probe(struct intel_th_device *thdev)
+{
+	struct device *dev = &thdev->dev;
+	struct sth_device *sth;
+	struct resource *res;
+	void __iomem *base, *channels;
+	int err;
+
+	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 0);
+	if (!res)
+		return -ENODEV;
+
+	base = devm_ioremap(dev, res->start, resource_size(res));
+	if (!base)
+		return -ENOMEM;
+
+	res = intel_th_device_get_resource(thdev, IORESOURCE_MEM, 1);
+	if (!res)
+		return -ENODEV;
+
+	channels = devm_ioremap(dev, res->start, resource_size(res));
+	if (!channels)
+		return -ENOMEM;
+
+	sth = devm_kzalloc(dev, sizeof(*sth), GFP_KERNEL);
+	if (!sth)
+		return -ENOMEM;
+
+	sth->dev = dev;
+	sth->base = base;
+	sth->channels = channels;
+	sth->channels_phys = res->start;
+	sth->stm.name = dev_name(dev);
+	sth->stm.packet = sth_stm_packet;
+	sth->stm.mmio_addr = sth_stm_mmio_addr;
+	sth->stm.sw_mmiosz = sizeof(struct intel_th_channel);
+	sth->stm.link = sth_stm_link;
+
+	err = intel_th_sw_init(sth);
+	if (err)
+		return err;
+
+	err = stm_register_device(dev, &sth->stm, THIS_MODULE);
+	if (err) {
+		dev_err(dev, "stm_register_device failed\n");
+		return err;
+	}
+
+	dev_set_drvdata(dev, sth);
+
+	return 0;
+}
+
+static void intel_th_sth_remove(struct intel_th_device *thdev)
+{
+	struct sth_device *sth = dev_get_drvdata(&thdev->dev);
+
+	stm_unregister_device(&sth->stm);
+}
+
+static struct intel_th_driver intel_th_sth_driver = {
+	.probe	= intel_th_sth_probe,
+	.remove	= intel_th_sth_remove,
+	.driver	= {
+		.name	= "sth",
+		.owner	= THIS_MODULE,
+	},
+};
+
+module_driver(intel_th_sth_driver,
+	      intel_th_driver_register,
+	      intel_th_driver_unregister);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Intel(R) Trace Hub Software Trace Hub driver");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@intel.com>");
diff --git a/drivers/hwtracing/intel_th/sth.h b/drivers/hwtracing/intel_th/sth.h
new file mode 100644
index 000000000..f1390cd4f
--- /dev/null
+++ b/drivers/hwtracing/intel_th/sth.h
@@ -0,0 +1,42 @@
+/*
+ * Intel(R) Trace Hub Software Trace Hub (STH) data structures
+ *
+ * Copyright (C) 2014-2015 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.
+ */
+
+#ifndef __INTEL_TH_STH_H__
+#define __INTEL_TH_STH_H__
+
+enum {
+	REG_STH_STHCAP0		= 0x0000, /* capabilities pt1 */
+	REG_STH_STHCAP1		= 0x0004, /* capabilities pt2 */
+	REG_STH_TRIG		= 0x0008, /* TRIG packet payload */
+	REG_STH_TRIG_TS		= 0x000c, /* TRIG_TS packet payload */
+	REG_STH_XSYNC		= 0x0010, /* XSYNC packet payload */
+	REG_STH_XSYNC_TS	= 0x0014, /* XSYNC_TS packet payload */
+	REG_STH_GERR		= 0x0018, /* GERR packet payload */
+};
+
+struct intel_th_channel {
+	u64	Dn;
+	u64	DnM;
+	u64	DnTS;
+	u64	DnMTS;
+	u64	USER;
+	u64	USER_TS;
+	u32	FLAG;
+	u32	FLAG_TS;
+	u32	MERR;
+	u32	__unused;
+} __packed;
+
+#endif /* __INTEL_TH_STH_H__ */
diff --git a/drivers/hwtracing/stm/Kconfig b/drivers/hwtracing/stm/Kconfig
new file mode 100644
index 000000000..83e9f591a
--- /dev/null
+++ b/drivers/hwtracing/stm/Kconfig
@@ -0,0 +1,26 @@
+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.
+
+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.
diff --git a/drivers/hwtracing/stm/Makefile b/drivers/hwtracing/stm/Makefile
new file mode 100644
index 000000000..f9312c38d
--- /dev/null
+++ b/drivers/hwtracing/stm/Makefile
@@ -0,0 +1,9 @@
+obj-$(CONFIG_STM)	+= stm_core.o
+
+stm_core-y		:= core.o policy.o
+
+obj-$(CONFIG_STM_DUMMY)	+= dummy_stm.o
+
+obj-$(CONFIG_STM_SOURCE_CONSOLE)	+= stm_console.o
+
+stm_console-y		:= console.o
diff --git a/drivers/hwtracing/stm/console.c b/drivers/hwtracing/stm/console.c
new file mode 100644
index 000000000..c9d9a8d2f
--- /dev/null
+++ b/drivers/hwtracing/stm/console.c
@@ -0,0 +1,80 @@
+/*
+ * Simple kernel console driver for STM devices
+ * Copyright (c) 2014, 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.
+ *
+ * STM console will send kernel messages over STM devices to a trace host.
+ */
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/console.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+
+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,
+		.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 <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/stm/core.c b/drivers/hwtracing/stm/core.c
new file mode 100644
index 000000000..b6445d9e5
--- /dev/null
+++ b/drivers/hwtracing/stm/core.c
@@ -0,0 +1,1032 @@
+/*
+ * System Trace Module (STM) infrastructure
+ * Copyright (c) 2014, 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.
+ *
+ * STM class implements generic infrastructure for  System Trace Module devices
+ * as defined in MIPI STPv2 specification.
+ */
+
+#include <linux/uaccess.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/compat.h>
+#include <linux/kdev_t.h>
+#include <linux/srcu.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include "stm.h"
+
+#include <uapi/linux/stm.h>
+
+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 struct attribute *stm_attrs[] = {
+	&dev_attr_masters.attr,
+	&dev_attr_channels.attr,
+	NULL,
+};
+
+ATTRIBUTE_GROUPS(stm);
+
+static struct class stm_class = {
+	.name		= "stm",
+	.dev_groups	= stm_groups,
+};
+
+static int stm_dev_match(struct device *dev, const void *data)
+{
+	const char *name = data;
+
+	return sysfs_streq(name, dev_name(dev));
+}
+
+/**
+ * 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(&stm_class, NULL, buf, stm_dev_match);
+	if (!dev)
+		return NULL;
+
+	stm = to_stm_device(dev);
+	if (!try_module_get(stm->owner)) {
+		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().
+ */
+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;
+	size_t size;
+
+	size = ALIGN(stm->data->sw_nchannels, 8) / 8;
+	size += sizeof(struct stp_master);
+	master = kzalloc(size, 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);
+
+	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);
+
+	bitmap_release_region(&master->chan_map[0], output->channel,
+			      ilog2(output->nr_chans));
+
+	output->nr_chans = 0;
+	master->nr_free += output->nr_chans;
+}
+
+/*
+ * 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;
+	}
+
+	return -1;
+}
+
+static unsigned 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;
+
+	if (policy_node) {
+		stp_policy_node_get_ranges(policy_node,
+					   &midx, &mend, &cidx, &cend);
+	} else {
+		midx = stm->data->sw_start;
+		cidx = 0;
+		mend = stm->data->sw_end;
+		cend = stm->data->sw_nchannels - 1;
+	}
+
+	spin_lock(&stm->mc_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)
+		goto unlock;
+
+	output->master = midx;
+	output->channel = cidx;
+	output->nr_chans = width;
+	stm_output_claim(stm, output);
+	dev_dbg(&stm->dev, "assigned %u:%u (+%u)\n", midx, cidx, width);
+
+	ret = 0;
+unlock:
+	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);
+	if (output->nr_chans)
+		stm_output_disclaim(stm, output);
+	spin_unlock(&stm->mc_lock);
+}
+
+static int major_match(struct device *dev, const void *data)
+{
+	unsigned int major = *(unsigned int *)data;
+
+	return MAJOR(dev->devt) == major;
+}
+
+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 = -ENODEV;
+
+	dev = class_find_device(&stm_class, NULL, &major, major_match);
+	if (!dev)
+		return -ENODEV;
+
+	stmf = kzalloc(sizeof(*stmf), GFP_KERNEL);
+	if (!stmf)
+		return -ENOMEM;
+
+	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);
+
+	return err;
+}
+
+static int stm_char_release(struct inode *inode, struct file *file)
+{
+	struct stm_file *stmf = file->private_data;
+
+	stm_output_free(stmf->stm, &stmf->output);
+	stm_put_device(stmf->stm);
+	kfree(stmf);
+
+	return 0;
+}
+
+static int stm_file_assign(struct stm_file *stmf, char *id, unsigned int width)
+{
+	struct stm_device *stm = stmf->stm;
+	int ret;
+
+	stmf->policy_node = stp_policy_node_lookup(stm, id);
+
+	ret = stm_output_assign(stm, width, stmf->policy_node, &stmf->output);
+
+	if (stmf->policy_node)
+		stp_policy_node_put(stmf->policy_node);
+
+	return ret;
+}
+
+static void 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;
+	size_t pos;
+	ssize_t sz;
+
+	for (pos = 0, p = buf; count > pos; pos += sz, p += sz) {
+		sz = min_t(unsigned int, count - pos, 8);
+		sz = data->packet(data, master, channel, STP_PACKET_DATA, flags,
+				  sz, p);
+		flags = 0;
+	}
+
+	data->packet(data, master, channel, STP_PACKET_FLAG, 0, 0, &nil);
+}
+
+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 no m/c have been assigned to this writer up to this
+	 * point, use "default" policy entry
+	 */
+	if (!stmf->output.nr_chans) {
+		err = stm_file_assign(stmf, "default", 1);
+		/*
+		 * EBUSY means that somebody else just assigned this
+		 * output, which is just fine for write()
+		 */
+		if (err && err != -EBUSY)
+			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;
+	}
+
+	stm_write(stm->data, stmf->output.master, stmf->output.channel, kbuf,
+		  count);
+
+	kfree(kbuf);
+
+	return count;
+}
+
+static int stm_char_mmap(struct file *file, struct vm_area_struct *vma)
+{
+	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 (vma->vm_pgoff)
+		return -EINVAL;
+
+	size = vma->vm_end - vma->vm_start;
+
+	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;
+
+	vma->vm_page_prot = pgprot_noncached(vma->vm_page_prot);
+	vma->vm_flags |= VM_IO | VM_DONTEXPAND | VM_DONTDUMP;
+	vm_iomap_memory(vma, 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;
+	int ret = -EINVAL;
+	u32 size;
+
+	if (stmf->output.nr_chans)
+		return -EBUSY;
+
+	if (copy_from_user(&size, arg, sizeof(size)))
+		return -EFAULT;
+
+	if (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 (id->width < 1 ||
+	    id->width > PAGE_SIZE / stm->data->sw_mmiosz)
+		goto err_free;
+
+	ret = stm_file_assign(stmf, id->id, id->width);
+	if (ret)
+		goto err_free;
+
+	ret = 0;
+
+	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);
+		stm_put_device(stmf->stm);
+	}
+
+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;
+}
+
+#ifdef CONFIG_COMPAT
+static long
+stm_char_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
+{
+	return stm_char_ioctl(file, cmd, (unsigned long)compat_ptr(arg));
+}
+#else
+#define stm_char_compat_ioctl	NULL
+#endif
+
+static const struct file_operations stm_fops = {
+	.open		= stm_char_open,
+	.release	= stm_char_release,
+	.write		= stm_char_write,
+	.mmap		= stm_char_mmap,
+	.unlocked_ioctl	= stm_char_ioctl,
+	.compat_ioctl	= stm_char_compat_ioctl,
+	.llseek		= no_llseek,
+};
+
+static void stm_device_release(struct device *dev)
+{
+	struct stm_device *stm = to_stm_device(dev);
+
+	kfree(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;
+	stm = kzalloc(sizeof(*stm) + nmasters * sizeof(void *), GFP_KERNEL);
+	if (!stm)
+		return -ENOMEM;
+
+	stm->major = register_chrdev(0, stm_data->name, &stm_fops);
+	if (stm->major < 0)
+		goto err_free;
+
+	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;
+
+	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;
+
+	spin_lock_init(&stm->link_lock);
+	INIT_LIST_HEAD(&stm->link_list);
+
+	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;
+
+	return 0;
+
+err_device:
+	put_device(&stm->dev);
+err_free:
+	kfree(stm);
+
+	return err;
+}
+EXPORT_SYMBOL_GPL(stm_register_device);
+
+static void __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;
+
+	spin_lock(&stm->link_lock);
+	list_for_each_entry_safe(src, iter, &stm->link_list, link_entry) {
+		__stm_source_link_drop(src, stm);
+	}
+	spin_unlock(&stm->link_lock);
+
+	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 = 0; i < stm->sw_nmasters; i++)
+		stp_master_free(stm, i);
+
+	device_unregister(&stm->dev);
+	stm_data->stm = NULL;
+}
+EXPORT_SYMBOL_GPL(stm_unregister_device);
+
+/**
+ * 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 *id;
+	int err;
+
+	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);
+
+	id = kstrdup(src->data->name, GFP_KERNEL);
+	if (id) {
+		src->policy_node =
+			stp_policy_node_lookup(stm, id);
+
+		kfree(id);
+	}
+
+	err = stm_output_assign(stm, src->data->nr_chans,
+				src->policy_node, &src->output);
+
+	if (src->policy_node)
+		stp_policy_node_put(src->policy_node);
+
+	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);
+	stm_put_device(stm);
+
+fail_detach:
+	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);
+
+	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_lock.
+ */
+static void __stm_source_link_drop(struct stm_source_device *src,
+				   struct stm_device *stm)
+{
+	struct stm_device *link;
+
+	spin_lock(&src->link_lock);
+	link = srcu_dereference_check(src->link, &stm_source_srcu, 1);
+	if (WARN_ON_ONCE(link != stm)) {
+		spin_unlock(&src->link_lock);
+		return;
+	}
+
+	stm_output_free(link, &src->output);
+	/* caller must hold stm::link_lock */
+	list_del_init(&src->link_entry);
+	/* matches stm_find_device() from stm_source_link_store() */
+	stm_put_device(link);
+	rcu_assign_pointer(src->link, NULL);
+
+	spin_unlock(&src->link_lock);
+}
+
+/**
+ * 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;
+
+	idx = srcu_read_lock(&stm_source_srcu);
+	stm = srcu_dereference(src->link, &stm_source_srcu);
+
+	if (stm) {
+		if (src->data->unlink)
+			src->data->unlink(src->data);
+
+		spin_lock(&stm->link_lock);
+		__stm_source_link_drop(src, stm);
+		spin_unlock(&stm->link_lock);
+	}
+
+	srcu_read_unlock(&stm_source_srcu, idx);
+}
+
+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) : "<none>");
+	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;
+
+	err = stm_source_link_add(src, link);
+	if (err)
+		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(sizeof(*src), GFP_KERNEL);
+	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;
+
+	err = device_add(&src->dev);
+	if (err)
+		goto err;
+
+	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);
+	kfree(src);
+
+	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_destroy(&stm_source_class, src->dev.devt);
+}
+EXPORT_SYMBOL_GPL(stm_source_unregister_device);
+
+int 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)
+		stm_write(stm->data, src->output.master,
+			  src->output.channel + chan,
+			  buf, count);
+	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);
+
+	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 <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/stm/dummy_stm.c b/drivers/hwtracing/stm/dummy_stm.c
new file mode 100644
index 000000000..3709bef0b
--- /dev/null
+++ b/drivers/hwtracing/stm/dummy_stm.c
@@ -0,0 +1,66 @@
+/*
+ * A dummy STM device for stm/stm_source class testing.
+ * Copyright (c) 2014, 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.
+ *
+ * STM class implements generic infrastructure for  System Trace Module devices
+ * as defined in MIPI STPv2 specification.
+ */
+
+#undef DEBUG
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+
+static ssize_t
+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;
+}
+
+static struct stm_data dummy_stm = {
+	.name		= "dummy_stm",
+	.sw_start	= 0x0000,
+	.sw_end		= 0xffff,
+	.sw_nchannels	= 0xffff,
+	.packet		= dummy_stm_packet,
+};
+
+static int dummy_stm_init(void)
+{
+	return stm_register_device(NULL, &dummy_stm, THIS_MODULE);
+}
+
+static void dummy_stm_exit(void)
+{
+	stm_unregister_device(&dummy_stm);
+}
+
+module_init(dummy_stm_init);
+module_exit(dummy_stm_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("dummy_stm device");
+MODULE_AUTHOR("Alexander Shishkin <alexander.shishkin@linux.intel.com>");
diff --git a/drivers/hwtracing/stm/policy.c b/drivers/hwtracing/stm/policy.c
new file mode 100644
index 000000000..11ab6d01a
--- /dev/null
+++ b/drivers/hwtracing/stm/policy.c
@@ -0,0 +1,472 @@
+/*
+ * System Trace Module (STM) master/channel allocation policy management
+ * Copyright (c) 2014, 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.
+ *
+ * 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 <linux/types.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/configfs.h>
+#include <linux/slab.h>
+#include <linux/stm.h>
+#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;
+};
+
+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 char *stp_policy_node_name(struct stp_policy_node *policy_node)
+{
+	return policy_node->group.cg_item.ci_name ? : "<none>";
+}
+
+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;
+}
+
+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 > INT_MAX || last > INT_MAX || 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)
+{
+	kfree(to_stp_policy_node(item));
+}
+
+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 struct config_item_type stp_policy_type;
+static struct config_item_type stp_policy_node_type;
+
+static struct config_group *
+stp_policy_node_make(struct config_group *group, const char *name)
+{
+	struct stp_policy_node *policy_node, *parent_node;
+	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);
+
+	policy_node = kzalloc(sizeof(struct stp_policy_node), GFP_KERNEL);
+	if (!policy_node)
+		return ERR_PTR(-ENOMEM);
+
+	config_group_init_type_name(&policy_node->group, name,
+				    &stp_policy_node_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 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 :
+			"<none>");
+
+	return count;
+}
+
+CONFIGFS_ATTR_RO(stp_policy_, device);
+
+static struct configfs_attribute *stp_policy_attrs[] = {
+	&stp_policy_attr_device,
+	NULL,
+};
+
+void stp_policy_unbind(struct stp_policy *policy)
+{
+	struct stm_device *stm = policy->stm;
+
+	if (WARN_ON_ONCE(!policy->stm))
+		return;
+
+	mutex_lock(&stm->policy_mutex);
+	stm->policy = NULL;
+	mutex_unlock(&stm->policy_mutex);
+
+	policy->stm = NULL;
+
+	stm_put_device(stm);
+}
+
+static void stp_policy_release(struct config_item *item)
+{
+	struct stp_policy *policy = to_stp_policy(item);
+
+	stp_policy_unbind(policy);
+	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 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_policies_make(struct config_group *group, const char *name)
+{
+	struct config_group *ret;
+	struct stm_device *stm;
+	char *devname, *p;
+
+	devname = kasprintf(GFP_KERNEL, "%s", name);
+	if (!devname)
+		return ERR_PTR(-ENOMEM);
+
+	/*
+	 * node must look like <device_name>.<policy_name>, where
+	 * <device_name> is the name of an existing stm device and
+	 * <policy_name> is an arbitrary string
+	 */
+	p = strchr(devname, '.');
+	if (!p) {
+		kfree(devname);
+		return ERR_PTR(-EINVAL);
+	}
+
+	*p++ = '\0';
+
+	stm = stm_find_device(devname);
+	kfree(devname);
+
+	if (!stm)
+		return ERR_PTR(-ENODEV);
+
+	mutex_lock(&stm->policy_mutex);
+	if (stm->policy) {
+		ret = ERR_PTR(-EBUSY);
+		goto unlock_policy;
+	}
+
+	stm->policy = kzalloc(sizeof(*stm->policy), GFP_KERNEL);
+	if (!stm->policy) {
+		ret = ERR_PTR(-ENOMEM);
+		goto unlock_policy;
+	}
+
+	config_group_init_type_name(&stm->policy->group, name,
+				    &stp_policy_type);
+	stm->policy->stm = stm;
+
+	ret = &stm->policy->group;
+
+unlock_policy:
+	mutex_unlock(&stm->policy_mutex);
+
+	if (IS_ERR(ret))
+		stm_put_device(stm);
+
+	return ret;
+}
+
+static struct configfs_group_operations stp_policies_group_ops = {
+	.make_group	= stp_policies_make,
+};
+
+static struct config_item_type stp_policies_type = {
+	.ct_group_ops	= &stp_policies_group_ops,
+	.ct_owner	= THIS_MODULE,
+};
+
+static struct configfs_subsystem stp_policy_subsys = {
+	.su_group = {
+		.cg_item = {
+			.ci_namebuf	= "stp-policy",
+			.ci_type	= &stp_policies_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;
+	struct list_head *head = &policy->group.cg_children;
+	struct config_item *item;
+	char *start, *end = s;
+
+	if (list_empty(head))
+		return NULL;
+
+	/* return the first entry if everything else fails */
+	item = list_entry(head->next, struct config_item, ci_entry);
+	ret = to_stp_policy_node(item);
+
+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);
+	mutex_unlock(&stp_policy_subsys.su_mutex);
+
+	return policy_node;
+}
+
+void stp_policy_node_put(struct stp_policy_node *policy_node)
+{
+	config_item_put(&policy_node->group.cg_item);
+}
+
+int __init stp_configfs_init(void)
+{
+	int err;
+
+	config_group_init(&stp_policy_subsys.su_group);
+	mutex_init(&stp_policy_subsys.su_mutex);
+	err = configfs_register_subsystem(&stp_policy_subsys);
+
+	return err;
+}
+
+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..95ece0292
--- /dev/null
+++ b/drivers/hwtracing/stm/stm.h
@@ -0,0 +1,87 @@
+/*
+ * System Trace Module (STM) infrastructure
+ * Copyright (c) 2014, 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.
+ *
+ * STM class implements generic infrastructure for  System Trace Module devices
+ * as defined in MIPI STPv2 specification.
+ */
+
+#ifndef _STM_STM_H_
+#define _STM_STM_H_
+
+struct stp_policy;
+struct stp_policy_node;
+
+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);
+int stp_configfs_init(void);
+void stp_configfs_exit(void);
+
+struct stp_master {
+	unsigned int	nr_free;
+	unsigned long	chan_map[0];
+};
+
+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;
+	spinlock_t		link_lock;
+	struct list_head	link_list;
+	/* master allocation */
+	spinlock_t		mc_lock;
+	struct stp_master	*masters[0];
+};
+
+#define to_stm_device(_d)				\
+	container_of((_d), struct stm_device, dev)
+
+struct stm_output {
+	unsigned int		master;
+	unsigned int		channel;
+	unsigned int		nr_chans;
+};
+
+struct stm_file {
+	struct stm_device	*stm;
+	struct stp_policy_node	*policy_node;
+	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 stp_policy_node	*policy_node;
+	struct stm_output	output;
+};
+
+#define to_stm_source_device(_d)				\
+	container_of((_d), struct stm_source_device, dev)
+
+#endif /* _STM_STM_H_ */