[PATCH] irqchip/gic-v3: Fix IPRIORITYR can't perform byte operations in GIC-600

[Lecopzer Chen]

When pseudo-NMI enabled, register_nmi() set priority of specific IRQ
by byte ops, and this doesn't work in GIC-600.

We have asked ARM Support [1]:
> Please refer to following description in
> "2.1.2 Distributor ACE-Lite slave interface" of GIC-600 TRM for
> the GIC600 ACE-lite slave interface supported sizes:
>   "The GIC-600 only accepts single beat accesses of the sizes for
>   each register that are shown in the Programmers model,
>   see Chapter 4 Programmer's model on page 4-102.
>   All other accesses are rejected and given either an
>   OKAY or SLVERR response that is based on the GICT_ERR0CTLR.UE bit.".

Thus the register needs to be written by double word operation and
the step will be: read 32bit, set byte and write it back.

[1] https://services.arm.com/support/s/case/5003t00001L4Pba

Signed-off-by: Lecopzer Chen <lecopzer.chen@xxxxxxxxxxxx>
---
 drivers/irqchip/irq-gic-v3.c | 13 ++++++++++++-
 1 file changed, 12 insertions(+), 1 deletion(-)

diff --git a/drivers/irqchip/irq-gic-v3.c b/drivers/irqchip/irq-gic-v3.c
index eb0ee356a629..cfc5a6ad30dc 100644
--- a/drivers/irqchip/irq-gic-v3.c
+++ b/drivers/irqchip/irq-gic-v3.c
@@ -440,10 +440,21 @@ static void gic_irq_set_prio(struct irq_data *d, u8 prio)
 {
 	void __iomem *base = gic_dist_base(d);
 	u32 offset, index;
+	u32 val, prio_offset_mask, prio_offset_shift;
 
 	offset = convert_offset_index(d, GICD_IPRIORITYR, &index);
 
-	writeb_relaxed(prio, base + offset + index);
+	/*
+	 * GIC-600 memory mapping register doesn't support byte opteration,
+	 * thus read 32-bits from register, set bytes and wtire back to it.
+	 */
+	prio_offset_shift = (index & 0x3) * 8;
+	prio_offset_mask = GENMASK(prio_offset_shift + 7, prio_offset_shift);
+	index &= ~0x3;
+	val = readl_relaxed(base + offset + index);
+	val &= ~prio_offset_mask;
+	val |= prio << prio_offset_shift;
+	writel_relaxed(val, base + offset + index);
 }
 
 static u32 gic_get_ppi_index(struct irq_data *d)
-- 
2.18.0