diff --git a/arch/arm/mach-msm/Kconfig b/arch/arm/mach-msm/Kconfig
index f3ae47e..db2eda5 100644
--- a/arch/arm/mach-msm/Kconfig
+++ b/arch/arm/mach-msm/Kconfig
@@ -133,6 +133,7 @@
 	select MSM_AUDIO_QDSP6 if SND_SOC
 	select CPU_HAS_L2_PMU
 	select MSM_SPM_V2
+	select HAS_QDSS
 	select MSM_L2_SPM
 	select MSM_NATIVE_RESTART
 	select DONT_MAP_HOLE_AFTER_MEMBANK0
@@ -869,13 +870,6 @@
 	help
 	 Select this if the L2 cache controller has a Performance Monitoring Unit.
 
-config MSM_JTAG_V7
-	depends on CPU_V7
-	default y if DEBUG_KERNEL
-        bool "JTAG debug support"
-	help
-          Add additional support for JTAG kernel debugging.
-
 config HTC_HEADSET
 	tristate "HTC 2 Wire detection driver"
 	default n
@@ -1682,25 +1676,43 @@
 		make the kernel reboot on a kernel panic - that must be
 		enabled via another mechanism.
 
+config HAS_QDSS
+	bool "QDSS Present"
+	help
+	 Select this if the chip has Qualcomm Debug Subsystem implemented.
+
+config MSM_DEBUG_ACROSS_PC
+	bool "Debug support across power collapse"
+	help
+	  Enables debug state to be saved and restored across power collapse.
+
+config MSM_JTAG_V7
+	depends on CPU_V7 && !HAS_QDSS
+	default y if DEBUG_KERNEL
+	bool "JTAG debug support"
+	select MSM_DEBUG_ACROSS_PC
+	help
+          Add additional support for JTAG kernel debugging.
+
 config MSM_TRACE_ACROSS_PC
 	bool "Trace support across power collapse"
-	depends on ARCH_MSM8X60 || ARCH_MSM8960
 	help
 	  Enables trace state to be saved and restored across power collapse.
 
 config MSM_ETM
 	tristate "Enable MSM ETM and ETB"
-	depends on ARCH_MSM8X60
+	depends on ARCH_MSM8X60 && !HAS_QDSS
 	select MSM_TRACE_ACROSS_PC
 	help
 	  Enables embedded trace collection on Qualcomm v7 CPUs.
 
 config MSM_QDSS
-	bool "Coresight tracing support"
-	depends on ARCH_MSM8960
+	bool "Qualcomm Debug Subsystem"
+	depends on HAS_QDSS
+	select MSM_DEBUG_ACROSS_PC
 	select MSM_TRACE_ACROSS_PC
 	help
-	  Enables support for Qualcomm debug subsystem.
+	  Enables support for Qualcomm Debug Subsystem.
 
 config MSM_SLEEP_STATS
 	bool "Enable exporting of MSM sleep stats to userspace"
diff --git a/arch/arm/mach-msm/Makefile b/arch/arm/mach-msm/Makefile
index 74f312a..60259ca 100644
--- a/arch/arm/mach-msm/Makefile
+++ b/arch/arm/mach-msm/Makefile
@@ -45,7 +45,13 @@
 
 msm-etm-objs := cp14.o etm.o
 obj-$(CONFIG_MSM_ETM) += msm-etm.o
-obj-$(CONFIG_MSM_QDSS) += qdss-etb.o qdss-tpiu.o qdss-funnel.o qdss-ptm.o
+
+ifdef CONFIG_MSM_QDSS
+	obj-y += qdss-debug.o
+	obj-y += qdss-etb.o qdss-tpiu.o
+	obj-y += qdss-funnel.o
+	obj-y += qdss-ptm.o
+endif
 
 quiet_cmd_mkrpcsym = MKCAP   $@
       cmd_mkrpcsym = $(PERL) $(srctree)/$(src)/mkrpcsym.pl $< $@
diff --git a/arch/arm/mach-msm/board-msm8960.c b/arch/arm/mach-msm/board-msm8960.c
index 41f8879..537605b 100644
--- a/arch/arm/mach-msm/board-msm8960.c
+++ b/arch/arm/mach-msm/board-msm8960.c
@@ -3790,6 +3790,7 @@
 	&msm_etb_device,
 	&msm_tpiu_device,
 	&msm_funnel_device,
+	&msm_debug_device,
 	&msm_ptm_device,
 #endif
 	&msm_device_dspcrashd_8960,
