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>

2 files changed, 203 insertions, 11 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);