diff --git a/drivers/irqchip/irq-imx-gpcv2.c b/drivers/irqchip/irq-imx-gpcv2.c index 4f74c15c475557ab2c2c6dd88634df077109e7d6..2f02c2dcb1213a46e8ef284a80ea66208407bb7a 100644 --- a/drivers/irqchip/irq-imx-gpcv2.c +++ b/drivers/irqchip/irq-imx-gpcv2.c @@ -3,11 +3,19 @@ * Copyright (C) 2015 Freescale Semiconductor, Inc. */ +#include <linux/arm-smccc.h> +#include <linux/mfd/syscon/imx6q-iomuxc-gpr.h> +#include <linux/mfd/syscon.h> #include <linux/of_address.h> #include <linux/of_irq.h> +#include <linux/regmap.h> #include <linux/slab.h> #include <linux/irqchip.h> #include <linux/syscore_ops.h> +#include <linux/smp.h> + +#define IMX_SIP_GPC 0xC2000004 +#define IMX_SIP_GPC_CORE_WAKE 0x00 #define IMR_NUM 4 #define GPC_MAX_IRQS (IMR_NUM * 32) @@ -70,6 +78,37 @@ static struct syscore_ops imx_gpcv2_syscore_ops = { .resume = gpcv2_wakeup_source_restore, }; +static void (*__gic_v3_smp_cross_call)(const struct cpumask *, unsigned int); + +static void imx_gpcv2_raise_softirq(const struct cpumask *mask, + unsigned int irq) +{ + struct arm_smccc_res res; + + /* call the hijacked smp cross call handler */ + __gic_v3_smp_cross_call(mask, irq); + + /* now call into EL3 and take care of the wakeup */ + arm_smccc_smc(IMX_SIP_GPC, IMX_SIP_GPC_CORE_WAKE, + *cpumask_bits(mask), 0, 0, 0, 0, 0, &res); +} + +static void imx_gpcv2_wake_request_fixup(void) +{ + struct regmap *iomux_gpr; + + /* hijack the already registered smp cross call handler */ + __gic_v3_smp_cross_call = __smp_cross_call; + + /* register our workaround handler for smp cross call */ + set_smp_cross_call(imx_gpcv2_raise_softirq); + + iomux_gpr = syscon_regmap_lookup_by_compatible("fsl,imx6q-iomuxc-gpr"); + if (!IS_ERR(iomux_gpr)) + regmap_update_bits(iomux_gpr, IOMUXC_GPR1, IMX6Q_GPR1_GINT, + IMX6Q_GPR1_GINT); +} + static int imx_gpcv2_irq_set_wake(struct irq_data *d, unsigned int on) { struct gpcv2_irqchip_data *cd = d->chip_data; @@ -267,6 +306,9 @@ static int __init imx_gpcv2_irqchip_init(struct device_node *node, cd->wakeup_sources[i] = ~0; } + if (of_property_read_bool(node, "broken-wake-request-signals")) + imx_gpcv2_wake_request_fixup(); + /* Let CORE0 as the default CPU to wake up by GPC */ cd->cpu2wakeup = GPC_IMR1_CORE0;