arm64: Mediate access to GICv3 sysregs at EL2

new file mode 100644
index 0000000000..4cc077fbb6
@@ -0,0 +1,86 @@
+/*
+ * xen/arch/arm/arm64/vgic-v3-sr.c
+ *
+ * Code to emulate group0/group1 traps for handling
+ * cavium erratum 30115
+ *
+ * This file merges code from Linux virt/kvm/arm/hyp/vgic-v3-sr.c
+ * which is : Copyright (C) 2012-2015 - ARM Ltd
+ * Author: Marc Zyngier <[email protected]>
+ *
+ * Xen-Merge: Manish Jaggi <[email protected]>
+ *
+ * Ths program is free software; you can redistribute it and/or
+ * modify it under the terms and conditions of the GNU General Public
+ * License, version 2, as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <asm/current.h>
+#include <asm/regs.h>
+#include <asm/system.h>
+#include <asm/traps.h>
+
+/* Provide wrappers to read write VMCR similar to linux */
+static uint64_t vgic_v3_read_vmcr(void)
+{
+    return READ_SYSREG32(ICH_VMCR_EL2);
+}
+
+static void vgic_v3_write_vmcr(uint32_t vmcr)
+{
+    WRITE_SYSREG32(vmcr, ICH_VMCR_EL2);
+}
+
+/* vgic_v3_handle_cpuif_access
+ * returns: true if the register is emulated
+ *          false if not a sysreg
+ */
+bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
+{
+    int rt;
+    uint32_t vmcr;
+    void (*fn)(struct cpu_user_regs *, u32, int);
+    bool is_read;
+    uint32_t sysreg;
+    bool ret = true;
+    const union hsr hsr = { .bits = regs->hsr };
+
+    sysreg = hsr.bits & HSR_SYSREG_REGS_MASK;
+    is_read = hsr.sysreg.read;
+    /* Disabling interrupts to prevent change in guest state */
+    local_irq_disable();
+    if ( hsr.ec != HSR_EC_SYSREG )
+    {
+        ret = false;
+        goto end;
+    }
+
+    switch ( sysreg )
+    {
+    default:
+        ret = false;
+        goto end;
+    }
+    /* Call the emulation hander */
+    vmcr = vgic_v3_read_vmcr();
+    rt = hsr.sysreg.reg;
+    fn(regs, vmcr, rt);
+end:
+    local_irq_enable();
+
+    return ret;
+}
+
+/*
+ * Local variables:
+ * mode: C
+ * c-file-style: "BSD"
+ * c-basic-offset: 4
+ * indent-tabs-mode: nil
+ * End:
+ */

index 2379b578cb..3c3911a69c 100644
@@ -3,6 +3,8 @@
 
 void inject_undef64_exception(struct cpu_user_regs *regs, int instr_len);
 
+bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs);
+
 void do_sysreg(struct cpu_user_regs *regs,
                const union hsr hsr);
 

index 4cc077fbb6..b894398dc6 100644
@@ -21,10 +21,13 @@
  */
 
 #include <asm/current.h>
+#include <asm/gic_v3_defs.h>
 #include <asm/regs.h>
 #include <asm/system.h>
 #include <asm/traps.h>
 
+#define vtr_to_nr_pre_bits(v)     ((((uint32_t)(v) >> 26) & 7) + 1)
+
 /* Provide wrappers to read write VMCR similar to linux */
 static uint64_t vgic_v3_read_vmcr(void)
 {

@@ -36,6 +39,60 @@ static void vgic_v3_write_vmcr(uint32_t vmcr)
     WRITE_SYSREG32(vmcr, ICH_VMCR_EL2);
 }
 
