DRVMGR/SPW_BUS: Mask instead of unregister interrupt from SpW ISR

... when temporarily disabling IRQ from the ISR (and corresponding for enable)

This is because low-level routines will return RTEMS_CALLED_FROM_ISR when
unregister is called from an ISR.

Implement SPW-BUS IRQ mask/unmask functions and bug fix interrupt
register/unregister functions.

4 files changed, 108 insertions, 71 deletions

diff --git a/c/src/lib/libbsp/sparc/shared/drvmgr/spw_bus.c b/c/src/lib/libbsp/sparc/shared/drvmgr/spw_bus.c
index da9466ee57..7cbe5f4475 100644
--- a/c/src/lib/libbsp/sparc/shared/drvmgr/spw_bus.c
+++ b/c/src/lib/libbsp/sparc/shared/drvmgr/spw_bus.c
@@ -67,6 +67,8 @@ int spw_bus_init1(struct drvmgr_bus *bus);
 int spw_bus_unite(struct drvmgr_drv *drv, struct drvmgr_dev *dev);
 int spw_bus_int_register(struct drvmgr_dev *dev, int index, const char *info, drvmgr_isr handler, void *arg);
 int spw_bus_int_unregister(struct drvmgr_dev *dev, int index, drvmgr_isr isr, void *arg);
+int spw_bus_int_mask(struct drvmgr_dev *dev, int index);
+int spw_bus_int_unmask(struct drvmgr_dev *dev, int index);
 int spw_bus_int_clear(struct drvmgr_dev *dev, int index);
 
 int spw_bus_freq_get(
@@ -102,6 +104,8 @@ struct drvmgr_bus_ops spw_bus_ops =
 	.unite		= spw_bus_unite,
 	.int_register	= spw_bus_int_register,
 	.int_unregister	= spw_bus_int_unregister,
+	.int_mask	= spw_bus_int_mask,
+	.int_unmask	= spw_bus_int_unmask,
 	.int_clear	= spw_bus_int_clear,
 	.get_params	= spw_bus_get_params,
 	.freq_get	= spw_bus_freq_get,
@@ -194,7 +198,8 @@ int spw_bus_dev_register(struct drvmgr_bus *bus, struct spw_node *node, int inde
 }
 
 /* Interrupt Service Routine, executes in interrupt context. This ISR:
- *  1. Disable/Mask IRQ on IRQ controller, this disables further interrupts on this IRQ number
+ *  1. Disable/Mask IRQ on IRQ controller, this disables further interrupts on
+       this IRQ number
  *  2. Mark in the private struct that the IRQ has happened
  *  3. Wake ISR TASK that will handle each marked IRQ
  *
@@ -212,8 +217,7 @@ void spw_bus_isr(void *arg)
 
 	priv = (struct spw_bus_priv *)(pvirq - offsetof(struct spw_bus_priv, virqs) - (virq-1));
 
-	/*drvmgr_interrupt_mask(priv->bus->dev, -irq);*/
-	gpiolib_irq_disable(priv->config->virq_table[virq-1].handle);
+	gpiolib_irq_mask(priv->config->virq_table[virq-1].handle);
 
 	/* Mark IRQ was received */
 	old_irq_mask = priv->irq_mask;
@@ -252,8 +256,7 @@ void spwbus_task(rtems_task_argument argument)
 				}
 
 				/* Reenable the handled IRQ */
-				/*drvmgr_interrupt_unmask(priv->bus->dev, -irq);*/
-				gpiolib_irq_enable(priv->config->virq_table[virq-1].handle);
+				gpiolib_irq_unmask(priv->config->virq_table[virq-1].handle);
 
 				virq++;
 				mask = mask >> 1;
