From 29657d0d5f5f604f7adf9b407a8dfa6e8147248a Mon Sep 17 00:00:00 2001
From: Siva Durga Prasad Paladugu <siva.durga.paladugu@xilinx.com>
Date: Mon, 30 Apr 2018 20:12:12 +0530
Subject: [PATCH] zynqmp: Add wdt timeout restart functionality

This patch adds support to restart system incase of wdt
timeout.

Signed-off-by: Siva Durga Prasad Paladugu <siva.durga.paladugu@xilinx.com>
---
 plat/xilinx/zynqmp/bl31_zynqmp_setup.c      |  43 ++++++
 plat/xilinx/zynqmp/include/platform_def.h   |  24 +++
 plat/xilinx/zynqmp/platform.mk              |   5 +
 plat/xilinx/zynqmp/pm_service/pm_svc_main.c | 155 ++++++++++++++++++++
 plat/xilinx/zynqmp/zynqmp_def.h             |   8 +
 plat/xilinx/zynqmp/zynqmp_private.h         |   8 +
 6 files changed, 243 insertions(+)

diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 471b1cc60..0b3106f7a 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -116,6 +116,39 @@ static void zynqmp_testing_setup(void)
 }
 #endif
 
+#if ZYNQMP_WDT_RESTART
+static interrupt_type_handler_t type_el3_interrupt_table[MAX_INTR_EL3];
+
+int request_intr_type_el3(uint32_t id, interrupt_type_handler_t handler)
+{
+	/* Validate 'handler' and 'id' parameters */
+	if (!handler || id >= MAX_INTR_EL3)
+		return -EINVAL;
+
+	/* Check if a handler has already been registered */
+	if (type_el3_interrupt_table[id])
+		return -EALREADY;
+
+	type_el3_interrupt_table[id] = handler;
+
+	return 0;
+}
+
+static uint64_t rdo_el3_interrupt_handler(uint32_t id, uint32_t flags,
+					  void *handle, void *cookie)
+{
+	uint32_t intr_id;
+	interrupt_type_handler_t handler;
+
+	intr_id = plat_ic_get_pending_interrupt_id();
+	handler = type_el3_interrupt_table[intr_id];
+	if (handler != NULL)
+		handler(intr_id, flags, handle, cookie);
+
+	return 0;
+}
+#endif
+
 void bl31_platform_setup(void)
 {
 	/* Initialize the gic cpu and distributor interfaces */
@@ -126,6 +159,16 @@ void bl31_platform_setup(void)
 
 void bl31_plat_runtime_setup(void)
 {
+#if ZYNQMP_WDT_RESTART
+	uint64_t flags = 0;
+	uint64_t rc;
+
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	rc = register_interrupt_type_handler(INTR_TYPE_EL3,
+					     rdo_el3_interrupt_handler, flags);
+	if (rc)
+		panic();
+#endif
 }
 
 /*
diff --git a/plat/xilinx/zynqmp/include/platform_def.h b/plat/xilinx/zynqmp/include/platform_def.h
index e74b9bbfd..ebbc8c2cd 100644
--- a/plat/xilinx/zynqmp/include/platform_def.h
+++ b/plat/xilinx/zynqmp/include/platform_def.h
@@ -96,6 +96,7 @@
  * terminology. On a GICv2 system or mode, the lists will be merged and treated
  * as Group 0 interrupts.
  */
+#if !ZYNQMP_WDT_RESTART
 #define PLAT_ARM_G1S_IRQ_PROPS(grp) \
 	INTR_PROP_DESC(ARM_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, grp, \
 			GIC_INTR_CFG_LEVEL), \
@@ -115,6 +116,29 @@
 			GIC_INTR_CFG_EDGE), \
 	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY, grp, \
 			GIC_INTR_CFG_EDGE)
+#else
+#define PLAT_ARM_G1S_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(ARM_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(IRQ_TTC3_1, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_0, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_1, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_2, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_3, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_4, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_5, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_6, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY, grp, \
+			GIC_INTR_CFG_EDGE)
+#endif
 
 #define PLAT_ARM_G0_IRQ_PROPS(grp)
 
diff --git a/plat/xilinx/zynqmp/platform.mk b/plat/xilinx/zynqmp/platform.mk
index e49a9cdf7..3ac9db959 100644
--- a/plat/xilinx/zynqmp/platform.mk
+++ b/plat/xilinx/zynqmp/platform.mk
@@ -9,6 +9,7 @@ override PROGRAMMABLE_RESET_ADDRESS := 1
 PSCI_EXTENDED_STATE_ID := 1
 A53_DISABLE_NON_TEMPORAL_HINT := 0
 SEPARATE_CODE_AND_RODATA := 1
