4 files changed, 24 insertions, 14 deletions

diff --git a/arch/arm/mach-mvebu/armada-370-xp.h b/arch/arm/mach-mvebu/armada-370-xp.h
index 237c86b83390..c3465f5b1250 100644
--- a/arch/arm/mach-mvebu/armada-370-xp.h
+++ b/arch/arm/mach-mvebu/armada-370-xp.h
@@ -20,8 +20,6 @@
 
 #define ARMADA_XP_MAX_CPUS 4
 
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq);
-void armada_xp_mpic_smp_cpu_init(void);
 void armada_xp_secondary_startup(void);
 extern struct smp_operations armada_xp_smp_ops;
 #endif
diff --git a/arch/arm/mach-mvebu/platsmp.c b/arch/arm/mach-mvebu/platsmp.c
index e01ae51bee56..32bf78e80c48 100644
--- a/arch/arm/mach-mvebu/platsmp.c
+++ b/arch/arm/mach-mvebu/platsmp.c
@@ -70,11 +70,6 @@ static void __init set_secondary_cpus_clock(void)
         }
 }
 
-static void armada_xp_secondary_init(unsigned int cpu)
-{
-        armada_xp_mpic_smp_cpu_init();
-}
-
 static int armada_xp_boot_secondary(unsigned int cpu, struct task_struct *idle)
 {
         int ret, hw_cpu;
@@ -98,8 +93,6 @@ static void __init armada_xp_smp_init_cpus(void)
 
         if (ncores == 0 || ncores > ARMADA_XP_MAX_CPUS)
                 panic("Invalid number of CPUs in DT\n");
-
-        set_smp_cross_call(armada_mpic_send_doorbell);
 }
 
 static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
@@ -132,7 +125,6 @@ static void __init armada_xp_smp_prepare_cpus(unsigned int max_cpus)
 struct smp_operations armada_xp_smp_ops __initdata = {
         .smp_init_cpus          = armada_xp_smp_init_cpus,
         .smp_prepare_cpus       = armada_xp_smp_prepare_cpus,
-        .smp_secondary_init     = armada_xp_secondary_init,
         .smp_boot_secondary     = armada_xp_boot_secondary,
 #ifdef CONFIG_HOTPLUG_CPU
         .cpu_die                = armada_xp_cpu_die,
diff --git a/drivers/irqchip/irq-armada-370-xp.c b/drivers/irqchip/irq-armada-370-xp.c
index 41be897df8d5..71f77848bc23 100644
--- a/drivers/irqchip/irq-armada-370-xp.c
+++ b/drivers/irqchip/irq-armada-370-xp.c
@@ -19,6 +19,7 @@
 #include <linux/irq.h>
 #include <linux/interrupt.h>
 #include <linux/irqchip/chained_irq.h>
+#include <linux/cpu.h>
 #include <linux/io.h>
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
@@ -315,7 +316,8 @@ static int armada_370_xp_mpic_irq_map(struct irq_domain *h,
 }
 
 #ifdef CONFIG_SMP
-void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
+static void armada_mpic_send_doorbell(const struct cpumask *mask,
+                                      unsigned int irq)
 {
         int cpu;
         unsigned long map = 0;
@@ -335,7 +337,7 @@ void armada_mpic_send_doorbell(const struct cpumask *mask, unsigned int irq)
                 ARMADA_370_XP_SW_TRIG_INT_OFFS);
 }
 
-void armada_xp_mpic_smp_cpu_init(void)
+static void armada_xp_mpic_smp_cpu_init(void)
 {
         /* Clear pending IPIs */
         writel(0, per_cpu_int_base + ARMADA_370_XP_IN_DRBEL_CAUSE_OFFS);
@@ -347,6 +349,20 @@ void armada_xp_mpic_smp_cpu_init(void)
         /* Unmask IPI interrupt */
         writel(0, per_cpu_int_base + ARMADA_370_XP_INT_CLEAR_MASK_OFFS);
 }
+
+static int armada_xp_mpic_secondary_init(struct notifier_block *nfb,
+                                         unsigned long action, void *hcpu)
+{
+        if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+                armada_xp_mpic_smp_cpu_init();
+        return NOTIFY_OK;
+}
+
+static struct notifier_block armada_370_xp_mpic_cpu_notifier = {
+        .notifier_call = armada_xp_mpic_secondary_init,
+        .priority = 100,
+};
+
 #endif /* CONFIG_SMP */
 
 static struct irq_domain_ops armada_370_xp_mpic_irq_ops = {
@@ -511,6 +527,10 @@ static int __init armada_370_xp_mpic_of_init(struct device_node *node,
         if (parent_irq <= 0) {
                 irq_set_default_host(armada_370_xp_mpic_domain);
                 set_handle_irq(armada_370_xp_handle_irq);
+#ifdef CONFIG_SMP
+                set_smp_cross_call(armada_mpic_send_doorbell);
+                register_cpu_notifier(&armada_370_xp_mpic_cpu_notifier);
+#endif
         } else {
                 irq_set_chained_handler(parent_irq,
                                         armada_370_xp_mpic_handle_cascade_irq);
diff --git a/drivers/irqchip/irq-orion.c b/drivers/irqchip/irq-orion.c
index e25f246cd2fb..34d18b48bb78 100644
--- a/drivers/irqchip/irq-orion.c
+++ b/drivers/irqchip/irq-orion.c
@@ -42,7 +42,7 @@ __exception_irq_entry orion_handle_irq(struct pt_regs *regs)
                 u32 stat = readl_relaxed(gc->reg_base + ORION_IRQ_CAUSE) &
                         gc->mask_cache;
                 while (stat) {
-                        u32 hwirq = ffs(stat) - 1;
+                        u32 hwirq = __fls(stat);
                         u32 irq = irq_find_mapping(orion_irq_domain,
                                                    gc->irq_base + hwirq);
                         handle_IRQ(irq, regs);
@@ -117,7 +117,7 @@ static void orion_bridge_irq_handler(unsigned int irq, struct irq_desc *desc)
                    gc->mask_cache;
 
         while (stat) {
-                u32 hwirq = ffs(stat) - 1;
+                u32 hwirq = __fls(stat);
 
                 generic_handle_irq(irq_find_mapping(d, gc->irq_base + hwirq));
                 stat &= ~(1 << hwirq);