@@ -469,41 +472,44 @@ int spw_bus_int_register(
 	/* Get IRQ number from index and device information */
 	virq = spw_bus_int_get(dev, index);
 	if ( virq <= 0 )
-		return -1;
+		return DRVMGR_FAIL;
 
 	bus = dev->parent;
 	priv = bus->priv;
-	
+
 	DBG("SpW-BUS: Register ISR for VIRQ%d\n", virq);
 
 	handle = priv->config->virq_table[virq-1].handle;
 	if ( handle == NULL )
-		return -1;
+		return DRVMGR_FAIL;
 
 	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 
 	status = genirq_register(priv->genirq, virq, handler, arg);
-	if ( status == 0 ) {
-		/* Register a ISR for the first registered handler */
-
-		/* Unmask the GPIO IRQ at the source (at the GPIO core), it is still masked by the IRQ
-		 * controller, it will be enabled later.
-		 */
-		struct gpiolib_config gpiocfg;
-		gpiocfg.mask = 1;
-		gpiocfg.irq_level = GPIOLIB_IRQ_LEVEL;
-		gpiocfg.irq_polarity = GPIOLIB_IRQ_POL_HIGH;
-		gpiolib_set_config(handle, &gpiocfg);
-
-		/* Already done 
-		gpioLib_ (priv->virq_table[virq].fd, 
-		drvmgr_interrupt_register(bus->dev, -irq, spw_bus_isr, priv);
-		*/
+	if ( status >= 0 ) {
+		status = genirq_enable(priv->genirq, virq, handler, arg);
+		if ( status == 0 ) {
+			/* Enable IRQ for first enabled handler only */
+
+			/* Unmask the GPIO IRQ at the source (at the GPIO core),
+			 * and at the IRQ controller
+			 */
+			struct gpiolib_config gpiocfg;
+			gpiocfg.mask = 1;
+			gpiocfg.irq_level = GPIOLIB_IRQ_LEVEL;
+			gpiocfg.irq_polarity = GPIOLIB_IRQ_POL_HIGH;
+			gpiolib_set_config(handle, &gpiocfg);
+
+			gpiolib_irq_enable(handle);
+		}
 	}
 
 	rtems_semaphore_release(priv->irqlock);
 
-	return 0;
+	if (status < 0)
+		return DRVMGR_FAIL;
+	else
+		return DRVMGR_OK;
 }
 
 int spw_bus_int_unregister(struct drvmgr_dev *dev, int index, drvmgr_isr isr, void *arg)
@@ -516,108 +522,112 @@ int spw_bus_int_unregister(struct drvmgr_dev *dev, int index, drvmgr_isr isr, vo
 	/* Get IRQ number from index and device information */
 	virq = spw_bus_int_get(dev, index);
 	if ( virq <= 0 )
-		return -1;
+		return DRVMGR_FAIL;
 
 	DBG("SpW-BUS: unregister ISR for VIRQ%d\n", virq);
 
 	bus = dev->parent;
 	priv = bus->priv;
-	
+
 	handle = priv->config->virq_table[virq-1].handle;
 	if ( handle == NULL )
-		return -1;
+		return DRVMGR_FAIL;
 
 	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 
-	status = genirq_unregister(priv->genirq, virq, isr, arg);
-	if ( status == 0 ) {
-		/* Register a ISR for the first registered handler */
-		/*drvmgr_interrupt_unregister(bus->dev, -irq, spw_bus_isr, priv);*/
+	status = genirq_disable(priv->genirq, virq, isr, arg);
+	if ( status >= 0 ) {
+		if ( status == 0 ) {
+			/* Disable IRQ only when no other handler is enabled */
+
+			/* Mask the GPIO IRQ at the source (at the GPIO core),
+			 * disable the IRQ at the interrupt controller.
+			 */
+			struct gpiolib_config gpiocfg;
+			gpiocfg.mask = 0;
+			gpiocfg.irq_level = GPIOLIB_IRQ_LEVEL;
+			gpiocfg.irq_polarity = GPIOLIB_IRQ_POL_HIGH;
+			gpiolib_set_config(handle, &gpiocfg);
+
+			gpiolib_irq_disable(handle);
+		}
+		status = genirq_unregister(priv->genirq, virq, isr, arg);
 	}
 
 	rtems_semaphore_release(priv->irqlock);
 
-	return 0;
+	if (status < 0)
+		return DRVMGR_FAIL;
+	else
+		return DRVMGR_OK;
 }
 
-#warning FIX SPW-BUS IRQ ENABLING/DISABLING
-#if 0
-/* Enable interrupt */
-int spw_bus_int_enable(struct drvmgr_dev *dev, int index, drvmgr_isr isr, void *arg)
+/* Unmask interrupt */
+int spw_bus_int_unmask(struct drvmgr_dev *dev, int index)
 {
 	struct drvmgr_bus *bus;
 	struct spw_bus_priv *priv;
-	int virq, status;
+	int virq;
 	void *handle;
 
 	/* Get IRQ number from index and device information */
 	virq = spw_bus_int_get(dev, index);
 	if ( virq <= 0 )
-		return -1;
+		return DRVMGR_FAIL;
 
 	bus = dev->parent;
 	priv = bus->priv;
-	
-	DBG("SpW-BUS: Enable IRQ for VIRQ%d\n", virq);
+
+	DBG("SpW-BUS: unmask IRQ for VIRQ%d\n", virq);
 
 	handle = priv->config->virq_table[virq-1].handle;
 	if ( handle == NULL )
-		return -1;
+		return DRVMGR_FAIL;
+
+	if ( genirq_check(priv->genirq, virq) )
+		return DRVMGR_FAIL;
 
 	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 
-	status = genirq_enable(priv->genirq, virq, isr, arg);
-	if ( status == 0 ) {
-		/* Register a ISR for the first registered handler */
-		if ( gpiolib_irq_enable(handle) ) {
-			DBG("SpW-BUS: Failed to Enable IRQ for VIRQ%d\n", virq);
-		}
-		/*
-		drvmgr_interrupt_enable(bus->dev, -irq, spw_bus_isr, priv);
-		*/
-	}
+	gpiolib_irq_unmask(handle);
 
 	rtems_semaphore_release(priv->irqlock);
 
-	return 0;
+	return DRVMGR_OK;
 }
 
-/* Disable interrupt */
-int spw_bus_int_disable(struct drvmgr_dev *dev, int index, drvmgr_isr isr, void *arg)
+/* mask interrupt */
+int spw_bus_int_mask(struct drvmgr_dev *dev, int index)
 {
 	struct drvmgr_bus *bus;
 	struct spw_bus_priv *priv;
-	int virq, status;
+	int virq;
 	void *handle;
 
 	/* Get IRQ number from index and device information */
 	virq = spw_bus_int_get(dev, index);
 	if ( virq <= 0 )
-		return -1;
+		return DRVMGR_FAIL;
 
 	bus = dev->parent;
 	priv = bus->priv;
 
 	handle = priv->config->virq_table[virq-1].handle;
 	if ( handle == NULL )
-		return -1;
+		return DRVMGR_FAIL;
+
+	if ( genirq_check(priv->genirq, virq) )
+		return DRVMGR_FAIL;
 
 	rtems_semaphore_obtain(priv->irqlock, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
 
-	status = genirq_disable(priv->genirq, virq, isr, arg);
-	if ( status == 0 ) {
-		/* Register a ISR for the first registered handler */
-		if ( gpiolib_irq_disable(handle) ) {
-			DBG("SpW-BUS: Failed to Disable IRQ for VIRQ%d\n", virq);
-		}
-		/*drvmgr_interrupt_disable(bus->dev, -irq, spw_bus_isr, priv);*/
-	}
+	/* Register a ISR for the first registered handler */
+	gpiolib_irq_mask(handle);
 
 	rtems_semaphore_release(priv->irqlock);
 
-	return 0;
+	return DRVMGR_OK;
 }
-#endif
 
 int spw_bus_int_clear(struct drvmgr_dev *dev, int index)
 {
@@ -629,14 +639,14 @@ int spw_bus_int_clear(struct drvmgr_dev *dev, int index)
 	/* Get IRQ number from index and device information */
 	virq = spw_bus_int_get(dev, index);
 	if ( virq < 0 )
-		return -1;
+		return DRVMGR_FAIL;
 
 	bus = dev->parent;
 	priv = bus->priv;
 
 	handle = priv->config->virq_table[virq-1].handle;
 	if ( handle == NULL )
-		return -1;
+		return DRVMGR_FAIL;
 
 	/* Register a ISR for the first registered handler */
 	/*drvmgr_interrupt_clear(bus->dev, -irq, spw_bus_isr, priv);*/
@@ -644,7 +654,7 @@ int spw_bus_int_clear(struct drvmgr_dev *dev, int index)
 		DBG("SpW-BUS: Failed to Clear IRQ for VIRQ%d\n", virq);
 	}
 
-	return 0;
+	return DRVMGR_OK;
 }
 
 void *spw_bus_rw_arg(struct drvmgr_dev *dev)
diff --git a/c/src/lib/libbsp/sparc/shared/gpio/gpiolib.c b/c/src/lib/libbsp/sparc/shared/gpio/gpiolib.c
index fe24c87f93..98ebd75002 100644
--- a/c/src/lib/libbsp/sparc/shared/gpio/gpiolib.c
+++ b/c/src/lib/libbsp/sparc/shared/gpio/gpiolib.c
@@ -255,6 +255,17 @@ int gpiolib_irq_disable(void *handle)
 	return gpiolib_irq_opts(handle, GPIOLIB_IRQ_DISABLE);
 }
 
+int gpiolib_irq_mask(void *handle)
+{
+	return gpiolib_irq_opts(handle, GPIOLIB_IRQ_MASK);
+}
+
+int gpiolib_irq_unmask(void *handle)
+{
+	return gpiolib_irq_opts(handle, GPIOLIB_IRQ_UNMASK);
+}
+
+
 /*** Initialization ***/
 int gpiolib_initialize(void)
 {
diff --git a/c/src/lib/libbsp/sparc/shared/gpio/grgpio.c b/c/src/lib/libbsp/sparc/shared/gpio/grgpio.c
index 1c3e7be8ac..ab08e1bfbb 100644
--- a/c/src/lib/libbsp/sparc/shared/gpio/grgpio.c
+++ b/c/src/lib/libbsp/sparc/shared/gpio/grgpio.c
@@ -267,6 +267,18 @@ int grgpio_grpiolib_irq_opts(void *handle, unsigned int options)
 			return -1;
 		}
 	}