+static int vgic_v3_bpr_min(void)
+{
+    /* See Pseudocode for VPriorityGroup */
+    return 8 - vtr_to_nr_pre_bits(READ_SYSREG32(ICH_VTR_EL2));
+}
+
+static unsigned int vgic_v3_get_bpr0(uint32_t vmcr)
+{
+    return (vmcr & ICH_VMCR_BPR0_MASK) >> ICH_VMCR_BPR0_SHIFT;
+}
+
+static unsigned int vgic_v3_get_bpr1(uint32_t vmcr)
+{
+    unsigned int bpr;
+
+    if ( vmcr & ICH_VMCR_CBPR_MASK )
+    {
+        bpr = vgic_v3_get_bpr0(vmcr);
+        if ( bpr < 7 )
+            bpr++;
+    }
+    else
+        bpr = (vmcr & ICH_VMCR_BPR1_MASK) >> ICH_VMCR_BPR1_SHIFT;
+
+    return bpr;
+}
+
+static void vgic_v3_read_bpr1(struct cpu_user_regs *regs, uint32_t vmcr,
+                              int rt)
+{
+    set_user_reg(regs, rt, vgic_v3_get_bpr1(vmcr));
+}
+
+static void vgic_v3_write_bpr1(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    register_t val = get_user_reg(regs, rt);
+    uint8_t bpr_min = vgic_v3_bpr_min();
+
+    if ( vmcr & ICH_VMCR_CBPR_MASK )
+        return;
+
+    /* Enforce BPR limiting */
+    if ( val < bpr_min )
+        val = bpr_min;
+
+    val <<= ICH_VMCR_BPR1_SHIFT;
+    val &= ICH_VMCR_BPR1_MASK;
+    vmcr &= ~ICH_VMCR_BPR1_MASK;
+    vmcr |= val;
+
+    vgic_v3_write_vmcr(vmcr);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -62,6 +119,14 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
 
     switch ( sysreg )
     {
+
+    case HSR_SYSREG_ICC_BPR1_EL1:
+        if ( is_read )
+            fn = vgic_v3_read_bpr1;
+        else
+            fn = vgic_v3_write_bpr1;
+        break;
+
     default:
         ret = false;
         goto end;

index 1811234249..6aa6deedfe 100644
@@ -89,6 +89,7 @@
 #define HSR_SYSREG_ICC_ASGI1R_EL1 HSR_SYSREG(3,1,c12,c11,6)
 #define HSR_SYSREG_ICC_SGI0R_EL1  HSR_SYSREG(3,2,c12,c11,7)
 #define HSR_SYSREG_ICC_SRE_EL1    HSR_SYSREG(3,0,c12,c12,5)
+#define HSR_SYSREG_ICC_BPR1_EL1   HSR_SYSREG(3,0,c12,c12,3)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)

index 10a2aeea93..e247327bf0 100644
@@ -164,6 +164,12 @@
 #define ICH_VMCR_VENG1               (1 << 1)
 #define ICH_VMCR_PRIORITY_MASK       0xff
 #define ICH_VMCR_PRIORITY_SHIFT      24
+#define ICH_VMCR_CBPR_SHIFT          4
+#define ICH_VMCR_CBPR_MASK           (1 << ICH_VMCR_CBPR_SHIFT)
+#define ICH_VMCR_BPR0_SHIFT          21
+#define ICH_VMCR_BPR0_MASK           (7 << ICH_VMCR_BPR0_SHIFT)
+#define ICH_VMCR_BPR1_SHIFT          18
+#define ICH_VMCR_BPR1_MASK           (7 << ICH_VMCR_BPR1_SHIFT)
 
 #define ICH_LR_VIRTUAL_MASK          0xffff
 #define ICH_LR_VIRTUAL_SHIFT         0

index b894398dc6..b49c53d1c5 100644
@@ -93,6 +93,25 @@ static void vgic_v3_write_bpr1(struct cpu_user_regs *regs, uint32_t vmcr,
     vgic_v3_write_vmcr(vmcr);
 }
 
+static void vgic_v3_read_igrpen1(struct cpu_user_regs *regs, uint32_t vmcr,
+                                 int rt)
+{
+    set_user_reg(regs, rt, !!(vmcr & ICH_VMCR_ENG1_MASK));
+}
+
+static void vgic_v3_write_igrpen1(struct cpu_user_regs *regs, uint32_t vmcr,
+                                  int rt)
+{
+    register_t val = get_user_reg(regs, rt);
+
+    if ( val & 1 )
+        vmcr |= ICH_VMCR_ENG1_MASK;
+    else
+        vmcr &= ~ICH_VMCR_ENG1_MASK;
+
+    vgic_v3_write_vmcr(vmcr);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -127,6 +146,13 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_bpr1;
         break;
 
+    case HSR_SYSREG_ICC_IGRPEN1_EL1:
+        if ( is_read )
+            fn = vgic_v3_read_igrpen1;
+        else
+            fn = vgic_v3_write_igrpen1;
+        break;
+
     default:
         ret = false;
         goto end;

index 6aa6deedfe..f4eff66380 100644
@@ -90,6 +90,7 @@
 #define HSR_SYSREG_ICC_SGI0R_EL1  HSR_SYSREG(3,2,c12,c11,7)
 #define HSR_SYSREG_ICC_SRE_EL1    HSR_SYSREG(3,0,c12,c12,5)
 #define HSR_SYSREG_ICC_BPR1_EL1   HSR_SYSREG(3,0,c12,c12,3)
+#define HSR_SYSREG_ICC_IGRPEN1_EL1 HSR_SYSREG(3,0,c12,c12,7)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)

index e247327bf0..8735ba3b1d 100644
@@ -170,6 +170,8 @@
 #define ICH_VMCR_BPR0_MASK           (7 << ICH_VMCR_BPR0_SHIFT)
 #define ICH_VMCR_BPR1_SHIFT          18
 #define ICH_VMCR_BPR1_MASK           (7 << ICH_VMCR_BPR1_SHIFT)
+#define ICH_VMCR_ENG1_SHIFT          1
+#define ICH_VMCR_ENG1_MASK           (1 << ICH_VMCR_ENG1_SHIFT)
 
 #define ICH_LR_VIRTUAL_MASK          0xffff
 #define ICH_LR_VIRTUAL_SHIFT         0

index b49c53d1c5..eea087902c 100644
@@ -39,6 +39,99 @@ static void vgic_v3_write_vmcr(uint32_t vmcr)
     WRITE_SYSREG32(vmcr, ICH_VMCR_EL2);
 }
 
+static void vgic_v3_write_ap0rn(uint32_t val, int n)
+{
+    switch (n)
+    {
+    case 0:
+        WRITE_SYSREG32(val, ICH_AP0R0_EL2);
+        break;
+    case 1:
+        WRITE_SYSREG32(val, ICH_AP0R1_EL2);
+        break;
+    case 2:
+        WRITE_SYSREG32(val, ICH_AP0R2_EL2);
+        break;
+    case 3:
+        WRITE_SYSREG32(val, ICH_AP0R3_EL2);
+        break;
+    default:
+        unreachable();
+    }
+}
+
+static void vgic_v3_write_ap1rn(uint32_t val, int n)
+{
+    switch (n)
+    {
+    case 0:
+        WRITE_SYSREG32(val, ICH_AP1R0_EL2);
+        break;
+    case 1:
+        WRITE_SYSREG32(val, ICH_AP1R1_EL2);
+        break;
+    case 2:
+        WRITE_SYSREG32(val, ICH_AP1R2_EL2);
+        break;
+    case 3:
+        WRITE_SYSREG32(val, ICH_AP1R3_EL2);
+        break;
+    default:
+        unreachable();
+    }
+}
+
+static uint32_t vgic_v3_read_ap0rn(int n)
+{
+    uint32_t val;
+
+    switch (n)
+    {
+    case 0:
+        val = READ_SYSREG32(ICH_AP0R0_EL2);
+        break;
+    case 1:
+        val = READ_SYSREG32(ICH_AP0R1_EL2);
+        break;
+    case 2:
+        val = READ_SYSREG32(ICH_AP0R2_EL2);
+        break;
+    case 3:
+        val = READ_SYSREG32(ICH_AP0R3_EL2);
+        break;
+    default:
+        unreachable();
+    }
+
+    return val;
+}
+
+static uint32_t vgic_v3_read_ap1rn(int n)
+{
+    uint32_t val;
+
+    switch (n)
+    {
+    case 0:
+        val = READ_SYSREG32(ICH_AP1R0_EL2);
+        break;
+    case 1:
+        val = READ_SYSREG32(ICH_AP1R1_EL2);
+        break;
+    case 2:
+        val = READ_SYSREG32(ICH_AP1R2_EL2);
+        break;
+    case 3:
+        val = READ_SYSREG32(ICH_AP1R3_EL2);
+        break;
+    default:
+        unreachable();
+    }
+
+    return val;
+}
+
+
 static int vgic_v3_bpr_min(void)
 {
     /* See Pseudocode for VPriorityGroup */

index eea087902c..8e9318d959 100644
@@ -39,6 +39,89 @@ static void vgic_v3_write_vmcr(uint32_t vmcr)
     WRITE_SYSREG32(vmcr, ICH_VMCR_EL2);
 }
 
+static uint64_t gicv3_ich_read_lr(int lr)
+{
+    switch ( lr )
+    {
+    case 0: return READ_SYSREG(ICH_LR0_EL2);
+    case 1: return READ_SYSREG(ICH_LR1_EL2);
+    case 2: return READ_SYSREG(ICH_LR2_EL2);
+    case 3: return READ_SYSREG(ICH_LR3_EL2);
+    case 4: return READ_SYSREG(ICH_LR4_EL2);
+    case 5: return READ_SYSREG(ICH_LR5_EL2);
+    case 6: return READ_SYSREG(ICH_LR6_EL2);
+    case 7: return READ_SYSREG(ICH_LR7_EL2);
+    case 8: return READ_SYSREG(ICH_LR8_EL2);
+    case 9: return READ_SYSREG(ICH_LR9_EL2);
+    case 10: return READ_SYSREG(ICH_LR10_EL2);
+    case 11: return READ_SYSREG(ICH_LR11_EL2);
+    case 12: return READ_SYSREG(ICH_LR12_EL2);
+    case 13: return READ_SYSREG(ICH_LR13_EL2);
+    case 14: return READ_SYSREG(ICH_LR14_EL2);
+    case 15: return READ_SYSREG(ICH_LR15_EL2);
+    default:
+        unreachable();
+    }
+}
+
+static void gicv3_ich_write_lr(int lr, uint64_t val)
+{
+    switch ( lr )
+    {
+    case 0:
+        WRITE_SYSREG(val, ICH_LR0_EL2);
+        break;
+    case 1:
+        WRITE_SYSREG(val, ICH_LR1_EL2);
+        break;
+    case 2:
+        WRITE_SYSREG(val, ICH_LR2_EL2);
+        break;
+    case 3:
+        WRITE_SYSREG(val, ICH_LR3_EL2);
+        break;
+    case 4:
+        WRITE_SYSREG(val, ICH_LR4_EL2);
+        break;
+    case 5:
+        WRITE_SYSREG(val, ICH_LR5_EL2);
+        break;
+    case 6:
+        WRITE_SYSREG(val, ICH_LR6_EL2);
+        break;
+    case 7:
+        WRITE_SYSREG(val, ICH_LR7_EL2);
+        break;
+    case 8:
+        WRITE_SYSREG(val, ICH_LR8_EL2);
+        break;
+    case 9:
+        WRITE_SYSREG(val, ICH_LR9_EL2);
+        break;
+    case 10:
+        WRITE_SYSREG(val, ICH_LR10_EL2);
+        break;
+    case 11:
+        WRITE_SYSREG(val, ICH_LR11_EL2);
+        break;
+    case 12:
+        WRITE_SYSREG(val, ICH_LR12_EL2);
+        break;
+    case 13:
+        WRITE_SYSREG(val, ICH_LR13_EL2);
+        break;
+    case 14:
+        WRITE_SYSREG(val, ICH_LR14_EL2);
+        break;
+    case 15:
+        WRITE_SYSREG(val, ICH_LR15_EL2);
+        break;
+    default:
+        return;
+    }
+    isb();
+}
+
 static void vgic_v3_write_ap0rn(uint32_t val, int n)
 {
     switch (n)

index 8e9318d959..1b47351bbb 100644
@@ -25,8 +25,17 @@
 #include <asm/regs.h>
 #include <asm/system.h>
 #include <asm/traps.h>
+#include <xen/sched.h>
+#include <asm/vtimer.h>
 
 #define vtr_to_nr_pre_bits(v)     ((((uint32_t)(v) >> 26) & 7) + 1)
+#define vtr_to_nr_apr_regs(v)     (1 << (vtr_to_nr_pre_bits(v) - 5))
+
+#define ESR_ELx_SYS64_ISS_CRM_SHIFT 1
+#define ESR_ELx_SYS64_ISS_CRM_MASK (0xf << ESR_ELx_SYS64_ISS_CRM_SHIFT)
+
+#define ICC_IAR1_EL1_SPURIOUS    0x3ff
+#define VGIC_MAX_SPI             1019
 
 /* Provide wrappers to read write VMCR similar to linux */
 static uint64_t vgic_v3_read_vmcr(void)

@@ -288,6 +297,172 @@ static void vgic_v3_write_igrpen1(struct cpu_user_regs *regs, uint32_t vmcr,
     vgic_v3_write_vmcr(vmcr);
 }
 
+static int vgic_v3_get_group(const union hsr hsr)
+{
+    uint8_t crm = (hsr.bits & ESR_ELx_SYS64_ISS_CRM_MASK) >>
+                   ESR_ELx_SYS64_ISS_CRM_SHIFT;
+
+    return crm != 8;
+}
+
+static int vgic_v3_highest_priority_lr(struct cpu_user_regs *regs,
+                                       uint32_t vmcr, uint64_t *lr_val)
+{
+    unsigned int i, lr = -1;
+    unsigned int used_lrs =  gic_get_nr_lrs();
+    uint8_t priority = GICV3_IDLE_PRIORITY;
+
+    for ( i = 0; i < used_lrs; i++ )
+    {
+        uint64_t val =  gicv3_ich_read_lr(i);
+        uint8_t lr_prio = (val & ICH_LR_PRIORITY_MASK) >> ICH_LR_PRIORITY_SHIFT;
+
+        /* Not pending in the state? */
+        if ( (val & ICH_LR_STATE) != ICH_LR_PENDING_BIT )
+            continue;
+
+        /* Group-0 interrupt, but Group-0 disabled? */
+        if ( !(val & ICH_LR_GROUP) && !(vmcr & ICH_VMCR_ENG0_MASK) )
+            continue;
+
+        /* Group-1 interrupt, but Group-1 disabled? */
+        if ( (val & ICH_LR_GROUP) && !(vmcr & ICH_VMCR_ENG1_MASK) )
+            continue;
+
+        /* Not the highest priority? */
+        if ( lr_prio >= priority )
+            continue;
+
+        /* This is a candidate */
+        priority = lr_prio;
+        *lr_val = val;
+        lr = i;
+    }
+
+    if ( lr == -1 )
+        *lr_val = ICC_IAR1_EL1_SPURIOUS;
+
+    return lr;
+}
+
+static int vgic_v3_get_highest_active_priority(void)
+{
+    unsigned int i;
+    uint32_t hap = 0;
+    uint8_t nr_apr_regs = vtr_to_nr_apr_regs(READ_SYSREG32(ICH_VTR_EL2));
+
+    for ( i = 0; i < nr_apr_regs; i++ )
+    {
+        uint32_t val;
+
+        /*
+         * The ICH_AP0Rn_EL2 and ICH_AP1Rn_EL2 registers
+         * contain the active priority levels for this VCPU
+         * for the maximum number of supported priority
+         * levels, and we return the full priority level only
+         * if the BPR is programmed to its minimum, otherwise
+         * we return a combination of the priority level and
+         * subpriority, as determined by the setting of the
+         * BPR, but without the full subpriority.
+         */
+        val  = vgic_v3_read_ap0rn(i);
+        val |= vgic_v3_read_ap1rn(i);
+        if ( !val )
+        {
+            hap += 32;
+            continue;
+        }
+
+        return (hap +  __ffs(val)) << vgic_v3_bpr_min();
+    }
+
+    return GICV3_IDLE_PRIORITY;
+}
+
+/*
+ * Convert a priority to a preemption level, taking the relevant BPR
+ * into account by zeroing the sub-priority bits.
+ */
+static uint8_t vgic_v3_pri_to_pre(uint8_t pri, uint32_t vmcr, int grp)
+{
+    unsigned int bpr;
+
+    if ( !grp )
+        bpr = vgic_v3_get_bpr0(vmcr) + 1;
+    else
+        bpr = vgic_v3_get_bpr1(vmcr);
+
+    return pri & (GENMASK(7, 0) << bpr);
+}
+
+/*
+ * The priority value is independent of any of the BPR values, so we
+ * normalize it using the minumal BPR value. This guarantees that no
+ * matter what the guest does with its BPR, we can always set/get the
+ * same value of a priority.
+ */
+static void vgic_v3_set_active_priority(uint8_t pri, uint32_t vmcr, int grp)
+{
+    uint8_t pre, ap;
+    uint32_t val;
+    int apr;
+
+    pre = vgic_v3_pri_to_pre(pri, vmcr, grp);
+    ap = pre >> vgic_v3_bpr_min();
+    apr = ap / 32;
+
+    if ( !grp )
+    {
+        val = vgic_v3_read_ap0rn(apr);
+        vgic_v3_write_ap0rn(val | BIT(ap % 32), apr);
+    }
+    else
+    {
+        val = vgic_v3_read_ap1rn(apr);
+        vgic_v3_write_ap1rn(val | BIT(ap % 32), apr);
+    }
+}
+
+static void vgic_v3_read_iar(struct cpu_user_regs *regs, uint32_t vmcr, int rt)
+{
+    uint64_t lr_val;
+    uint8_t lr_prio, pmr;
+    int lr, grp;
+    const union hsr hsr = { .bits = regs->hsr };
+
+    grp = vgic_v3_get_group(hsr);
+
+    lr = vgic_v3_highest_priority_lr(regs, vmcr, &lr_val);
+    if ( lr < 0 )
+        goto spurious;
+
+    if ( grp != !!(lr_val & ICH_LR_GROUP) )
+        goto spurious;
+
+    pmr = (vmcr & ICH_VMCR_PMR_MASK) >> ICH_VMCR_PMR_SHIFT;
+    lr_prio = (lr_val & ICH_LR_PRIORITY_MASK) >> ICH_LR_PRIORITY_SHIFT;
+    if ( pmr <= lr_prio )
+        goto spurious;
+
+    if ( vgic_v3_get_highest_active_priority() <=
+         vgic_v3_pri_to_pre(lr_prio, vmcr, grp) )
+        goto spurious;
+
+    lr_val &= ~ICH_LR_STATE;
+    /* No active state for LPIs */
+    if ( (lr_val & ICH_LR_VIRTUAL_ID_MASK) <= VGIC_MAX_SPI )
+        lr_val |= ICH_LR_ACTIVE_BIT;
+
+    gicv3_ich_write_lr(lr, lr_val);
+    vgic_v3_set_active_priority(lr_prio, vmcr, grp);
+    set_user_reg(regs, rt,  lr_val & ICH_LR_VIRTUAL_ID_MASK);
+
+    return;
+
+spurious:
+     set_user_reg(regs, rt, ICC_IAR1_EL1_SPURIOUS);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -329,6 +504,10 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_igrpen1;
         break;
 
+    case HSR_SYSREG_ICC_IAR1_EL1:
+        fn = vgic_v3_read_iar;
+        break;
+
     default:
         ret = false;
         goto end;

index f4eff66380..dc8bcabe4c 100644
@@ -91,6 +91,7 @@
 #define HSR_SYSREG_ICC_SRE_EL1    HSR_SYSREG(3,0,c12,c12,5)
 #define HSR_SYSREG_ICC_BPR1_EL1   HSR_SYSREG(3,0,c12,c12,3)
 #define HSR_SYSREG_ICC_IGRPEN1_EL1 HSR_SYSREG(3,0,c12,c12,7)
+#define HSR_SYSREG_ICC_IAR1_EL1   HSR_SYSREG(3,0,c12,c12,0)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)

index 58b910fe6a..15ad578e98 100644
@@ -84,6 +84,7 @@
 
 #define GICH_HCR        (0x00)
 #define GICH_VTR        (0x04)
+#define GICH_VTR_NRLRGS  0x3f
 #define GICH_VMCR       (0x08)
 #define GICH_MISR       (0x10)
 #define GICH_EISR0      (0x20)

index 8735ba3b1d..50316477bb 100644
@@ -69,6 +69,7 @@
  */
 #define GICV3_GICD_IIDR_VAL          0x34c
 #define GICV3_GICR_IIDR_VAL          GICV3_GICD_IIDR_VAL
+#define GICV3_IDLE_PRIORITY          0xff
 
 /* Two pages for the RD_base and SGI_base register frame. */
 #define GICV3_GICR_SIZE              (2 * SZ_64K)

@@ -172,6 +173,10 @@
 #define ICH_VMCR_BPR1_MASK           (7 << ICH_VMCR_BPR1_SHIFT)
 #define ICH_VMCR_ENG1_SHIFT          1
 #define ICH_VMCR_ENG1_MASK           (1 << ICH_VMCR_ENG1_SHIFT)
+#define ICH_VMCR_ENG0_SHIFT          0
+#define ICH_VMCR_ENG0_MASK           (1 << ICH_VMCR_ENG0_SHIFT)
+#define ICH_VMCR_PMR_SHIFT           24
+#define ICH_VMCR_PMR_MASK            (0xffUL << ICH_VMCR_PMR_SHIFT)
 
 #define ICH_LR_VIRTUAL_MASK          0xffff
 #define ICH_LR_VIRTUAL_SHIFT         0

@@ -192,6 +197,14 @@
 #define ICH_LR_MAINTENANCE_IRQ       (1UL<<41)
 #define ICH_LR_GRP1                  (1UL<<60)
 #define ICH_LR_HW                    (1UL<<61)
+#define ICH_LR_EOI                   (1ULL << 41)
+#define ICH_LR_GROUP                 (1ULL << 60)
+#define ICH_LR_STATE                 (3ULL << 62)
+#define ICH_LR_PENDING_BIT           (1ULL << 62)
+#define ICH_LR_ACTIVE_BIT            (1ULL << 63)
+#define ICH_LR_PHYS_ID_SHIFT         32
+#define ICH_LR_PHYS_ID_MASK          (0x3ffULL << ICH_LR_PHYS_ID_SHIFT)
+#define ICH_LR_VIRTUAL_ID_MASK       ((1ULL << 32) - 1)
 
 #define ICH_VTR_NRLRGS               0x3f
 #define ICH_VTR_PRIBITS_MASK         0x7

index 1b47351bbb..2192547cc4 100644
@@ -21,6 +21,7 @@
  */
 
 #include <asm/current.h>
+#include <asm-arm/gic.h>
 #include <asm/gic_v3_defs.h>
 #include <asm/regs.h>
 #include <asm/system.h>

@@ -36,6 +37,7 @@
 
 #define ICC_IAR1_EL1_SPURIOUS    0x3ff
 #define VGIC_MAX_SPI             1019
+#define VGIC_MIN_LPI             8192
 
 /* Provide wrappers to read write VMCR similar to linux */
 static uint64_t vgic_v3_read_vmcr(void)

@@ -463,6 +465,134 @@ spurious:
      set_user_reg(regs, rt, ICC_IAR1_EL1_SPURIOUS);
 }
 
+static int vgic_v3_find_active_lr(int intid, uint64_t *lr_val)
+{
+    int i;
+    unsigned int used_lr;
+    unsigned int nr_lrs =  gic_get_nr_lrs();
+    unsigned long *lr_mask = (unsigned long *) &this_cpu(lr_mask);
+
+    for_each_set_bit(used_lr, lr_mask, nr_lrs)
+    {
+        uint64_t val = gicv3_ich_read_lr(used_lr);
+
+        if ( (val & ICH_LR_VIRTUAL_ID_MASK) == intid &&
+            (val & ICH_LR_ACTIVE_BIT) )
+        {
+            *lr_val = val;
+            return i;
+        }
+    }
+
+    *lr_val = ICC_IAR1_EL1_SPURIOUS;
+    return -1;
+}
+
+static int vgic_v3_clear_highest_active_priority(void)
+{
+    int i;
+    uint32_t hap = 0;
+    uint8_t nr_apr_regs = vtr_to_nr_apr_regs(READ_SYSREG32(ICH_VTR_EL2));
+
+    for ( i = 0; i < nr_apr_regs; i++ )
+    {
+        uint32_t ap0, ap1;
+        int c0, c1;
+
+        ap0 = vgic_v3_read_ap0rn(i);
+        ap1 = vgic_v3_read_ap1rn(i);
+        if ( !ap0 && !ap1 )
+        {
+            hap += 32;
+            continue;
+        }
+
+        c0 = ap0 ? __ffs(ap0) : 32;
+        c1 = ap1 ? __ffs(ap1) : 32;
+
+        /* Always clear the LSB, which is the highest priority */
+        if ( c0 < c1 )
+        {
+            ap0 &= ~BIT(c0);
+            vgic_v3_write_ap0rn(ap0, i);
+            hap += c0;
+        }
+        else
+        {
+            ap1 &= ~BIT(c1);
+            vgic_v3_write_ap1rn(ap1, i);
+            hap += c1;
+        }
+
+        /* Rescale to 8 bits of priority */
+        return hap << vgic_v3_bpr_min();
+    }
+
+    return GICV3_IDLE_PRIORITY;
+}
+
+static void vgic_v3_clear_active_lr(int lr, uint64_t lr_val)
+{
+    lr_val &= ~ICH_LR_ACTIVE_BIT;
+    if ( lr_val & ICH_LR_HW )
+    {
+        uint32_t pid;
+
+        pid = (lr_val & ICH_LR_PHYS_ID_MASK) >> ICH_LR_PHYS_ID_SHIFT;
+        WRITE_SYSREG32(pid, ICC_DIR_EL1);
+    }
+    gicv3_ich_write_lr(lr, lr_val);
+}
+
+static void vgic_v3_bump_eoicount(void)
+{
+    uint32_t hcr;
+
+    hcr = READ_SYSREG32(ICH_HCR_EL2);
+    hcr += 1 << ICH_HCR_EOIcount_SHIFT;
+    WRITE_SYSREG32(hcr, ICH_HCR_EL2);
+}
+
+static void vgic_v3_write_eoir(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    uint64_t lr_val;
+    uint8_t lr_prio, act_prio;
+    int lr, grp;
+    const union hsr hsr = { .bits = regs->hsr };
+    register_t vid = get_user_reg(regs, hsr.sysreg.reg);
+
+    grp = vgic_v3_get_group(hsr);
+
+    /* Drop priority in any case */
+    act_prio = vgic_v3_clear_highest_active_priority();
+
+    /* If EOIing an LPI, no deactivate to be performed */
+    if ( vid >= VGIC_MIN_LPI )
+        return;
+
+    /* EOImode == 1, nothing to be done here */
+    if ( vmcr & ICH_VMCR_EOIM_MASK )
+        return;
+
+    lr = vgic_v3_find_active_lr(vid, &lr_val);
+    if ( lr == -1 )
+    {
+        vgic_v3_bump_eoicount();
+        return;
+    }
+
+    lr_prio = (lr_val & ICH_LR_PRIORITY_MASK) >> ICH_LR_PRIORITY_SHIFT;
+
+    /* If priorities or group do not match, the guest has fscked-up. */
+    if ( grp != !!(lr_val & ICH_LR_GROUP) ||
+         vgic_v3_pri_to_pre(lr_prio, vmcr, grp) != act_prio )
+        return;
+
+    /* Let's now perform the deactivation */
+    vgic_v3_clear_active_lr(lr, lr_val);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -508,6 +638,10 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
         fn = vgic_v3_read_iar;
         break;
 
+    case HSR_SYSREG_ICC_EOIR1_EL1:
+        fn = vgic_v3_write_eoir;
+        break;
+
     default:
         ret = false;
         goto end;

index dc8bcabe4c..f7422f1649 100644
@@ -92,6 +92,7 @@
 #define HSR_SYSREG_ICC_BPR1_EL1   HSR_SYSREG(3,0,c12,c12,3)
 #define HSR_SYSREG_ICC_IGRPEN1_EL1 HSR_SYSREG(3,0,c12,c12,7)
 #define HSR_SYSREG_ICC_IAR1_EL1   HSR_SYSREG(3,0,c12,c12,0)
+#define HSR_SYSREG_ICC_EOIR1_EL1  HSR_SYSREG(3,0,c12,c12,1)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)

index 50316477bb..52af0466df 100644
@@ -177,6 +177,10 @@
 #define ICH_VMCR_ENG0_MASK           (1 << ICH_VMCR_ENG0_SHIFT)
 #define ICH_VMCR_PMR_SHIFT           24
 #define ICH_VMCR_PMR_MASK            (0xffUL << ICH_VMCR_PMR_SHIFT)
+#define ICH_VMCR_EOIM_SHIFT          9
+#define ICH_VMCR_EOIM_MASK           (1 << ICH_VMCR_EOIM_SHIFT)
+#define ICH_HCR_EOIcount_SHIFT       27
+#define ICH_HCR_EOIcount_MASK        (0x1f << ICH_HCR_EOIcount_SHIFT)
 
 #define ICH_LR_VIRTUAL_MASK          0xffff
 #define ICH_LR_VIRTUAL_SHIFT         0

index 2192547cc4..c067278499 100644
@@ -593,6 +593,27 @@ static void vgic_v3_write_eoir(struct cpu_user_regs *regs, uint32_t vmcr,
     vgic_v3_clear_active_lr(lr, lr_val);
 }
 
+static void vgic_v3_read_hppir(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    uint64_t lr_val;
+    int lr, lr_grp, grp;
+    const union hsr hsr = { .bits = regs->hsr };
+
+    grp = vgic_v3_get_group(hsr);
+    lr = vgic_v3_highest_priority_lr(regs, vmcr, &lr_val);
+
+    if ( lr == -1 )
+        goto spurious;
+
+    lr_grp = !!(lr_val & ICH_LR_GROUP);
+    if ( lr_grp != grp )
+        lr_val = ICC_IAR1_EL1_SPURIOUS;
+
+spurious:
+    set_user_reg(regs, rt, lr_val & ICH_LR_VIRTUAL_ID_MASK);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -642,6 +663,10 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
         fn = vgic_v3_write_eoir;
         break;
 
+    case HSR_SYSREG_ICC_HPPIR1_EL1:
+        fn = vgic_v3_read_hppir;
+        break;
+
     default:
         ret = false;
         goto end;

index f7422f1649..b9230fe795 100644
@@ -93,6 +93,7 @@
 #define HSR_SYSREG_ICC_IGRPEN1_EL1 HSR_SYSREG(3,0,c12,c12,7)
 #define HSR_SYSREG_ICC_IAR1_EL1   HSR_SYSREG(3,0,c12,c12,0)
 #define HSR_SYSREG_ICC_EOIR1_EL1  HSR_SYSREG(3,0,c12,c12,1)
+#define HSR_SYSREG_ICC_HPPIR1_EL1 HSR_SYSREG(3,0,c12,c12,2)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)

index c067278499..f3b6e5367c 100644
@@ -280,6 +280,33 @@ static void vgic_v3_write_bpr1(struct cpu_user_regs *regs, uint32_t vmcr,
     vgic_v3_write_vmcr(vmcr);
 }
 
+static void vgic_v3_read_bpr0(struct cpu_user_regs *regs, uint32_t vmcr,
+                              int rt)
+{
+    set_user_reg(regs, rt, vgic_v3_get_bpr0(vmcr));
+}
+
+static void vgic_v3_write_bpr0(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    register_t val = get_user_reg(regs, rt);
+    uint8_t bpr_min = vgic_v3_bpr_min();
+
+    if ( vmcr & ICH_VMCR_CBPR_MASK )
+        return;
+
+    /* Enforce BPR limiting */
+    if ( val < bpr_min )
+        val = bpr_min;
+
+    val <<= ICH_VMCR_BPR0_SHIFT;
+    val &= ICH_VMCR_BPR0_MASK;
+    vmcr &= ~ICH_VMCR_BPR0_MASK;
+    vmcr |= val;
+
+    vgic_v3_write_vmcr(vmcr);
+}
+
 static void vgic_v3_read_igrpen1(struct cpu_user_regs *regs, uint32_t vmcr,
                                  int rt)
 {

@@ -641,6 +668,13 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
     switch ( sysreg )
     {
 
+    case HSR_SYSREG_ICC_BPR0_EL1:
+        if ( is_read )
+            fn = vgic_v3_read_bpr0;
+        else
+            fn = vgic_v3_write_bpr0;
+        break;
+
     case HSR_SYSREG_ICC_BPR1_EL1:
         if ( is_read )
             fn = vgic_v3_read_bpr1;

index b9230fe795..e03b6edf4d 100644
@@ -94,6 +94,7 @@
 #define HSR_SYSREG_ICC_IAR1_EL1   HSR_SYSREG(3,0,c12,c12,0)
 #define HSR_SYSREG_ICC_EOIR1_EL1  HSR_SYSREG(3,0,c12,c12,1)
 #define HSR_SYSREG_ICC_HPPIR1_EL1 HSR_SYSREG(3,0,c12,c12,2)
+#define HSR_SYSREG_ICC_BPR0_EL1	  HSR_SYSREG(3,0,c12,c8,3)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)

index f3b6e5367c..0fd3f929e4 100644
@@ -641,6 +641,25 @@ spurious:
     set_user_reg(regs, rt, lr_val & ICH_LR_VIRTUAL_ID_MASK);
 }
 
+static void vgic_v3_read_igrpen0(struct cpu_user_regs *regs, uint32_t vmcr,
+                                 int rt)
+{
+    set_user_reg(regs, rt, !!(vmcr & ICH_VMCR_ENG0_MASK));
+}
+
+static void vgic_v3_write_igrpen0(struct cpu_user_regs *regs, uint32_t vmcr,
+                                  int rt)
+{
+    register_t val = get_user_reg(regs, rt);
+
+    if ( val & 1 )
+        vmcr |= ICH_VMCR_ENG0_MASK;
+    else
+        vmcr &= ~ICH_VMCR_ENG0_MASK;
+
+    WRITE_SYSREG32(vmcr, ICH_VMCR_EL2);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -701,6 +720,13 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
         fn = vgic_v3_read_hppir;
         break;
 
+    case HSR_SYSREG_ICC_IGRPEN0_EL1:
+        if (is_read)
+            fn = vgic_v3_read_igrpen0;
+        else
+            fn = vgic_v3_write_igrpen0;
+        break;
+
     default:
         ret = false;
         goto end;

index e03b6edf4d..3c4b5587a1 100644
@@ -90,6 +90,7 @@
 #define HSR_SYSREG_ICC_SGI0R_EL1  HSR_SYSREG(3,2,c12,c11,7)
 #define HSR_SYSREG_ICC_SRE_EL1    HSR_SYSREG(3,0,c12,c12,5)
 #define HSR_SYSREG_ICC_BPR1_EL1   HSR_SYSREG(3,0,c12,c12,3)
+#define HSR_SYSREG_ICC_IGRPEN0_EL1 HSR_SYSREG(3,0,c12,c12,6)
 #define HSR_SYSREG_ICC_IGRPEN1_EL1 HSR_SYSREG(3,0,c12,c12,7)
 #define HSR_SYSREG_ICC_IAR1_EL1   HSR_SYSREG(3,0,c12,c12,0)
 #define HSR_SYSREG_ICC_EOIR1_EL1  HSR_SYSREG(3,0,c12,c12,1)

index 0fd3f929e4..a09d7544f3 100644
@@ -660,6 +660,78 @@ static void vgic_v3_write_igrpen0(struct cpu_user_regs *regs, uint32_t vmcr,
     WRITE_SYSREG32(vmcr, ICH_VMCR_EL2);
 }
 
+static void vgic_v3_read_apxrn(struct cpu_user_regs *regs, int rt, int n)
+{
+    uint32_t val;
+    const union hsr hsr = { .bits = regs->hsr };
+
+    if ( !vgic_v3_get_group(hsr) )
+        val = vgic_v3_read_ap0rn(n);
+    else
+        val = vgic_v3_read_ap1rn(n);
+
+    set_user_reg(regs, rt, val);
+}
+
+static void vgic_v3_write_apxrn(struct cpu_user_regs *regs, int rt, int n)
+{
+    u32 val = get_user_reg(regs, rt);
+    const union hsr hsr = { .bits = regs->hsr };
+
+    if ( !vgic_v3_get_group(hsr) )
+        vgic_v3_write_ap0rn(val, n);
+    else
+        vgic_v3_write_ap1rn(val, n);
+}
+
+static void vgic_v3_read_apxr0(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    vgic_v3_read_apxrn(regs, rt, 0);
+}
+
+static void vgic_v3_read_apxr1(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    vgic_v3_read_apxrn(regs, rt, 1);
+}
+
+static void vgic_v3_read_apxr2(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    vgic_v3_read_apxrn(regs, rt, 2);
+}
+
+static void vgic_v3_read_apxr3(struct cpu_user_regs *regs, uint32_t vmcr,
+                               int rt)
+{
+    vgic_v3_read_apxrn(regs, rt, 3);
+}
+
+static void vgic_v3_write_apxr0(struct cpu_user_regs *regs, uint32_t vmcr,
+                                int rt)
+{
+    vgic_v3_write_apxrn(regs, rt, 0);
+}
+
+static void vgic_v3_write_apxr1(struct cpu_user_regs *regs, uint32_t vmcr,
+                                int rt)
+{
+    vgic_v3_write_apxrn(regs, rt, 1);
+}
+
+static void vgic_v3_write_apxr2(struct cpu_user_regs *regs, uint32_t vmcr,
+                                int rt)
+{
+    vgic_v3_write_apxrn(regs, rt, 2);
+}
+
+static void vgic_v3_write_apxr3(struct cpu_user_regs *regs, uint32_t vmcr,
+                                int rt)
+{
+    vgic_v3_write_apxrn(regs, rt, 3);
+}
+
 /* vgic_v3_handle_cpuif_access
  * returns: true if the register is emulated
  *          false if not a sysreg

@@ -727,6 +799,34 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_igrpen0;
         break;
 
+    case HSR_SYSREG_ICC_AP1Rn_EL1(0):
+        if (is_read)
+            fn = vgic_v3_read_apxr0;
+        else
+            fn = vgic_v3_write_apxr0;
+        break;
+
+    case HSR_SYSREG_ICC_AP1Rn_EL1(1):
+        if (is_read)
+            fn = vgic_v3_read_apxr1;
+        else
+            fn = vgic_v3_write_apxr1;
+        break;
+
+    case HSR_SYSREG_ICC_AP1Rn_EL1(2):
+        if (is_read)
+            fn = vgic_v3_read_apxr2;
+        else
+            fn = vgic_v3_write_apxr2;
+        break;
+
+    case HSR_SYSREG_ICC_AP1Rn_EL1(3):
+        if (is_read)
+            fn = vgic_v3_read_apxr3;
+        else
+            fn = vgic_v3_write_apxr3;
+        break;
+
     default:
         ret = false;
         goto end;

index 3c4b5587a1..0e583f820d 100644
@@ -97,6 +97,7 @@
 #define HSR_SYSREG_ICC_HPPIR1_EL1 HSR_SYSREG(3,0,c12,c12,2)
 #define HSR_SYSREG_ICC_BPR0_EL1	  HSR_SYSREG(3,0,c12,c8,3)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
+#define HSR_SYSREG_ICC_AP1Rn_EL1(n) HSR_SYSREG(3,0,c12,c9, n)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)
 #define HSR_SYSREG_PMCNTENSET_EL0 HSR_SYSREG(3,3,c9,c12,1)

index a09d7544f3..27f5139dcb 100644
@@ -780,14 +780,17 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_igrpen1;
         break;
 
+    case HSR_SYSREG_ICC_IAR0_EL1:
     case HSR_SYSREG_ICC_IAR1_EL1:
         fn = vgic_v3_read_iar;
         break;
 
+    case HSR_SYSREG_ICC_EOIR0_EL1:
     case HSR_SYSREG_ICC_EOIR1_EL1:
         fn = vgic_v3_write_eoir;
         break;
 
+    case HSR_SYSREG_ICC_HPPIR0_EL1:
     case HSR_SYSREG_ICC_HPPIR1_EL1:
         fn = vgic_v3_read_hppir;
         break;

@@ -799,6 +802,7 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_igrpen0;
         break;
 
+    case HSR_SYSREG_ICC_AP0Rn_EL1(0):
     case HSR_SYSREG_ICC_AP1Rn_EL1(0):
         if (is_read)
             fn = vgic_v3_read_apxr0;

@@ -806,6 +810,7 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_apxr0;
         break;
 
+    case HSR_SYSREG_ICC_AP0Rn_EL1(1):
     case HSR_SYSREG_ICC_AP1Rn_EL1(1):
         if (is_read)
             fn = vgic_v3_read_apxr1;

@@ -813,6 +818,7 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_apxr1;
         break;
 
+    case HSR_SYSREG_ICC_AP0Rn_EL1(2):
     case HSR_SYSREG_ICC_AP1Rn_EL1(2):
         if (is_read)
             fn = vgic_v3_read_apxr2;

@@ -820,6 +826,7 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
             fn = vgic_v3_write_apxr2;
         break;
 
+    case HSR_SYSREG_ICC_AP0Rn_EL1(3):
     case HSR_SYSREG_ICC_AP1Rn_EL1(3):
         if (is_read)
             fn = vgic_v3_read_apxr3;

index 0e583f820d..f83326943a 100644
@@ -95,9 +95,13 @@
 #define HSR_SYSREG_ICC_IAR1_EL1   HSR_SYSREG(3,0,c12,c12,0)
 #define HSR_SYSREG_ICC_EOIR1_EL1  HSR_SYSREG(3,0,c12,c12,1)
 #define HSR_SYSREG_ICC_HPPIR1_EL1 HSR_SYSREG(3,0,c12,c12,2)
+#define HSR_SYSREG_ICC_IAR0_EL1   HSR_SYSREG(3,0,c12,c8,0)
+#define HSR_SYSREG_ICC_EOIR0_EL1  HSR_SYSREG(3,0,c12,c8,1)
+#define HSR_SYSREG_ICC_HPPIR0_EL1 HSR_SYSREG(3,0,c12,c8,2)
 #define HSR_SYSREG_ICC_BPR0_EL1	  HSR_SYSREG(3,0,c12,c8,3)
 #define HSR_SYSREG_CONTEXTIDR_EL1 HSR_SYSREG(3,0,c13,c0,1)
 #define HSR_SYSREG_ICC_AP1Rn_EL1(n) HSR_SYSREG(3,0,c12,c9, n)
+#define HSR_SYSREG_ICC_AP0Rn_EL1(n) HSR_SYSREG(3,0,c12,c8,4|n)
 
 #define HSR_SYSREG_PMCR_EL0       HSR_SYSREG(3,3,c9,c12,0)
 #define HSR_SYSREG_PMCNTENSET_EL0 HSR_SYSREG(3,3,c9,c12,1)

index 222a02dd99..2f75131333 100644
@@ -43,6 +43,7 @@
 })
 
 #define ARM_CPU_IMP_ARM             0x41
+#define ARM_CPU_IMP_CAVIUM          0x43
 
 #define ARM_CPU_PART_CORTEX_A12     0xC0D
 #define ARM_CPU_PART_CORTEX_A17     0xC0E

@@ -53,6 +54,10 @@
 #define ARM_CPU_PART_CORTEX_A73     0xD09
 #define ARM_CPU_PART_CORTEX_A75     0xD0A
 
+#define CAVIUM_CPU_PART_THUNDERX      0x0A1
+#define CAVIUM_CPU_PART_THUNDERX_81XX 0x0A2
+#define CAVIUM_CPU_PART_THUNDERX_83XX 0x0A3
+
 #define MIDR_CORTEX_A12 MIDR_CPU_MODEL(ARM_CPU_IMP_ARM, ARM_CPU_PART_CORTEX_A12)
 #define MIDR_CORTEX_A17 MIDR_CPU_MODEL(ARM_CPU_IMP_ARM, ARM_CPU_PART_CORTEX_A17)
 #define MIDR_CORTEX_A15 MIDR_CPU_MODEL(ARM_CPU_IMP_ARM, ARM_CPU_PART_CORTEX_A15)

@@ -62,6 +67,10 @@
 #define MIDR_CORTEX_A73 MIDR_CPU_MODEL(ARM_CPU_IMP_ARM, ARM_CPU_PART_CORTEX_A73)
 #define MIDR_CORTEX_A75 MIDR_CPU_MODEL(ARM_CPU_IMP_ARM, ARM_CPU_PART_CORTEX_A75)
 
+#define MIDR_THUNDERX      MIDR_CPU_MODEL(ARM_CPU_IMP_CAVIUM, CAVIUM_CPU_PART_THUNDERX)
+#define MIDR_THUNDERX_81XX MIDR_CPU_MODEL(ARM_CPU_IMP_CAVIUM, CAVIUM_CPU_PART_THUNDERX_81XX)
+#define MIDR_THUNDERX_83XX MIDR_CPU_MODEL(ARM_CPU_IMP_CAVIUM, CAVIUM_CPU_PART_THUNDERX_83XX)
+
 /* MPIDR Multiprocessor Affinity Register */
 #define _MPIDR_UP           (30)
 #define MPIDR_UP            (_AC(1,U) << _MPIDR_UP)

index 1baa20654b..6095566085 100644
@@ -271,6 +271,27 @@ static const struct arm_cpu_capabilities arm_errata[] = {
         MIDR_RANGE(MIDR_CORTEX_A53, 0x00, 0x02),
     },
 #endif
+#ifdef CONFIG_CAVIUM_ERRATUM_30115
+    {
+        /* Cavium ThunderX, T88 pass 1.x - 2.2 */
+        .desc = "Cavium erratum 30115",
+        .capability = ARM64_WORKAROUND_CAVIUM_30115,
+        MIDR_RANGE(MIDR_THUNDERX, 0x00,
+                   (1 << MIDR_VARIANT_SHIFT) | 2),
+    },
+    {
+        /* Cavium ThunderX, T81 pass 1.0 - 1.2 */
+        .desc = "Cavium erratum 30115",
+        .capability = ARM64_WORKAROUND_CAVIUM_30115,
+        MIDR_RANGE(MIDR_THUNDERX_81XX, 0x00, 0x02),
+    },
+    {
+        /* Cavium ThunderX, T83 pass 1.0 */
+        .desc = "Cavium erratum 30115",
+        .capability = ARM64_WORKAROUND_CAVIUM_30115,
+        MIDR_RANGE(MIDR_THUNDERX_83XX, 0x00, 0x00),
+    },
+#endif
 #ifdef CONFIG_ARM64_ERRATUM_819472
     {
         /* Cortex-A53 r0[01] */

index 4e45b237c8..fd7135cbe8 100644
@@ -27,6 +27,7 @@ static inline bool check_workaround_##erratum(void)             \
 
 CHECK_WORKAROUND_HELPER(766422, ARM32_WORKAROUND_766422, CONFIG_ARM_32)
 CHECK_WORKAROUND_HELPER(834220, ARM64_WORKAROUND_834220, CONFIG_ARM_64)
+CHECK_WORKAROUND_HELPER(cavium_30115, ARM64_WORKAROUND_CAVIUM_30115, CONFIG_ARM_64)
 
 #undef CHECK_WORKAROUND_HELPER
 

index e557a095af..75f32802bd 100644
@@ -43,8 +43,9 @@
 #define SKIP_SYNCHRONIZE_SERROR_ENTRY_EXIT 5
 #define SKIP_CTXT_SWITCH_SERROR_SYNC 6
 #define ARM_HARDEN_BRANCH_PREDICTOR 7
+#define ARM64_WORKAROUND_CAVIUM_30115 8
 
-#define ARM_NCAPS           8
+#define ARM_NCAPS           9
 
 #ifndef __ASSEMBLY__
 

index 5c18e918b0..bc851d5e26 100644
@@ -2044,6 +2044,27 @@ void do_trap_guest_sync(struct cpu_user_regs *regs)
 {
     const union hsr hsr = { .bits = regs->hsr };
 
+    if ( check_workaround_cavium_30115() )
+    {
+        if ( vgic_v3_handle_cpuif_access(regs) )
+        {
+	        /*
+            * if true, g0/g1 vgic register trap is emulated for errata
+	         * so rest of handling of do_trap_guest_sync is not required.
+	         */
+            advance_pc(regs, hsr);
+            /*
+             * enter_hypervisor_head is not invoked when workaround 30115
+             * is in place. enter_hypervisor_head and leave_hypervisor_tail
+             * are invoked in sync, if one is not called other one should be
+             * skipped, otherwise guest vGIC state be out-of-date.
+             */
+            get_cpu_info()->skip_hyp_tail = true;
+
+            return;
+        }
+    }
+
     enter_hypervisor_head(regs);
 
     switch (hsr.ec) {

@@ -2242,6 +2263,16 @@ void do_trap_fiq(struct cpu_user_regs *regs)
 
 void leave_hypervisor_tail(void)
 {
+    /*
+     * if skip_hyp_tail is set simply retrun;
+     */
+    if ( unlikely(get_cpu_info()->skip_hyp_tail) )
+    {
+        /* clear it, so that it is false when not handling g0/g1 traps */
+        get_cpu_info()->skip_hyp_tail = false;
+        return;
+    }
+
     while (1)
     {
         local_irq_disable();

index 7a0971fdea..6dce06289f 100644
@@ -21,7 +21,14 @@ DECLARE_PER_CPU(struct vcpu *, curr_vcpu);
 struct cpu_info {
     struct cpu_user_regs guest_cpu_user_regs;
     unsigned long elr;
-    unsigned int pad;
+/*
+ * Flag is used to skip leave_hypervisor_tail when enter_hypervisor_head
+ * is not invoked. enter_hypervisor_head andleave_hypervisor_tail are
+ * invoked in sync, if one is not called other one should be skipped,
+ * otherwise guest vGIC state be out-of-date.
+ */
+    bool skip_hyp_tail:1;
+    unsigned int pad:31;
 };
 
 static inline struct cpu_info *get_cpu_info(void)

index 27f5139dcb..ac39eb5e1d 100644
@@ -782,6 +782,11 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
 
     case HSR_SYSREG_ICC_IAR0_EL1:
     case HSR_SYSREG_ICC_IAR1_EL1:
+        if ( unlikely(!is_read) )
+        {
+            ret = false;
+            goto end;
+        }
         fn = vgic_v3_read_iar;
         break;
 

@@ -792,6 +797,11 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
 
     case HSR_SYSREG_ICC_HPPIR0_EL1:
     case HSR_SYSREG_ICC_HPPIR1_EL1:
+        if ( unlikely(!is_read) )
+        {
+            ret = false;
+            goto end;
+        }
         fn = vgic_v3_read_hppir;
         break;
 

index ac39eb5e1d..1b36b104bc 100644
@@ -792,6 +792,11 @@ bool vgic_v3_handle_cpuif_access(struct cpu_user_regs *regs)
 
     case HSR_SYSREG_ICC_EOIR0_EL1:
     case HSR_SYSREG_ICC_EOIR1_EL1:
+        if ( unlikely(is_read) )
+        {
+            ret = false;
+            goto end;
+        }
         fn = vgic_v3_write_eoir;
         break;