From ae203e0d4190d8830a8091cde975e84d142016e1 Mon Sep 17 00:00:00 2001
From: Daniel Hellstrom <daniel@gaisler.com>
Date: Fri, 7 Apr 2017 08:31:48 +0200
Subject: leon, ambapp_bus: IRQ affinity for on-chip AMBAPP bus

---
 c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus.c  | 38 +++++++++++++++++++++
 .../libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c  | 39 ++++++++++++++++++++++
 .../sparc/shared/include/drvmgr/ambapp_bus.h       |  4 +++
 3 files changed, 81 insertions(+)

diff --git a/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus.c b/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus.c
index 23335d4a4b..d17affbf81 100644
--- a/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus.c
+++ b/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus.c
@@ -55,6 +55,13 @@ int ambapp_bus_freq_get(
 	unsigned int *freq_hz);
 void ambapp_dev_info(struct drvmgr_dev *, void (*print)(void *p, char *str), void *p);
 
+#ifdef RTEMS_SMP
+int ambapp_int_set_affinity(
+	struct drvmgr_dev *dev,
+	int index,
+	Processor_mask cpus);
+#endif
+
 struct drvmgr_bus_ops ambapp_bus_ops =
 {
 	.init		= 
@@ -70,6 +77,9 @@ struct drvmgr_bus_ops ambapp_bus_ops =
 	.int_unregister	= ambapp_int_unregister,
 	.int_clear	= ambapp_int_clear,
 	.int_mask	= ambapp_int_mask,
+#ifdef RTEMS_SMP
+	.int_set_affinity = ambapp_int_set_affinity,
+#endif
 	.int_unmask	= ambapp_int_unmask,
 	.get_params	= ambapp_get_params,
 	.get_freq	= ambapp_bus_freq_get,
@@ -782,3 +792,31 @@ int ambapp_bus_remove(struct drvmgr_bus *bus)
 {
 	return DRVMGR_OK;
 }
+
+#ifdef RTEMS_SMP
+int ambapp_int_set_affinity(
+	struct drvmgr_dev *dev,
+	int index,
+	Processor_mask cpus)
+{
+	struct ambapp_priv *priv;
+	int irq;
+
+	priv = dev->parent->priv;
+
+	/* Get IRQ number from index and device information */
+	irq = ambapp_int_get(dev, index);
+	if (irq < 0)
+		return DRVMGR_EINVAL;
+
+	DBG("Set interrupt affinity on 0x%x for dev 0x%x (IRQ: %d)\n",
+		(unsigned int)dev->parent->dev, (unsigned int)dev, irq);
+
+	if (priv->config->ops->int_set_affinity) {
+		/* Let device override driver default */
+		return priv->config->ops->int_set_affinity(dev, irq, cpus);
+	} else {
+		return DRVMGR_ENOSYS;
+	}
+}
+#endif
diff --git a/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c b/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c
index b0c47ffc9b..5305527346 100644
--- a/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c
+++ b/c/src/lib/libbsp/sparc/shared/drvmgr/ambapp_bus_grlib.c
@@ -46,6 +46,12 @@ int ambapp_grlib_int_mask(
 int ambapp_grlib_int_unmask(
 	struct drvmgr_dev *dev,
 	int irq);
+#ifdef RTEMS_SMP
+int ambapp_grlib_int_set_affinity(
+	struct drvmgr_dev *dev,
+	int irq,
+	Processor_mask cpus);
+#endif
 int ambapp_grlib_get_params(
 	struct drvmgr_dev *dev,
 	struct drvmgr_bus_params *params);
@@ -63,6 +69,9 @@ struct ambapp_ops ambapp_grlib_ops = {
 	.int_clear = ambapp_grlib_int_clear,
 	.int_mask = ambapp_grlib_int_mask,
 	.int_unmask = ambapp_grlib_int_unmask,
+#ifdef RTEMS_SMP
+	.int_set_affinity = ambapp_grlib_int_set_affinity,
+#endif
 	.get_params = ambapp_grlib_get_params
 };
 
@@ -219,6 +228,36 @@ int ambapp_grlib_int_unmask
 	return DRVMGR_OK;
 }
 
+#ifdef RTEMS_SMP
+int ambapp_grlib_int_set_affinity
+	(
+	struct drvmgr_dev *dev,
+	int irq,
+	Processor_mask cpus
+	)
+{
+	uint32_t cpu_count = rtems_get_processor_count();
+	uint32_t cpu_index;
+	int enabled_cnt = 0;
+
+	for (cpu_index = 0; cpu_index < cpu_count; cpu_index++) {
+		if (_Processor_mask_Is_set(cpus, cpu_index)) {
+			BSP_Cpu_Unmask_interrupt(irq, cpu_index);
+			enabled_cnt++;
+		}
+	}
+
+	/* Propagate the interrupt to all CPUs */
+	if (enabled_cnt > 1) {
+		LEON_Enable_interrupt_broadcast(irq);
+	} else {
+		LEON_Disable_interrupt_broadcast(irq);
+	}
+
+	return DRVMGR_OK;
+}
+#endif
+
 int ambapp_grlib_get_params(struct drvmgr_dev *dev, struct drvmgr_bus_params *params)
 {
 	/* Leave params->freq_hz untouched for default */
diff --git a/c/src/lib/libbsp/sparc/shared/include/drvmgr/ambapp_bus.h b/c/src/lib/libbsp/sparc/shared/include/drvmgr/ambapp_bus.h
index 1e8dde8753..ee4a15253e 100644
--- a/c/src/lib/libbsp/sparc/shared/include/drvmgr/ambapp_bus.h
+++ b/c/src/lib/libbsp/sparc/shared/include/drvmgr/ambapp_bus.h
@@ -85,6 +85,10 @@ struct ambapp_ops {
 	int	(*int_clear)(struct drvmgr_dev *dev, int index);
 	int	(*int_mask)(struct drvmgr_dev *dev, int index);
 	int	(*int_unmask)(struct drvmgr_dev *dev, int index);
+#ifdef RTEMS_SMP
+	int 	(*int_set_affinity)(struct drvmgr_dev *dev, int index,
+				Processor_mask cpus);
+#endif
 	int	(*get_params)
 		(struct drvmgr_dev *, struct drvmgr_bus_params *);
 };
-- 
cgit v1.2.3