[PATCH 2/3] bsp/atsam: Add additional PIO helper.

[Christian Mauderer]

---
 .../contrib/libraries/libchip/source/pio_it.c      | 46 ++++++++++++++++++++++
 bsps/arm/atsam/include/libchip/include/pio_it.h    | 13 ++++++
 2 files changed, 59 insertions(+)

diff --git a/bsps/arm/atsam/contrib/libraries/libchip/source/pio_it.c b/bsps/arm/atsam/contrib/libraries/libchip/source/pio_it.c
index f3745281dd..f539b090e4 100644
--- a/bsps/arm/atsam/contrib/libraries/libchip/source/pio_it.c
+++ b/bsps/arm/atsam/contrib/libraries/libchip/source/pio_it.c
@@ -37,6 +37,8 @@
 
 #include "chip.h"
 
+#include <string.h>
+
 #include <rtems/irq-extension.h>
 #include <rtems/sysinit.h>
 #include <bsp/fatal.h>
@@ -285,3 +287,47 @@ void PIO_ConfigureIt(const Pin *pPin, void (*handler)(const Pin *, void *arg),
 	/* Define new source */
 	TRACE_DEBUG("PIO_ConfigureIt: Defining new source #%d.\n\r", _dwNumSources);
 }
+
+/**
+ * Search for a PIO interrupt and remove it if it is there.
+ * \param pPin  Pointer to a Pin instance.
+ * \param handler  Interrupt handler function pointer.
+ * \param arg Pointer to interrupt handler argument
+ * \return RTEMS_SUCCESSFUL if removed.
+ * \return RTEMS_UNSATISFIED if the handler couldn't be found
+ */
+rtems_status_code PIO_RemoveIt(const Pin *pPin,
+    void (*handler)(const Pin *, void *arg), void *arg)
+{
+	InterruptSource *pSource;
+	rtems_interrupt_level level;
+	rtems_status_code sc = RTEMS_UNSATISFIED;
+	uint32_t i;
+	uint32_t j;
+
+	TRACE_DEBUG("PIO_RemoveIt()\n\r");
+
+	rtems_interrupt_disable(level);
+
+	for(i = 0; i < _dwNumSources; ++i) {
+		pSource = &(_aIntSources[_dwNumSources]);
+		if(pSource->pPin == pPin &&
+		    pSource->handler == handler &&
+		    pSource->arg == arg) {
+			if(i + 1 < _dwNumSources) {
+				TRACE_DEBUG("PIO_RemoveIt: Remove #%d.\n\r", i);
+				/* Move remaining sources */
+				memcpy(pSource,
+				    &(_aIntSources[i+1]),
+				    sizeof(_aIntSources[0])*(_dwNumSources-i-1)
+				    );
+			}
+			_dwNumSources--;
+			sc = RTEMS_SUCCESSFUL;
+		}
+	}
+
+	rtems_interrupt_enable(level);
+
+	return sc;
+}
diff --git a/bsps/arm/atsam/include/libchip/include/pio_it.h b/bsps/arm/atsam/include/libchip/include/pio_it.h
index b03973caa1..e3b6221205 100644
--- a/bsps/arm/atsam/include/libchip/include/pio_it.h
+++ b/bsps/arm/atsam/include/libchip/include/pio_it.h
@@ -66,6 +66,7 @@
  */
 
 #include "pio.h"
+#include <rtems.h>
 
 #ifdef __cplusplus
 extern "C" {
@@ -80,6 +81,8 @@ extern void PIO_InitializeInterrupts(uint32_t dwPriority);
 extern void PIO_ConfigureIt(const Pin *pPin,
     void (*handler)(const Pin *, void *arg), void *arg);
 
+extern rtems_status_code PIO_RemoveIt(const Pin *pPin,
+    void (*handler)(const Pin *, void *arg), void *arg);
 
 /**
  * Enables the given interrupt source if it has been configured. The status
@@ -103,6 +106,16 @@ static inline void PIO_DisableIt(const Pin *pPin)
 	pPin->pio->PIO_IDR = pPin->mask;
 }
 
+/**
+ * Check whether a given interrupt source is active.
+ *
+ * \param pPin  Interrupt source to check.
+ */
+static inline bool PIO_ItIsActive(const Pin *pPin)
+{
+	return ((pPin->pio->PIO_IMR & pPin->mask) != 0);
+}
+
 extern void PIO_IT_InterruptHandler(void);
 
 extern void PioInterruptHandler(uint32_t id, Pio *pPio);
-- 
2.16.4