diff --git a/include/lib/aarch64/arch.h b/include/lib/aarch64/arch.h index 042720823a10a5e3b9031a8b3a7de163098a00c1..1e0f2d5c1f55b8c679a213531cd117d880458d3f 100644 --- a/include/lib/aarch64/arch.h +++ b/include/lib/aarch64/arch.h @@ -35,8 +35,12 @@ /******************************************************************************* * MIDR bit definitions ******************************************************************************/ +#define MIDR_VAR_MASK 0xf +#define MIDR_REV_MASK 0xf #define MIDR_PN_MASK 0xfff -#define MIDR_PN_SHIFT 0x4 +#define MIDR_VAR_SHIFT 20 +#define MIDR_REV_SHIFT 0 +#define MIDR_PN_SHIFT 4 #define MIDR_PN_AEM 0xd0f #define MIDR_PN_A57 0xd07 #define MIDR_PN_A53 0xd03 @@ -78,6 +82,7 @@ * Implementation defined sysreg encodings ******************************************************************************/ #define CPUECTLR_EL1 S3_1_C15_C2_1 +#define CPUACTLR_EL1 S3_1_C15_C2_0 /******************************************************************************* * Generic timer memory mapped registers & offsets @@ -133,9 +138,6 @@ #define SCTLR_WXN_BIT (1 << 19) #define SCTLR_EE_BIT (1 << 25) -/* CPUECTLR definitions */ -#define CPUECTLR_SMP_BIT (1 << 6) - /* CPACR_El1 definitions */ #define CPACR_EL1_FPEN(x) (x << 20) #define CPACR_EL1_FP_TRAP_EL0 0x1 @@ -173,6 +175,10 @@ #define EL0VTEN_BIT (1 << 8) #define EL0PCTEN_BIT (1 << 0) #define EL0VCTEN_BIT (1 << 1) +#define EVNTEN_BIT (1 << 2) +#define EVNTDIR_BIT (1 << 3) +#define EVNTI_SHIFT 4 +#define EVNTI_MASK 0xf /* CPTR_EL3 definitions */ #define TCPAC_BIT (1 << 31) @@ -417,6 +423,16 @@ #define EC_BITS(x) (x >> ESR_EC_SHIFT) & ESR_EC_MASK +/******************************************************************************* + * Imp. Def. register defines. + ******************************************************************************/ +/* CPUECTLR definitions */ +#define CPUECTLR_SMP_BIT (1 << 6) + +/* A57 CPUACTLR definitions */ +#define CPUACTLR_NO_ALLOC_WBWA (1 << 49) +#define CPUACTLR_DCC_AS_DCCI (1 << 44) + /******************************************************************************* * Definitions of register offsets and fields in the CNTCTLBase Frame of the * system level implementation of the Generic Timer. diff --git a/lib/aarch64/cpu_helpers.S b/lib/aarch64/cpu_helpers.S index b8be34027cfde0d9fc4f53b807bf1159aabb4ab0..b85e74301d4c07a6da288b627adfc62ee8f5e135 100644 --- a/lib/aarch64/cpu_helpers.S +++ b/lib/aarch64/cpu_helpers.S @@ -40,19 +40,38 @@ func cpu_reset_handler * --------------------------------------------- */ mrs x0, midr_el1 - lsr x0, x0, #MIDR_PN_SHIFT - and x0, x0, #MIDR_PN_MASK - cmp x0, #MIDR_PN_A57 + lsr x1, x0, #MIDR_PN_SHIFT + and x1, x1, #MIDR_PN_MASK + cmp x1, #MIDR_PN_A57 b.eq a57_setup_begin - cmp x0, #MIDR_PN_A53 + cmp x1, #MIDR_PN_A53 b.eq smp_setup_begin b smp_setup_end a57_setup_begin: + ubfx x1, x0, #MIDR_VAR_SHIFT, #4 + cmp x1, #0 // Major Revision 0 + b.ne smp_setup_begin + ubfx x1, x0, #MIDR_REV_SHIFT, #4 + cmp x1, #0 // Minor Revision 0 + b.ne smp_setup_begin + mov x1, #CPUACTLR_NO_ALLOC_WBWA + orr x1, x1, #CPUACTLR_DCC_AS_DCCI + mrs x0, CPUACTLR_EL1 + orr x0, x0, x1 + msr CPUACTLR_EL1, x0 mov x0, #0x082 msr s3_1_c11_c0_2, x0 smp_setup_begin: + /* --------------------------------------------- + * Enable the event stream every 65536 cycles + * --------------------------------------------- + */ + mov x0, #(0xf << EVNTI_SHIFT) + orr x0, x0, #EVNTEN_BIT + msr CNTKCTL_EL1, x0 + mrs x0, CPUECTLR_EL1 orr x0, x0, #CPUECTLR_SMP_BIT msr CPUECTLR_EL1, x0