fsl_bman: add cpu hotplug support

- Initialize all the portals at the boot time based cpu_possible_mask. Migrate
  the portals on offline cpus to bootcpu when there is less cpu online at boot
  time.
- Register cpu hotplug notifier to migrate the portals to boot cpu when cpus
  are offlined, and migrate the portal back to original cpu when cpu is onlined.
- Re-direct irq of the portal in the migrate function.

Signed-off-by: Haiying Wang <Haiying.Wang@freescale.com>
Change-Id: I863d5c15c7f35f9de4ea3d985e4ff467167924b7
Reviewed-on: http://git.am.freescale.net:8181/5453
Tested-by: Review Code-CDREVIEW <CDREVIEW@freescale.com>
Reviewed-by: Ladouceur Jeffrey-R11498 <Jeffrey.Ladouceur@freescale.com>
Reviewed-by: Rivera Jose-B46482 <German.Rivera@freescale.com>

4 files changed, 130 insertions, 41 deletions

diff --git a/drivers/staging/fsl_qbman/bman_driver.c b/drivers/staging/fsl_qbman/bman_driver.c
index ed7dde0..2a59721 100644
--- a/drivers/staging/fsl_qbman/bman_driver.c
+++ b/drivers/staging/fsl_qbman/bman_driver.c
@@ -30,6 +30,9 @@
  */
 
 #include "bman_private.h"
+#ifdef CONFIG_HOTPLUG_CPU
+#include <linux/cpu.h>
+#endif
 
 /*
  * Global variables of the max portal/pool number this bman version supported
@@ -49,6 +52,7 @@ static int num_shared_portals;
 static int shared_portals_idx;
 static LIST_HEAD(unused_pcfgs);
 static DEFINE_SPINLOCK(unused_pcfgs_lock);
+static uintptr_t affine_bportals[NR_CPUS];
 
 static int __init fsl_bpool_init(struct device_node *node)
 {
@@ -240,36 +244,32 @@ void bm_put_unused_portal(struct bm_portal_config *pcfg)
 static struct bman_portal *init_pcfg(struct bm_portal_config *pcfg)
 {
 	struct bman_portal *p;
-	struct cpumask oldmask = *tsk_cpus_allowed(current);
-	set_cpus_allowed_ptr(current, get_cpu_mask(pcfg->public_cfg.cpu));
 	p = bman_create_affine_portal(pcfg);
 	if (p) {
 #ifdef CONFIG_FSL_DPA_PIRQ_SLOW
-		bman_irqsource_add(BM_PIRQ_RCRI | BM_PIRQ_BSCN);
+		bman_p_irqsource_add(p, BM_PIRQ_RCRI | BM_PIRQ_BSCN);
 #endif
 		pr_info("Bman portal %sinitialised, cpu %d\n",
 			pcfg->public_cfg.is_shared ? "(shared) " : "",
 			pcfg->public_cfg.cpu);
+		affine_bportals[pcfg->public_cfg.cpu] = (uintptr_t)p;
 	} else
 		pr_crit("Bman portal failure on cpu %d\n",
 			pcfg->public_cfg.cpu);
-	set_cpus_allowed_ptr(current, &oldmask);
 	return p;
 }
 
 static void init_slave(int cpu)
 {
 	struct bman_portal *p;
-	struct cpumask oldmask = *tsk_cpus_allowed(current);
-	set_cpus_allowed_ptr(current, get_cpu_mask(cpu));
-	p = bman_create_affine_slave(shared_portals[shared_portals_idx++]);
+	p = bman_create_affine_slave(shared_portals[shared_portals_idx++], cpu);
 	if (!p)
 		pr_err("Bman slave portal failure on cpu %d\n", cpu);
 	else
 		pr_info("Bman portal %sinitialised, cpu %d\n", "(slave) ", cpu);
-	set_cpus_allowed_ptr(current, &oldmask);
 	if (shared_portals_idx >= num_shared_portals)
 		shared_portals_idx = 0;
+	affine_bportals[cpu] = (uintptr_t)p;
 }
 
 /* Bootarg "bportals=[...]" has the same syntax as "qportals=", and so the
@@ -293,6 +293,47 @@ static int __init parse_bportals(char *str)
 }
 __setup("bportals=", parse_bportals);
 
+static void bman_offline_cpu(unsigned int cpu)
+{
+	struct bman_portal *p;
+	p = (struct bman_portal *)affine_bportals[cpu];
+	if (p && (!bman_portal_is_sharing_redirect(p)))
+		bman_migrate_portal(p);
+}
+
+static void bman_online_cpu(unsigned int cpu)
+{
+	struct bman_portal *p;
+	p = (struct bman_portal *)affine_bportals[cpu];
+	if (p && (!bman_portal_is_sharing_redirect(p)))
+		bman_migrate_portal_back(p, cpu);
+}
+
+#ifdef CONFIG_HOTPLUG_CPU
+static int __cpuinit bman_hotplug_cpu_callback(struct notifier_block *nfb,
+					unsigned long action, void *hcpu)
+{
+	unsigned int cpu = (unsigned long)hcpu;
+
+	switch (action) {
+	case CPU_ONLINE:
+	case CPU_ONLINE_FROZEN:
+		bman_online_cpu(cpu);
+		break;
+	case CPU_DOWN_PREPARE:
+	case CPU_DOWN_PREPARE_FROZEN:
+		bman_offline_cpu(cpu);
+	default:
+		break;
+	}
+	return NOTIFY_OK;
+}
+
+static struct notifier_block bman_hotplug_cpu_notifier = {
+	.notifier_call = bman_hotplug_cpu_callback,
+};
+#endif /* CONFIG_HOTPLUG_CPU */
+
 /* Initialise the Bman driver. The meat of this function deals with portals. The
  * following describes the flow of portal-handling, the code "steps" refer to
  * this description;
@@ -331,6 +372,7 @@ __init int bman_init(void)
 	struct bm_portal_config *pcfg;
 	struct bman_portal *p;
 	int cpu, ret;
+	struct cpumask offline_cpus;
 
 	/* Initialise the Bman (CCSR) device */
 	for_each_compatible_node(dn, NULL, "fsl,bman") {
@@ -354,27 +396,29 @@ __init int bman_init(void)
 			list_add_tail(&pcfg->list, &unused_pcfgs);
 	}
 	/* Step 2. */
