From 879cc61c83c3fe3685c6ab3625a8ebc6df6323d5 Mon Sep 17 00:00:00 2001
From: Soren Brinkmann <soren.brinkmann@xilinx.com>
Date: Sun, 12 Apr 2015 20:24:17 -0700
Subject: [PATCH] xilpm: self-suspend: Set RVBAR on A53

swbeta2 commit 7ebba935262ebff0df604fd560f4a023d32a7c72

To ensure the application is resumed at the vector table, set the RVBAR
accordingly.

Signed-off-by: Soren Brinkmann <soren.brinkmann@xilinx.com>
---
 .../examples/xilpm_selfsuspend_example.c      | 30 +++++++++++++++++++
 1 file changed, 30 insertions(+)

diff --git a/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c b/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c
index 5b71c496..cd315ea5 100644
--- a/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c
+++ b/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c
@@ -62,6 +62,8 @@
 #include "timer.h"
 #include "pm_client.h"
 
+extern void *_vector_table;
+
 #ifdef __aarch64__
 	/* Use OCM for saving context */
 	#define CONTEXT_MEM_BASE	0xFFFC0000U
@@ -108,6 +110,21 @@ static void RestoreContext(void)
 	pm_dbg("Restored context (tick_count = %d)\n", TickCount);
 }
 
+static uint32_t GetCpuId(void)
+{
+#ifdef __aarch64__
+	u64 id;
+
+	__asm__ volatile("mrs	%0, MPIDR_EL1\n"
+			: "=r"(id)
+	);
+
+	return id & 0xff;
+#else
+	return XST_FAILURE;
+#endif
+}
+
 /**
  * PrepareSuspend() - save context and request suspend
  */
@@ -116,6 +133,9 @@ static void PrepareSuspend(void)
 	SaveContext();
 /* usleep is used to prevents UART prints from overlapping */
 #ifdef __aarch64__
+	u64 rvbar;
+	u64 vector_base = (u64)&_vector_table;
+
 	/* APU */
 	XPm_SelfSuspend(NODE_APU_0, MAX_LATENCY, 0);
 	usleep(100000);
@@ -127,6 +147,16 @@ static void PrepareSuspend(void)
 	usleep(100000);
 	XPm_SetRequirement(NODE_OCM_BANK_3, PM_CAP_CONTEXT, 0, REQ_ACK_NO);
 	usleep(100000);
+
+	/*
+	 * Set RVBAR to ensure we resume at the expected address
+	 * FIXME: This should be communicated to FW which has to set this.
+	 */
+	rvbar = APU_RVBARADDR0L;
+	rvbar += 8 * GetCpuId();
+	Xil_Out32(rvbar, vector_base & 0xffffffff);
+	rvbar += 4;
+	Xil_Out32(rvbar, vector_base >> 32);
 #else
 	/* RPU */
 	XPm_SelfSuspend(NODE_RPU_0, MAX_LATENCY, 0);