FIQ: Implement WDT FIQ debugger

Implement Tegra WDT FIQ debug function.
Default is disabled.

Bug 1581432

Change-Id: Ic81ab4cd3285080016b37191e6e0fab18e330a30
Signed-off-by: Renn Wu <rewu@nvidia.com>
Reviewed-on: http://git-master/r/#/c/271988
Reviewed-on: http://git-master/r/662550
Reviewed-by: Bibek Basu <bbasu@nvidia.com>
Reviewed-by: Venkat Moganty <vmoganty@nvidia.com>

5 files changed, 407 insertions, 16 deletions

diff --git a/drivers/irqchip/irq-gic.c b/drivers/irqchip/irq-gic.c
index 65ab1b442c09..882ed97f7b9e 100644
--- a/drivers/irqchip/irq-gic.c
+++ b/drivers/irqchip/irq-gic.c
@@ -2,6 +2,7 @@
  *  linux/arch/arm/common/gic.c
  *
  *  Copyright (C) 2002 ARM Limited, All Rights Reserved.
+ *  Copyright (c) 2012-2015, NVIDIA CORPORATION.  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 as
@@ -426,7 +427,18 @@ static void __init gic_dist_init(struct gic_chip_data *gic)
 	for (i = 32; i < gic_irqs; i += 32)
 		writel_relaxed(0xffffffff, base + GIC_DIST_ENABLE_CLEAR + i * 4 / 32);
 
-	writel_relaxed(1, base + GIC_DIST_CTRL);
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	/*
+	 * Set all interrupts in group 1
+	 */
+	for (i = 0; i < gic_irqs; i += 32)
+		writel_relaxed(0xffffffff, base + GIC_DIST_IGROUP + i * 4 / 32);
+
+	writel_relaxed(0x3, base + GIC_DIST_CTRL);
+#else
+	writel_relaxed(0x1, base + GIC_DIST_CTRL);
+#endif
 }
 
 static void __cpuinit gic_cpu_init(struct gic_chip_data *gic)
@@ -465,7 +477,12 @@ static void __cpuinit gic_cpu_init(struct gic_chip_data *gic)
 		writel_relaxed(0xa0a0a0a0, dist_base + GIC_DIST_PRI + i * 4 / 4);
 
 	writel_relaxed(0xf0, base + GIC_CPU_PRIMASK);
-	writel_relaxed(1, base + GIC_CPU_CTRL);
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel_relaxed(0x1ef, base + GIC_CPU_CTRL);
+#else
+	writel_relaxed(0x1, base + GIC_CPU_CTRL);
+#endif
 }
 
 #ifdef CONFIG_CPU_PM
@@ -545,7 +562,15 @@ static void gic_dist_restore(unsigned int gic_nr)
 		writel_relaxed(gic_data[gic_nr].saved_spi_enable[i],
 			dist_base + GIC_DIST_ENABLE_SET + i * 4);
 
-	writel_relaxed(1, dist_base + GIC_DIST_CTRL);
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	for (i = 0; i < gic_irqs; i += 32)
+		writel_relaxed(0xffffffff, dist_base + GIC_DIST_IGROUP + i * 4 / 32);
+
+	writel_relaxed(0x3, dist_base + GIC_DIST_CTRL);
+#else
+	writel_relaxed(0x1, dist_base + GIC_DIST_CTRL);
+#endif
 }
 
 static void gic_cpu_save(unsigned int gic_nr)
@@ -601,8 +626,17 @@ static void gic_cpu_restore(unsigned int gic_nr)
 	for (i = 0; i < DIV_ROUND_UP(32, 4); i++)
 		writel_relaxed(0xa0a0a0a0, dist_base + GIC_DIST_PRI + i * 4);
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel_relaxed(0xffffffff, dist_base + GIC_DIST_IGROUP);
+#endif
 	writel_relaxed(0xf0, cpu_base + GIC_CPU_PRIMASK);
-	writel_relaxed(1, cpu_base + GIC_CPU_CTRL);
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel_relaxed(0x1ef, cpu_base + GIC_CPU_CTRL);
+#else
+	writel_relaxed(0x1, cpu_base + GIC_CPU_CTRL);
+#endif
 }
 
 static int gic_notifier(struct notifier_block *self, unsigned long cmd,	void *v)
