From 1eca099af7accdfeb16d12287e6008a4445068bf Mon Sep 17 00:00:00 2001 From: Soren Brinkmann Date: Sun, 12 Apr 2015 20:24:18 -0700 Subject: [PATCH] xilpm: self-suspend: Set VINITH on R5 swbeta2 commit 8e5bf013a42c56c713efcfa1ab00c78e648b2333 To ensure we resume at the correct vector address, set the VINITH bit accordingly. Signed-off-by: Soren Brinkmann --- .../examples/xilpm_selfsuspend_example.c | 31 +++++++++++++++++-- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c b/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c index cd315ea5..503a957c 100644 --- a/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c +++ b/lib/sw_services/xilpm/examples/xilpm_selfsuspend_example.c @@ -118,11 +118,15 @@ static uint32_t GetCpuId(void) __asm__ volatile("mrs %0, MPIDR_EL1\n" : "=r"(id) ); +#else + u32 id; + + __asm__ volatile("mrc p15, 0, %0, c0, c0, 5\n" + : "=r"(id) + ); +#endif return id & 0xff; -#else - return XST_FAILURE; -#endif } /** @@ -158,6 +162,9 @@ static void PrepareSuspend(void) rvbar += 4; Xil_Out32(rvbar, vector_base >> 32); #else + u32 reg, rpuctrl; + u32 vector_base = (u32)&_vector_table; + /* RPU */ XPm_SelfSuspend(NODE_RPU_0, MAX_LATENCY, 0); usleep(100000); @@ -169,6 +176,24 @@ static void PrepareSuspend(void) usleep(100000); XPm_SetRequirement(NODE_TCM_1_B, PM_CAP_CONTEXT, 0, REQ_ACK_NO); usleep(100000); + + /* + * Set VINITH to ensure we resume at the expected address + * FIXME: This should be communicated to FW which has to set this. + */ + if (GetCpuId() == 0U) { + rpuctrl = RPU_RPU_0_CFG; + } else { + rpuctrl = RPU_RPU_1_CFG; + } + + reg = Xil_In32(rpuctrl); + if (vector_base == 0) { + reg &= ~RPU_RPU_0_CFG_VINITHI_MASK; + } else { + reg |= RPU_RPU_0_CFG_VINITHI_MASK; + } + Xil_Out32(rpuctrl, reg); #endif /* __aarch64__ */ }