Update for MPC55XX changes

1 files changed, 153 insertions, 160 deletions

diff --git a/c/src/lib/libcpu/powerpc/mpc55xx/edma/edma.c b/c/src/lib/libcpu/powerpc/mpc55xx/edma/edma.c
index 1572cce0b2..dc7adc136e 100644
--- a/c/src/lib/libcpu/powerpc/mpc55xx/edma/edma.c
+++ b/c/src/lib/libcpu/powerpc/mpc55xx/edma/edma.c
@@ -22,164 +22,148 @@
 #include <mpc55xx/edma.h>
 #include <mpc55xx/mpc55xx.h>
 
-#include <bsp/irq.h>
-
 #include <string.h>
 
+#include <bsp/irq.h>
+#include <bsp/utility.h>
+
 #define RTEMS_STATUS_CHECKS_USE_PRINTK
 
 #include <rtems/status-checks.h>
 
-#define MPC55XX_EDMA_CHANNEL_NUMBER 64
-#define MPC55XX_EDMA_INVALID_CHANNEL UINT8_MAX
-#define MPC55XX_EDMA_IS_CHANNEL_INVALID( i) ((i) < 0 || (i) >= MPC55XX_EDMA_CHANNEL_NUMBER)
-
-#define MPC55XX_EDMA_IRQ_PRIORITY MPC55XX_INTC_MIN_PRIORITY
+#define MPC55XX_EDMA_CHANNEL_NUMBER 64U
 
-typedef struct {
-	uint8_t channel;
-	rtems_id transfer_update;
-	uint32_t *error_status;
-} mpc55xx_edma_channel_entry;
+#define MPC55XX_EDMA_INVALID_CHANNEL MPC55XX_EDMA_CHANNEL_NUMBER
 
-static mpc55xx_edma_channel_entry mpc55xx_edma_channel_table [MPC55XX_EDMA_CHANNEL_NUMBER];
+#define MPC55XX_EDMA_IS_CHANNEL_INVALID( i) ((unsigned) (i) >= MPC55XX_EDMA_CHANNEL_NUMBER)
 
-static uint32_t mpc55xx_edma_channel_occupation_low = 0;
+#define MPC55XX_EDMA_IS_CHANNEL_VALID( i) ((unsigned) (i) < MPC55XX_EDMA_CHANNEL_NUMBER)
 
-static uint32_t mpc55xx_edma_channel_occupation_high = 0;
+#define MPC55XX_EDMA_IRQ_PRIORITY MPC55XX_INTC_DEFAULT_PRIORITY
 
-static rtems_id mpc55xx_edma_channel_occupation_mutex = RTEMS_ID_NONE;
+#define MPC55XX_EDMA_CHANNEL_FLAG( channel) ((uint64_t) 1 << (channel))
 
-static uint8_t mpc55xx_edma_irq_error_low_channel = 0;
+static uint64_t mpc55xx_edma_channel_occupation = 0;
 
-static uint8_t mpc55xx_edma_irq_error_high_channel = 32;
+static rtems_chain_control mpc55xx_edma_channel_chain;
 
-static void mpc55xx_edma_irq_handler( rtems_vector_number vector, void *data)
+static void mpc55xx_edma_interrupt_handler( rtems_vector_number vector, void *arg)
 {
-	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	mpc55xx_edma_channel_entry *e = (mpc55xx_edma_channel_entry *) data;
+	mpc55xx_edma_channel_entry *e = (mpc55xx_edma_channel_entry *) arg;
+
 #ifdef DEBUG
 	uint32_t citer = EDMA.TCD [e->channel].CITERE_LINK ? EDMA.TCD [e->channel].CITER & EDMA_TCD_BITER_LINKED_MASK : EDMA.TCD [e->channel].CITER;
-	RTEMS_DEBUG_PRINT( "Channel %i (CITER = %i)\n", e->channel, citer);
+	RTEMS_DEBUG_PRINT( "channel %i (CITER = %i)\n", e->channel, citer);
 #endif /* DEBUG */
-	EDMA.CIRQR.R = e->channel;
-	sc = rtems_semaphore_release( e->transfer_update);
-	RTEMS_SYSLOG_WARNING_SC( sc, "Transfer update semaphore release");
-}
 
