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 de7bc3180c8da6b7313b35c2a4482367a7de8796..ecf255063887de135ccfe57298691521a09c8497 100644 (file)
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -95,7 +95,7 @@ void bl1_main(void)
 
        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 ac28559118bace5688f0182167b3bad4f5869add..4a54bf1d2d0dd63e9f8eb07eb19574df0f32cc26 100644 (file)
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -140,7 +140,7 @@ void bl2_main(void)
         * 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 @@ void bl2_main(void)
         * 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 86b0cc5cba23c72fd5cf51a42f55d6ac7d595195..037d0ff27ea02533d593b9c25be08eff37ad748d 100644 (file)
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -122,20 +122,6 @@ void __dead2 change_el(el_change_info_t *info)
                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 9fba9c067d3ea71966e6a17c3fb9004a9e2b6cd1..1569962584262f039babb70293482fda668e0938 100644 (file)
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -118,7 +118,6 @@ extern void change_security_state(unsigned int);
 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 d7e65b38ebea584f019f18cead0817cf726cd003..1c11af3f5879fb9603ba9c8023339616092d1054 100644 (file)
--- 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 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 SPSR32_EE_BIT          (1 << 9)
-#define SPSR32_T_BIT           (1 << 5)
+#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) & 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 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 a4c393630c80400b88c1547465086f9483eae1e1..d3fe5ddf5880c313682bc69e0623cb5e3bfd8a40 100644 (file)
--- a/services/spd/tspd/tspd_common.c
+++ b/services/spd/tspd/tspd_common.c
@@ -91,7 +91,7 @@ int32_t tspd_init_secure_context(uint64_t entrypoint,
        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 f24a2f02ae3598ae04b3545318b5eff985dde759..025d8b434a8d95e3bf3edf53a26b655aa318eef8 100644 (file)
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -303,6 +303,7 @@ int psci_set_ns_entry_info(unsigned int index,
        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 @@ int psci_set_ns_entry_info(unsigned int index,
                        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 @@ int psci_set_ns_entry_info(unsigned int index,
                 * 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;