Introduce macros to manipulate the SPSR
This patch introduces macros (SPSR_64 and SPSR_32) to
create a SPSR for both aarch32 and aarch64 execution
states. These macros allow the user to set fields
in the SPSR depending upon its format.
The make_spsr() function which did not allow
manipulation of all the fields in the aarch32 SPSR
has been replaced by these new macros.
Change-Id: I9425dda0923e8d5f03d03ddb8fa0e28392c4c61e
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
index de7bc31..ecf2550 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -95,7 +95,7 @@
if (bl2_base) {
bl1_arch_next_el_setup();
- spsr = make_spsr(MODE_EL1, MODE_SP_ELX, MODE_RW_64);
+ spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
printf("Booting trusted firmware boot loader stage 2\n\r");
#if DEBUG
printf("BL2 address = 0x%llx \n\r", (unsigned long long) bl2_base);
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index ac28559..4a54bf1 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -140,7 +140,7 @@
* well.
*/
bl2_to_bl31_args->bl33_image_info.spsr =
- make_spsr(mode, MODE_SP_ELX, MODE_RW_64);
+ SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
bl2_to_bl31_args->bl33_image_info.security_state = NON_SECURE;
if (bl32_base) {
@@ -165,7 +165,7 @@
* BL31 as an argument.
*/
run_image(bl31_base,
- make_spsr(MODE_EL3, MODE_SP_ELX, MODE_RW_64),
+ SPSR_64(MODE_EL3, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS),
SECURE,
(void *) bl2_to_bl31_args,
NULL);
diff --git a/common/bl_common.c b/common/bl_common.c
index 86b0cc5..037d0ff 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -122,20 +122,6 @@
raise_el(&info->args);
}
-/* TODO: add a parameter for DAIF. not needed right now */
-unsigned long make_spsr(unsigned long target_el,
- unsigned long target_sp,
- unsigned long target_rw)
-{
- unsigned long spsr;
-
- /* Disable all exceptions & setup the EL */
- spsr = (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT | DAIF_DBG_BIT)
- << PSR_DAIF_SHIFT;
- spsr |= PSR_MODE(target_rw, target_el, target_sp);
-
- return spsr;
-}
/*******************************************************************************
* The next two functions are the weak definitions. Platform specific
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 9fba9c0..1569962 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -118,7 +118,6 @@
extern void __dead2 drop_el(aapcs64_params_t *, unsigned long, unsigned long);
extern void __dead2 raise_el(aapcs64_params_t *);
extern void __dead2 change_el(el_change_info_t *);
-extern unsigned long make_spsr(unsigned long, unsigned long, unsigned long);
extern void init_bl2_mem_layout(meminfo_t *,
meminfo_t *,
unsigned int,
diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h
index d7e65b3..1c11af3 100644
--- a/include/lib/aarch64/arch.h
+++ b/include/lib/aarch64/arch.h
@@ -175,7 +175,25 @@
#define DAIF_IRQ_BIT (1 << 1)
#define DAIF_ABT_BIT (1 << 2)
#define DAIF_DBG_BIT (1 << 3)
-#define PSR_DAIF_SHIFT 0x6
+#define SPSR_DAIF_SHIFT 6
+#define SPSR_DAIF_MASK 0xf
+
+#define SPSR_AIF_SHIFT 6
+#define SPSR_AIF_MASK 0x7
+
+#define SPSR_E_SHIFT 9
+#define SPSR_E_MASK 0x1
+#define SPSR_E_LITTLE 0x0
+#define SPSR_E_BIG 0x1
+
+#define SPSR_T_SHIFT 5
+#define SPSR_T_MASK 0x1
+#define SPSR_T_ARM 0x0
+#define SPSR_T_THUMB 0x1
+
+#define DISABLE_ALL_EXCEPTIONS \
+ (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT | DAIF_DBG_BIT)
+
/*
* TCR defintions
@@ -198,29 +216,53 @@
#define TCR_SH_OUTER_SHAREABLE (0x2 << 12)
#define TCR_SH_INNER_SHAREABLE (0x3 << 12)
-#define MODE_RW_64 0x0
-#define MODE_RW_32 0x1
+#define MODE_SP_SHIFT 0x0
+#define MODE_SP_MASK 0x1
#define MODE_SP_EL0 0x0
#define MODE_SP_ELX 0x1
+
+#define MODE_RW_SHIFT 0x4
+#define MODE_RW_MASK 0x1
+#define MODE_RW_64 0x0
+#define MODE_RW_32 0x1
+
+#define MODE_EL_SHIFT 0x2
+#define MODE_EL_MASK 0x3
#define MODE_EL3 0x3
#define MODE_EL2 0x2
#define MODE_EL1 0x1
#define MODE_EL0 0x0
-#define MODE_RW_SHIFT 0x4
-#define MODE_EL_SHIFT 0x2
-#define MODE_SP_SHIFT 0x0
+#define MODE32_SHIFT 0
+#define MODE32_MASK 0xf
+#define MODE32_usr 0x0
+#define MODE32_fiq 0x1
+#define MODE32_irq 0x2
+#define MODE32_svc 0x3
+#define MODE32_mon 0x6
+#define MODE32_abt 0x7
+#define MODE32_hyp 0xa
+#define MODE32_und 0xb
+#define MODE32_sys 0xf
-#define GET_RW(mode) ((mode >> MODE_RW_SHIFT) & 0x1)
-#define GET_EL(mode) ((mode >> MODE_EL_SHIFT) & 0x3)
-#define PSR_MODE(rw, el, sp) (rw << MODE_RW_SHIFT | el << MODE_EL_SHIFT \
- | sp << MODE_SP_SHIFT)
+#define GET_RW(mode) (((mode) >> MODE_RW_SHIFT) & MODE_RW_MASK)
+#define GET_EL(mode) (((mode) >> MODE_EL_SHIFT) & MODE_EL_MASK)
+#define GET_SP(mode) (((mode) >> MODE_SP_SHIFT) & MODE_SP_MASK)
+#define GET_M32(mode) (((mode) >> MODE32_SHIFT) & MODE32_MASK)
-#define SPSR32_EE_BIT (1 << 9)
-#define SPSR32_T_BIT (1 << 5)
+#define SPSR_64(el, sp, daif) \
+ (MODE_RW_64 << MODE_RW_SHIFT | \
+ ((el) & MODE_EL_MASK) << MODE_EL_SHIFT | \
+ ((sp) & MODE_SP_MASK) << MODE_SP_SHIFT | \
+ ((daif) & SPSR_DAIF_MASK) << SPSR_DAIF_SHIFT)
+
+#define SPSR_MODE32(mode, isa, endian, aif) \
+ (MODE_RW_32 << MODE_RW_SHIFT | \
+ ((mode) & MODE32_MASK) << MODE32_SHIFT | \
+ ((isa) & SPSR_T_MASK) << SPSR_T_SHIFT | \
+ ((endian) & SPSR_E_MASK) << SPSR_E_SHIFT | \
+ ((aif) & SPSR_AIF_MASK) << SPSR_AIF_SHIFT)
-#define AARCH32_MODE_SVC 0x13
-#define AARCH32_MODE_HYP 0x1a
/* Miscellaneous MMU related constants */
#define NUM_2MB_IN_GB (1 << 9)
diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c
index a4c3936..d3fe5dd 100644
--- a/services/spd/tspd/tspd_common.c
+++ b/services/spd/tspd/tspd_common.c
@@ -91,7 +91,7 @@
tsp_ctx->mpidr = mpidr;
cm_set_context(mpidr, &tsp_ctx->cpu_ctx, SECURE);
- spsr = make_spsr(MODE_EL1, MODE_SP_ELX, rw);
+ spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
cm_set_el3_eret_context(SECURE, entrypoint, spsr, scr);
return 0;
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index f24a2f0..025d8b4 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -303,6 +303,7 @@
unsigned int rw, mode, ee, spsr = 0;
unsigned long id_aa64pfr0 = read_id_aa64pfr0_el1(), scr = read_scr();
unsigned long el_status;
+ unsigned long daif;
/* Figure out what mode do we enter the non-secure world in */
el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) &
@@ -330,24 +331,18 @@
ee = read_sctlr_el1() & SCTLR_EE_BIT;
}
- spsr = DAIF_DBG_BIT | DAIF_ABT_BIT;
- spsr |= DAIF_IRQ_BIT | DAIF_FIQ_BIT;
- spsr <<= PSR_DAIF_SHIFT;
- spsr |= make_spsr(mode, MODE_SP_ELX, !rw);
+ spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
psci_ns_entry_info[index].sctlr |= ee;
psci_ns_entry_info[index].scr |= SCR_RW_BIT;
} else {
- /* Check whether aarch32 has to be entered in Thumb mode */
- if (entrypoint & 0x1)
- spsr = SPSR32_T_BIT;
if (el_status && (scr & SCR_HCE_BIT)) {
- mode = AARCH32_MODE_HYP;
+ mode = MODE32_hyp;
ee = read_sctlr_el2() & SCTLR_EE_BIT;
} else {
- mode = AARCH32_MODE_SVC;
+ mode = MODE32_svc;
ee = read_sctlr_el1() & SCTLR_EE_BIT;
}
@@ -355,11 +350,9 @@
* TODO: Choose async. exception bits if HYP mode is not
* implemented according to the values of SCR.{AW, FW} bits
*/
- spsr |= DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
- spsr <<= PSR_DAIF_SHIFT;
- if (ee)
- spsr |= SPSR32_EE_BIT;
- spsr |= mode;
+ daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+
+ spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif);
/* Ensure that the CSPR.E and SCTLR.EE bits match */
psci_ns_entry_info[index].sctlr |= ee;