From ae213cee8c38af64ba82c1c9e94598d7efe46327 Mon Sep 17 00:00:00 2001
From: Jens Wiklander <jens.wiklander@linaro.org>
Date: Thu, 4 Sep 2014 10:23:27 +0200
Subject: [PATCH] Initialize SCTLR_EL1 based on MODE_RW bit

Initializes SCTLR_EL1 based on MODE_RW bit in SPSR for the entry
point. The RES1 bits for SCTLR_EL1 differs for Aarch64 and Aarch32
mode.
---
 bl31/context_mgmt.c        | 5 ++++-
 include/lib/aarch64/arch.h | 4 ++++
 2 files changed, 8 insertions(+), 1 deletion(-)

diff --git a/bl31/context_mgmt.c b/bl31/context_mgmt.c
index 489d45444..4dd297eff 100644
--- a/bl31/context_mgmt.c
+++ b/bl31/context_mgmt.c
@@ -182,7 +182,10 @@ void cm_init_context(uint64_t mpidr, const entry_point_info_t *ep)
 	 * against the CPU support, security state, endianess and pc
 	 */
 	sctlr_elx = EP_GET_EE(ep->h.attr) ? SCTLR_EE_BIT : 0;
-	sctlr_elx |= SCTLR_EL1_RES1;
+	if (GET_RW(ep->spsr) == MODE_RW_64)
+		sctlr_elx |= SCTLR_EL1_RES1;
+	else
+		sctlr_elx |= SCTLR_AARCH32_EL1_RES1;
 	write_ctx_reg(get_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_elx);
 
 	if ((GET_RW(ep->spsr) == MODE_RW_64
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index 405626077..e5b2bf8ca 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -121,6 +121,10 @@
 
 #define SCTLR_EL1_RES1  ((1 << 29) | (1 << 28) | (1 << 23) | (1 << 22) | \
 			(1 << 11))
+#define SCTLR_AARCH32_EL1_RES1 \
+			((1 << 23) | (1 << 22) | (1 << 11) | (1 << 4) | \
+			(1 << 3))
+
 #define SCTLR_M_BIT		(1 << 0)
 #define SCTLR_A_BIT		(1 << 1)
 #define SCTLR_C_BIT		(1 << 2)
-- 
GitLab