[PATCH 2/3] zynq: Add function to enable JTAG

[lukas.funke-oss at weidmueller.com]

From: Stefan Herbrechtsmeier <stefan.herbrechtsmeier at weidmueller.com>

In non-secure boot mode jtag is restored by the BootROM. In secure boot
mode jtag has to be restored by the trusted application, i.e. the
bootloader.

This commit adds a function to enable the jtag interface on zynq
devices from u-boot.

Signed-off-by: Stefan Herbrechtsmeier <stefan.herbrechtsmeier at weidmueller.com>
Signed-off-by: Lukas Funke <lukas.funke at weidmueller.com>
---

 arch/arm/mach-zynq/cpu.c                    | 19 +++++++++++++++++++
 arch/arm/mach-zynq/include/mach/sys_proto.h |  1 +
 2 files changed, 20 insertions(+)

diff --git a/arch/arm/mach-zynq/cpu.c b/arch/arm/mach-zynq/cpu.c
index 3d2866422e..b8d413b69a 100644
--- a/arch/arm/mach-zynq/cpu.c
+++ b/arch/arm/mach-zynq/cpu.c
@@ -14,6 +14,13 @@
 #include <asm/arch/ps7_init_gpl.h>
 #include <asm/arch/sys_proto.h>
 
+#define ZYNQ_DEV_CFG_CTRL_DAP_EN		GENMASK(0, 2)
+#define ZYNQ_DEV_CFG_CTRL_DBGEN			BIT(3)
+#define ZYNQ_DEV_CFG_CTRL_NIDEN			BIT(4)
+#define ZYNQ_DEV_CFG_CTRL_SPIDEM		BIT(5)
+#define ZYNQ_DEV_CFG_CTRL_SPNIDEN		BIT(6)
+#define ZYNQ_DEV_CFG_CTRL_JTAG_CHAIN_DIS	BIT(23)
+
 #define ZYNQ_SILICON_VER_MASK		0xF0000000
 #define ZYNQ_SILICON_VER_SHIFT		28
 #define ZYNQ_MULTIBOOT_ADDR_MASK	0x00001FFF
@@ -74,6 +81,18 @@ int arch_cpu_init(void)
 	return 0;
 }
 
+void zynq_enable_jtag(void)
+{
+	unsigned int v;
+
+	v = readl(&devcfg_base->ctrl);
+	v &= ~ZYNQ_DEV_CFG_CTRL_JTAG_CHAIN_DIS;
+	v |= ZYNQ_DEV_CFG_CTRL_DAP_EN | ZYNQ_DEV_CFG_CTRL_DBGEN
+	   | ZYNQ_DEV_CFG_CTRL_NIDEN | ZYNQ_DEV_CFG_CTRL_NIDEN
+	   | ZYNQ_DEV_CFG_CTRL_SPIDEM | ZYNQ_DEV_CFG_CTRL_SPNIDEN;
+	writel(v, &devcfg_base->ctrl);
+}
+
 unsigned int zynq_get_silicon_version(void)
 {
 	return (readl(&devcfg_base->mctrl) & ZYNQ_SILICON_VER_MASK)
diff --git a/arch/arm/mach-zynq/include/mach/sys_proto.h b/arch/arm/mach-zynq/include/mach/sys_proto.h
index 6b85682808..f583ef090d 100644
--- a/arch/arm/mach-zynq/include/mach/sys_proto.h
+++ b/arch/arm/mach-zynq/include/mach/sys_proto.h
@@ -15,6 +15,7 @@ extern u32 zynq_slcr_get_boot_mode(void);
 extern u32 zynq_slcr_get_idcode(void);
 extern int zynq_slcr_get_mio_pin_status(const char *periph);
 extern void zynq_ddrc_init(void);
+extern void zynq_enable_jtag(void);
 extern unsigned int zynq_get_silicon_version(void);
 extern unsigned int zynq_get_mulitboot_addr(void);
 extern void zynq_set_mulitboot_addr(unsigned int);
-- 
2.30.2