+	if ( options & GPIOLIB_IRQ_MASK ) {
+		/* Mask (disable) interrupt at interrupt controller */
+		if ( drvmgr_interrupt_mask(priv->dev, portnr) ) {
+			return -1;
+		}
+	}
+	if ( options & GPIOLIB_IRQ_UNMASK ) {
+		/* Unmask (enable) interrupt at interrupt controller */
+		if ( drvmgr_interrupt_unmask(priv->dev, portnr) ) {
+			return -1;
+		}
+	}
 
 	return 0;
 }
diff --git a/c/src/lib/libbsp/sparc/shared/include/gpiolib.h b/c/src/lib/libbsp/sparc/shared/include/gpiolib.h
index 94b8b7dd79..4b4ab1bc8f 100644
--- a/c/src/lib/libbsp/sparc/shared/include/gpiolib.h
+++ b/c/src/lib/libbsp/sparc/shared/include/gpiolib.h
@@ -52,6 +52,8 @@ extern int gpiolib_get(void *handle, int *inval);
 extern int gpiolib_irq_clear(void *handle);
 extern int gpiolib_irq_enable(void *handle);
 extern int gpiolib_irq_disable(void *handle);
+extern int gpiolib_irq_mask(void *handle);
+extern int gpiolib_irq_unmask(void *handle);
 extern int gpiolib_irq_force(void *handle);
 extern int gpiolib_irq_register(void *handle, void *func, void *arg);
 
@@ -76,6 +78,8 @@ struct gpiolib_drv_ops {
 #define GPIOLIB_IRQ_DISABLE 0x02
 #define GPIOLIB_IRQ_CLEAR   0x04
 #define GPIOLIB_IRQ_FORCE   0x08
+#define GPIOLIB_IRQ_MASK    0x10
+#define GPIOLIB_IRQ_UNMASK  0x20
 
 struct gpiolib_drv {
 	struct gpiolib_drv_ops	*ops;