leon, grspw_pkt: added work-task configuration options

Following changes:
     * possible for user to create work-tasks and assign custom  message queues.
     * possible for user to override default ISR message to implement custom
       handling of DMA error, DMA RX/TX and link error from ISR.
     * work-task now checks message to determine which work to perform rather than
       looking at registers only, this makes it possible for user to implement
       custom handling.
     * exported work-queue message definitions and separated them so that a user
       can assign custom DMA RX/TX handling of a specific DMA channel.
     * added a work-task event callback to let user add custom handling or
       monitoring of DMA Stop, DMA error, Link Error or work-task exits etc.

c/src/lib/libbsp/sparc/shared/include/grspw_pkt.h

@@ -311,6 +311,50 @@ struct grspw_dma_stats {
 	int rx_work_enabled;	/* No. RX BDs enabled by work thread */
 };
 
+/* ISR message sending call back. Compatible with rtems_message_queue_send().
+ * The 'buf' parameter has a pointer to a WORK-TASK message defined by the
+ * WORK_* macros below. The message indicates what GRSPW device operations
+ * are pending, thus what caused the interrupt.
+ *
+ * \param data   defined by grspw_work_config.msgisr_arg, default a rtems_id.
+ * \param buf    Pointer to a 32-bit message word
+ * \param n      Always 4 (byte size of buf).
+ */
+typedef int (*grspw_msgqisr_t)(void *data, unsigned int *buf, unsigned int n);
+
+/* Work message definitions, the int sent to *buf
+ * Bits 31..24: reserved.
+ * Bits 23..16: GRSPW device number message is associated with.
+ * Bit  15:     reserved.
+ * Bit  14:     work-task shall delete message queue on exit.
+ * Bit  13:     work-task shall exit and delete itself.
+ * Bit  12:     link error - shut down all DMA operations (stop DMA channels).
+ * Bit  11..8:  Indicats DMA error on DMA channel 3..0.
+ * Bit  7..0:   Indicats RX and/or TX packets completed on channel 3..0.
+ */
+#define WORK_NONE         0
+#define WORK_SHUTDOWN     0x1000 /* Signal shut down */
+#define WORK_QUIT_TASK    0x2000 /* Work task shall exit (delete itself) */
+#define WORK_FREE_MSGQ    0x4000 /* Delete MsgQ (valid when WORK_QUIT_TASK) */
+#define WORK_DMA(chan, rxtx) (((rxtx) & 0x3) << ((chan) * 2))
+#define WORK_DMA_TX(chan) WORK_DMA(chan, 1)
+#define WORK_DMA_RX(chan) WORK_DMA(chan, 2)
+#define WORK_DMA_ER(chan) (0x1 << ((chan) + 8))
+#define WORK_DMA_MASK     0xfff /* max 4 channels all work */
+#define WORK_DMA_TX_MASK  0x055 /* max 4 channels TX work */
+#define WORK_DMA_RX_MASK  0x0aa /* max 4 channels RX work */
+#define WORK_DMA_ER_MASK  0xf00 /* max 4 channels Error work */
+#define WORK_DMA_CHAN_MASK(chan) (WORK_DMA_ER(chan) | WORK_DMA(chan, 0x3))
+#define WORK_CORE_BIT     16
+#define WORK_CORE_MASK    0x00ff0000
+#define WORK_CORE(device) ((device) << WORK_CORE_BIT)
+
+/* Message Q used to send messages to work task */
+struct grspw_work_config {
+	grspw_msgqisr_t msgisr;
+	void *msgisr_arg; /* example: rtems_id to Msg Q */
+};
+
 extern void grspw_initialize_user(
 	/* Callback every time a GRSPW device is found. Args: DeviceIndex */
 	void *(*devfound)(int),
@@ -320,6 +364,60 @@ extern void grspw_initialize_user(
 	 */
 	void (*devremove)(int,void*)
 	);
+
+/* Creates a MsgQ (optional) and spawns a worker task associated with the
+ * message Q. The task can also be associated with a custom msgQ if *msgQ.
+ * is non-zero.
+ *
+ * \param prio     Task priority, set to -1 for default.
+ * \param stack    Task stack size, set to 0 for default.
+ * \param msgQ     pMsgQ=NULL: illegal,
+ *                 pMsqQ==0: create new MsgQ with task and place in *pMsgQ,
+ *                 *pmsqQ!=0: pointer to MsgQ used for task.
+ * \param msgMax   Maximum number of messages, set to 0 for default.
+ * \return         0 on failure, task id on success.
+ */
+extern rtems_id grspw_work_spawn(int prio, int stack, rtems_id *pMsgQ, int msgMax);
+
+/* Free task associated with message queue and optionally also the message
+ * queue itself. The message queue is deleted by the work task and is therefore
+ * delayed until it the work task resumes its execution.
+ */
+extern rtems_status_code grspw_work_free(rtems_id msgQ, int freeMsgQ);
+
+/* Configure a GRSPW device Work task and Message Q set up.
+ * This affects messages to:
+ *  - DMA AHB error interrupt handling (mandatory)
+ *  - Link status interrupt handling (optional)
+ *  - RX DMA, defaults to common msgQ (configured per DMA channel) 
+ */
+extern void grspw_work_cfg(void *d, struct grspw_work_config *wc);
+
+/* Work-task function, called only from the work task. The function is provided
+ * as a way for the user to create its own work tasks.
+ * The argument determines which message queue the task shall read its
+ * work jobs from.
+ *
+ * The messages are always 32-bit words and follows the format defined by the
+ * WORK_* macros above.
+ */
+extern void grspw_work_func(rtems_id msgQ);
+
+enum grspw_worktask_ev {
+	WORKTASK_EV_NONE = 0,
+	WORKTASK_EV_QUIT = 1,
+	WORKTASK_EV_SHUTDOWN = 2,
+	WORKTASK_EV_DMA_STOP = 3,
+};
+
+/* Weak function to let user override. Function called every time one of the
+ * events above is handled by the work-task. The message 'msg' is the current
+ * message being processed by the work-task.
+ * The user can for example add custom code to invoke on a DMA error, link
+ * error or monitor when the work-task exits after a call to grspw_work_free().
+ */
+extern void grspw_work_event(enum grspw_worktask_ev ev, unsigned int msg);
+
 extern int grspw_dev_count(void);
 extern void *grspw_open(int dev_no);
 extern int grspw_close(void *d);

c/src/lib/libbsp/sparc/shared/spw/grspw_pkt.c

@@ -478,13 +478,15 @@ struct grspw_priv {
 	/* Bit mask for link status bits to clear by ISR */
 	unsigned int stscfg;
 
+	/*** Message Queue Handling ***/
+	struct grspw_work_config wc;
+
 	/* "Core Global" Statistics gathered, not dependent on DMA channel */
 	struct grspw_core_stats stats;
 };
 
 int grspw_initialized = 0;
 int grspw_count = 0;
-struct workqueue_struct *grspw_workq = NULL;
 rtems_id grspw_sem;
 static struct grspw_priv *priv_tab[GRSPW_MAX];
 
@@ -492,20 +494,22 @@ static struct grspw_priv *priv_tab[GRSPW_MAX];
 void *(*grspw_dev_add)(int) = NULL;
 void (*grspw_dev_del)(int,void*) = NULL;
 
+/* Defaults to do nothing - user can override this function.
+ * Called from work-task.
+ */
+void __attribute__((weak)) grspw_work_event(
+	enum grspw_worktask_ev ev,
+	unsigned int msg)
+{
+
+}
+
 /* USER OVERRIDABLE - The work task priority. Set to -1 to disable creating
  * the work-task and work-queue to save space.
  */
 int grspw_work_task_priority __attribute__((weak)) = 100;
-int grspw_task_stop = 0;
 rtems_id grspw_work_task;
-rtems_id grspw_work_queue = 0;
+static struct grspw_work_config grspw_wc_def;
-#define WORK_NONE         0
-#define WORK_SHUTDOWN     0x100
-#define WORK_DMA(channel) (0x1 << (channel))
-#define WORK_DMA_MASK     0xf /* max 4 channels */
-#define WORK_CORE_BIT     16
-#define WORK_CORE_MASK    0xffff
-#define WORK_CORE(device) ((device) << WORK_CORE_BIT)
 
 STATIC void grspw_hw_stop(struct grspw_priv *priv);
 STATIC void grspw_hw_dma_stop(struct grspw_dma_priv *dma);
@@ -546,6 +550,11 @@ void *grspw_open(int dev_no)
 	priv->icisr_arg = NULL;
 	priv->stscfg = LINKSTS_MASK;
 
+	/* Default to common work queue and message queue, if not created
+	 * during initialization then its disabled.
+	 */
+	grspw_work_cfg(priv, &grspw_wc_def);
+
 	grspw_stats_clr(priv);
 
 	/* Allocate TX & RX Descriptor memory area for all DMA
@@ -2531,7 +2540,7 @@ static void grspw_work_shutdown_func(struct grspw_priv *priv)
 }
 
 /* Do DMA work on one channel, invoked indirectly from ISR */
-static void grspw_work_dma_func(struct grspw_dma_priv *dma)
+static void grspw_work_dma_func(struct grspw_dma_priv *dma, unsigned int msg)
 {
 	int tx_cond_true, rx_cond_true;
 	unsigned int ctrl;
@@ -2557,19 +2566,32 @@ static void grspw_work_dma_func(struct grspw_dma_priv *dma)
 	if (ctrl & GRSPW_DMA_STATUS_ERROR) {
 		/* DMA error -> Stop DMA channel (both RX and TX) */
 		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
+		if (msg & WORK_DMA_ER_MASK) {
+			/* DMA error and user wants work-task to handle error */
 			grspw_dma_stop(dma);
-	} else if (ctrl & (GRSPW_DMACTRL_PR | GRSPW_DMACTRL_PS)) {
+			grspw_work_event(WORKTASK_EV_DMA_STOP, msg);
-		/* DMA has finished a TX/RX packet */
+		}
+	} else if (((msg & WORK_DMA_RX_MASK) && (ctrl & GRSPW_DMACTRL_PR)) ||
+		   ((msg & WORK_DMA_TX_MASK) && (ctrl & GRSPW_DMACTRL_PS))) {
+		/* DMA has finished a TX/RX packet and user wants work-task to
+		 * take care of DMA table processing.
+		 */
 		ctrl &= ~GRSPW_DMACTRL_AT;
-		if (dma->cfg.rx_irq_en_cnt != 0 ||
+
+		if ((msg & WORK_DMA_RX_MASK) == 0)
+			ctrl &= ~GRSPW_DMACTRL_PR;
+		else if (dma->cfg.rx_irq_en_cnt != 0 ||
 			 (dma->cfg.flags & DMAFLAG2_RXIE))
 			ctrl |= GRSPW_DMACTRL_RI;
-		if (dma->cfg.tx_irq_en_cnt != 0 ||
+		if ((msg & WORK_DMA_TX_MASK) == 0)
-		    (dma->cfg.flags & DMAFLAG2_TXIE))
+			ctrl &= ~GRSPW_DMACTRL_PS;
+		else if ((dma->cfg.tx_irq_en_cnt != 0 ||
+			 (dma->cfg.flags & DMAFLAG2_TXIE)))
 			ctrl |= GRSPW_DMACTRL_TI;
+
 		REG_WRITE(&dma->regs->ctrl, ctrl);
 		SPIN_UNLOCK_IRQ(&priv->devlock, irqflags);
-		if (ctrl & GRSPW_DMACTRL_PR) {
+		if ((msg & WORK_DMA_RX_MASK) && (ctrl & GRSPW_DMACTRL_PR)) {
 			/* Do RX Work */
 
 			/* Take DMA channel RX lock */
@@ -2590,7 +2612,7 @@ static void grspw_work_dma_func(struct grspw_dma_priv *dma)
 			}
 			rtems_semaphore_release(dma->sem_rxdma);
 		}
-		if (ctrl & GRSPW_DMACTRL_PS) {
+		if ((msg & WORK_DMA_TX_MASK) && (ctrl & GRSPW_DMACTRL_PS)) {
 			/* Do TX Work */
 
 			/* Take DMA channel TX lock */
@@ -2624,42 +2646,50 @@ static void grspw_work_dma_func(struct grspw_dma_priv *dma)
 /* Work task is receiving work for the work message queue posted from
  * the ISR.
  */
-static void grspw_work_func(rtems_task_argument unused)
+void grspw_work_func(rtems_id msgQ)
 {
-	rtems_status_code status;
+	unsigned int message = 0, msg;
-	unsigned int message;
 	size_t size;
 	struct grspw_priv *priv;
 	int i;
 
-	while (grspw_task_stop == 0) {
 	/* Wait for ISR to schedule work */
-		status = rtems_message_queue_receive(
+	while (rtems_message_queue_receive(msgQ, &message, &size,
-			grspw_work_queue, &message,
+	       RTEMS_WAIT, RTEMS_NO_TIMEOUT) == RTEMS_SUCCESSFUL) {
-			&size, RTEMS_WAIT, RTEMS_NO_TIMEOUT);
+		if (message & WORK_QUIT_TASK)
-		if (status != RTEMS_SUCCESSFUL)
 			break;
 
 		/* Handle work */
 		priv = priv_tab[message >> WORK_CORE_BIT];
-		if (message & WORK_SHUTDOWN)
+		if (message & WORK_SHUTDOWN) {
 			grspw_work_shutdown_func(priv);
-		else if (message & WORK_DMA_MASK) {
+				
-			for (i = 0; i < 4; i++) {
+			grspw_work_event(WORKTASK_EV_SHUTDOWN, message);
-				if (message & WORK_DMA(i))
+		} else if (message & WORK_DMA_MASK) {
-					grspw_work_dma_func(&priv->dma[i]);
+			for (i = 0; i < priv->hwsup.ndma_chans; i++) {
+				msg = message &
+				      (WORK_CORE_MASK | WORK_DMA_CHAN_MASK(i));
+				if (msg)
+					grspw_work_dma_func(&priv->dma[i], msg);
 			}
 		}
+		message = 0;
 	}
+
+	if (message & WORK_FREE_MSGQ)
+		rtems_message_queue_delete(msgQ);
+
+	grspw_work_event(WORKTASK_EV_QUIT, message);
 	rtems_task_delete(RTEMS_SELF);
 }
 
 STATIC void grspw_isr(void *data)
 {
 	struct grspw_priv *priv = data;
-	unsigned int dma_stat, stat, stat_clrmsk, ctrl, icctrl, timecode;
+	unsigned int dma_stat, stat, stat_clrmsk, ctrl, icctrl, timecode, irqs;
 	unsigned int rxirq, rxack, intto;
-	int i, handled = 0, message = WORK_NONE, call_user_int_isr;
+	int i, handled = 0, call_user_int_isr;
+	unsigned int message = WORK_NONE;
 #ifdef RTEMS_HAS_SMP
 	IRQFLAGS_TYPE irqflags;
 #endif
@@ -2767,12 +2797,11 @@ STATIC void grspw_isr(void *data)
 		/* Check for Errors and if Packets been sent or received if
 		 * respective IRQ are enabled
 		 */
-#ifdef HW_WITH_GI
+		irqs = (((dma_stat << 3) & (GRSPW_DMACTRL_PR | GRSPW_DMACTRL_PS))
-		if ( dma_stat & (GRSPW_DMA_STATUS_ERROR | GRSPW_DMACTRL_GI) ) {
+			| GRSPW_DMA_STATUS_ERROR) & dma_stat;
-#else
+		if (!irqs)
-		if ( (((dma_stat << 3) & (GRSPW_DMACTRL_PR | GRSPW_DMACTRL_PS))
+			continue;
-		     | GRSPW_DMA_STATUS_ERROR) & dma_stat ) {
+
-#endif
 		/* Disable Further IRQs (until enabled again)
 		 * from this DMA channel. Let the status
 		 * bit remain so that they can be handled by
@@ -2783,8 +2812,17 @@ STATIC void grspw_isr(void *data)
 			GRSPW_DMACTRL_PR|GRSPW_DMACTRL_PS|
 			GRSPW_DMACTRL_RA|GRSPW_DMACTRL_TA|
 			GRSPW_DMACTRL_AT));
-			message |= WORK_DMA(i);
 		handled = 1;
+
+		/* DMA error has priority, if error happens it is assumed that
+		 * the common work-queue stops the DMA operation for that
+		 * channel and makes the DMA tasks exit from their waiting
+		 * functions (both RX and TX tasks).
+		 */
+		if (irqs & GRSPW_DMA_STATUS_ERROR) {
+			message |= WORK_DMA_ER(i);
+		} else {
+			message |= WORK_DMA(i, irqs >> GRSPW_DMACTRL_PS_BIT);
 		}
 	}
 	SPIN_UNLOCK(&priv->devlock, irqflags);
@@ -2793,12 +2831,19 @@ STATIC void grspw_isr(void *data)
 		priv->stats.irq_cnt++;
 
 	/* Schedule work by sending message to work thread */
-	if ((message != WORK_NONE) && grspw_work_queue) {
+	if (message != WORK_NONE && priv->wc.msgisr) {
+		int status;
 		message |= WORK_CORE(priv->index);
-		stat = rtems_message_queue_send(grspw_work_queue, &message, 4);
+		/* func interface compatible with msgQSend() on purpose, but
-		if (stat != RTEMS_SUCCESSFUL)
+		 * at the same time the user can assign a custom function to
+		 * handle DMA RX/TX operations as indicated by the "message"
+		 * and clear the handled bits before given to msgQSend().
+		 */
+		status = priv->wc.msgisr(priv->wc.msgisr_arg, &message, 4);
+		if (status != RTEMS_SUCCESSFUL) {
 			printk("grspw_isr(%d): message fail %d (0x%x)\n",
-				priv->index, stat, message);
+				priv->index, status, message);
+		}
 	}
 }
 
@@ -3070,6 +3115,72 @@ static int grspw2_init3(struct drvmgr_dev *dev)
 }
 
 /******************* Driver Implementation ***********************/
+/* Creates a MsgQ (optional) and spawns a worker task associated with the
+ * message Q. The task can also be associated with a custom msgQ if *msgQ.
+ * is non-zero.
+ */
+rtems_id grspw_work_spawn(int prio, int stack, rtems_id *pMsgQ, int msgMax)
+{
+	rtems_id tid;
+	int created_msgq = 0;
+
+	if (pMsgQ == NULL)
+		return OBJECTS_ID_NONE;
+
+	if (*pMsgQ == OBJECTS_ID_NONE) {
+		if (msgMax <= 0)
+			msgMax = 32;
+
+		if (rtems_message_queue_create(
+			rtems_build_name('S', 'G', 'L', 'Q'),
+			msgMax, 4, RTEMS_FIFO, pMsgQ) !=
+			RTEMS_SUCCESSFUL)
+			return OBJECTS_ID_NONE;
+		created_msgq = 1;
+	}
+
+	if (prio < 0)
+		prio = grspw_work_task_priority; /* default prio */
+	if (stack < 0x800)
+		stack = RTEMS_MINIMUM_STACK_SIZE; /* default stack size */
+
+	if (rtems_task_create(rtems_build_name('S', 'G', 'L', 'T'),
+		prio, stack, RTEMS_PREEMPT | RTEMS_NO_ASR,
+		RTEMS_NO_FLOATING_POINT, &tid) != RTEMS_SUCCESSFUL)
+		tid = OBJECTS_ID_NONE;
+	else if (rtems_task_start(tid, (rtems_task_entry)grspw_work_func, *pMsgQ) !=
+		    RTEMS_SUCCESSFUL) {
+		rtems_task_delete(tid);
+		tid = OBJECTS_ID_NONE;
+	}
+
+	if (tid == OBJECTS_ID_NONE && created_msgq) {
+		rtems_message_queue_delete(*pMsgQ);
+		*pMsgQ = OBJECTS_ID_NONE;
+	}
+	return tid;
+}
+
+/* Free task associated with message queue and optionally also the message
+ * queue itself. The message queue is deleted by the work task and is therefore
+ * delayed until it the work task resumes its execution.
+ */
+rtems_status_code grspw_work_free(rtems_id msgQ, int freeMsgQ)
+{
+	int msg = WORK_QUIT_TASK;
+	if (freeMsgQ)
+		msg |= WORK_FREE_MSGQ;
+	return rtems_message_queue_send(msgQ, &msg, 4);
+}
+
+void grspw_work_cfg(void *d, struct grspw_work_config *wc)
+{
+	struct grspw_priv *priv = (struct grspw_priv *)d;
+
+	if (wc == NULL)
+		wc = &grspw_wc_def; /* use default config */
+	priv->wc = *wc;
+}
 
 static int grspw_common_init(void)
 {
@@ -3090,21 +3201,16 @@ static int grspw_common_init(void)
 	 * user can disable it when interrupt is not used to save resources
 	 */
 	if (grspw_work_task_priority != -1) {
-		if (rtems_message_queue_create(
+		grspw_work_task = grspw_work_spawn(-1, 0,
-		    rtems_build_name('S', 'G', 'L', 'Q'), 32, 4, RTEMS_FIFO,
+			(rtems_id *)&grspw_wc_def.msgisr_arg, 0);
-		    &grspw_work_queue) != RTEMS_SUCCESSFUL)
+		if (grspw_work_task == OBJECTS_ID_NONE)
-			return -1;
+			return -2;
-
+		grspw_wc_def.msgisr =
-		if (rtems_task_create(rtems_build_name('S', 'G', 'L', 'T'),
+			(grspw_msgqisr_t) rtems_message_queue_send;
-		    grspw_work_task_priority, RTEMS_MINIMUM_STACK_SIZE,
+	} else {
-		    RTEMS_PREEMPT | RTEMS_NO_ASR, RTEMS_NO_FLOATING_POINT,
+		grspw_wc_def.msgisr = NULL;
-		    &grspw_work_task) != RTEMS_SUCCESSFUL)
+		grspw_wc_def.msgisr_arg = NULL;
-			return -1;
+	}
-
-		if (rtems_task_start(grspw_work_task, grspw_work_func, 0) !=
-		    RTEMS_SUCCESSFUL)
-			return -1;
-}
 
 	grspw_initialized = 1;
 	return 0;