@@ -676,7 +710,15 @@ void gic_raise_softirq(const struct cpumask *mask, unsigned int irq)
 	dsb();
 
 	/* this always happens on GIC0 */
-	writel_relaxed(map << 16 | irq, gic_data_dist_base(&gic_data[0]) + GIC_DIST_SOFTINT);
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel_relaxed(map << 16 | irq | 1<<15,
+		       gic_data_dist_base(&gic_data[0]) + GIC_DIST_SOFTINT);
+#else
+	writel_relaxed(map << 16 | irq ,
+		       gic_data_dist_base(&gic_data[0]) + GIC_DIST_SOFTINT);
+#endif
 }
 #endif
 
@@ -727,6 +769,18 @@ static int __cpuinit gic_secondary_init(struct notifier_block *nfb,
 	return NOTIFY_OK;
 }
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+void __cpuinit gic_secondary_init_t124(unsigned int gic_nr)
+{
+	void __iomem *dist_base = gic_data_dist_base(&gic_data[gic_nr]);
+
+	BUG_ON(gic_nr >= MAX_GIC_NR);
+
+	writel_relaxed(0xffffffff, dist_base + GIC_DIST_IGROUP);
+	gic_cpu_init(&gic_data[gic_nr]);
+}
+#endif
 /*
  * Notifier for enabling the GIC CPU interface. Set an arbitrarily high
  * priority because the GIC needs to be up before the ARM generic timers.
diff --git a/drivers/irqchip/irq-tegra.c b/drivers/irqchip/irq-tegra.c
index 7fde2967f9d8..87846e55f2e8 100644
--- a/drivers/irqchip/irq-tegra.c
+++ b/drivers/irqchip/irq-tegra.c
@@ -4,7 +4,7 @@
  * Author:
  *	Colin Cross <ccross@android.com>
  *
- * Copyright (c) 2010-2013, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2010-2015, NVIDIA CORPORATION.  All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms and conditions of the GNU General Public License,
@@ -85,6 +85,35 @@ static u32 ictlr_wake_mask[NUM_ICTLRS];
 #endif
 
 #ifdef CONFIG_FIQ
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+void tegra_fiq_ack(unsigned int fiq)
+{
+	void __iomem *base = IO_ADDRESS(TEGRA_ARM_PERIF_BASE + 0x2000);
+	readl_relaxed(base + GIC_CPU_INTACK);
+}
+
+static void tegra_legacy_avp_irq_mask(unsigned int irq)
+{
+	void __iomem *base;
+	pr_debug("%s: %d\n", __func__, irq);
+
+	irq -= FIRST_LEGACY_IRQ;
+	base = ictlr_reg_base[irq>>5];
+	writel(1 << (irq & 31), base + ICTLR_COP_IER_CLR);
+}
+
+static void tegra_legacy_avp_irq_unmask(unsigned int irq)
+{
+	void __iomem *base;
+	pr_debug("%s: %d\n", __func__, irq);
+
+	irq -= FIRST_LEGACY_IRQ;
+	base = ictlr_reg_base[irq>>5];
+	writel(1 << (irq & 31), base + ICTLR_COP_IER_SET);
+}
+#else
+
 static void tegra_legacy_select_fiq(unsigned int irq, bool fiq)
 {
 	void __iomem *base;
@@ -120,22 +149,33 @@ static void tegra_fiq_unmask(struct irq_data *d)
 	base = ictlr_reg_base[leg_irq >> 5];
 	writel(1 << (leg_irq & 31), base + ICTLR_CPU_IER_SET);
 }
-
+#endif
 void tegra_fiq_enable(int irq)
 {
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	tegra_legacy_avp_irq_mask(irq);
+#else
+	void __iomem *base = IO_ADDRESS(TEGRA_ARM_PERIF_BASE + 0x100);
 	/* enable FIQ */
-	u32 val = readl(gic_cpu_base + GIC_CPU_CTRL);
+	u32 val = readl(base + GIC_CPU_CTRL);
 	val &= ~8; /* pass FIQs through */
 	val |= 2; /* enableNS */
-	writel(val, gic_cpu_base + GIC_CPU_CTRL);
+	writel(val, base + GIC_CPU_CTRL);
 	tegra_legacy_select_fiq(irq, true);
 	tegra_fiq_unmask(irq_get_irq_data(irq));