+ZYNQMP_WDT_RESTART := 0
 override RESET_TO_BL31 := 1
 
 # Do not enable SVE
@@ -41,6 +42,10 @@ endif
 ZYNQMP_CONSOLE	?=	cadence
 $(eval $(call add_define_val,ZYNQMP_CONSOLE,ZYNQMP_CONSOLE_ID_${ZYNQMP_CONSOLE}))
 
+ifdef ZYNQMP_WDT_RESTART
+$(eval $(call add_define,ZYNQMP_WDT_RESTART))
+endif
+
 PLAT_INCLUDES		:=	-Iinclude/plat/arm/common/			\
 				-Iinclude/plat/arm/common/aarch64/		\
 				-Iplat/xilinx/zynqmp/include/			\
diff --git a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
index 9b356a76f..dd9bbc8c7 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
+++ b/plat/xilinx/zynqmp/pm_service/pm_svc_main.c
@@ -15,6 +15,13 @@
 #include "pm_api_sys.h"
 #include "pm_client.h"
 #include "pm_ipi.h"
+#if ZYNQMP_WDT_RESTART
+#include <arch_helpers.h>
+#include <gicv2.h>
+#include <mmio.h>
+#include <platform.h>
+#include <spinlock.h>
+#endif
 
 #define PM_SET_SUSPEND_MODE	0xa02
 #define PM_GET_TRUSTZONE_VERSION	0xa03
@@ -22,6 +29,12 @@
 /* !0 - UP, 0 - DOWN */
 static int32_t pm_up = 0;
 
