diff --git a/src/arch/aarch32/boot.S b/src/arch/aarch32/boot.S index 56e000f6315a572fa0b11ceaaf57bc2de2aae02c..d154e6afa071e5e259c5f4b0a6625895b3b75e0f 100644 --- a/src/arch/aarch32/boot.S +++ b/src/arch/aarch32/boot.S @@ -107,6 +107,13 @@ func entrypoint mov r13, #0 /* r13_sys */ mov r14, #0 /* r14_sys */ + /* + * Initialize vector base address + */ + ldr r0, =vector_table + stcopr r0, VBAR + isb + ldcopr r0, MPIDR bl plat_get_core_pos /* r0 holds CPU number */ diff --git a/src/arch/aarch32/exceptions.S b/src/arch/aarch32/exceptions.S new file mode 100644 index 0000000000000000000000000000000000000000..60035bc2afc654dc34f8946fb4acae547bfef87a --- /dev/null +++ b/src/arch/aarch32/exceptions.S @@ -0,0 +1,118 @@ +#include +#include + + + +/** + * @brief Undefined instruction exception handler + * + * An undefined instruction (UNDEF) exception is generated when an undefined + * instruction, or a VFP instruction when the VFP is not enabled, is + * encountered. + */ +func arm_undef_instruction + /* + * The undefined instruction address is offset by 2 if the previous + * mode is Thumb; otherwise, it is offset by 4. + */ + push {r0} + mrs r0, spsr + tst r0, #T_BIT + subeq lr, #4 /* ARM (!T_BIT) */ + subne lr, #2 /* Thumb (T_BIT) */ + pop {r0} + + exception_entry MODE32_und + bl plat_handle_exception + exception_exit + +endfunc arm_undef_instruction + + +/** + * @brief Prefetch abort exception handler + * + * A prefetch abort (PABT) exception is generated when the processor marks the + * prefetched instruction as invalid and the instruction is executed. + */ +func arm_prefetch_abort + /* + * The faulting instruction address is always offset by 4 for the + * prefetch abort exceptions. + */ + sub lr, #4 + + exception_entry MODE32_abt + bl plat_handle_exception + exception_exit + +endfunc arm_prefetch_abort + +/** + * @brief Data abort exception handler + * + * A data abort (DABT) exception is generated when an error occurs on a data + * memory access. This exception can be either synchronous or asynchronous, + * depending on the type of fault that caused it. + */ +func arm_data_abort + /* + * The faulting instruction address is always offset by 8 for the data + * abort exceptions. + */ + sub lr, #8 + + exception_entry MODE32_abt + bl plat_handle_exception + exception_exit + +endfunc arm_data_abort + +/** + * @brief irq handle + */ +func arm_handle_irq + /* + * The faulting instruction address is always offset by 4 for the + * prefetch abort exceptions. + */ + sub lr, #4 + + exception_entry MODE32_irq + bl plat_handle_interrupt + exception_exit + +endfunc arm_handle_irq + +/** + * @brief fiq handle + */ +func arm_handle_fiq + /* + * The faulting instruction address is always offset by 4 for the + * prefetch abort exceptions. + */ + sub lr, #4 + + exception_entry MODE32_fiq + bl plat_handle_exception + exception_exit + +endfunc arm_handle_fiq + + .global vector_table + +/* ----------------------------------------------------- + * Setup the vector table to support SVC & MON mode. + * ----------------------------------------------------- + */ +vector_base vector_table + ldr pc, =entrypoint /* offset 0 */ + ldr pc, =arm_undef_instruction /* undef instruction offset 4 */ + nop /* svc offset 8 */ + ldr pc, =arm_prefetch_abort /* prefetch abort offset 0xc */ + ldr pc, =arm_data_abort /* data abort offset 0x10 */ + nop /* offset 0x14 */ + ldr pc, =arm_handle_irq /* IRQ offset 0x18 */ + ldr pc, =arm_handle_fiq /* FIQ offset 0x1c */ + diff --git a/src/arch/aarch32/gicv3.c b/src/arch/aarch32/gicv3.c new file mode 100644 index 0000000000000000000000000000000000000000..e9479561c932078018d29fb79fda89d4534984bb --- /dev/null +++ b/src/arch/aarch32/gicv3.c @@ -0,0 +1,610 @@ +#include +#include +#include +#include +#include +#include + +volatile gicd_t* gicd = (void*)GICD_BASE; +volatile gicr_t* gicr = (void*)GICR_BASE; + +static spinlock_t gicd_lock; +static spinlock_t gicr_lock; + +/* GIC-600 specific register offsets */ +#define IIDR_MODEL_ARM_GIC_600 0x0200043bU +#define IIDR_MODEL_ARM_GIC_600AE 0x0300043bU + +#define GICR_PWRR 0x24U +/* GICR_PWRR fields */ +#define PWRR_RDPD_SHIFT 0 +#define PWRR_RDAG_SHIFT 1 +#define PWRR_RDGPD_SHIFT 2 +#define PWRR_RDGPO_SHIFT 3 +#define PWRR_RDPD (1U << PWRR_RDPD_SHIFT) +#define PWRR_RDAG (1U << PWRR_RDAG_SHIFT) +#define PWRR_RDGPD (1U << PWRR_RDGPD_SHIFT) +#define PWRR_RDGPO (1U << PWRR_RDGPO_SHIFT) + +/* + * Values to write to GICR_PWRR register to power redistributor + * for operating through the core (PWRR.RDAG = 0) + */ +#define PWRR_ON (0U << PWRR_RDPD_SHIFT) +#define PWRR_OFF (1U << PWRR_RDPD_SHIFT) + +static void gic600_wait_group_not_in_transit(void) +{ + uint32_t pwrr; + + do { + pwrr = gicr[get_cpuid()].PWRR; + + /* Check group not transitioning: RDGPD == RDGPO */ + } while (((pwrr & PWRR_RDGPD) >> PWRR_RDGPD_SHIFT) != + ((pwrr & PWRR_RDGPO) >> PWRR_RDGPO_SHIFT)); +} + +static void gic600_pwr_on(void) +{ + do { /* Wait until group not transitioning */ + gic600_wait_group_not_in_transit(); + + /* Power on redistributor */ + gicr[get_cpuid()].PWRR = PWRR_ON; + + /* + * Wait until the power on state is reflected. + * If RDPD == 0 then powered on. + */ + } while ((gicr[get_cpuid()].PWRR & PWRR_RDPD) != PWRR_ON); +} + +static bool gicr_need_power_mgmt() +{ + uint32_t reg = gicr[get_cpuid()].IIDR; + + return (((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600) || + ((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600AE)); +} + +static inline uint64_t gic_num_irqs() +{ + uint32_t itlinenumber = + bit_extract(gicd->TYPER, GICD_TYPER_ITLN_OFF, GICD_TYPER_ITLN_LEN); + return 32 * itlinenumber + 1; +} + +/* + * Wait for register write pending + * TODO: add timed wait + */ +static inline int gic_wait_rwp(uint32_t intid) +{ + uint32_t rwp_mask; + + if (intid < 32) { + while(gicr[get_cpuid()].CTLR & GICR_CTLR_UWP); + } else { + while(gicd->CTLR & GICR_CTLR_UWP); + } + + return 0; +} + +/* + * Wake up GIC redistributor. + * clear ProcessorSleep and wait till ChildAsleep is cleared. + * ProcessSleep to be cleared only when ChildAsleep is set + * Check if redistributor is not powered already. + */ +static void gicr_enable(void) +{ + /* + * The WAKER_PS_BIT should be changed to 0 + * only when WAKER_CA_BIT is 1. + */ + if (!gicr[get_cpuid()].WAKER & GICR_ChildrenASleep_BIT) + return; + + gicr[get_cpuid()].WAKER &= ~ GICR_ProcessorSleep_BIT; + while(gicr[get_cpuid()].WAKER & GICR_ChildrenASleep_BIT); +} + +static inline void gicc_init() +{ + /* Switch to SRE=1 */ + MSR(ICC_SRE_EL1, 0x1); + + /* Set priority mask to 0xff, binary point=0x7 */ + MSR(ICC_PMR_EL1, -1); + MSR(ICC_BPR0_EL1, 0x7); + MSR(ICC_BPR1_EL1, 0x7); + + /* Enable Group1 Secure interrupts */ + MSR(ICC_IGRPEN1_EL1, ICC_IGRPEN_EL1_ENB_BIT); + ISB(); +} + +static inline void gicr_init() +{ + gicr[get_cpuid()].ICENABLER0 = -1; + gicr[get_cpuid()].ICPENDR0 = -1; + gicr[get_cpuid()].ICACTIVER0 = -1; + gic_wait_rwp(0); + + /* + * Configure all SGIs/PPIs as G1S or G1NS + * All interrupts should be delivered as irq + */ + gicr[get_cpuid()].IGROUPR0 = -1; + /*gicr[get_cpuid()].IGRPMODR0 = -1;*/ + + for (int i = 0; i < GIC_NUM_PRIO_REGS(GIC_CPU_PRIV); i++) { + gicr[get_cpuid()].IPRIORITYR[i] = -1; + } +} + +void gic_cpu_init() +{ + if (gicr_need_power_mgmt()) { + gic600_pwr_on(); + } + + gicr_init(); + gicr_enable(); + gicc_init(); +} + +void gicd_init() +{ + size_t int_num = gic_num_irqs(); + +#if defined(GIC_SINGLE_SECURITY_STATE) + /* + * Before configuration, we need to check whether + * the GIC single security state mode is supported. + * Make sure GICD_CTRL_NS is 1. + */ +#endif + + /* Bring distributor to known state */ + for (int i = GIC_NUM_PRIVINT_REGS; i < GIC_NUM_INT_REGS(int_num); i++) { + /** + * Make sure all interrupts are not enabled, non pending, + * non active. + */ + gicd->ICENABLER[i] = -1; + gicd->ICPENDR[i] = -1; + gicd->ICACTIVER[i] = -1; + + /* G1NS */ + gicd->IGROUPR[i] = -1; + /* gicd->IGPRMODR[i] = -1; */ + } + + /* wait for rwp on GICD */ + gic_wait_rwp(32); + + /* All interrupts have lowest priority possible by default */ + for (int i = GIC_CPU_PRIV; i < GIC_NUM_PRIO_REGS(int_num); i++) + gicd->IPRIORITYR[i] = -1; + + /* No CPU targets for any interrupt by default */ + for (int i = GIC_CPU_PRIV; i < GIC_NUM_TARGET_REGS(int_num); i++) + gicd->ITARGETSR[i] = 0; + + /* No need to setup gicd->NSACR as all interrupts are setup to group 1 */ + + +#if defined(GIC_SINGLE_SECURITY_STATE) + /* + * For GIC single security state, the config GIC_SINGLE_SECURITY_STATE + * means the GIC is under single security state which has only two + * groups: group 0 and group 1. + * Then set GICD_CTLR_ARE and GICD_CTLR_ENABLE_G1 to enable Group 1 + * interrupt. + * Since the GICD_CTLR_ARE and GICD_CTRL_ARE_S share BIT(4), and + * similarly the GICD_CTLR_ENABLE_G1 and GICD_CTLR_ENABLE_G1NS share + * BIT(1), we can reuse them. + */ + gicd->CTLR |= GICD_CTLR_ARE_S_BIT | GICD_CTLR_ENA_BIT; +#else + /* Enable distributor and affinity routing */ + gicd->CTLR |= GICD_CTLR_ARE_NS_BIT | GICD_CTLR_EN_BIT; + //gicd->CTLR |= 0x37; +#endif + + /* wait for pending write GICD_CTRL.RWP */ + while((gicd->CTLR >> 31) & 0x1); +} + +void gic_init() +{ + if (get_cpuid() == 0) { + gicd_init(); + } + + gic_cpu_init(); +} + +void gic_handle(void) +{ + unsigned long ack; + uint64_t id; + + //TODO:only support G1S now + /* Service all pending interrupts */ + while(1) { + ack = MRS(ICC_IAR1_EL1); + id = ack & ((1UL << 24) -1); + + if (id >= 1022) { + /* no pending interrupt to this PE */ + return; + } + + irq_handle(id); + + fence_sync(); + + /* Clear the interrupt */ + MSR(ICC_EOIR1_EL1, ack); + } +} + +uint64_t gicd_get_prio(uint64_t int_id) +{ + uint64_t reg_ind = GIC_PRIO_REG(int_id); + uint64_t off = GIC_PRIO_OFF(int_id); + + spin_lock(&gicd_lock); + + uint64_t prio = + gicd->IPRIORITYR[reg_ind] >> off & BIT_MASK(off, GIC_PRIO_BITS); + + spin_unlock(&gicd_lock); + + return prio; +} + +void gicd_set_icfgr(uint64_t int_id, uint8_t cfg) +{ + spin_lock(&gicd_lock); + + uint64_t reg_ind = (int_id * GIC_CONFIG_BITS) / (sizeof(uint32_t) * 8); + uint64_t off = (int_id * GIC_CONFIG_BITS) % (sizeof(uint32_t) * 8); + uint64_t mask = ((1U << GIC_CONFIG_BITS) - 1) << off; + + gicd->ICFGR[reg_ind] = (gicd->ICFGR[reg_ind] & ~mask) | ((cfg << off) & mask); + + spin_unlock(&gicd_lock); +} + +void gicd_set_prio(uint64_t int_id, uint8_t prio) +{ + uint64_t reg_ind = GIC_PRIO_REG(int_id); + uint64_t off = GIC_PRIO_OFF(int_id); + uint64_t mask = BIT_MASK(off, GIC_PRIO_BITS); + + spin_lock(&gicd_lock); + + gicd->IPRIORITYR[reg_ind] = + (gicd->IPRIORITYR[reg_ind] & ~mask) | ((prio << off) & mask); + + spin_unlock(&gicd_lock); +} + +enum int_state gicd_get_state(uint64_t int_id) +{ + uint64_t reg_ind = GIC_INT_REG(int_id); + uint64_t mask = GIC_INT_MASK(int_id); + + spin_lock(&gicd_lock); + + enum int_state pend = (gicd->ISPENDR[reg_ind] & mask) ? PEND : 0; + enum int_state act = (gicd->ISACTIVER[reg_ind] & mask) ? ACT : 0; + + spin_unlock(&gicd_lock); + + return pend | act; +} + +static void gicd_set_pend(uint64_t int_id, bool pend) +{ + spin_lock(&gicd_lock); + + if (gic_is_sgi(int_id)) { + uint64_t reg_ind = GICD_SGI_REG(int_id); + uint64_t off = GICD_SGI_OFF(int_id); + + if (pend) { + gicd->SPENDSGIR[reg_ind] = (1U) << (off + get_cpuid()); + } else { + gicd->CPENDSGIR[reg_ind] = BIT_MASK(off, 8); + } + } else { + uint64_t reg_ind = GIC_INT_REG(int_id); + + if (pend) { + gicd->ISPENDR[reg_ind] = GIC_INT_MASK(int_id); + } else { + gicd->ICPENDR[reg_ind] = GIC_INT_MASK(int_id); + } + } + + spin_unlock(&gicd_lock); +} + +void gicd_set_act(uint64_t int_id, bool act) +{ + uint64_t reg_ind = GIC_INT_REG(int_id); + + spin_lock(&gicd_lock); + + if (act) { + gicd->ISACTIVER[reg_ind] = GIC_INT_MASK(int_id); + } else { + gicd->ICACTIVER[reg_ind] = GIC_INT_MASK(int_id); + } + + spin_unlock(&gicd_lock); +} + +void gicd_set_state(uint64_t int_id, enum int_state state) +{ + gicd_set_act(int_id, state & ACT); + gicd_set_pend(int_id, state & PEND); +} + +void gicd_set_trgt(uint64_t int_id, uint8_t trgt) +{ + uint64_t reg_ind = GIC_TARGET_REG(int_id); + uint64_t off = GIC_TARGET_OFF(int_id); + uint32_t mask = BIT_MASK(off, GIC_TARGET_BITS); + + spin_lock(&gicd_lock); + + gicd->ITARGETSR[reg_ind] = + (gicd->ITARGETSR[reg_ind] & ~mask) | ((trgt << off) & mask); + + spin_unlock(&gicd_lock); +} + +void gicd_set_route(uint64_t int_id, uint64_t trgt) +{ + if (gic_is_priv(int_id)) return; + + gicd->IROUTER[int_id] = trgt; +} + +void gicd_set_enable(uint64_t int_id, bool en) +{ + uint64_t bit = GIC_INT_MASK(int_id); + + uint64_t reg_ind = GIC_INT_REG(int_id); + spin_lock(&gicd_lock); + if (en) + gicd->ISENABLER[reg_ind] = bit; + else + gicd->ICENABLER[reg_ind] = bit; + spin_unlock(&gicd_lock); +} + +void gicr_set_prio(uint64_t int_id, uint8_t prio, uint32_t gicr_id) +{ + uint64_t reg_ind = GIC_PRIO_REG(int_id); + uint64_t off = GIC_PRIO_OFF(int_id); + uint64_t mask = BIT_MASK(off, GIC_PRIO_BITS); + + spin_lock(&gicr_lock); + + gicr[gicr_id].IPRIORITYR[reg_ind] = + (gicr[gicr_id].IPRIORITYR[reg_ind] & ~mask) | ((prio << off) & mask); + + spin_unlock(&gicr_lock); +} + +uint64_t gicr_get_prio(uint64_t int_id, uint32_t gicr_id) +{ + uint64_t reg_ind = GIC_PRIO_REG(int_id); + uint64_t off = GIC_PRIO_OFF(int_id); + + spin_lock(&gicr_lock); + + uint64_t prio = + gicr[gicr_id].IPRIORITYR[reg_ind] >> off & BIT_MASK(off, GIC_PRIO_BITS); + + spin_unlock(&gicr_lock); + + return prio; +} + +void gicr_set_icfgr(uint64_t int_id, uint8_t cfg, uint32_t gicr_id) +{ + spin_lock(&gicr_lock); + + uint64_t reg_ind = (int_id * GIC_CONFIG_BITS) / (sizeof(uint32_t) * 8); + uint64_t off = (int_id * GIC_CONFIG_BITS) % (sizeof(uint32_t) * 8); + uint64_t mask = ((1U << GIC_CONFIG_BITS) - 1) << off; + + if (reg_ind == 0) { + gicr[gicr_id].ICFGR0 = + (gicr[gicr_id].ICFGR0 & ~mask) | ((cfg << off) & mask); + } else { + gicr[gicr_id].ICFGR1 = + (gicr[gicr_id].ICFGR1 & ~mask) | ((cfg << off) & mask); + } + + spin_unlock(&gicr_lock); +} + +enum int_state gicr_get_state(uint64_t int_id, uint32_t gicr_id) +{ + uint64_t mask = GIC_INT_MASK(int_id); + + spin_lock(&gicr_lock); + + enum int_state pend = (gicr[gicr_id].ISPENDR0 & mask) ? PEND : 0; + enum int_state act = (gicr[gicr_id].ISACTIVER0 & mask) ? ACT : 0; + + spin_unlock(&gicr_lock); + + return pend | act; +} + +static void gicr_set_pend(uint64_t int_id, bool pend, uint32_t gicr_id) +{ + spin_lock(&gicr_lock); + if (pend) { + gicr[gicr_id].ISPENDR0 = (1U) << (int_id); + } else { + gicr[gicr_id].ICPENDR0 = (1U) << (int_id); + } + spin_unlock(&gicr_lock); +} + +void gicr_set_act(uint64_t int_id, bool act, uint32_t gicr_id) +{ + spin_lock(&gicr_lock); + + if (act) { + gicr[gicr_id].ISACTIVER0 = GIC_INT_MASK(int_id); + } else { + gicr[gicr_id].ICACTIVER0 = GIC_INT_MASK(int_id); + } + + spin_unlock(&gicr_lock); +} + +void gicr_set_state(uint64_t int_id, enum int_state state, uint32_t gicr_id) +{ + gicr_set_act(int_id, state & ACT, gicr_id); + gicr_set_pend(int_id, state & PEND, gicr_id); +} + +void gicr_set_trgt(uint64_t int_id, uint8_t trgt, uint32_t gicr_id) +{ + spin_lock(&gicr_lock); + + spin_unlock(&gicr_lock); +} + +void gicr_set_route(uint64_t int_id, uint8_t trgt, uint32_t gicr_id) +{ + gicr_set_trgt(int_id, trgt, gicr_id); +} + +void gicr_set_enable(uint64_t int_id, bool en, uint32_t gicr_id) +{ + uint64_t bit = GIC_INT_MASK(int_id); + + spin_lock(&gicr_lock); + if (en) + gicr[gicr_id].ISENABLER0 = bit; + else + gicr[gicr_id].ICENABLER0 = bit; + spin_unlock(&gicr_lock); +} + +static bool irq_in_gicd(uint64_t int_id) +{ + if (int_id > 32 && int_id < 1025) return true; + else return false; +} + +void gic_send_sgi(uint64_t cpu_target, uint64_t sgi_num) +{ + if (sgi_num >= GIC_MAX_SGIS) return; + + //TODO: 这里是简陋的实现,后续要考虑多个cluster的情形 + uint64_t sgi = (1UL << (cpu_target & 0xffull)) | (sgi_num << 24); + MSR(ICC_SGI1R, sgi); +} + +void gic_set_prio(uint64_t int_id, uint8_t prio) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_prio(int_id, prio); + } else { + return gicr_set_prio(int_id, prio, get_cpuid()); + } +} + +uint64_t gic_get_prio(uint64_t int_id) +{ + if (irq_in_gicd(int_id)) { + return gicd_get_prio(int_id); + } else { + return gicr_get_prio(int_id, get_cpuid()); + } +} + +void gic_set_icfgr(uint64_t int_id, uint8_t cfg) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_icfgr(int_id, cfg); + } else { + return gicr_set_icfgr(int_id, cfg, get_cpuid()); + } +} + +enum int_state gic_get_state(uint64_t int_id) +{ + if (irq_in_gicd(int_id)) { + return gicd_get_state(int_id); + } else { + return gicr_get_state(int_id, get_cpuid()); + } +} + +static void gic_set_pend(uint64_t int_id, bool pend) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_pend(int_id, pend); + } else { + return gicr_set_pend(int_id, pend, get_cpuid()); + } +} + +void gic_set_act(uint64_t int_id, bool act) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_act(int_id, act); + } else { + return gicr_set_act(int_id, act, get_cpuid()); + } +} + +void gic_set_state(uint64_t int_id, enum int_state state) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_state(int_id, state); + } else { + return gicr_set_state(int_id, state, get_cpuid()); + } +} + +void gic_set_trgt(uint64_t int_id, uint8_t trgt) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_trgt(int_id, trgt); + } else { + return gicr_set_trgt(int_id, trgt, get_cpuid()); + } +} + +void gic_set_route(uint64_t int_id, uint64_t trgt) +{ + return gicd_set_route(int_id, trgt); +} + +void gic_set_enable(uint64_t int_id, bool en) +{ + if (irq_in_gicd(int_id)) { + return gicd_set_enable(int_id, en); + } else { + return gicr_set_enable(int_id, en, get_cpuid()); + } +} + diff --git a/src/arch/aarch32/inc/arch/aarch32_helpers.h b/src/arch/aarch32/inc/arch/aarch32_helpers.h index b749f9492f87130da569a8f5547e9cf57219105b..5fc77b8273ddeef932bc79248819c4778ae956cb 100644 --- a/src/arch/aarch32/inc/arch/aarch32_helpers.h +++ b/src/arch/aarch32/inc/arch/aarch32_helpers.h @@ -81,19 +81,27 @@ MAKE_REG64_HELPER(cntv_cval, 3, 14); /* ICC_PMR */ MAKE_REG_HELPER(ICC_PMR_EL1, 0, 4, 6, 0); +/* ICC_BPR0 */ +MAKE_REG_HELPER(ICC_BPR0_EL1, 0, 12, 8, 3); +/* ICC_BPR1 */ +MAKE_REG_HELPER(ICC_BPR1_EL1, 0, 12, 12, 3); + /* ICC_IAR1 */ MAKE_REG_HELPER(ICC_IAR1_EL1, 0, 12, 12, 0); /* ICC_EOIR1 */ MAKE_REG_HELPER(ICC_EOIR1_EL1, 0, 12, 12, 1); /* ICC_SRE */ MAKE_REG_HELPER(ICC_SRE_EL1, 0, 12, 12, 5); +/* ICC_IGRPEN0 */ +MAKE_REG_HELPER(ICC_IGRPEN0_EL1, 0, 12, 12, 6); /* ICC_IGRPEN1 */ MAKE_REG_HELPER(ICC_IGRPEN1_EL1, 0, 12, 12, 7); +#define ICC_IGRPEN_EL1_ENB_BIT (0x1) #define write_sysreg(val, reg) write_##reg(val) #define read_sysreg(reg) read_##reg() -#define MSR(reg, var) write_##reg(val) +#define MSR(reg, val) write_##reg(val) #define MRS(reg) read_##reg() #endif /* __ASSEMBLER__ */ diff --git a/src/arch/aarch32/inc/asm_macros.h b/src/arch/aarch32/inc/asm_macros.h index 81d2e7b43692cddf0aff975fd407b0fddaf34ce9..a838b045525ba28a91e4ecaa70fe8eb519d9983f 100644 --- a/src/arch/aarch32/inc/asm_macros.h +++ b/src/arch/aarch32/inc/asm_macros.h @@ -252,4 +252,36 @@ div2: cmp \temp, \bot bhs div2 .endm + +.macro exception_entry mode + /* + * 以下摘至, G1.11.3 + * For an exception taken to a non-EL2 mode, + * on exception entry, the exception handler can use the SRS instruction to + * store the return state onto the stack of any mode at the same Exception level and in the same Security state + * and can use the CPS instruction to change mode + */ + + /* + * Store r0-r3, r12, lr, lr_und and spsr_und into the stack to + * construct an exception stack frame. + */ + srsdb sp!, #\mode + stmfd sp, {r0-r3, r12, lr}^ + sub sp, #24 + + /* sp may Invoked by fault handler */ + mov r0, sp +.endm + +.macro exception_exit + /* + * Restore r0-r3, r12, lr, lr_und and spsr_und from the exception stack + * and return to the current thread. + */ + ldmia sp, {r0-r3, r12, lr}^ + add sp, #24 + rfeia sp! +.endm + #endif /* ASM_MACROS_S */ diff --git a/src/arch/aarch32/inc/gic.h b/src/arch/aarch32/inc/gic.h index f44b609fe6932b69327b4d262c746b3d4e712c5d..9046e01c0828905493e3d1d2c129d9e7510fde27 100644 --- a/src/arch/aarch32/inc/gic.h +++ b/src/arch/aarch32/inc/gic.h @@ -6,4 +6,328 @@ #include #include +#define GICV2 (2) +#define GICV3 (3) + +#define GIC_MAX_INTERUPTS 1024 +#define GIC_MAX_SGIS 16 +#define GIC_MAX_PPIS 16 +#define GIC_CPU_PRIV (GIC_MAX_SGIS + GIC_MAX_PPIS) +#define GIC_MAX_SPIS (GIC_MAX_INTERUPTS - GIC_CPU_PRIV) +#define GIC_PRIO_BITS 8 +#define GIC_TARGET_BITS 8 +#define GIC_MAX_TARGETS GIC_TARGET_BITS +#define GIC_CONFIG_BITS 2 +#define GIC_SEC_BITS 2 +#define GIC_SGI_BITS 8 + +/* GICR_IIDR bit definitions */ +#define IIDR_PRODUCT_ID_MASK 0xff000000U +#define IIDR_VARIANT_MASK 0x000f0000U +#define IIDR_REVISION_MASK 0x0000f000U +#define IIDR_IMPLEMENTER_MASK 0x00000fffU +#define IIDR_MODEL_MASK (IIDR_PRODUCT_ID_MASK | \ + IIDR_IMPLEMENTER_MASK) + +#define GIC_INT_REG(NINT) (NINT / (sizeof(uint32_t) * 8)) +#define GIC_INT_MASK(NINT) (1U << NINT % (sizeof(uint32_t) * 8)) +#define GIC_NUM_INT_REGS(NINT) GIC_INT_REG(NINT) +#define GIC_NUM_PRIVINT_REGS (GIC_CPU_PRIV / (sizeof(uint32_t) * 8)) + +#define GIC_PRIO_REG(NINT) ((NINT * GIC_PRIO_BITS) / (sizeof(uint32_t) * 8)) +#define GIC_NUM_PRIO_REGS(NINT) GIC_PRIO_REG(NINT) +#define GIC_PRIO_OFF(NINT) (NINT * GIC_PRIO_BITS) % (sizeof(uint32_t) * 8) + +#define GIC_TARGET_REG(NINT) ((NINT * GIC_TARGET_BITS) / (sizeof(uint32_t) * 8)) +#define GIC_NUM_TARGET_REGS(NINT) GIC_TARGET_REG(NINT) +#define GIC_TARGET_OFF(NINT) (NINT * GIC_TARGET_BITS) % (sizeof(uint32_t) * 8) + +#define GIC_CONFIG_REG(NINT) ((NINT * GIC_CONFIG_BITS) / (sizeof(uint32_t) * 8)) +#define GIC_NUM_CONFIG_REGS(NINT) GIC_CONFIG_REG(NINT) +#define GIC_CONFIG_OFF(NINT) (NINT * GIC_CONFIG_BITS) % (sizeof(uint32_t) * 8) + +#define GIC_NUM_SEC_REGS(NINT) ((NINT * GIC_SEC_BITS) / (sizeof(uint32_t) * 8)) + +#define GIC_NUM_SGI_REGS \ + ((GIC_MAX_SGIS * GIC_SGI_BITS) / (sizeof(uint32_t) * 8)) +#define GICD_SGI_REG(NINT) (NINT / 4) +#define GICD_SGI_OFF(NINT) ((NINT % 4) * 8) + +#define GIC_NUM_APR_REGS ((1UL << (GIC_PRIO_BITS - 1)) / (sizeof(uint32_t) * 8)) +#define GIC_NUM_LIST_REGS (64) + +/* Distributor Control Register, GICD_CTLR */ + +#define GICD_CTLR_EN_BIT (0x1) +#define GICD_CTLR_ENA_BIT (0x2) +#define GICD_CTRL_DS (0x40) +#define GICD_CTLR_ARE_S_BIT (0x10) +#define GICD_CTLR_ARE_NS_BIT (0x20) +/* GICD_CTLR Register write progress bit */ +#define GICD_CTLR_RWP (1 << 31) + +/* Interrupt Controller Type Register, GICD_TYPER */ + +#define GICD_TYPER_ITLINENUM_OFF (0) +#define GICD_TYPER_ITLINENUM_LEN (5) +#define GICD_TYPER_CPUNUM_OFF (5) +#define GICD_TYPER_CPUNUM_LEN (3) +#define GICD_TYPER_SECUREXT_BIT (1UL << 10) +#define GICD_TYPER_LSPI_OFF (11) +#define GICD_TYPER_LSPI_LEN (6) + +/* Software Generated Interrupt Register, GICD_SGIR */ + +#define GICD_TYPER_ITLN_OFF 0 +#define GICD_TYPER_ITLN_LEN 5 +#define GICD_TYPER_ITLN_MSK BIT_MASK(GICD_TYPER_ITLN_OFF, GICD_TYPER_ITLN_LEN) +#define GICD_TYPER_CPUN_OFF 5 +#define GICD_TYPER_CPUN_LEN 3 +#define GICD_TYPER_CPUN_MSK BIT_MASK(GICD_TYPER_CPUN_OFF, GICD_TYPER_CPUN_LEN) + +#define GICD_SGIR_SGIINTID_OFF 0 +#define GICD_SGIR_SGIINTID_LEN 4 +#define GICD_SGIR_SGIINTID_MSK \ + (BIT_MASK(GICD_SGIR_SGIINTID_OFF, GICD_SGIR_SGIINTID_LEN)) +#define GICD_SGIR_SGIINTID(sgir) \ + bit_extract(sgir, GICD_SGIR_SGIINTID_OFF, GICD_SGIR_SGIINTID_LEN) +#define GICD_SGIR_CPUTRGLST_OFF 16 +#define GICD_SGIR_CPUTRGLST_LEN 8 +#define GICD_SGIR_CPUTRGLST(sgir) \ + bit_extract(sgir, GICD_SGIR_CPUTRGLST_OFF, GICD_SGIR_CPUTRGLST_LEN) +#define GICD_SGIR_TRGLSTFLT_OFF 24 +#define GICD_SGIR_TRGLSTFLT_LEN 2 +#define GICD_SGIR_TRGLSTFLT(sgir) \ + bit_extract(sgir, GICD_SGIR_TRGLSTFLT_OFF, GICD_SGIR_TRGLSTFLT_LEN) + +typedef struct { + uint32_t CTLR; + uint32_t TYPER; + uint32_t IIDR; + uint8_t pad0[0x0010 - 0x000C]; + uint32_t STATUSR; + uint8_t pad1[0x0040 - 0x0014]; + uint32_t SETSPI_NSR; + uint8_t pad2[0x0048 - 0x0044]; + uint32_t CLRSPI_NSR; + uint8_t pad3[0x0050 - 0x004C]; + uint32_t SETSPI_SR; + uint8_t pad4[0x0058 - 0x0054]; + uint32_t CLRSPI_SR; + uint8_t pad9[0x0080 - 0x005C]; + uint32_t IGROUPR[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; // banked CPU + uint32_t ISENABLER[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ICENABLER[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ISPENDR[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ICPENDR[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ISACTIVER[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ICACTIVER[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint32_t IPRIORITYR[GIC_NUM_PRIO_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ITARGETSR[GIC_NUM_TARGET_REGS(GIC_MAX_INTERUPTS)]; + uint32_t ICFGR[GIC_NUM_CONFIG_REGS(GIC_MAX_INTERUPTS)]; + uint32_t IGPRMODR[GIC_NUM_INT_REGS(GIC_MAX_INTERUPTS)]; + uint8_t pad5[0x0E00 - 0x0D80]; + uint32_t NSACr[GIC_NUM_SEC_REGS(GIC_MAX_INTERUPTS)]; + uint32_t SGIR; + uint8_t pad6[0x0F10 - 0x0F04]; + uint32_t CPENDSGIR[GIC_NUM_SGI_REGS]; + uint32_t SPENDSGIR[GIC_NUM_SGI_REGS]; + uint8_t pad7[0x6000 - 0x0F30]; + uint64_t IROUTER[GIC_MAX_INTERUPTS]; + uint8_t pad8[0xFFD0 - 0x8000]; + uint32_t ID[(0x10000 - 0xFFD0) / sizeof(uint32_t)]; +} __attribute__((__packed__, aligned(0x10000))) gicd_t; + +/* Redistributor Wake Register, GICD_WAKER */ + +#define GICR_CTLR_UWP (1 << 31) /* No upstream writes pending. */ +#define GICR_ProcessorSleep_BIT (0x2) +#define GICR_ChildrenASleep_BIT (0x4) +typedef struct { + /* RD_base frame */ + uint32_t CTLR; + uint32_t IIDR; + uint64_t TYPER; + uint32_t STATUSR; + uint32_t WAKER; + uint8_t pad17[0x0024 - 0x0018]; + uint32_t PWRR; /* GIC-600 specific register offsets */ + uint8_t pad0[0x0040 - 0x0028]; + uint64_t SETLPIR; + uint64_t CLRLPIR; + uint8_t pad1[0x0070 - 0x0050]; + uint64_t PROPBASER; + uint64_t PENDBASER; + uint8_t pad2[0x00A0 - 0x0080]; + uint64_t INVLPIR; + uint8_t pad3[0x00B0 - 0x00A8]; + uint64_t INVALLR; + uint8_t pad4[0x00c0 - 0x00b8]; + uint64_t SYNCR; + uint8_t pad5[0xFFD0 - 0x00c8]; + uint32_t ID[(0x10000 - 0xFFD0) / sizeof(uint32_t)]; + + /* SGI_base frame */ + uint8_t sgi_base[0] __attribute__((aligned(0x10000))); + uint8_t pad6[0x0080 - 0x000]; + uint32_t IGROUPR0; + uint8_t pad7[0x0100 - 0x084]; + uint32_t ISENABLER0; + uint8_t pad8[0x0180 - 0x104]; + uint32_t ICENABLER0; + uint8_t pad9[0x0200 - 0x184]; + uint32_t ISPENDR0; + uint8_t pad10[0x0280 - 0x204]; + uint32_t ICPENDR0; + uint8_t pad11[0x0300 - 0x284]; + uint32_t ISACTIVER0; + uint8_t pad12[0x0380 - 0x304]; + uint32_t ICACTIVER0; + uint8_t pad13[0x0400 - 0x384]; + uint32_t IPRIORITYR[GIC_NUM_PRIO_REGS(GIC_CPU_PRIV)]; + uint8_t pad14[0x0c00 - 0x420]; + uint32_t ICFGR0; + uint32_t ICFGR1; + uint8_t pad15[0x0D00 - 0xc08]; + uint32_t IGRPMODR0; + uint8_t pad16[0x0e00 - 0xd04]; + uint32_t NSACr; +} __attribute__((__packed__, aligned(0x10000))) gicr_t; + +/* CPU Interface Control Register, GICC_CTLR */ + +#define GICC_CTLR_EN_BIT (0x1) +#define GICC_CTLR_EOImodeNS_BIT (1UL << 9) +#define GICC_CTLR_WR_MSK (0x1) +#define GICC_IAR_ID_OFF (0) +#define GICC_IAR_ID_LEN (10) +#define GICC_IAR_ID_MSK (BIT_MASK(GICC_IAR_ID_OFF, GICC_IAR_ID_LEN)) +#define GICC_IAR_CPU_OFF (10) +#define GICC_IAR_CPU_LEN (3) +#define GICC_IAR_CPU_MSK (BIT_MASK(GICC_IAR_CPU_OFF, GICC_IAR_CPU_LEN)) + +typedef struct { + uint32_t CTLR; + uint32_t PMR; + uint32_t BPR; + uint32_t IAR; + uint32_t EOIR; + uint32_t RPR; + uint32_t HPPIR; + uint32_t ABPR; + uint32_t AIAR; + uint32_t AEOIR; + uint32_t AHPPIR; + uint8_t pad0[0x00D0 - 0x002C]; + uint32_t APR[GIC_NUM_APR_REGS]; + uint32_t NSAPR[GIC_NUM_APR_REGS]; + uint8_t pad1[0x00FC - 0x00F0]; + uint32_t IIDR; + uint8_t pad2[0x1000 - 0x0100]; + uint32_t DIR; +} __attribute__((__packed__, aligned(0x1000))) gicc_t; + +#define GICH_HCR_En_BIT (1 << 0) +#define GICH_HCR_UIE_BIT (1 << 1) +#define GICH_HCR_LRENPIE_BIT (1 << 2) +#define GICH_HCR_NPIE_BIT (1 << 3) +#define GICH_HCR_VGrp0DIE_BIT (1 << 4) +#define GICH_HCR_VGrp0EIE_BIT (1 << 5) +#define GICH_HCR_VGrp1EIE_BIT (1 << 6) +#define GICH_HCR_VGrp1DIE_BIT (1 << 7) +#define GICH_HCR_EOICount_OFF (27) +#define GICH_HCR_EOICount_LEN (5) +#define GICH_HCR_EOICount_MASK \ + BIT_MASK(GICH_HCR_EOICount_OFF, GICH_HCR_EOICount_LEN) + +#define GICH_VTR_OFF (0) +#define GICH_VTR_LEN (6) +#define GICH_VTR_MSK BIT_MASK(GICH_VTR_OFF, GICH_VTR_LEN) + +#define GICH_LR_VID_OFF (0) +#define GICH_LR_VID_LEN (10) +#define GICH_LR_VID_MSK BIT_MASK(GICH_LR_VID_OFF, GICH_LR_VID_LEN) +#define GICH_LR_VID(LR) (bit_extract(LR, GICH_LR_VID_OFF, GICH_LR_VID_LEN)) + +#define GICH_LR_PID_OFF (10) +#define GICH_LR_PID_LEN (10) +#define GICH_LR_PID_MSK BIT_MASK(GICH_LR_PID_OFF, GICH_LR_PID_LEN) +#define GICH_LR_CPUID_OFF (10) +#define GICH_LR_CPUID_LEN (3) +#define GICH_LR_CPUID_MSK BIT_MASK(GICH_LR_CPUID_OFF, GICH_LR_CPUID_LEN) +#define GICH_LR_CPUID(LR) \ + (bit_extract(LR, GICH_LR_CPUID_OFF, GICH_LR_CPUID_LEN)) +#define GICH_LR_PRIO_OFF (23) +#define GICH_LR_PRIO_LEN (5) +#define GICH_LR_PRIO_MSK BIT_MASK(GICH_LR_PRIO_OFF, GICH_LR_PRIO_LEN) +#define GICH_LR_STATE_OFF (28) +#define GICH_LR_STATE_LEN (2) +#define GICH_LR_STATE_MSK BIT_MASK(GICH_LR_STATE_OFF, GICH_LR_STATE_LEN) +#define GICH_LR_STATE(LR) \ + (bit_extract(LR, GICH_LR_STATE_OFF, GICH_LR_STATE_LEN)) + +#define GICH_LR_STATE_INV ((0 << GICH_LR_STATE_OFF) & GICH_LR_STATE_MSK) +#define GICH_LR_STATE_PND ((1 << GICH_LR_STATE_OFF) & GICH_LR_STATE_MSK) +#define GICH_LR_STATE_ACT ((2 << GICH_LR_STATE_OFF) & GICH_LR_STATE_MSK) +#define GICH_LR_STATE_ACTPEND ((3 << GICH_LR_STATE_OFF) & GICH_LR_STATE_MSK) + +#define GICH_LR_HW_BIT (1U << 31) +#define GICH_LR_EOI_BIT (1U << 19) + +#define GICH_MISR_EOI (1U << 0) +#define GICH_MISR_U (1U << 1) +#define GICH_MISR_LRPEN (1U << 2) +#define GICH_MISR_NP (1U << 3) +#define GICH_MISR_VGrp0E (1U << 4) +#define GICH_MISR_VGrp0D (1U << 5) +#define GICH_MISR_VGrp1E (1U << 6) +#define GICH_MISR_VGrp1D (1U << 7) + +enum int_state { INV, PEND, ACT, PENDACT }; + +void gic_init(); +void gic_cpu_init(); +void gic_send_sgi(uint64_t cpu_target, uint64_t sgi_num); + +void gic_set_enable(uint64_t int_id, bool en); +void gic_set_prio(uint64_t int_id, uint8_t prio); +void gic_set_icfgr(uint64_t int_id, uint8_t cfg); +void gic_set_act(uint64_t int_id, bool act); +void gic_set_state(uint64_t int_id, enum int_state state); +void gic_set_trgt(uint64_t int_id, uint8_t trgt); +void gic_set_route(uint64_t int_id, uint64_t trgt); +uint64_t gic_get_prio(uint64_t int_id); +uint8_t gic_get_trgt(uint64_t int_id); +enum int_state gic_get_state(uint64_t int_id); + +void gicd_set_enable(uint64_t int_id, bool en); +void gicd_set_prio(uint64_t int_id, uint8_t prio); +void gicd_set_icfgr(uint64_t int_id, uint8_t cfg); +void gicd_set_act(uint64_t int_id, bool act); +void gicd_set_state(uint64_t int_id, enum int_state state); +void gicd_set_trgt(uint64_t int_id, uint8_t trgt); +void gicd_set_route(uint64_t int_id, uint64_t trgt); +uint64_t gicd_get_prio(uint64_t int_id); +enum int_state gicd_get_state(uint64_t int_id); + +void gicr_set_enable(uint64_t int_id, bool en, uint32_t gicr_id); +void gicr_set_prio(uint64_t int_id, uint8_t prio, uint32_t gicr_id); +void gicr_set_icfgr(uint64_t int_id, uint8_t cfg, uint32_t gicr_id); +void gicr_set_act(uint64_t int_id, bool act, uint32_t gicr_id); +void gicr_set_state(uint64_t int_id, enum int_state state, uint32_t gicr_id); +void gicr_set_trgt(uint64_t int_id, uint8_t trgt, uint32_t gicr_id); +void gicr_set_route(uint64_t int_id, uint8_t trgt, uint32_t gicr_id); +uint64_t gicr_get_prio(uint64_t int_id, uint32_t gicr_id); +enum int_state gicr_get_state(uint64_t int_id, uint32_t gicr_id); + +static inline bool gic_is_sgi(uint64_t int_id) +{ + return int_id < GIC_MAX_SGIS; +} + +static inline bool gic_is_priv(uint64_t int_id) +{ + return int_id < GIC_CPU_PRIV; +} + #endif /* __GIC_H__ */ diff --git a/src/arch/aarch32/interrupts.c b/src/arch/aarch32/interrupts.c index 03923e097f2212f8b345e45def6e706f489e20bc..c4afcd7c027c23b826d9ce1758c3317a516c76fc 100644 --- a/src/arch/aarch32/interrupts.c +++ b/src/arch/aarch32/interrupts.c @@ -6,28 +6,29 @@ void arch_irq_enable(unsigned id) { -// gic_set_enable(id, true); -// gic_set_route(id, get_cpuid()); + gic_set_enable(id, true); + gic_set_route(id, get_cpuid()); } void arch_irq_disable(unsigned id) { - // gic_set_enable(id, false); + gic_set_enable(id, false); } void arch_irq_set_prio(unsigned id, unsigned prio) { - //gic_set_prio(id, (uint8_t) prio); + gic_set_prio(id, (uint8_t) prio); } void arch_irq_send_ipi(uint64_t target_cpu, uint64_t ipi_id) { -// if (ipi_id < GIC_MAX_SGIS) - // gic_send_sgi(target_cpu, ipi_id); + if (ipi_id < GIC_MAX_SGIS) + gic_send_sgi(target_cpu, ipi_id); } void arch_interrupts_init(void) { + gic_init(); arch_local_irq_enable(); } diff --git a/src/arch/aarch32/plat_helpers.S b/src/arch/aarch32/plat_helpers.S index e911e769dbe11a231488f5fe57b023accc1ced55..a66f85cdf49c66aa30c3dfb366eb3fcfdf196280 100644 --- a/src/arch/aarch32/plat_helpers.S +++ b/src/arch/aarch32/plat_helpers.S @@ -4,6 +4,8 @@ .global plat_get_core_pos .global plat_set_stack + .global plat_handle_interrupt + .global plat_handle_exception .global bzero @@ -61,6 +63,24 @@ func plat_set_stack bx r4 endfunc plat_set_stack +/* + * void plat_handle_interrupt(void); + * platform specific interrup handler + */ +func plat_handle_interrupt + cpsid i + bl gic_handle + cpsie i +endfunc plat_handle_interrupt + +/* + * void plat_handle_exception(void); + * platform specific exception handler + */ +func plat_handle_exception + b . +endfunc plat_handle_exception + /* ----------------------------------------------------------------------- * void bzero(void *mem, unsigned int length); * diff --git a/src/arch/aarch32/sources.mk b/src/arch/aarch32/sources.mk index 12bce15c05231dff18cc5fe8122c8550aae4d117..55fa505d8be5f0d4725bf7f51b8fb2e158662726 100644 --- a/src/arch/aarch32/sources.mk +++ b/src/arch/aarch32/sources.mk @@ -1,2 +1,2 @@ -arch_c_srcs:= interrupts.c cpu.c -arch_s_srcs:= boot.S plat_helpers.S spinlock.S +arch_c_srcs:= interrupts.c cpu.c gicv3.c +arch_s_srcs:= boot.S plat_helpers.S spinlock.S exceptions.S diff --git a/src/arch/aarch64/boot.S b/src/arch/aarch64/boot.S index 38b23c5855694471252d0c1dacd1c45f16851ba6..963e58c5e2fdf92498b95662e710fa6c3ced2992 100644 --- a/src/arch/aarch64/boot.S +++ b/src/arch/aarch64/boot.S @@ -79,7 +79,7 @@ func entrypoint msr sctlr_el3, x0 isb -/* + /* * Initialize vector base address register for EL3 */ adr x1, rom_exceptions diff --git a/src/arch/aarch64/gicv3.c b/src/arch/aarch64/gicv3.c index 6eb36cfdf7ea2fdd29c6cb77f3855834c1f5636a..10229e16a7258a43fd9f2ae39767342fd302a4f5 100644 --- a/src/arch/aarch64/gicv3.c +++ b/src/arch/aarch64/gicv3.c @@ -69,13 +69,48 @@ static bool gicr_need_power_mgmt() ((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600AE)); } -inline uint64_t gic_num_irqs() +static inline uint64_t gic_num_irqs() { uint32_t itlinenumber = bit_extract(gicd->TYPER, GICD_TYPER_ITLN_OFF, GICD_TYPER_ITLN_LEN); return 32 * itlinenumber + 1; } +/* + * Wait for register write pending + * TODO: add timed wait + */ +static inline int gic_wait_rwp(uint32_t intid) +{ + uint32_t rwp_mask; + + if (intid < 32) { + while(gicr[get_cpuid()].CTLR & GICR_CTLR_UWP); + } else { + while(gicd->CTLR & GICR_CTLR_UWP); + } + + return 0; +} + +/* + * Wake up GIC redistributor. + * clear ProcessorSleep and wait till ChildAsleep is cleared. + * ProcessSleep to be cleared only when ChildAsleep is set + * Check if redistributor is not powered already. + */ +static void gicr_enable(void) +{ + /* + * The WAKER_PS_BIT should be changed to 0 + * only when WAKER_CA_BIT is 1. + */ + if (!gicr[get_cpuid()].WAKER & GICR_ChildrenASleep_BIT) + return; + + gicr[get_cpuid()].WAKER &= ~ GICR_ProcessorSleep_BIT; + while(gicr[get_cpuid()].WAKER & GICR_ChildrenASleep_BIT); +} static inline void gicc_init() { @@ -94,64 +129,24 @@ static inline void gicc_init() static inline void gicr_init() { - if (gicr_need_power_mgmt()) { - gic600_pwr_on(); - } - gicr[get_cpuid()].ICENABLER0 = -1; gicr[get_cpuid()].ICPENDR0 = -1; gicr[get_cpuid()].ICACTIVER0 = -1; + gic_wait_rwp(0); for (int i = 0; i < GIC_NUM_PRIO_REGS(GIC_CPU_PRIV); i++) { gicr[get_cpuid()].IPRIORITYR[i] = -1; } } -//void gicc_save_state(gicc_state_t *state) -//{ -// state->CTLR = MRS(ICC_CTLR_EL1); -// state->PMR = MRS(ICC_PMR_EL1); -// //state->IAR = MRS(ICC_IAR1_EL1); -// state->BPR = MRS(ICC_BPR1_EL1); -// //state->EOIR = MRS(ICC_EOIR1_EL1); -// state->RPR = MRS(ICC_RPR_EL1); -// state->HPPIR = MRS(ICC_HPPIR1_EL1); -// state->priv_ISENABLER = gicr[get_cpuid()].ISENABLER0; -// -// for (int i = 0; i < GIC_NUM_PRIO_REGS(GIC_CPU_PRIV); i++) { -// state->priv_IPRIORITYR[i] = gicr[get_cpuid()].IPRIORITYR[i]; -// } -// -// state->HCR = MRS(ICH_HCR_EL2); -// for (int i = 0; i < gich_num_lrs(); i++) { -// state->LR[i] = gich_read_lr(i); -// } -//} -// -//void gicc_restore_state(gicc_state_t *state) -//{ -// MSR(ICC_CTLR_EL1, state->CTLR); -// MSR(ICC_PMR_EL1, state->PMR); -// MSR(ICC_BPR1_EL1, state->BPR); -// //MSR(ICC_EOIR1_EL1, state->EOIR); -// //MSR(ICC_IAR1_EL1, state->IAR); -// MSR(ICC_RPR_EL1, state->RPR); -// MSR(ICC_HPPIR1_EL1, state->HPPIR); -// gicr[get_cpuid()].ISENABLER0 = state->priv_ISENABLER; -// -// for (int i = 0; i < GIC_NUM_PRIO_REGS(GIC_CPU_PRIV); i++) { -// gicr[get_cpuid()].IPRIORITYR[i] = state->priv_IPRIORITYR[i]; -// } -// -// MSR(ICH_HCR_EL2, state->HCR); -// for (int i = 0; i < gich_num_lrs(); i++) { -// gich_write_lr(i, state->LR[i]); -// } -//} - void gic_cpu_init() { + if (gicr_need_power_mgmt()) { + gic600_pwr_on(); + } + gicr_init(); + gicr_enable(); gicc_init(); } @@ -170,6 +165,9 @@ void gicd_init() gicd->ICACTIVER[i] = -1; } + /* wait for rwp on GICD */ + gic_wait_rwp(32); + /* All interrupts have lowest priority possible by default */ for (int i = GIC_CPU_PRIV; i < GIC_NUM_PRIO_REGS(int_num); i++) gicd->IPRIORITYR[i] = -1; @@ -196,15 +194,13 @@ void gic_init() gicd_init(); } - //cpu_sync_barrier(&cpu_glb_sync); - gic_cpu_init(); } void gic_handle(void) { - uint64_t ack; + unsigned long ack; uint64_t id; //TODO:only support G1S now diff --git a/src/arch/aarch64/inc/gic.h b/src/arch/aarch64/inc/gic.h index 24ea8f8742e37297b328be9058c2c02fe07fd05b..651c0a137239c334ddd2aeb18f6b4fc968f85b6f 100644 --- a/src/arch/aarch64/inc/gic.h +++ b/src/arch/aarch64/inc/gic.h @@ -60,7 +60,11 @@ #define GICD_CTLR_EN_BIT (0x1) #define GICD_CTLR_ENA_BIT (0x2) -#define GICD_CTLR_ARE_NS_BIT (0x10) +#define GICD_CTRL_DS (0x40) +#define GICD_CTLR_ARE_S_BIT (0x10) +#define GICD_CTLR_ARE_NS_BIT (0x20) +/* GICD_CTLR Register write progress bit */ +#define GICD_CTLR_RWP (1 << 31) /* Interrupt Controller Type Register, GICD_TYPER */ @@ -136,7 +140,7 @@ typedef struct { /* Redistributor Wake Register, GICD_WAKER */ -#define GICR_CTRL_DS_BIT (1 << 6) +#define GICR_CTLR_UWP (1 << 31) /* No upstream writes pending. */ #define GICR_ProcessorSleep_BIT (0x2) #define GICR_ChildrenASleep_BIT (0x4) typedef struct { @@ -305,7 +309,6 @@ void gicd_set_trgt(uint64_t int_id, uint8_t trgt); void gicd_set_route(uint64_t int_id, uint64_t trgt); uint64_t gicd_get_prio(uint64_t int_id); enum int_state gicd_get_state(uint64_t int_id); -uint64_t gic_num_irqs(); void gicr_set_enable(uint64_t int_id, bool en, uint32_t gicr_id); void gicr_set_prio(uint64_t int_id, uint8_t prio, uint32_t gicr_id); diff --git a/src/platform/fvp_armv8r/inc/plat.h b/src/platform/fvp_armv8r/inc/plat.h index 3b1e1172de011fe253d1f7420b2695794e9ea3e1..7be122524261eed380a4d7440796a471c25fd0ea 100644 --- a/src/platform/fvp_armv8r/inc/plat.h +++ b/src/platform/fvp_armv8r/inc/plat.h @@ -6,13 +6,14 @@ #define PLAT_STACK_SIZE 0x1000 /* stack for each CPU */ -#define PLAT_NR_CPUS 2 +#define PLAT_NR_CPUS 4 #define PLAT_CLUSTER_COUNT 1 #define PLAT_CORE_COUNT (PLAT_NR_CPUS * PLAT_CLUSTER_COUNT) #define UART_BASE (0x9c090000ULL) #define UART_IRQ_ID 37 +#define GIC_SINGLE_SECURITY_STATE 1 #define GICD_BASE (0xaf000000ULL) #define GICR_BASE (0xaf100000ULL)