+#endif
 }
 
 void tegra_fiq_disable(int irq)
 {
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	tegra_legacy_avp_irq_unmask(irq);
+#else
 	tegra_fiq_mask(irq_get_irq_data(irq));
 	tegra_legacy_select_fiq(irq, false);
+#endif
 }
 #endif /* CONFIG_FIQ */
 
@@ -156,6 +196,17 @@ void tegra_gic_cpu_disable(bool disable_pass_through)
 }
 #endif
 
+void tegra_gic_cpu_enable(void)
+{
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel(0x1EF, gic_cpu_base + GIC_CPU_CTRL);
+#else
+	writel(0x1, gic_cpu_base + GIC_CPU_CTRL);
+#endif
+}
+
+
 #ifdef CONFIG_PM_SLEEP
 int tegra_gic_pending_interrupt(void)
 {
@@ -173,7 +224,12 @@ void tegra_gic_dist_disable(void)
 
 void tegra_gic_dist_enable(void)
 {
-	writel(1, gic_dist_base + GIC_DIST_CTRL);
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel(0x3, gic_dist_base + GIC_DIST_CTRL);
+#else
+	writel(0x1, gic_dist_base + GIC_DIST_CTRL);
+#endif
 }
 
 void tegra_gic_disable_affinity(void)
@@ -228,6 +284,39 @@ void tegra_gic_affinity_to_cpu0(void)
 }
 #endif /* CONFIG_ARCH_TEGRA_2x_SOC */
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+static void tegra_gic_cpu_restore(void)
+{
+	u32 val = 0;
+	val = readl_relaxed(gic_dist_base + GIC_DIST_IGROUP +
+			    (INT_WDT_AVP/32)*4);
+	val &= ~(1 << (INT_WDT_AVP%32));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_IGROUP +
+		       (INT_WDT_AVP/32)*4);
+	val = readl_relaxed(gic_dist_base + GIC_DIST_PRI +
+			    (INT_WDT_AVP/4)*4);
+	val &= ~(0xff << ((INT_WDT_AVP%4)*8));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_PRI +
+		       (INT_WDT_AVP/4)*4);
+
+}
+static void tegra_gic_dist_restore(void)
+{
+	u32 val = 0;
+	val = readl_relaxed(gic_dist_base + GIC_DIST_IGROUP +
+			    (INT_WDT_AVP/32)*4);
+	val &= ~(1 << (INT_WDT_AVP%32));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_IGROUP +
+		       (INT_WDT_AVP/32)*4);
+	val = readl_relaxed(gic_dist_base + GIC_DIST_PRI +
+			    (INT_WDT_AVP/4)*4);
+	val &= ~(0xff << ((INT_WDT_AVP%4)*8));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_PRI +
+		       (INT_WDT_AVP/4)*4);
+}
+#endif
+
 static int tegra_gic_notifier(struct notifier_block *self,
 					unsigned long cmd, void *v)
 {
@@ -235,6 +324,17 @@ static int tegra_gic_notifier(struct notifier_block *self,
 	case CPU_PM_ENTER:
 		writel(0x1E0, gic_cpu_base + GIC_CPU_CTRL);
 		break;
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	case CPU_PM_ENTER_FAILED:
+	case CPU_PM_EXIT:
+		tegra_gic_cpu_restore();
+		break;
+	case CPU_CLUSTER_PM_ENTER_FAILED:
+	case CPU_CLUSTER_PM_EXIT:
+		tegra_gic_dist_restore();
+		break;
+#endif
 	}
 
 	return NOTIFY_OK;
@@ -245,6 +345,41 @@ static struct notifier_block tegra_gic_notifier_block = {
 };
 #endif /* CONFIG_PM_SLEEP */
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+static void __init tegra_gic_dist_init(void)
+{
+	u32 val = 0;
+	val = readl_relaxed(gic_dist_base + GIC_DIST_IGROUP +
+			    (INT_WDT_AVP/32)*4);
+	val &= ~(1 << (INT_WDT_AVP%32));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_IGROUP +
+		       (INT_WDT_AVP/32)*4);
+	val = readl_relaxed(gic_dist_base + GIC_DIST_PRI +
+			    (INT_WDT_AVP/4)*4);
+	val &= ~(0xff << ((INT_WDT_AVP%4)*8));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_PRI +
+		       (INT_WDT_AVP/4)*4);
+}
+
+void __cpuinit tegra_gic_secondary_init(void)
+{
+	u32 val = 0;
+	val = readl_relaxed(gic_dist_base + GIC_DIST_IGROUP +
+			    (INT_WDT_AVP/32)*4);
+	val &= ~(1 << (INT_WDT_AVP%32));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_IGROUP +
+		       (INT_WDT_AVP/32)*4);
+	val = readl_relaxed(gic_dist_base + GIC_DIST_PRI +
+			    (INT_WDT_AVP/4)*4);
+	val &= ~(0xff << ((INT_WDT_AVP%4)*8));
+	writel_relaxed(val, gic_dist_base + GIC_DIST_PRI +
+		       (INT_WDT_AVP/4)*4);
+}
+
+#endif
+
+
 int tegra_update_lp1_irq_wake(unsigned int irq, bool enable)
 {
 #ifdef CONFIG_PM_SLEEP
@@ -503,7 +638,10 @@ static int __init tegra_gic_of_init(struct device_node *node,
 	if (gic_version == GIC_V2)
 		cpu_pm_register_notifier(&tegra_gic_notifier_block);
 #endif
-
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	tegra_gic_dist_init();
+#endif
 	return 0;
 }
 IRQCHIP_DECLARE(tegra_gic, "nvidia,tegra-gic", tegra_gic_of_init);
diff --git a/drivers/platform/tegra/tegra_fiq_debugger.c b/drivers/platform/tegra/tegra_fiq_debugger.c
index 302b081e43a9..64f90d1b0cc3 100644
--- a/drivers/platform/tegra/tegra_fiq_debugger.c
+++ b/drivers/platform/tegra/tegra_fiq_debugger.c
@@ -4,7 +4,7 @@
  * Serial Debugger Interface for Tegra
  *
  * Copyright (C) 2008 Google, Inc.
- * Copyright (c) 2012-2013, NVIDIA CORPORATION.  All rights reserved.
+ * Copyright (c) 2012-2015, NVIDIA CORPORATION.  All rights reserved.
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -124,6 +124,12 @@ static void fiq_enable(struct platform_device *pdev, unsigned int irq, bool on)
 		tegra_fiq_disable(irq);
 }
 
