1 files changed, 65 insertions, 60 deletions

diff --git a/c/src/lib/libbsp/arm/atsam/libraries/libchip/source/xdmad.c b/c/src/lib/libbsp/arm/atsam/libraries/libchip/source/xdmad.c
index 972b0e7ec3..a2dd3a7458 100644
--- a/c/src/lib/libbsp/arm/atsam/libraries/libchip/source/xdmad.c
+++ b/c/src/lib/libbsp/arm/atsam/libraries/libchip/source/xdmad.c
@@ -64,6 +64,8 @@
 #include "chip.h"
 #ifdef __rtems__
 #include "../../../utils/utility.h"
+#include <rtems/irq-extension.h>
+#include <bsp/fatal.h>
 #endif /* __rtems__ */
 #include <assert.h>
 static uint8_t xDmad_Initialized = 0;
@@ -129,6 +131,11 @@ static uint32_t XDMAD_AllocateXdmacChannel(sXdmad *pXdmad,
 	return XDMAD_ALLOC_FAILED;
 }
 
+void XDMAD_DoNothingCallback(uint32_t Channel, void *pArg)
+{
+	/* Do nothing */
+}
+
 /*----------------------------------------------------------------------------
  *        Exported functions
  *----------------------------------------------------------------------------*/
@@ -144,6 +151,7 @@ void XDMAD_Initialize(sXdmad *pXdmad, uint8_t bPollingMode)
 {
 	uint32_t j;
 	uint32_t volatile timer = 0x7FF;
+	rtems_status_code sc;
 
 	assert(pXdmad);
 	LockMutex(pXdmad->xdmaMutex, timer);
@@ -159,16 +167,18 @@ void XDMAD_Initialize(sXdmad *pXdmad, uint8_t bPollingMode)
 	pXdmad->numChannels    = (XDMAC_GTYPE_NB_CH(XDMAC_GetType(XDMAC)) + 1);
 
 	for (j = 0; j < pXdmad->numChannels; j ++) {
-		pXdmad->XdmaChannels[j].fCallback = 0;
-		pXdmad->XdmaChannels[j].pArg      = 0;
-		pXdmad->XdmaChannels[j].bIrqOwner    = 0;
-		pXdmad->XdmaChannels[j].bSrcPeriphID = 0;
-		pXdmad->XdmaChannels[j].bDstPeriphID = 0;
-		pXdmad->XdmaChannels[j].bSrcTxIfID   = 0;
-		pXdmad->XdmaChannels[j].bSrcRxIfID   = 0;
-		pXdmad->XdmaChannels[j].bDstTxIfID   = 0;
-		pXdmad->XdmaChannels[j].bDstRxIfID   = 0;
-		pXdmad->XdmaChannels[j].state = XDMAD_STATE_FREE;
+		pXdmad->XdmaChannels[j].fCallback = XDMAD_DoNothingCallback;
+	}
+
+	sc = rtems_interrupt_handler_install(
+		ID_XDMAC,
+		"XDMA",
+		RTEMS_INTERRUPT_UNIQUE,
+		XDMAD_Handler,
+		pXdmad
+	);
+	if (sc != RTEMS_SUCCESSFUL) {
+		bsp_fatal(ATSAM_FATAL_XDMA_IRQ_INSTALL);
 	}
 
 	xDmad_Initialized = 1;
@@ -302,79 +312,74 @@ eXdmadRC XDMAD_PrepareChannel(sXdmad *pXdmad, uint32_t dwChannel)
 
 /**
  * \brief xDMA interrupt handler
- * \param pxDmad Pointer to DMA driver instance.
+ * \param arg Pointer to DMA driver instance.
  */
-void XDMAD_Handler(sXdmad *pDmad)
+void XDMAD_Handler(void *arg)
 {
+	sXdmad *pDmad;
 	Xdmac *pXdmac;
 	sXdmadChannel *pCh;
 	uint32_t xdmaChannelIntStatus, xdmaGlobaIntStatus, xdmaGlobalChStatus;
-	uint8_t bExec = 0;
+	uint8_t bExec;
 	uint8_t _iChannel;
-	assert(pDmad != NULL);
 
+	pDmad = arg;
 	pXdmac = pDmad->pXdmacs;
-	xdmaGlobaIntStatus = XDMAC_GetGIsr(pXdmac);
-
-	if ((xdmaGlobaIntStatus & 0xFFFFFF) != 0) {
-		xdmaGlobalChStatus = XDMAC_GetGlobalChStatus(pXdmac);
+	xdmaGlobaIntStatus = XDMAC_GetGIsr(pXdmac) & 0xFFFFFF;
+	xdmaGlobalChStatus = XDMAC_GetGlobalChStatus(pXdmac);
 
-		for (_iChannel = 0; _iChannel < pDmad->numChannels; _iChannel ++) {
-			if (!(xdmaGlobaIntStatus & (1 << _iChannel))) continue;
+	while (xdmaGlobaIntStatus != 0) {
+		_iChannel = 31 - __builtin_clz(xdmaGlobaIntStatus);
+		xdmaGlobaIntStatus &= ~(UINT32_C(1) << _iChannel);
 
-			pCh = &pDmad->XdmaChannels[_iChannel];
+		pCh = &pDmad->XdmaChannels[_iChannel];
+		bExec = 0;
 
-			if (pCh->state == XDMAD_STATE_FREE) return;
+		if ((xdmaGlobalChStatus & (XDMAC_GS_ST0 << _iChannel)) == 0) {
+			xdmaChannelIntStatus = XDMAC_GetMaskChannelIsr(pXdmac, _iChannel);
 
-			if ((xdmaGlobalChStatus & (XDMAC_GS_ST0 << _iChannel)) == 0) {
-				bExec = 0;
-				xdmaChannelIntStatus = XDMAC_GetMaskChannelIsr(pXdmac, _iChannel);
-
-				if (xdmaChannelIntStatus & XDMAC_CIS_BIS) {
-					if ((XDMAC_GetChannelItMask(pXdmac, _iChannel) & XDMAC_CIM_LIM)
-						== 0) {
-						pCh->state = XDMAD_STATE_DONE;
-						bExec = 1;
-					}
-
-					TRACE_DEBUG("XDMAC_CIS_BIS\n\r");
+			if (xdmaChannelIntStatus & XDMAC_CIS_BIS) {
+				if ((XDMAC_GetChannelItMask(pXdmac, _iChannel) & XDMAC_CIM_LIM) == 0) {
+					pCh->state = XDMAD_STATE_DONE;
+					bExec = 1;
 				}
 
-				if (xdmaChannelIntStatus & XDMAC_CIS_FIS)
-					TRACE_DEBUG("XDMAC_CIS_FIS\n\r");
+				TRACE_DEBUG("XDMAC_CIS_BIS\n\r");
+			}
 
-				if (xdmaChannelIntStatus & XDMAC_CIS_RBEIS)
-					TRACE_DEBUG("XDMAC_CIS_RBEIS\n\r");
+			if (xdmaChannelIntStatus & XDMAC_CIS_FIS)
+				TRACE_DEBUG("XDMAC_CIS_FIS\n\r");
 
-				if (xdmaChannelIntStatus & XDMAC_CIS_WBEIS)
-					TRACE_DEBUG("XDMAC_CIS_WBEIS\n\r");
+			if (xdmaChannelIntStatus & XDMAC_CIS_RBEIS)
+				TRACE_DEBUG("XDMAC_CIS_RBEIS\n\r");
 
-				if (xdmaChannelIntStatus & XDMAC_CIS_ROIS)
-					TRACE_DEBUG("XDMAC_CIS_ROIS\n\r");
+			if (xdmaChannelIntStatus & XDMAC_CIS_WBEIS)
+				TRACE_DEBUG("XDMAC_CIS_WBEIS\n\r");
 
-				if (xdmaChannelIntStatus & XDMAC_CIS_LIS) {
-					TRACE_DEBUG("XDMAC_CIS_LIS\n\r");
-					pCh->state = XDMAD_STATE_DONE;
-					bExec = 1;
-				}
+			if (xdmaChannelIntStatus & XDMAC_CIS_ROIS)
+				TRACE_DEBUG("XDMAC_CIS_ROIS\n\r");
 
-				if (xdmaChannelIntStatus & XDMAC_CIS_DIS) {
-					pCh->state = XDMAD_STATE_DONE;
-					bExec = 1;
-				}
+			if (xdmaChannelIntStatus & XDMAC_CIS_LIS) {
+				TRACE_DEBUG("XDMAC_CIS_LIS\n\r");
+				pCh->state = XDMAD_STATE_DONE;
+				bExec = 1;
+			}
 
-			} else {
-				/* Block end interrupt for LLI dma mode */
-				if (XDMAC_GetChannelIsr(pXdmac, _iChannel) & XDMAC_CIS_BIS) {
-					/* Execute callback */
-					pCh->fCallback(_iChannel, pCh->pArg);
-				}
+			if (xdmaChannelIntStatus & XDMAC_CIS_DIS) {
+				pCh->state = XDMAD_STATE_DONE;
+				bExec = 1;
 			}
 
-			/* Execute callback */
-			if (bExec && pCh->fCallback)
-				pCh->fCallback(_iChannel, pCh->pArg);
+		} else {
+			/* Block end interrupt for LLI dma mode */
+			if (XDMAC_GetChannelIsr(pXdmac, _iChannel) & XDMAC_CIS_BIS) {
+				bExec = 1;
+			}
 		}
+
+		/* Execute callback */
+		if (bExec)
+			pCh->fCallback(_iChannel, pCh->pArg);
 	}
 }