-	for_each_cpu(cpu, &want_shared) {
-		pcfg = get_pcfg(&unused_pcfgs);
-		if (!pcfg)
-			break;
-		pcfg->public_cfg.cpu = cpu;
-		list_add_tail(&pcfg->list, &shared_pcfgs);
-		cpumask_set_cpu(cpu, &shared_cpus);
-	}
-	for_each_cpu(cpu, &want_unshared) {
-		if (cpumask_test_cpu(cpu, &shared_cpus))
-			continue;
-		pcfg = get_pcfg(&unused_pcfgs);
-		if (!pcfg)
-			break;
-		pcfg->public_cfg.cpu = cpu;
-		list_add_tail(&pcfg->list, &unshared_pcfgs);
-		cpumask_set_cpu(cpu, &unshared_cpus);
+	for_each_possible_cpu(cpu) {
+		if (cpumask_test_cpu(cpu, &want_shared)) {
+			pcfg = get_pcfg(&unused_pcfgs);
+			if (!pcfg)
+				break;
+			pcfg->public_cfg.cpu = cpu;
+			list_add_tail(&pcfg->list, &shared_pcfgs);
+			cpumask_set_cpu(cpu, &shared_cpus);
+		}
+		if (cpumask_test_cpu(cpu, &want_unshared)) {
+			if (cpumask_test_cpu(cpu, &shared_cpus))
+				continue;
+			pcfg = get_pcfg(&unused_pcfgs);
+			if (!pcfg)
+				break;
+			pcfg->public_cfg.cpu = cpu;
+			list_add_tail(&pcfg->list, &unshared_pcfgs);
+			cpumask_set_cpu(cpu, &unshared_cpus);
+		}
 	}
 	if (list_empty(&shared_pcfgs) && list_empty(&unshared_pcfgs)) {
 		/* Default, give an unshared portal to each online cpu */
-		for_each_online_cpu(cpu) {
+		for_each_possible_cpu(cpu) {
 			pcfg = get_pcfg(&unused_pcfgs);
 			if (!pcfg)
 				break;
@@ -384,7 +428,7 @@ __init int bman_init(void)
 		}
 	}
 	/* Step 3. */
-	cpumask_andnot(&slave_cpus, cpu_online_mask, &shared_cpus);
+	cpumask_andnot(&slave_cpus, cpu_possible_mask, &shared_cpus);
 	cpumask_andnot(&slave_cpus, &slave_cpus, &unshared_cpus);
 	if (cpumask_empty(&slave_cpus)) {
 		/* No sharing required */
@@ -427,6 +471,12 @@ __init int bman_init(void)
 		for_each_cpu(cpu, &slave_cpus)
 			init_slave(cpu);
 	pr_info("Bman portals initialised\n");
+	cpumask_andnot(&offline_cpus, cpu_possible_mask, cpu_online_mask);
+	for_each_cpu(cpu, &offline_cpus)
+		bman_offline_cpu(cpu);
+#ifdef CONFIG_HOTPLUG_CPU
+	register_hotcpu_notifier(&bman_hotplug_cpu_notifier);
+#endif
 	return 0;
 }
 
diff --git a/drivers/staging/fsl_qbman/bman_high.c b/drivers/staging/fsl_qbman/bman_high.c
index c439e55..3e82672 100644
--- a/drivers/staging/fsl_qbman/bman_high.c
+++ b/drivers/staging/fsl_qbman/bman_high.c
@@ -301,14 +301,9 @@ fail_rcr:
 struct bman_portal *bman_create_affine_portal(
 			const struct bm_portal_config *config)
 {
-	struct bman_portal *portal = get_raw_affine_portal();
+	struct bman_portal *portal;
 
-	/*This function is called from the context which is already affine to
-	 *CPU or in other words this in non-migratable to other CPUs. Call
-	 *put_affine_portal() on entry which allows subsequent functions to
-	 *sleep.
-	 */
-	put_affine_portal();
+	portal = &per_cpu(bman_affine_portal, config->public_cfg.cpu);
 	portal = bman_create_portal(portal, config);
 	if (portal) {
 		spin_lock(&affine_mask_lock);
@@ -319,10 +314,12 @@ struct bman_portal *bman_create_affine_portal(
 }
 
 
-struct bman_portal *bman_create_affine_slave(struct bman_portal *redirect)
+struct bman_portal *bman_create_affine_slave(struct bman_portal *redirect,
+								int cpu)
 {
 #ifdef CONFIG_FSL_DPA_PORTAL_SHARE
-	struct bman_portal *p = get_raw_affine_portal();
+	struct bman_portal *p;
+	p = &per_cpu(bman_affine_portal, cpu);
 	BUG_ON(p->config);
 	BUG_ON(p->is_shared);
 	BUG_ON(!redirect->config->public_cfg.is_shared);
@@ -480,13 +477,11 @@ u32 bman_irqsource_get(void)
 }
 EXPORT_SYMBOL(bman_irqsource_get);
 
-int bman_irqsource_add(__maybe_unused u32 bits)
+int bman_p_irqsource_add(struct bman_portal *p, __maybe_unused u32 bits)
 {
-	struct bman_portal *p = get_raw_affine_portal();
-	int ret = 0;
 #ifdef CONFIG_FSL_DPA_PORTAL_SHARE
 	if (p->sharing_redirect)
-		ret = -EINVAL;
+		return -EINVAL;
 	else
 #endif
 	{
@@ -496,6 +491,15 @@ int bman_irqsource_add(__maybe_unused u32 bits)
 		bm_isr_enable_write(&p->p, p->irq_sources);
 		PORTAL_IRQ_UNLOCK(p, irqflags);
 	}
+	return 0;
+}
+EXPORT_SYMBOL(bman_p_irqsource_add);
+
+int bman_irqsource_add(__maybe_unused u32 bits)
+{
+	struct bman_portal *p = get_raw_affine_portal();
+	int ret = 0;
+	ret = bman_p_irqsource_add(p, bits);
 	put_affine_portal();
 	return ret;
 }
@@ -1043,3 +1047,26 @@ int bman_shutdown_pool(u32 bpid)
 	return ret;
 }
 EXPORT_SYMBOL(bman_shutdown_pool);
+
+int bman_portal_is_sharing_redirect(struct bman_portal *portal)
+{
+	return portal->sharing_redirect ? 1 : 0;
+}
+
+/* Migrate the portal to the boot cpu(cpu0) for offline cpu */
+void bman_migrate_portal(struct bman_portal *portal)
+{
+	unsigned long irqflags __maybe_unused;
+	PORTAL_IRQ_LOCK(portal, irqflags);
+	irq_set_affinity(portal->config->public_cfg.irq, cpumask_of(0));
+	PORTAL_IRQ_UNLOCK(portal, irqflags);
+}
+
+/* Migrate the portal back to the affined cpu once that cpu reappears.*/
+void bman_migrate_portal_back(struct bman_portal *portal, unsigned int cpu)
+{
+	unsigned long irqflags __maybe_unused;
+	PORTAL_IRQ_LOCK(portal, irqflags);
+	irq_set_affinity(portal->config->public_cfg.irq, cpumask_of(cpu));
+	PORTAL_IRQ_UNLOCK(portal, irqflags);
+}
diff --git a/drivers/staging/fsl_qbman/bman_private.h b/drivers/staging/fsl_qbman/bman_private.h
index 7e54701..cdd8c00 100644
--- a/drivers/staging/fsl_qbman/bman_private.h
+++ b/drivers/staging/fsl_qbman/bman_private.h
@@ -73,7 +73,8 @@ struct bman_portal *bman_create_portal(
 				       const struct bm_portal_config *config);
 struct bman_portal *bman_create_affine_portal(
 			const struct bm_portal_config *config);
-struct bman_portal *bman_create_affine_slave(struct bman_portal *redirect);
+struct bman_portal *bman_create_affine_slave(struct bman_portal *redirect,
+								int cpu);
 void bman_destroy_portal(struct bman_portal *bm);
 
 const struct bm_portal_config *bman_destroy_affine_portal(void);
@@ -155,4 +156,8 @@ u32 bm_pool_free_buffers(u32 bpid);
 __init int bman_init(void);
 __init int bman_resource_init(void);
 
+/* Portal migration */
+int bman_portal_is_sharing_redirect(struct bman_portal *portal);
+void bman_migrate_portal(struct bman_portal *portal);
+void bman_migrate_portal_back(struct bman_portal *portal, unsigned int cpu);
 #endif /* CONFIG_FSL_BMAN_CONFIG */
diff --git a/include/linux/fsl_bman.h b/include/linux/fsl_bman.h
index 04797e71..4bb5551 100644
--- a/include/linux/fsl_bman.h
+++ b/include/linux/fsl_bman.h
@@ -514,6 +514,13 @@ u32 bman_query_free_buffers(struct bman_pool *pool);
  */
 int bman_update_pool_thresholds(struct bman_pool *pool, const u32 *thresholds);
 #endif
+
+/**
+ * The below bman_p_***() variant might be called in a situation that the cpu
+ * which the portal affine to is not online yet.
+ * @bman_portal specifies which portal the API will use.
+*/
+int bman_p_irqsource_add(struct bman_portal *p, __maybe_unused u32 bits);
 #ifdef __cplusplus
 }
 #endif