+#if ZYNQMP_WDT_RESTART
+static spinlock_t inc_lock;
+static int active_cores = 0;
+#endif
+
+
 /**
  * pm_context - Structure which contains data for power management
  * @api_version		version of PM API, must match with one on PMU side
@@ -33,6 +46,142 @@ static struct {
 	uint32_t payload[PAYLOAD_ARG_CNT];
 } pm_ctx;
 
+#if ZYNQMP_WDT_RESTART
+/**
+ * trigger_wdt_restart() - Trigger warm restart event to APU cores
+ *
+ * This function triggers SGI for all active APU CPUs. SGI handler then
+ * power down CPU and call system reset.
+ */
+static void trigger_wdt_restart(void)
+{
+	uint32_t core_count = 0;
+	uint32_t core_status[3];
+	uint32_t target_cpu_list = 0;
+	int i;
+
+	for (i = 0; i < 4; i++) {
+		pm_get_node_status(NODE_APU_0 + i, core_status);
+		if (core_status[0] == 1) {
+			core_count++;
+			target_cpu_list |= (1 << i);
+		}
+	}
+
+	spin_lock(&inc_lock);
+	active_cores = core_count;
+	spin_unlock(&inc_lock);
+
+	INFO("Active Cores: %d\n", active_cores);
+
+	/* trigger SGI to active cores */
+	gicv2_raise_sgi(ARM_IRQ_SEC_SGI_7, target_cpu_list);
+}
+
+/**
+ * ttc_fiq_handler() - TTC Handler for timer event
+ * @id         number of the highest priority pending interrupt of the type
+ *             that this handler was registered for
+ * @flags      security state, bit[0]
+ * @handler    pointer to 'cpu_context' structure of the current CPU for the
+ *             security state specified in the 'flags' parameter
+ * @cookie     unused
+ *
+ * Function registered as INTR_TYPE_EL3 interrupt handler
+ *
+ * When WDT event is received in PMU, PMU needs to notify master to do cleanup
+ * if required. PMU sets up timer and starts timer to overflow in zero time upon
+ * WDT event. ATF handles this timer event and takes necessary action required
+ * for warm restart.
+ *
+ * In presence of non-secure software layers (EL1/2) sets the interrupt
+ * at registered entrance in GIC and informs that PMU responsed or demands
+ * action.
+ */
+static uint64_t ttc_fiq_handler(uint32_t id, uint32_t flags, void *handle,
+                               void *cookie)
+{
+	INFO("BL31: Got TTC FIQ\n");
+
+	/* Clear TTC interrupt by reading interrupt register */
+	mmio_read_32(TTC3_INTR_REGISTER_1);
+
+	/* Disable the timer interrupts */
+	mmio_write_32(TTC3_INTR_ENABLE_1, 0);
+
+	trigger_wdt_restart();
+
+	return 0;
+}
+
+/**
+ * zynqmp_sgi7_irq() - Handler for SGI7 IRQ
+ * @id         number of the highest priority pending interrupt of the type
+ *             that this handler was registered for
+ * @flags      security state, bit[0]
+ * @handler    pointer to 'cpu_context' structure of the current CPU for the
+ *             security state specified in the 'flags' parameter
+ * @cookie     unused
+ *
+ * Function registered as INTR_TYPE_EL3 interrupt handler
+ *
+ * On receiving WDT event from PMU, ATF generates SGI7 to all running CPUs.
+ * In response to SGI7 interrupt, each CPUs do clean up if required and last
+ * running CPU calls system restart.
+ */
+static uint64_t __unused __dead2 zynqmp_sgi7_irq(uint32_t id, uint32_t flags,
+                                                void *handle, void *cookie)
+{
+	int i;
+	/* enter wfi and stay there */
+	INFO("Entering wfi\n");
+
+	spin_lock(&inc_lock);
+	active_cores--;
+
+	for (i = 0; i < 4; i++) {
+		mmio_write_32(BASE_GICD_BASE + GICD_CPENDSGIR + 4 * i,
+				0xffffffff);
+	}
+
+	spin_unlock(&inc_lock);
+
+	if (active_cores == 0) {
+		pm_system_shutdown(PMF_SHUTDOWN_TYPE_RESET,
+				PMF_SHUTDOWN_SUBTYPE_SUBSYSTEM);
+	}
+
+	/* enter wfi and stay there */
+	while (1)
+		wfi();
+}
+
+/**
+ * pm_wdt_restart_setup() - Setup warm restart interrupts
+ *
+ * This function sets up handler for SGI7 and TTC interrupts
+ * used for warm restart.
+ */
+static int pm_wdt_restart_setup(void)
+{
+	int ret;
+
+	/* register IRQ handler for SGI7 */
+	ret = request_intr_type_el3(ARM_IRQ_SEC_SGI_7, zynqmp_sgi7_irq);
+	if (ret) {
+		WARN("BL31: registering SGI7 interrupt failed\n");
+		goto err;
+	}
+
+	ret = request_intr_type_el3(IRQ_TTC3_1, ttc_fiq_handler);
+	if (ret)
+		WARN("BL31: registering TTC3 interrupt failed\n");
+
+err:
+	return ret;
+}
+#endif
+
 /**
  * pm_setup() - PM service setup
  *
@@ -52,6 +201,12 @@ int pm_setup(void)
 
 	status = pm_ipi_init(primary_proc);
 
+#if ZYNQMP_WDT_RESTART
+	status = pm_wdt_restart_setup();
+	if (status)
+		WARN("BL31: warm-restart setup failed\n");
+#endif
+
 	if (status >= 0) {
 		INFO("BL31: PM Service Init Complete: API v%d.%d\n",
 		     PM_VERSION_MAJOR, PM_VERSION_MINOR);
diff --git a/plat/xilinx/zynqmp/zynqmp_def.h b/plat/xilinx/zynqmp/zynqmp_def.h
index cf90bcd07..22256ebfa 100644
--- a/plat/xilinx/zynqmp/zynqmp_def.h
+++ b/plat/xilinx/zynqmp/zynqmp_def.h
@@ -101,6 +101,14 @@
 #define BASE_GICH_BASE		0xF9040000
 #define BASE_GICV_BASE		0xF9060000
 
+#if ZYNQMP_WDT_RESTART
+#define IRQ_SEC_IPI_APU		67
+#define IRQ_TTC3_1		77
+#define TTC3_BASE_ADDR		0xFF140000
+#define TTC3_INTR_REGISTER_1	(TTC3_BASE_ADDR + 0x54)
+#define TTC3_INTR_ENABLE_1	(TTC3_BASE_ADDR + 0x60)
+#endif
+
 #define ARM_IRQ_SEC_PHY_TIMER		29
 
 #define ARM_IRQ_SEC_SGI_0		8
diff --git a/plat/xilinx/zynqmp/zynqmp_private.h b/plat/xilinx/zynqmp/zynqmp_private.h
index a66f31fd2..08a54107a 100644
--- a/plat/xilinx/zynqmp/zynqmp_private.h
+++ b/plat/xilinx/zynqmp/zynqmp_private.h
@@ -24,6 +24,14 @@ enum fsbl_handoff {
 	FSBL_HANDOFF_TOO_MANY_PARTS,
 };
 
+#if ZYNQMP_WDT_RESTART
+/*
+ * Register handler to specific GIC entrance
+ * for INTR_TYPE_EL3 type of interrupt
+ */
+int request_intr_type_el3(uint32_t, interrupt_type_handler_t);
+#endif
+
 enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32_image_ep_info,
 		       entry_point_info_t *bl33_image_ep_info);
 
-- 
GitLab