diff --git a/arch/arm/mach-msm/devices-8960.c b/arch/arm/mach-msm/devices-8960.c
index d9bf7db..c5eee63 100644
--- a/arch/arm/mach-msm/devices-8960.c
+++ b/arch/arm/mach-msm/devices-8960.c
@@ -2365,6 +2365,7 @@
 #define MSM_ETB_PHYS_BASE		(MSM_QDSS_PHYS_BASE + 0x1000)
 #define MSM_TPIU_PHYS_BASE		(MSM_QDSS_PHYS_BASE + 0x3000)
 #define MSM_FUNNEL_PHYS_BASE		(MSM_QDSS_PHYS_BASE + 0x4000)
+#define MSM_DEBUG_PHYS_BASE		(MSM_QDSS_PHYS_BASE + 0x10000)
 #define MSM_PTM_PHYS_BASE		(MSM_QDSS_PHYS_BASE + 0x1C000)
 
 static struct resource msm_etb_resources[] = {
@@ -2412,6 +2413,26 @@
 	.resource      = msm_funnel_resources,
 };
 
+static struct resource msm_debug_resources[] = {
+	{
+		.start = MSM_DEBUG_PHYS_BASE,
+		.end   = MSM_DEBUG_PHYS_BASE + SZ_4K - 1,
+		.flags = IORESOURCE_MEM,
+	},
+	{
+		.start = MSM_DEBUG_PHYS_BASE + (SZ_4K * 2),
+		.end   = MSM_DEBUG_PHYS_BASE + (SZ_4K * 2) + SZ_4K - 1,
+		.flags = IORESOURCE_MEM,
+	},
+};
+
+struct platform_device msm_debug_device = {
+	.name          = "msm_debug",
+	.id            = 0,
+	.num_resources = ARRAY_SIZE(msm_debug_resources),
+	.resource      = msm_debug_resources,
+};
+
 static struct resource msm_ptm_resources[] = {
 	{
 		.start = MSM_PTM_PHYS_BASE,
diff --git a/arch/arm/mach-msm/devices-msm8x60.h b/arch/arm/mach-msm/devices-msm8x60.h
index 6b7d141..1d5dee1 100644
--- a/arch/arm/mach-msm/devices-msm8x60.h
+++ b/arch/arm/mach-msm/devices-msm8x60.h
@@ -69,12 +69,6 @@
 #ifdef CONFIG_MSM_DSPS
 extern struct platform_device msm_dsps_device;
 #endif
-#ifdef CONFIG_MSM_QDSS
-extern struct platform_device msm_etb_device;
-extern struct platform_device msm_tpiu_device;
-extern struct platform_device msm_funnel_device;
-extern struct platform_device msm_ptm_device;
-#endif
 
 #if defined(CONFIG_MSM_RPM_STATS_LOG)
 extern struct platform_device msm_rpm_stat_device;
diff --git a/arch/arm/mach-msm/devices.h b/arch/arm/mach-msm/devices.h
index b706fcd..7196224 100644
--- a/arch/arm/mach-msm/devices.h
+++ b/arch/arm/mach-msm/devices.h
@@ -209,4 +209,10 @@
 extern struct platform_device msm8660_device_watchdog;
 extern struct platform_device msm8064_device_watchdog;
 extern struct platform_device msm9615_device_watchdog;
+
+extern struct platform_device msm_etb_device;
+extern struct platform_device msm_tpiu_device;
+extern struct platform_device msm_funnel_device;
+extern struct platform_device msm_debug_device;
+extern struct platform_device msm_ptm_device;
 #endif
diff --git a/arch/arm/mach-msm/idle-v7.S b/arch/arm/mach-msm/idle-v7.S
index a6e8fdb..b0c075b 100644
--- a/arch/arm/mach-msm/idle-v7.S
+++ b/arch/arm/mach-msm/idle-v7.S
@@ -32,15 +32,7 @@
 #endif
 
 ENTRY(msm_arch_idle)
-#ifdef CONFIG_MSM_JTAG_V7
-	stmfd   sp!, {lr}
-	bl      msm_save_jtag_debug
-#endif
 	wfi
-#ifdef CONFIG_MSM_JTAG_V7
-	bl      msm_restore_jtag_debug
-	ldmfd   sp!, {lr}
-#endif
 	bx	lr
 
 ENTRY(msm_pm_collapse)
@@ -85,7 +77,7 @@
 	stmia   r0!, {r1-r3}
 #endif
 
-#ifdef CONFIG_MSM_JTAG_V7
+#ifdef CONFIG_MSM_DEBUG_ACROSS_PC
 	bl      msm_save_jtag_debug
 #endif
 #ifdef CONFIG_MSM_TRACE_ACROSS_PC
@@ -132,7 +124,7 @@
 #ifdef CONFIG_MSM_TRACE_ACROSS_PC
 	bl	etm_restore_reg_check
 #endif
-#ifdef CONFIG_MSM_JTAG_V7
+#ifdef CONFIG_MSM_DEBUG_ACROSS_PC
 	bl	msm_restore_jtag_debug
 #endif
 	ldr     r0, =saved_state        /* restore registers */
@@ -220,7 +212,7 @@
 #ifdef CONFIG_MSM_TRACE_ACROSS_PC
 	bl      etm_restore_reg_check
 #endif
-#ifdef CONFIG_MSM_JTAG_V7
+#ifdef CONFIG_MSM_DEBUG_ACROSS_PC
 	bl      msm_restore_jtag_debug
 #endif
 	ldmfd   sp!, {lr}
diff --git a/arch/arm/mach-msm/pm-8x60.c b/arch/arm/mach-msm/pm-8x60.c
index ad6da6a..cb5fcff 100644
--- a/arch/arm/mach-msm/pm-8x60.c
+++ b/arch/arm/mach-msm/pm-8x60.c
@@ -1111,6 +1111,7 @@
 		return 0;
 	}
 	etm_restore_reg_check();
+	msm_restore_jtag_debug();
 #ifdef CONFIG_VFP
 	vfp_reinit();
 #endif
diff --git a/arch/arm/mach-msm/qdss-debug.c b/arch/arm/mach-msm/qdss-debug.c
new file mode 100644
index 0000000..b2d38b1
--- /dev/null
+++ b/arch/arm/mach-msm/qdss-debug.c
@@ -0,0 +1,285 @@
+/* Copyright (c) 2011, Code Aurora Forum. All rights reserved.
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License version 2 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that 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/kernel.h>
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/types.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/smp.h>
+
+#include "qdss.h"
+
+#define debug_writel(debug, cpu, val, off)	\
+			__raw_writel((val), debug.base[cpu] + off)
+#define debug_readl(debug, cpu, off)		\
+			__raw_readl(debug.base[cpu] + off)
+
+#define DBGDIDR			(0x000)
+#define DBGWFAR			(0x018)
+#define DBGVCR			(0x01C)
+#define DBGECR			(0x024)
+#define DBGDTRRX		(0x080)
+#define DBGITR			(0x084)
+#define DBGDSCR			(0x088)
+#define DBGDTRTX		(0x08C)
+#define DBGDRCR			(0x090)
+#define DBGEACR			(0x094)
+#define DBGPCSR			(0x0A0)
+#define DBGCIDSR		(0x0A4)
+#define DBGVIDSR		(0x0A8)
+#define DBGBVRm(n)		(0x100 + (n * 4))
+#define DBGBCRm(n)		(0x140 + (n * 4))
+#define DBGWVRm(n)		(0x180 + (n * 4))
+#define DBGWCRm(n)		(0x1C0 + (n * 4))
+#define DBGBXVRm(n)		(0x240 + (n * 4))
+#define DBGOSLAR		(0x300)
+#define DBGOSLSR		(0x304)
+#define DBGPRCR			(0x310)
+#define DBGPRSR			(0x314)
+
+
+#define DEBUG_LOCK(cpu)							\
+do {									\
+	mb();								\
+	debug_writel(debug, cpu, MAGIC2, CS_LAR);			\
+} while (0)
+#define DEBUG_UNLOCK(cpu)						\
+do {									\
+	debug_writel(debug, cpu, MAGIC1, CS_LAR);			\
+	mb();								\
+} while (0)
+
+#define DEBUG_OS_LOCK(cpu)						\
+do {									\
+	debug_writel(debug, cpu, MAGIC1, DBGOSLAR);			\
+	mb();								\
+} while (0)
+#define DEBUG_OS_UNLOCK(cpu)						\
+do {									\
+	mb();								\
+	debug_writel(debug, cpu, MAGIC2, DBGOSLAR);			\
+	mb();								\
+} while (0)
+
+#define MAX_DEBUG_REGS		(90)
+#define MAX_STATE_SIZE		(MAX_DEBUG_REGS * num_possible_cpus())
+#define DBGDSCR_MASK		(0x6C30FC3C)
+
+struct debug_config {
+	/* read only config register */
+	uint32_t	dbg_id;
+	/* derived values */
+	uint8_t		nr_watch_pts;
+	uint8_t		nr_brk_pts;
+	uint8_t		nr_ctx_comp;
+};
+
+struct debug_ctx {
+	struct debug_config		cfg;
+	void __iomem			**base;
+	uint32_t			*state;
+	struct device			*dev;
+};
+
+static struct debug_ctx debug;
+
+static void debug_save_reg(int cpu)
+{
+	uint32_t i;
+	int j;
+
+	DEBUG_UNLOCK(cpu);
+	DEBUG_OS_LOCK(cpu);
+
+	i = cpu * MAX_DEBUG_REGS;
+
+	debug.state[i++] = debug_readl(debug, cpu, DBGWFAR);
+	for (j = 0; j < debug.cfg.nr_brk_pts; j++) {
+		debug.state[i++] = debug_readl(debug, cpu, DBGBCRm(j));
+		debug.state[i++] = debug_readl(debug, cpu, DBGBVRm(j));
+	}
+	for (j = 0; j < debug.cfg.nr_ctx_comp; j++)
+		debug.state[i++] = debug_readl(debug, cpu, DBGBXVRm(j));
+	for (j = 0; j < debug.cfg.nr_watch_pts; j++) {
+		debug.state[i++] = debug_readl(debug, cpu, DBGWVRm(j));
+		debug.state[i++] = debug_readl(debug, cpu, DBGWCRm(j));
+	}
+	debug.state[i++] = debug_readl(debug, cpu, DBGVCR);
+	debug.state[i++] = debug_readl(debug, cpu, CS_CLAIMSET);
+	debug.state[i++] = debug_readl(debug, cpu, CS_CLAIMCLR);
+	debug.state[i++] = debug_readl(debug, cpu, DBGDTRTX);
+	debug.state[i++] = debug_readl(debug, cpu, DBGDTRRX);
+	debug.state[i++] = debug_readl(debug, cpu, DBGDSCR);
+
+	DEBUG_LOCK(cpu);
+}
+
+static void debug_restore_reg(int cpu)
+{
+	uint32_t i;
+	int j;
+
+	DEBUG_UNLOCK(cpu);
+	DEBUG_OS_LOCK(cpu);
+
+	i = cpu * MAX_DEBUG_REGS;
+
+	debug_writel(debug, cpu, debug.state[i++], DBGWFAR);
+	for (j = 0; j < debug.cfg.nr_brk_pts; j++) {
+		debug_writel(debug, cpu, debug.state[i++], DBGBCRm(j));
+		debug_writel(debug, cpu, debug.state[i++], DBGBVRm(j));
+	}
+	for (j = 0; j < debug.cfg.nr_ctx_comp; j++)
+		debug_writel(debug, cpu, debug.state[i++], DBGBXVRm(j));
+	for (j = 0; j < debug.cfg.nr_watch_pts; j++) {
+		debug_writel(debug, cpu, debug.state[i++], DBGWVRm(j));
+		debug_writel(debug, cpu, debug.state[i++], DBGWCRm(j));
+	}
+	debug_writel(debug, cpu, debug.state[i++], DBGVCR);
+	debug_writel(debug, cpu, debug.state[i++], CS_CLAIMSET);
+	debug_writel(debug, cpu, debug.state[i++], CS_CLAIMCLR);
+	debug_writel(debug, cpu, debug.state[i++], DBGDTRTX);
+	debug_writel(debug, cpu, debug.state[i++], DBGDTRRX);
+	debug_writel(debug, cpu, debug.state[i++] & DBGDSCR_MASK, DBGDSCR);
+
+	DEBUG_OS_UNLOCK(cpu);
+	DEBUG_LOCK(cpu);
+}
+
+/* msm_save_jtag_debug and msm_restore_jtag_debug should be fast
+ *
+ * These functions will be called either from:
+ * 1. per_cpu idle thread context for idle power collapses.
+ * 2. per_cpu idle thread context for hotplug/suspend power collapse for
+ *    nonboot cpus.
+ * 3. suspend thread context for core0.
+ *
+ * In all cases we are guaranteed to be running on the same cpu for the
+ * entire duration.
+ */
+void msm_save_jtag_debug(void)
+{
+	int cpu = smp_processor_id();
+	debug_save_reg(cpu);
+}
+
+void msm_restore_jtag_debug(void)
+{
+	int cpu = smp_processor_id();
+	debug_restore_reg(cpu);
+}
+
+static void debug_cfg_ro_init(void)
+{
+	/* use cpu 0 for setup */
+	int cpu = 0;
+
+	DEBUG_UNLOCK(cpu);
+
+	debug.cfg.dbg_id = debug_readl(debug, cpu, DBGDIDR);
+	debug.cfg.nr_ctx_comp = BMVAL(debug.cfg.dbg_id, 20, 23) + 1;
+	debug.cfg.nr_brk_pts = BMVAL(debug.cfg.dbg_id, 24, 27) + 1;
+	debug.cfg.nr_watch_pts = BMVAL(debug.cfg.dbg_id, 28, 31) + 1;
+
+	DEBUG_LOCK(cpu);
+}
+
+static int __devinit debug_probe(struct platform_device *pdev)
+{
+	int i, ret;
+	struct resource *res;
+
+	debug.base = kzalloc(pdev->num_resources * sizeof(void *), GFP_KERNEL);
+	if (!debug.base) {
+		ret = -ENOMEM;
+		goto err_base_kzalloc;
+	}
+
+	for (i = 0; i < pdev->num_resources; i++) {
+		res = platform_get_resource(pdev, IORESOURCE_MEM, i);
+		if (!res) {
+			ret = -EINVAL;
+			goto err_res;
+		}
+
+		debug.base[i] = ioremap_nocache(res->start, resource_size(res));
+		if (!debug.base[i]) {
+			ret = -EINVAL;
+			goto err_ioremap;
+		}
+	}
+
+	debug.dev = &pdev->dev;
+
+	debug.state = kzalloc(MAX_STATE_SIZE * sizeof(uint32_t), GFP_KERNEL);
+	if (!debug.state) {
+		ret = -ENOMEM;
+		goto err_state_kzalloc;
+	}
+
+	debug_cfg_ro_init();
+
+	dev_info(debug.dev, "Debug intialized.\n");
+
+	return 0;
+
+err_state_kzalloc:
+err_ioremap:
+err_res:
+	while (i) {
+		iounmap(debug.base[i-1]);
+		i--;
+	}
+	kfree(debug.base);
+err_base_kzalloc:
+	return ret;
+}
+
+static int __devexit debug_remove(struct platform_device *pdev)
+{
+	int i;
+
+	kfree(debug.state);
+	for (i = pdev->num_resources; i > 0; i--)
+		iounmap(debug.base[i-1]);
+	kfree(debug.base);
+
+	return 0;
+}
+
+static struct platform_driver debug_driver = {
+	.probe          = debug_probe,
+	.remove         = __devexit_p(debug_remove),
+	.driver         = {
+		.name   = "msm_debug",
+	},
+};
+
+static int __init debug_init(void)
+{
+	return platform_driver_register(&debug_driver);
+}
+module_init(debug_init);
+
+static void __exit debug_exit(void)
+{
+	platform_driver_unregister(&debug_driver);
+}
+module_exit(debug_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_DESCRIPTION("Coresight Debug driver");
diff --git a/arch/arm/mach-msm/qdss-ptm.c b/arch/arm/mach-msm/qdss-ptm.c
index 4e8add7..0d971d7 100644
--- a/arch/arm/mach-msm/qdss-ptm.c
+++ b/arch/arm/mach-msm/qdss-ptm.c
@@ -119,6 +119,7 @@
 } while (0)
 #define PTM_OS_UNLOCK(cpu)						\
 do {									\
+	mb();								\
 	ptm_writel(ptm, cpu, MAGIC2, ETMOSLAR);				\
 	mb();								\
 } while (0)
diff --git a/arch/arm/mach-msm/qdss.h b/arch/arm/mach-msm/qdss.h
index a41d7e2..8d346c4 100644
--- a/arch/arm/mach-msm/qdss.h
+++ b/arch/arm/mach-msm/qdss.h
@@ -56,6 +56,13 @@
 void funnel_enable(uint8_t id, uint32_t port_mask);
 void funnel_disable(uint8_t id, uint32_t port_mask);
 
+#ifdef CONFIG_MSM_DEBUG_ACROSS_PC
+extern void msm_save_jtag_debug(void);
+extern void msm_restore_jtag_debug(void);
+#else
+static inline void msm_save_jtag_debug(void) {}
+static inline void msm_restore_jtag_debug(void) {}
+#endif
 #ifdef CONFIG_MSM_TRACE_ACROSS_PC
 extern void etm_save_reg_check(void);
 extern void etm_restore_reg_check(void);