-static void mpc55xx_edma_irq_update_error_table( uint8_t *link_table, uint8_t *error_table, int channel)
-{
-	int i = 0;
-	error_table [channel] = 1;
-	for (i = 0; i < MPC55XX_EDMA_CHANNEL_NUMBER; ++i) {
-		if (channel == link_table [i] && error_table [i] == 0) {
-			mpc55xx_edma_irq_update_error_table( link_table, error_table, i);
-		}
-	}
+	/* Clear interrupt */
+	EDMA.CIRQR.R = (uint8_t) e->channel;
+
+	/* Notify user */
+	e->done( e, 0);
 }
 
-static void mpc55xx_edma_irq_error_handler( rtems_vector_number vector, void *data)
+static void mpc55xx_edma_interrupt_error_handler( rtems_vector_number vector, void *arg)
 {
-	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	uint8_t channel_start = *((uint8_t *) data);
-	uint8_t channel_end = (uint8_t) (channel_start + 32);
-	int i = 0;
-	uint32_t mask = 0x1;
-	uint32_t error_register = 0;
-	uint8_t channel_link_table [MPC55XX_EDMA_CHANNEL_NUMBER];
-	uint8_t channel_error_table [MPC55XX_EDMA_CHANNEL_NUMBER];
-
-	/* Error register */
-	if (channel_start < 32) {
-		error_register = EDMA.ERL.R;
-	} else if (channel_start < 64) {
-		error_register = EDMA.ERH.R;
-	}
-	RTEMS_DEBUG_PRINT( "Error register %s: 0x%08x\n", channel_start < 32 ? "low" : "high", error_register);
+	rtems_chain_control *chain = &mpc55xx_edma_channel_chain;
+	rtems_chain_node *node = chain->first;
+	unsigned i = 0;
+	uint64_t error_status = EDMA.ESR.R;
+	uint64_t error_channels = ((uint64_t) EDMA.ERH.R << 32) | EDMA.ERL.R;
+	uint64_t error_channels_update = 0;
+
+	RTEMS_DEBUG_PRINT( "error channels: %08x %08x\n", (unsigned) (error_channels >> 32), (unsigned) error_channels);
+
+	/* Mark all channels that are linked to a channel with errors */
+	do {
+		error_channels_update = 0;
+
+		for (i = 0; i < MPC55XX_EDMA_CHANNEL_NUMBER; ++i) {
+			uint64_t channel_flags = 0;
+			unsigned minor_link = i;
+			unsigned major_link = i;
+
+			/* Check if we have linked channels */
+			if (EDMA.TCD [i].BMF.B.BITERE_LINK) {
+				minor_link = EDMA_TCD_BITER_LINK( i);
+			}
+			if (EDMA.TCD [i].BMF.B.MAJORE_LINK) {
+				major_link = EDMA.TCD [i].BMF.B.MAJORLINKCH;
+			}
 
-	/* Fill channel link table */
-	for (i = 0; i < MPC55XX_EDMA_CHANNEL_NUMBER; ++i) {
-		if (EDMA.TCD [i].BITERE_LINK && EDMA.TCD [i].CITER != EDMA.TCD [i].BITER) {
-			channel_link_table [i] = (uint8_t) EDMA_TCD_BITER_LINK( i);
-		} else if (EDMA.TCD [i].MAJORE_LINK && EDMA.TCD [i].CITER == EDMA.TCD [i].BITER) {
-			channel_link_table [i] = EDMA.TCD [i].MAJORLINKCH;
-		} else {
-			channel_link_table [i] = MPC55XX_EDMA_INVALID_CHANNEL;
-		}
-		channel_error_table [i] = 0;
-	}
+			/* Set flags related to this channel */
+			channel_flags = MPC55XX_EDMA_CHANNEL_FLAG( i) | MPC55XX_EDMA_CHANNEL_FLAG( minor_link) | MPC55XX_EDMA_CHANNEL_FLAG( major_link);
+
+			/* Any errors in these channels? */
+			if (IS_ANY_FLAG_SET( error_channels, channel_flags)) {
+				/* Get new error channels */
+				uint64_t update = (error_channels & channel_flags) ^ channel_flags;
 
-	/* Search for channels with errors */
-	for (i = channel_start; i < channel_end; ++i) {
-		if ((error_register & mask) != 0) {
-			mpc55xx_edma_irq_update_error_table( channel_link_table, channel_error_table, i);
+				/* Update error channels */
+				error_channels = SET_FLAGS( error_channels, channel_flags);
+
+				/* Contribute to the update of this round */
+				error_channels_update = SET_FLAGS( error_channels_update, update);
+			}
 		}
-		mask <<= 1;
-	}
+	} while (error_channels_update != 0);
+
+	RTEMS_DEBUG_PRINT( "error channels (all): %08x %08x\n", (unsigned) (error_channels >> 32), (unsigned) error_channels);
 
 	/* Process the channels related to errors */
-	error_register = EDMA.ESR.R;
-	for (i = 0; i < MPC55XX_EDMA_CHANNEL_NUMBER; ++i) {
-		if (channel_error_table [i]) {
-			mpc55xx_edma_channel_entry *e = &mpc55xx_edma_channel_table [i];
-			if (e->error_status != NULL) {
-				*e->error_status = error_register;
-			}
-			sc = mpc55xx_edma_enable_hardware_requests( i, false);
-			RTEMS_SYSLOG_ERROR_SC( sc, "Disable hardware requests");
-			sc = rtems_semaphore_release( e->transfer_update);
-			RTEMS_SYSLOG_WARNING_SC( sc, "Transfer update semaphore release");
+	while (!rtems_chain_is_tail( chain, node)) {
+		mpc55xx_edma_channel_entry *e = (mpc55xx_edma_channel_entry *) node;
+
+		if (IS_FLAG_SET( error_channels, MPC55XX_EDMA_CHANNEL_FLAG( e->channel))) {
+			mpc55xx_edma_enable_hardware_requests( e->channel, false);
+
+			/* Notify user */
+			e->done( e, error_status);
 		}
+
+		node = node->next;
 	}
 
 	/* Clear the error interrupt requests */
 	for (i = 0; i < MPC55XX_EDMA_CHANNEL_NUMBER; ++i) {
-		if (channel_error_table [i]) {
+		if (IS_FLAG_SET( error_channels, MPC55XX_EDMA_CHANNEL_FLAG( i))) {
 			EDMA.CER.R = (uint8_t) i;
 		}
 	}
 }
 
-rtems_status_code mpc55xx_edma_enable_hardware_requests( int channel, bool enable)
+void mpc55xx_edma_enable_hardware_requests( unsigned channel, bool enable)
 {
-	if (MPC55XX_EDMA_IS_CHANNEL_INVALID( channel)) {
-		return RTEMS_INVALID_NUMBER;
-	}
-	if (enable) {
-		EDMA.SERQR.R = (uint8_t) channel;
+	if (MPC55XX_EDMA_IS_CHANNEL_VALID( channel)) {
+		if (enable) {
+			EDMA.SERQR.R = (uint8_t) channel;
+		} else {
+			EDMA.CERQR.R = (uint8_t) channel;
+		}
 	} else {
-		EDMA.CERQR.R = (uint8_t) channel;
+		RTEMS_SYSLOG_ERROR( "invalid channel number\n");
 	}
-	return RTEMS_SUCCESSFUL;
 }
 
-rtems_status_code mpc55xx_edma_enable_error_interrupts( int channel, bool enable)
+void mpc55xx_edma_enable_error_interrupts( unsigned channel, bool enable)
 {
-	if (MPC55XX_EDMA_IS_CHANNEL_INVALID( channel)) {
-		return RTEMS_INVALID_NUMBER;
-	}
-	if (enable) {
-		EDMA.SEEIR.R = channel;
+	if (MPC55XX_EDMA_IS_CHANNEL_VALID( channel)) {
+		if (enable) {
+			EDMA.SEEIR.R = (uint8_t) channel;
+		} else {
+			EDMA.CEEIR.R = (uint8_t) channel;
+		}
 	} else {
-		EDMA.CEEIR.R = channel;
+		RTEMS_SYSLOG_ERROR( "invalid channel number\n");
 	}
-	return RTEMS_SUCCESSFUL;
 }
 
 rtems_status_code mpc55xx_edma_init()
 {
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	int i = 0;
-
-	/* Channel occupation mutex */
-	sc = rtems_semaphore_create (
-		rtems_build_name ( 'D', 'M', 'A', 'O'),
-		1,
-		RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_INHERIT_PRIORITY | RTEMS_PRIORITY,
-		RTEMS_NO_PRIORITY,
-		&mpc55xx_edma_channel_occupation_mutex
-	);
-	RTEMS_CHECK_SC( sc, "Create channel occupation mutex");
+
+	/* Initialize channel chain */
+	rtems_chain_initialize_empty( &mpc55xx_edma_channel_chain);
 
 	/* Arbitration mode: round robin */
 	EDMA.CR.B.ERCA = 1;
@@ -188,92 +172,101 @@ rtems_status_code mpc55xx_edma_init()
 	/* Clear TCDs */
 	memset( &EDMA.TCD [0], 0, sizeof( EDMA.TCD));
 
-	/* Channel table */
-	for (i = 0; i < MPC55XX_EDMA_CHANNEL_NUMBER; ++i) {
-		mpc55xx_edma_channel_table [i].channel = i;
-		mpc55xx_edma_channel_table [i].transfer_update = RTEMS_ID_NONE;
-		mpc55xx_edma_channel_table [i].error_status = NULL;
-	}
-
-	/* Error interrupt handler */
+	/* Error interrupt handlers */
 	sc = mpc55xx_interrupt_handler_install(
 		MPC55XX_IRQ_EDMA_ERROR_LOW,
-		MPC55XX_EDMA_IRQ_PRIORITY,
 		"eDMA Error (Low)",
 		RTEMS_INTERRUPT_UNIQUE,
-		mpc55xx_edma_irq_error_handler,
-		&mpc55xx_edma_irq_error_low_channel
+		MPC55XX_EDMA_IRQ_PRIORITY,
+		mpc55xx_edma_interrupt_error_handler,
+		NULL
 	);
-	RTEMS_CHECK_SC( sc, "Install low error interrupt handler");
+	RTEMS_CHECK_SC( sc, "install low error interrupt handler");
 	sc = mpc55xx_interrupt_handler_install(
 		MPC55XX_IRQ_EDMA_ERROR_HIGH,
-		MPC55XX_EDMA_IRQ_PRIORITY,
 		"eDMA Error (High)",
 		RTEMS_INTERRUPT_UNIQUE,
-		mpc55xx_edma_irq_error_handler,
-		&mpc55xx_edma_irq_error_high_channel
+		MPC55XX_EDMA_IRQ_PRIORITY,
+		mpc55xx_edma_interrupt_error_handler,
+		NULL
 	);
-	RTEMS_CHECK_SC( sc, "Install high error interrupt handler");
+	RTEMS_CHECK_SC( sc, "install high error interrupt handler");
 
 	return RTEMS_SUCCESSFUL;
 }
 
-rtems_status_code mpc55xx_edma_obtain_channel( int channel, uint32_t *error_status, rtems_id transfer_update)
+rtems_status_code mpc55xx_edma_obtain_channel( mpc55xx_edma_channel_entry *e)
 {
 	rtems_status_code sc = RTEMS_SUCCESSFUL;
-	int channel_occupied = 1;
+	rtems_interrupt_level level;
+	uint64_t channel_occupation = 0;
 
-	if (MPC55XX_EDMA_IS_CHANNEL_INVALID( channel)) {
+	if (MPC55XX_EDMA_IS_CHANNEL_INVALID( e->channel)) {
 		return RTEMS_INVALID_NUMBER;
 	}
 
-	/* Check occupation */
-	sc = rtems_semaphore_obtain( mpc55xx_edma_channel_occupation_mutex, RTEMS_WAIT, 0);
-	RTEMS_CHECK_SC( sc, "Obtain channel occupation mutex");
-	if (channel < 32) {
-		channel_occupied = mpc55xx_edma_channel_occupation_low & (0x1 << channel);
-		if (!channel_occupied) {
-			mpc55xx_edma_channel_occupation_low |= 0x1 << channel;
-		}
-	} else if (channel < 64) {
-		channel_occupied = mpc55xx_edma_channel_occupation_high & (0x1 << (channel - 32));
-		if (!channel_occupied) {
-			mpc55xx_edma_channel_occupation_high |= 0x1 << (channel - 32);
-		}
+	/* Test and set channel occupation flag */
+	rtems_interrupt_disable( level);
+	channel_occupation = mpc55xx_edma_channel_occupation;
+	if (IS_FLAG_CLEARED( channel_occupation, MPC55XX_EDMA_CHANNEL_FLAG( e->channel))) {
+		mpc55xx_edma_channel_occupation = SET_FLAG( channel_occupation, MPC55XX_EDMA_CHANNEL_FLAG( e->channel));
 	}
-	if (channel_occupied) {
-		sc = rtems_semaphore_release( mpc55xx_edma_channel_occupation_mutex);
-		RTEMS_SYSLOG_WARNING_SC( sc, "Release occupation mutex");
+	rtems_interrupt_enable( level);
+
+	/* Check channel occupation flag */
+	if (IS_FLAG_SET( channel_occupation, MPC55XX_EDMA_CHANNEL_FLAG( e->channel))) {
 		return RTEMS_RESOURCE_IN_USE;
-	} else {
-		sc = rtems_semaphore_release( mpc55xx_edma_channel_occupation_mutex);
-		RTEMS_CHECK_SC( sc, "Release channel occupation mutex");
 	}
 
-	/* Channel data */
-	mpc55xx_edma_channel_table [channel].transfer_update = transfer_update;
-	mpc55xx_edma_channel_table [channel].error_status = error_status;
-
 	/* Interrupt handler */
 	sc = mpc55xx_interrupt_handler_install(
-		MPC55XX_IRQ_EDMA_GET_REQUEST( channel),
-		MPC55XX_EDMA_IRQ_PRIORITY,
+		MPC55XX_IRQ_EDMA_GET_REQUEST( e->channel),
 		"eDMA Channel",
 		RTEMS_INTERRUPT_SHARED,
-		mpc55xx_edma_irq_handler,
-		&mpc55xx_edma_channel_table [channel]
+		MPC55XX_EDMA_IRQ_PRIORITY,
+		mpc55xx_edma_interrupt_handler,
+		e
 	);
-	RTEMS_CHECK_SC( sc, "Install channel interrupt handler");
+	RTEMS_CHECK_SC( sc, "install channel interrupt handler");
 
 	/* Enable error interrupts */
-	sc = mpc55xx_edma_enable_error_interrupts( channel, true);
-	RTEMS_CHECK_SC( sc, "Enable error interrupts");
+	mpc55xx_edma_enable_error_interrupts( e->channel, true);
+
+	/* Prepend channel entry to channel list */
+	rtems_chain_prepend( &mpc55xx_edma_channel_chain, &e->node);
 
 	return RTEMS_SUCCESSFUL;
 }
 
-rtems_status_code mpc55xx_edma_release_channel( int channel)
+rtems_status_code mpc55xx_edma_release_channel( mpc55xx_edma_channel_entry *e)
 {
-	// TODO
-	return RTEMS_NOT_IMPLEMENTED;
+	rtems_status_code sc = RTEMS_SUCCESSFUL;
+	rtems_interrupt_level level;
+
+	/* Clear channel occupation flag */
+	rtems_interrupt_disable( level);
+	mpc55xx_edma_channel_occupation = CLEAR_FLAG( mpc55xx_edma_channel_occupation, MPC55XX_EDMA_CHANNEL_FLAG( e->channel));
+	rtems_interrupt_enable( level);
+
+	/* Disable hardware requests */
+	mpc55xx_edma_enable_hardware_requests( e->channel, false);
+
+	/* Disable error interrupts */
+	mpc55xx_edma_enable_error_interrupts( e->channel, false);
+
+	/* Extract channel entry from channel chain */
+	rtems_chain_extract( &e->node);
+
+	/* Remove interrupt handler */
+	sc = rtems_interrupt_handler_remove(
+		MPC55XX_IRQ_EDMA_GET_REQUEST( e->channel),
+		mpc55xx_edma_interrupt_handler,
+		e
+	);
+	RTEMS_CHECK_SC( sc, "remove channel interrupt handler");
+
+	/* Notify user */
+	e->done( e, 0);
+
+	return RTEMS_SUCCESSFUL;
 }