+
+static void fiq_ack(struct platform_device *pdev, unsigned int fiq)
+{
+	tegra_fiq_ack(fiq);
+}
+
 static int tegra_fiq_debugger_id;
 
 static void __tegra_serial_debug_init(unsigned int base, int fiq, int irq,
@@ -145,7 +151,7 @@ static void __tegra_serial_debug_init(unsigned int base, int fiq, int irq,
 	t->pdata.uart_putc = debug_putc;
 	t->pdata.uart_flush = debug_flush;
 	t->pdata.fiq_enable = fiq_enable;
-
+	t->pdata.fiq_ack = fiq_ack;
 	t->debug_port_base = ioremap(base, PAGE_SIZE);
 	if (!t->debug_port_base) {
 		pr_err("Failed to ioremap for fiq debugger\n");
diff --git a/drivers/staging/android/fiq_debugger/fiq_debugger.c b/drivers/staging/android/fiq_debugger/fiq_debugger.c
index 7d6b4ae8a2cd..f466831a95ff 100644
--- a/drivers/staging/android/fiq_debugger/fiq_debugger.c
+++ b/drivers/staging/android/fiq_debugger/fiq_debugger.c
@@ -4,6 +4,7 @@
  * Serial Debugger Interface accessed through an FIQ interrupt.
  *
  * Copyright (C) 2008 Google, Inc.
+ * Copyright (c) 2012-2015, NVIDIA CORPORATION.  All rights reserved.
  *
  * This software is licensed under the terms of the GNU General Public
  * License version 2, as published by the Free Software Foundation, and
@@ -49,7 +50,6 @@
 #define MAX_UNHANDLED_FIQ_COUNT 1000000
 
 #define MAX_FIQ_DEBUGGER_PORTS 4
-
 struct fiq_debugger_state {
 #ifdef CONFIG_FIQ_GLUE
 	struct fiq_glue_handler handler;
diff --git a/drivers/watchdog/tegra_wdt.c b/drivers/watchdog/tegra_wdt.c
index a4533d4de8fa..d83adeff021e 100644
--- a/drivers/watchdog/tegra_wdt.c
+++ b/drivers/watchdog/tegra_wdt.c
@@ -3,7 +3,7 @@
  *
  * watchdog driver for NVIDIA tegra internal watchdog
  *
- * Copyright (c) 2012-2014, NVIDIA CORPORATION. All rights reserved.
+ * Copyright (c) 2012-2015, NVIDIA CORPORATION. All rights reserved.
  *
  * based on drivers/watchdog/softdog.c and drivers/watchdog/omap_wdt.c
  *
@@ -74,13 +74,19 @@ struct tegra_wdt {
 	struct resource		*res_avp_src;
 	void __iomem		*wdt_avp_source;
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	struct resource         *res_avp_src;
+	void __iomem            *wdt_avp_source;
+#endif
 };
 
 /*
  * For spinlock lockup detection to work, the heartbeat should be 2*lockup
  * for cases where the spinlock disabled irqs.
  */
-static int heartbeat = 120; /* must be greater than MIN_WDT_PERIOD and lower than MAX_WDT_PERIOD */
+static int heartbeat = 80; /* must be greater than MIN_WDT_PERIOD and lower than MAX_WDT_PERIOD */
 
 #if defined(CONFIG_ARCH_TEGRA_2x_SOC)
 
@@ -161,6 +167,12 @@ static inline void tegra_wdt_ping(struct tegra_wdt *wdt)
 	if (!watchdog_get_lockup_state())
 		writel(WDT_CMD_START_COUNTER, wdt->wdt_avp_source + WDT_CMD);
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel(WDT_CMD_START_COUNTER, wdt->wdt_avp_source + WDT_CMD);
+#endif
+
 }
 
 #if !defined(CONFIG_ARCH_TEGRA_12x_SOC) || !defined(CONFIG_FIQ_DEBUGGER)
@@ -178,6 +190,22 @@ static void tegra_wdt_int_priority(struct tegra_wdt *wdt)
 #endif
 #endif
 
+#if defined(CONFIG_TRUSTED_FOUNDATIONS) || \
+	!defined(CONFIG_ARCH_TEGRA_12x_SOC) || !defined(CONFIG_FIQ_DEBUGGER)
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+static void tegra_wdt_int_priority(struct tegra_wdt *wdt)
+{
+	unsigned val = 0;
+
+	if (!wdt->int_base)
+		return;
+	val = readl(wdt->int_base + ICTLR_IEP_CLASS);
+	val &= ~(1 << (INT_WDT_CPU & 31));
+	writel(val, wdt->int_base + ICTLR_IEP_CLASS);
+}
+#endif
+#endif
+
 static void tegra_wdt_enable(struct tegra_wdt *wdt)
 {
 	u32 val;
@@ -201,9 +229,26 @@ static void tegra_wdt_enable(struct tegra_wdt *wdt)
 	val |= WDT_CFG_FIQ_INT_EN;
 #endif
 #endif
+
+#if defined(CONFIG_TRUSTED_FOUNDATIONS) || \
+	!defined(CONFIG_ARCH_TEGRA_12x_SOC) || !defined(CONFIG_FIQ_DEBUGGER)
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+	val |= WDT_CFG_FIQ_INT_EN;
+#endif
+#endif
+
 	writel(val, wdt->wdt_source + WDT_CFG);
 	writel(WDT_CMD_START_COUNTER, wdt->wdt_source + WDT_CMD);
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	val = wdt->tmrsrc | (WDT_CFG_PERIOD << 1) | /*WDT_CFG_INT_EN |*/
+		/*WDT_CFG_SYS_RST_EN |*/ WDT_CFG_PMC2CAR_RST_EN;
+	writel(val, wdt->wdt_avp_source + WDT_CFG);
+	writel(WDT_CMD_START_COUNTER, wdt->wdt_avp_source + WDT_CMD);
+
+#endif
+
 #if defined(CONFIG_TRUSTED_LITTLE_KERNEL) && \
 	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
 	val = wdt->tmrsrc | (WDT_CFG_PERIOD << 1) | /*WDT_CFG_INT_EN |*/
@@ -225,6 +270,11 @@ static void tegra_wdt_disable(struct tegra_wdt *wdt)
 	writel(WDT_CMD_DISABLE_COUNTER, wdt->wdt_avp_source + WDT_CMD);
 #endif
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	writel(WDT_UNLOCK_PATTERN, wdt->wdt_avp_source + WDT_UNLOCK);
+	writel(WDT_CMD_DISABLE_COUNTER, wdt->wdt_avp_source + WDT_CMD);
+#endif
 	writel(0, wdt->wdt_timer + TIMER_PTV);
 }
 
@@ -252,6 +302,15 @@ static irqreturn_t tegra_wdt_avp_interrupt(int irq, void *dev_id)
 }
 #endif
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+static irqreturn_t tegra_wdt_avp_interrupt(int irq, void *dev_id)
+{
+	return IRQ_HANDLED;
+}
+#endif
+
+
 #endif
 
 static int tegra_wdt_notify(struct notifier_block *this,
@@ -436,6 +495,12 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
 	struct resource *res_avp_src, *res_avp_irq;
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	struct resource *res_avp_src, *res_avp_irq;
+#endif
+
 	struct resource	*res_int_base = NULL;
 	struct tegra_wdt *wdt;
 	int ret = 0;
@@ -456,13 +521,27 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 	res_avp_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
 #endif
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	res_avp_src = platform_get_resource(pdev, IORESOURCE_MEM, 4);
+	res_avp_irq = platform_get_resource(pdev, IORESOURCE_IRQ, 1);
+#endif
+
+
 #if defined(CONFIG_TRUSTED_LITTLE_KERNEL) && \
 	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
 	if (!res_src || !res_wdt || !res_avp_src || (!pdev->id && !res_irq) ||
 	    !res_pmc) {
 #else
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	if (!res_src || !res_wdt || !res_avp_src || (!pdev->id && !res_irq)) {
+#else
 	if (!res_src || !res_wdt || (!pdev->id && !res_irq) || !res_pmc) {
 #endif
+
+#endif
 		dev_err(&pdev->dev, "incorrect resources\n");
 		return -ENOENT;
 	}
@@ -477,6 +556,18 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 #endif
 #endif
 
+#if defined(CONFIG_TRUSTED_FOUNDATIONS) || \
+	!defined(CONFIG_ARCH_TEGRA_12x_SOC) || !defined(CONFIG_FIQ_DEBUGGER)
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+	res_int_base = platform_get_resource(pdev, IORESOURCE_MEM, 3);
+	if (!pdev->id && !res_int_base) {
+		dev_err(&pdev->dev, "FIQ_DBG: INT base not defined\n");
+		return -ENOENT;
+	}
+#endif
+#endif
+
+
 	if (pdev->id == -1 && !res_irq) {
 		dev_err(&pdev->dev, "incorrect irq\n");
 		return -ENOENT;
@@ -522,12 +613,26 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 					 pdev->name);
 #endif
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	res_avp_src = request_mem_region(res_avp_src->start,
+					 resource_size(res_avp_src),
+					 pdev->name);
+#endif
+
 #if defined(CONFIG_TRUSTED_LITTLE_KERNEL) && \
 	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
 	if (!res_src || !res_wdt || !res_avp_src || !res_pmc) {
 #else
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	if (!res_src || !res_wdt || !res_avp_src || !res_pmc) {
+#else
 	if (!res_src || !res_wdt || !res_pmc) {
 #endif
+
+#endif
 		dev_err(&pdev->dev, "unable to request memory resources\n");
 		ret = -EBUSY;
 		goto fail;
@@ -542,6 +647,13 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 	wdt->wdt_avp_source = ioremap(res_avp_src->start,
 				      resource_size(res_avp_src));
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	wdt->wdt_avp_source = ioremap(res_avp_src->start,
+				      resource_size(res_avp_src));
+#endif
+
 	/* tmrsrc will be used to set WDT_CFG */
 	wdt->tmrsrc = (TMR_SRC_START + pdev->id) % 10;
 
@@ -550,8 +662,15 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 	if (!wdt->wdt_source || !wdt->wdt_timer || !wdt->wdt_avp_source ||
 	    !wdt->pmc_base) {
 #else
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	if (!wdt->wdt_source || !wdt->wdt_timer || !wdt->wdt_avp_source ||
+	    !wdt->pmc_base) {
+#else
 	if (!wdt->wdt_source || !wdt->wdt_timer || !wdt->pmc_base) {
 #endif
+
+#endif
 		dev_err(&pdev->dev, "unable to map registers\n");
 		ret = -ENOMEM;
 		goto fail;
@@ -583,6 +702,28 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 		tegra_wdt_int_priority(wdt);
 #endif
 #endif
+
+#if defined(CONFIG_TRUSTED_FOUNDATIONS) || \
+	!defined(CONFIG_ARCH_TEGRA_12x_SOC) || !defined(CONFIG_FIQ_DEBUGGER)
+#ifdef CONFIG_TEGRA_FIQ_DEBUGGER
+		/* FIQ debugger enables FIQ priority for INT_WDT_CPU.
+		 * But that will disable IRQ on WDT expiration.
+		 * Reset the priority back to IRQ on INT_WDT_CPU so
+		 * that tegra_wdt_interrupt gets its chance to restart the
+		 * counter before expiration.
+		 */
+		res_int_base = request_mem_region(res_int_base->start,
+						  resource_size(res_int_base),
+						  pdev->name);
+		if (!res_int_base)
+			goto fail;
+		wdt->int_base = ioremap(res_int_base->start,
+					resource_size(res_int_base));
+		if (!wdt->int_base)
+			goto fail;
+		tegra_wdt_int_priority(wdt);
+#endif
+#endif
 		ret = request_irq(res_irq->start, tegra_wdt_interrupt,
 				  IRQF_DISABLED, dev_name(&pdev->dev), wdt);
 		if (ret) {
@@ -600,6 +741,15 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 		}
 #endif
 
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+		ret = request_irq(res_avp_irq->start, tegra_wdt_avp_interrupt,
+				IRQF_DISABLED, "avp_wdt", wdt);
+		if (ret) {
+			dev_err(&pdev->dev, "unable to configure WDT AVP IRQ\n");
+			goto fail;
+		}
+#endif
 		wdt->irq = res_irq->start;
 	}
 
@@ -609,6 +759,11 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
 	wdt->res_avp_src = res_avp_src;
 #endif
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	wdt->res_avp_src = res_avp_src;
+#endif
+
 	wdt->res_wdt = res_wdt;
 	wdt->res_int_base = res_int_base;
 	wdt->res_pmc = res_pmc;
@@ -648,6 +803,15 @@ static int tegra_wdt_probe(struct platform_device *pdev)
 		writel(val, wdt->wdt_avp_source + WDT_CFG);
 
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+		val = readl(wdt->wdt_avp_source + WDT_CFG);
+		val |= WDT_CFG_INT_EN;
+		writel(val, wdt->wdt_avp_source + WDT_CFG);
+
+#endif
+
 		pr_info("WDT heartbeat enabled on probe\n");
 	}
 #endif
@@ -684,6 +848,15 @@ fail:
 		release_mem_region(res_avp_src->start,
 				   resource_size(res_avp_src));
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	if (wdt->wdt_avp_source)
+		iounmap(wdt->wdt_avp_source);
+	if (res_avp_src)
+		release_mem_region(res_avp_src->start,
+				   resource_size(res_avp_src));
+#endif
 	kfree(wdt);
 	return ret;
 }
@@ -704,6 +877,11 @@ static int tegra_wdt_remove(struct platform_device *pdev)
 	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
 	iounmap(wdt->wdt_avp_source);
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	iounmap(wdt->wdt_avp_source);
+#endif
 	iounmap(wdt->wdt_timer);
 	if (wdt->int_base)
 		iounmap(wdt->int_base);
@@ -716,6 +894,13 @@ static int tegra_wdt_remove(struct platform_device *pdev)
 	release_mem_region(wdt->res_avp_src->start,
 			   resource_size(wdt->res_avp_src));
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+	release_mem_region(wdt->res_avp_src->start,
+			   resource_size(wdt->res_avp_src));
+#endif
+
 	release_mem_region(wdt->res_wdt->start, resource_size(wdt->res_wdt));
 	if (wdt->res_int_base)
 		release_mem_region(wdt->res_int_base->start,
@@ -758,6 +943,14 @@ static int tegra_wdt_resume(struct platform_device *pdev)
 		val |= WDT_CFG_INT_EN;
 		writel(val, wdt->wdt_avp_source + WDT_CFG);
 #endif
+
+#if !defined(CONFIG_TRUSTED_FOUNDATIONS) && \
+	defined(CONFIG_ARCH_TEGRA_12x_SOC) && defined(CONFIG_FIQ_DEBUGGER)
+		val = readl(wdt->wdt_avp_source + WDT_CFG);
+		val |= WDT_CFG_INT_EN;
+		writel(val, wdt->wdt_avp_source + WDT_CFG);
+#endif
+
 		pr_info("WDT heartbeat enabled on probe\n");
 	}
 #endif