[RFC PATCH v2 44/45] irqchip/gic-v5: Add NMI support for PPIs, SPIs and LPIs

@@ -22,6 +22,7 @@
 static u8 pri_bits __ro_after_init = 5;
 
 #define GICV5_IRQ_PRI_MASK	0x1f
+#define GICV5_IRQ_PRI_NMI	0
 #define GICV5_IRQ_PRI_MI	(GICV5_IRQ_PRI_MASK & GENMASK(4, 5 - pri_bits))
 
 #define PPI_NR	128

@@ -109,6 +110,56 @@ static void gicv5_hwirq_init(irq_hw_number_t hwirq, u8 priority, u8 hwirq_type)
 	}
 }
 
+static void gicv5_ppi_irq_set_priority(struct irq_data *d, u8 priority)
+{
+	u64 hwirq_id_offset = d->hwirq / 8;
+	u64 hwirq_id_index = d->hwirq % 8;
+	u64 hwirq_id_clear = (u64)GICV5_IRQ_PRI_MASK << (hwirq_id_index * 8);
+	u64 hwirq_id_set = (u64)priority << (hwirq_id_index * 8);
+
+#define SET_ICC_PPI_PRIORITYRn_EL1(cnt)	\
+	case cnt: sysreg_clear_set_s(SYS_ICC_PPI_PRIORITYR ## cnt ## _EL1, hwirq_id_clear, hwirq_id_set); break
+
+	switch (hwirq_id_offset) {
+	SET_ICC_PPI_PRIORITYRn_EL1(0);
+	SET_ICC_PPI_PRIORITYRn_EL1(1);
+	SET_ICC_PPI_PRIORITYRn_EL1(2);
+	SET_ICC_PPI_PRIORITYRn_EL1(3);
+	SET_ICC_PPI_PRIORITYRn_EL1(4);
+	SET_ICC_PPI_PRIORITYRn_EL1(5);
+	SET_ICC_PPI_PRIORITYRn_EL1(6);
+	SET_ICC_PPI_PRIORITYRn_EL1(7);
+	SET_ICC_PPI_PRIORITYRn_EL1(8);
+	SET_ICC_PPI_PRIORITYRn_EL1(9);
+	SET_ICC_PPI_PRIORITYRn_EL1(10);
+	SET_ICC_PPI_PRIORITYRn_EL1(11);
+	SET_ICC_PPI_PRIORITYRn_EL1(12);
+	SET_ICC_PPI_PRIORITYRn_EL1(13);
+	SET_ICC_PPI_PRIORITYRn_EL1(14);
+	SET_ICC_PPI_PRIORITYRn_EL1(15);
+#undef SET_ICC_PPI_PRIORITYRn_EL1
+	default:
+		WARN_ON(1);
+	}
+	/*
+	 * Context syncronization required to make sure system register writes
+	 * effects are synchronised.
+	 */
+	isb();
+}
+
+static void gicv5_hwirq_set_priority(irq_hw_number_t hwirq, u8 priority, u8 hwirq_type) {
+	u64 cdpri;
+
+	cdpri = FIELD_PREP(GICV5_GIC_CDPRI_PRIORITY_MASK, priority)	|
+		FIELD_PREP(GICV5_GIC_CDPRI_TYPE_MASK, hwirq_type)	|
+		FIELD_PREP(GICV5_GIC_CDPRI_ID_MASK, hwirq);
+
+	gic_insn(cdpri, CDPRI);
+
+	return;
+}
+
 static void gicv5_ppi_irq_mask(struct irq_data *d)
 {
 	u64 hwirq_id_bit = BIT_ULL(d->hwirq % 64);

@@ -524,7 +575,134 @@ static int gicv5_ppi_irq_set_vcpu_affinity(struct irq_data *d, void *vcpu)
 	return 0;
 }
 
-static const struct irq_chip gicv5_ppi_irq_chip = {
+static int gicv5_hwirq_peek_irq(irq_hw_number_t hwirq, u8 hwirq_type)
+{
+	u64 icsr, cdrcfg;
+
+	cdrcfg = hwirq | FIELD_PREP(GICV5_GIC_CDRCFG_TYPE_MASK, hwirq_type);
+
+	gic_insn(cdrcfg, CDRCFG);
+	isb();
+	icsr = read_sysreg_s(SYS_ICC_ICSR_EL1);
+
+	if (FIELD_GET(ICC_ICSR_EL1_F, icsr)) {
+		pr_err("ICSR_EL1 is invalid\n");
+		return -EINVAL;
+	}
+
+	return !!(FIELD_GET(ICC_ICSR_EL1_Enabled, icsr));
+
+}
+
+static int gicv5_ppi_irq_nmi_setup(struct irq_data *d)
+{
+	unsigned long hwirq_id_bit = d->hwirq % 64;
+	unsigned long enable;
+
+	if (!system_uses_nmi())
+		return -EINVAL;
+
+	if (d->hwirq < 64)
+		enable = read_sysreg_s(SYS_ICC_PPI_ENABLER0_EL1);
+	else
+		enable = read_sysreg_s(SYS_ICC_PPI_ENABLER1_EL1);
+
+	if (test_bit(hwirq_id_bit, &enable))
+		return -EINVAL;
+
+	gicv5_ppi_irq_set_priority(d, GICV5_IRQ_PRI_NMI);
+
+	return 0;
+}
+
+static void gicv5_ppi_irq_nmi_teardown(struct irq_data *d)
+{
+	unsigned long hwirq_id_bit = d->hwirq % 64;
+	unsigned long enable;
+
+	if (WARN_ON(!system_uses_nmi()))
+		return;
+
+	if (d->hwirq < 64)
+		enable = read_sysreg_s(SYS_ICC_PPI_ENABLER0_EL1);
+	else
+		enable = read_sysreg_s(SYS_ICC_PPI_ENABLER1_EL1);
+
+	if (test_bit(hwirq_id_bit, &enable))
+		return;
+
+	gicv5_ppi_irq_set_priority(d, GICV5_IRQ_PRI_MI);
+
+	return;
+}
+
+static int gicv5_hwirq_irq_nmi_setup(irq_hw_number_t hwirq, u8 hwirq_type)
+{
+	if (!system_uses_nmi())
+		return -EINVAL;
+
+	if (WARN_ON(hwirq_type != GICV5_HWIRQ_TYPE_LPI && hwirq_type != GICV5_HWIRQ_TYPE_SPI))
+		return -EINVAL;
+
+	if (gicv5_hwirq_peek_irq(hwirq, hwirq_type))
+		return -EINVAL;
+
+	gicv5_hwirq_set_priority(hwirq, GICV5_IRQ_PRI_NMI, hwirq_type);
+
+	return 0;
+}
+
+static void gicv5_hwirq_irq_nmi_teardown(irq_hw_number_t hwirq, u8 hwirq_type)
+{
+	if (WARN_ON(!system_uses_nmi()))
+		return;
+
+	if (WARN_ON(hwirq_type != GICV5_HWIRQ_TYPE_LPI && hwirq_type != GICV5_HWIRQ_TYPE_SPI))
+		return;
+
+	if (gicv5_hwirq_peek_irq(hwirq, hwirq_type))
+		return;
+
+	gicv5_hwirq_set_priority(hwirq, GICV5_IRQ_PRI_MI, hwirq_type);
+
+	return;
+}
+
+static int gicv5_lpi_irq_nmi_setup(struct irq_data *d)
+{
+	int ret = gicv5_hwirq_irq_nmi_setup(d->hwirq, GICV5_HWIRQ_TYPE_LPI);
+
+	if (!ret)
+		irq_to_desc(d->irq)->handle_irq = handle_fasteoi_nmi;
+
+	return ret;
+}
+
+static void gicv5_lpi_irq_nmi_teardown(struct irq_data *d)
+{
+	gicv5_hwirq_irq_nmi_teardown(d->hwirq, GICV5_HWIRQ_TYPE_LPI);
+	irq_to_desc(d->irq)->handle_irq = handle_fasteoi_irq;
+}
+
+static int gicv5_spi_irq_nmi_setup(struct irq_data *d)
+{
+	int ret = gicv5_hwirq_irq_nmi_setup(d->hwirq, GICV5_HWIRQ_TYPE_SPI);
+
+	if (!ret)
+		irq_to_desc(d->irq)->handle_irq = handle_fasteoi_nmi;
+
+	return ret;
+}
+
+static void gicv5_spi_irq_nmi_teardown(struct irq_data *d)
+{
+	gicv5_hwirq_irq_nmi_teardown(d->hwirq, GICV5_HWIRQ_TYPE_SPI);
+	irq_to_desc(d->irq)->handle_irq = handle_fasteoi_irq;
+}
+
+}
+
+static struct irq_chip gicv5_ppi_irq_chip = {
 	.name			= "GICv5-PPI",
 	.irq_mask		= gicv5_ppi_irq_mask,
 	.irq_unmask		= gicv5_ppi_irq_unmask,

@@ -533,11 +711,13 @@ static const struct irq_chip gicv5_ppi_irq_chip = {
 	.irq_get_irqchip_state	= gicv5_ppi_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gicv5_ppi_irq_set_irqchip_state,
 	.irq_set_vcpu_affinity	= gicv5_ppi_irq_set_vcpu_affinity,
+	.irq_nmi_setup		= gicv5_ppi_irq_nmi_setup,
+	.irq_nmi_teardown	= gicv5_ppi_irq_nmi_teardown,
 	.flags			= IRQCHIP_SKIP_SET_WAKE	  |
 				  IRQCHIP_MASK_ON_SUSPEND,
 };
 
-static const struct irq_chip gicv5_spi_irq_chip = {
+static struct irq_chip gicv5_spi_irq_chip = {
 	.name			= "GICv5-SPI",
 	.irq_mask		= gicv5_spi_irq_mask,
 	.irq_unmask		= gicv5_spi_irq_unmask,

@@ -547,12 +727,14 @@ static const struct irq_chip gicv5_spi_irq_chip = {
 	.irq_retrigger		= gicv5_spi_irq_retrigger,
 	.irq_get_irqchip_state	= gicv5_spi_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gicv5_spi_irq_set_irqchip_state,
+	.irq_nmi_setup		= gicv5_spi_irq_nmi_setup,
+	.irq_nmi_teardown	= gicv5_spi_irq_nmi_teardown,
 	.flags			= IRQCHIP_SET_TYPE_MASKED |
 				  IRQCHIP_SKIP_SET_WAKE	  |
 				  IRQCHIP_MASK_ON_SUSPEND,
 };
 
-static const struct irq_chip gicv5_lpi_irq_chip = {
+static struct irq_chip gicv5_lpi_irq_chip = {
 	.name			= "GICv5-LPI",
 	.irq_mask		= gicv5_lpi_irq_mask,
 	.irq_unmask		= gicv5_lpi_irq_unmask,

@@ -561,6 +743,8 @@ static const struct irq_chip gicv5_lpi_irq_chip = {
 	.irq_retrigger		= gicv5_lpi_irq_retrigger,
 	.irq_get_irqchip_state	= gicv5_lpi_irq_get_irqchip_state,
 	.irq_set_irqchip_state	= gicv5_lpi_irq_set_irqchip_state,
+	.irq_nmi_setup		= gicv5_lpi_irq_nmi_setup,
+	.irq_nmi_teardown	= gicv5_lpi_irq_nmi_teardown,
 	.flags			= IRQCHIP_SKIP_SET_WAKE	  |
 				  IRQCHIP_MASK_ON_SUSPEND,
 };

@@ -914,11 +1098,12 @@ static const struct irq_domain_ops gicv5_irq_ipi_domain_ops = {
 	.free	= gicv5_irq_ipi_domain_free,
 };
 
-static void handle_irq_per_domain(u32 hwirq)
+static void handle_irq_per_domain(u32 hwirq, bool is_nmi)
 {
 	u8 hwirq_type = FIELD_GET(GICV5_HWIRQ_TYPE, hwirq);
 	u32 hwirq_id = FIELD_GET(GICV5_HWIRQ_ID, hwirq);
 	struct irq_domain *domain;
+	int ret;
 
 	switch (hwirq_type) {
 	case GICV5_HWIRQ_TYPE_PPI:

@@ -935,26 +1120,32 @@ static void handle_irq_per_domain(u32 hwirq)
 		return;
 	}
 
-	if (generic_handle_domain_irq(domain, hwirq_id)) {
-		pr_err_once("Could not handle, hwirq = 0x%x", hwirq_id);
+	if (is_nmi)
+		ret = generic_handle_domain_nmi(domain, hwirq_id);
+	else
+		ret = generic_handle_domain_irq(domain, hwirq_id);
+
+	if (ret) {
+		pr_err_once("Could not handle, hwirq = 0x%x (%d)", hwirq_id, ret);
 		gicv5_hwirq_eoi(hwirq_id, hwirq_type);
 	}
 }
 
 static void __exception_irq_entry gicv5_handle_irq(struct pt_regs *regs)
 {
+	bool is_nmi = in_nmi();
 	bool valid;
 	u32 hwirq;
 	u64 ia;
 
-	ia = gicr_insn(CDIA);
+	ia = is_nmi ? gicr_insn(CDNMIA) : gicr_insn(CDIA);
 	valid = GICV5_GICR_CDIA_VALID(ia);
 
 	if (!valid)
 		return;
 
 	/*
-	 * Ensure that the CDIA instruction effects (ie IRQ activation) are
+	 * Ensure that the CD{NM}IA instruction effects (ie IRQ activation) are
 	 * completed before handling the interrupt.
 	 */
 	gsb_ack();

@@ -974,7 +1165,7 @@ static void __exception_irq_entry gicv5_handle_irq(struct pt_regs *regs)
 
 	hwirq = FIELD_GET(GICV5_HWIRQ_INTID, ia);
 
-	handle_irq_per_domain(hwirq);
+	handle_irq_per_domain(hwirq, is_nmi);
 }
 
 static void gicv5_cpu_disable_interrupts(void)

@@ -1014,6 +1205,16 @@ static int gicv5_starting_cpu(unsigned int cpu)
 	return gicv5_irs_register_cpu(cpu);
 }
 
+static void gicv5_enable_nmi_support(void)
+{
+	if (!system_uses_nmi())
+		return;
+
+	gicv5_ppi_irq_chip.flags |= IRQCHIP_SUPPORTS_NMI;
+	gicv5_spi_irq_chip.flags |= IRQCHIP_SUPPORTS_NMI;
+	gicv5_lpi_irq_chip.flags |= IRQCHIP_SUPPORTS_NMI;
+}
+
 static void __init gicv5_smp_init(void)
 {
 	unsigned int num_ipis = GICV5_IPIS_PER_CPU * nr_cpu_ids;

@@ -1180,6 +1381,8 @@ static int __init gicv5_init_common(struct fwnode_handle *parent_domain)
 	if (ret)
 		goto out_int;
 
+	gicv5_enable_nmi_support();
+
 	gicv5_smp_init();
 
 	gicv5_irs_its_probe();