Added taskqueue to resolve linking issues with the RealTek Nic.

5 files changed, 599 insertions, 2 deletions

diff --git a/Makefile b/Makefile
index f6c9d384..cd06210f 100644
--- a/Makefile
+++ b/Makefile
@@ -374,7 +374,8 @@ C_FILES += \
 	rtemsbsd/src/rtems-bsd-sysctl.c \
 	rtemsbsd/src/rtems-bsd-sysctlbyname.c \
 	rtemsbsd/src/rtems-bsd-sysctlnametomib.c \
-	rtemsbsd/src/rtems-bsd-uma.c
+	rtemsbsd/src/rtems-bsd-uma.c \
+	rtemsbsd/src/rtems-bsd-taskqueue.c
 
 ifeq ($(RTEMS_CPU),arm)
 C_FILES += \
diff --git a/freebsd-to-rtems.py b/freebsd-to-rtems.py
index f5e49f88..cbea9143 100755
--- a/freebsd-to-rtems.py
+++ b/freebsd-to-rtems.py
@@ -487,6 +487,7 @@ rtems_headerFiles = [
 	'rtems/machine/rtems-bsd-cache.h',
 	'rtems/machine/rtems-bsd-sysinit.h',
         'rtems/machine/rtems-bsd-select.h',
+        'rtems/machine/rtems-bsd-taskqueue.h',
         #'rtems/machine/vm.h',
 	'bsd.h',
 	]
@@ -526,6 +527,7 @@ rtems_sourceFiles = [
         'src/rtems-bsd-sysctlbyname.c',
         'src/rtems-bsd-sysctlnametomib.c',
         'src/rtems-bsd-uma.c',
+        'src/rtems-bsd-taskqueue.c',
 	]
 # RTEMS files handled separately from modules
 # rtems = Module('rtems')
@@ -1273,7 +1275,7 @@ netDeps.addHeaderFiles(
 		'sys/sdt.h',
 		'sys/_task.h',
 		'sys/sbuf.h',
-		#'sys/smp.h',
+		'sys/smp.h',
 		'sys/syslog.h',
 		'sys/jail.h',
 		'sys/protosw.h',
diff --git a/freebsd/sys/smp.h b/freebsd/sys/smp.h
new file mode 100644
index 00000000..c73bec25
--- /dev/null
+++ b/freebsd/sys/smp.h
@@ -0,0 +1,183 @@
+/*-
+ * ----------------------------------------------------------------------------
+ * "THE BEER-WARE LICENSE" (Revision 42):
+ * <phk@FreeBSD.org> wrote this file.  As long as you retain this notice you
+ * can do whatever you want with this stuff. If we meet some day, and you think
+ * this stuff is worth it, you can buy me a beer in return.   Poul-Henning Kamp
+ * ----------------------------------------------------------------------------
+ *
+ * $FreeBSD$
+ */
+
+#ifndef _SYS_SMP_HH_
+#define _SYS_SMP_HH_
+
+#ifdef _KERNEL
+
+#ifndef LOCORE
+
+#ifdef SMP
+
+/*
+ * Topology of a NUMA or HTT system.
+ *
+ * The top level topology is an array of pointers to groups.  Each group
+ * contains a bitmask of cpus in its group or subgroups.  It may also
+ * contain a pointer to an array of child groups.
+ *
+ * The bitmasks at non leaf groups may be used by consumers who support
+ * a smaller depth than the hardware provides.
+ *
+ * The topology may be omitted by systems where all CPUs are equal.
+ */
+
+struct cpu_group {
+	struct cpu_group *cg_parent;	/* Our parent group. */
+	struct cpu_group *cg_child;	/* Optional children groups. */
+	cpumask_t	cg_mask;	/* Mask of cpus in this group. */
+	int8_t		cg_count;	/* Count of cpus in this group. */
+	int8_t		cg_children;	/* Number of children groups. */
+	int8_t		cg_level;	/* Shared cache level. */
+	int8_t		cg_flags;	/* Traversal modifiers. */
+};
+
+/*
+ * Defines common resources for CPUs in the group.  The highest level
+ * resource should be used when multiple are shared.
+ */
+#define	CG_SHARE_NONE	0
+#define	CG_SHARE_L1	1
+#define	CG_SHARE_L2	2
+#define	CG_SHARE_L3	3
+
+/*
+ * Behavior modifiers for load balancing and affinity.
+ */
+#define	CG_FLAG_HTT	0x01		/* Schedule the alternate core last. */
+#define	CG_FLAG_SMT	0x02		/* New age htt, less crippled. */
+#define	CG_FLAG_THREAD	(CG_FLAG_HTT | CG_FLAG_SMT)	/* Any threading. */
+
+/*
+ * Convenience routines for building topologies.
+ */
+struct cpu_group *smp_topo(void);
+struct cpu_group *smp_topo_none(void);
+struct cpu_group *smp_topo_1level(int l1share, int l1count, int l1flags);
+struct cpu_group *smp_topo_2level(int l2share, int l2count, int l1share,
+    int l1count, int l1flags);
+struct cpu_group *smp_topo_find(struct cpu_group *top, int cpu);
+
+extern void (*cpustop_restartfunc)(void);
+extern int smp_active;
+extern int smp_cpus;
+extern volatile cpumask_t started_cpus;
+extern volatile cpumask_t stopped_cpus;
+extern cpumask_t idle_cpus_mask;
+extern cpumask_t hlt_cpus_mask;
+extern cpumask_t logical_cpus_mask;
+#endif /* SMP */
+
+extern u_int mp_maxid;
+extern int mp_maxcpus;
+extern int mp_ncpus;
+extern volatile int smp_started;
+
+extern cpumask_t all_cpus;
+
+/*
+ * Macro allowing us to determine whether a CPU is absent at any given
+ * time, thus permitting us to configure sparse maps of cpuid-dependent
+ * (per-CPU) structures.
+ */
+#define	CPU_ABSENT(x_cpu)	((all_cpus & (1 << (x_cpu))) == 0)
+
+/*
+ * Macros to iterate over non-absent CPUs.  CPU_FOREACH() takes an
+ * integer iterator and iterates over the available set of CPUs.
+ * CPU_FIRST() returns the id of the first non-absent CPU.  CPU_NEXT()
+ * returns the id of the next non-absent CPU.  It will wrap back to
+ * CPU_FIRST() once the end of the list is reached.  The iterators are
+ * currently implemented via inline functions.
+ */
+#define	CPU_FOREACH(i)							\
+	for ((i) = 0; (i) <= mp_maxid; (i)++)				\
+		if (!CPU_ABSENT((i)))
+
+static __inline int
+cpu_first(void)
+{
+	int i;
+
+	for (i = 0;; i++)
+		if (!CPU_ABSENT(i))
+			return (i);
+}
+
+static __inline int
+cpu_next(int i)
+{
+
+	for (;;) {
+		i++;
+		if (i > mp_maxid)
+			i = 0;
+		if (!CPU_ABSENT(i))
+			return (i);
+	}
+}
+
+#define	CPU_FIRST()	cpu_first()
+#define	CPU_NEXT(i)	cpu_next((i))
+
+#ifdef SMP
+/*
+ * Machine dependent functions used to initialize MP support.
+ *
+ * The cpu_mp_probe() should check to see if MP support is present and return
+ * zero if it is not or non-zero if it is.  If MP support is present, then
+ * cpu_mp_start() will be called so that MP can be enabled.  This function
+ * should do things such as startup secondary processors.  It should also
+ * setup mp_ncpus, all_cpus, and smp_cpus.  It should also ensure that
+ * smp_active and smp_started are initialized at the appropriate time.
+ * Once cpu_mp_start() returns, machine independent MP startup code will be
+ * executed and a simple message will be output to the console.  Finally,
+ * cpu_mp_announce() will be called so that machine dependent messages about
+ * the MP support may be output to the console if desired.
+ *
+ * The cpu_setmaxid() function is called very early during the boot process
+ * so that the MD code may set mp_maxid to provide an upper bound on CPU IDs
+ * that other subsystems may use.  If a platform is not able to determine
+ * the exact maximum ID that early, then it may set mp_maxid to MAXCPU - 1.
+ */
+struct thread;
+
+struct cpu_group *cpu_topo(void);
+void	cpu_mp_announce(void);
+int	cpu_mp_probe(void);
+void	cpu_mp_setmaxid(void);
+void	cpu_mp_start(void);
+
+void	forward_signal(struct thread *);
+int	restart_cpus(cpumask_t);
+int	stop_cpus(cpumask_t);
+int	stop_cpus_hard(cpumask_t);
+#if defined(__amd64__)
+int	suspend_cpus(cpumask_t);
+#endif
+void	smp_rendezvous_action(void);
+extern	struct mtx smp_ipi_mtx;
+
+#endif /* SMP */
+void	smp_no_rendevous_barrier(void *);
+void	smp_rendezvous(void (*)(void *), 
+		       void (*)(void *),
+		       void (*)(void *),
+		       void *arg);
+void	smp_rendezvous_cpus(cpumask_t,
+		       void (*)(void *), 
+		       void (*)(void *),
+		       void (*)(void *),
+		       void *arg);
+#endif /* !LOCORE */
+#endif /* _KERNEL */
+#endif /* _SYS_SMP_HH_ */
diff --git a/rtemsbsd/freebsd/machine/rtems-bsd-taskqueue.h b/rtemsbsd/freebsd/machine/rtems-bsd-taskqueue.h
new file mode 100644
index 00000000..d57442ef
--- /dev/null
+++ b/rtemsbsd/freebsd/machine/rtems-bsd-taskqueue.h
@@ -0,0 +1,86 @@
+/**
+ * @file
+ *
+ * @ingroup rtems_bsd_rtems
+ *
+ * @brief TODO.
+ */
+
+/*
+ *  COPYRIGHT (c) 1989-2012.
+ *  On-Line Applications Research Corporation (OAR).
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#ifndef RTEMS_TASKQUEUE_H
+#define RTEMS_TASKQUEUE_H
+
+#include <stdarg.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct taskqueue;
+
+typedef void (*task_fn)(void *ctxt, int pending);
+
+/* forwarded 'ctxt' that was passed to taskqueue_create() */
+typedef void (*tq_enq_fn)(void *ctxt);
+
+struct task {
+	struct task *ta_next;
+	int		     ta_pending;
+	int		     ta_priority;
+	task_fn      ta_fn;
+	void        *ta_fn_arg;
+};
+
+struct taskqueue *
+taskqueue_create(const char *name, int mflags, tq_enq_fn, void *ctxt);
+
+struct taskqueue *
+taskqueue_create_fast(const char *name, int mflags, tq_enq_fn, void *ctxt);
+
+int
+taskqueue_enqueue(struct taskqueue *tq, struct task *ta);
+
+#define taskqueue_enqueue_fast(_q,_t) taskqueue_enqueue(_q,_t)
+
+void
+taskqueue_thread_enqueue(void *ctxt);
+
+#define PI_NET	150
+/* Returns 0 on success */
+int
+taskqueue_start_threads(struct taskqueue **ptq, int count, int prio, const char *fmt, ...);
+
+void
+taskqueue_drain(struct taskqueue *tq, struct task *ta);
+
+void
+taskqueue_free(struct taskqueue *tq);
+
+#define TASK_INIT(task, pri, fn, arg) \
+	do { \
+		(task)->ta_next     = 0; \
+		(task)->ta_priority = (pri); \
+		(task)->ta_pending  = 0; \
+		(task)->ta_fn       = (fn); \
+		(task)->ta_fn_arg   = (arg); \
+	} while (0)
+
+extern struct taskqueue *taskqueue_fast;
+
+/* Initialize taskqueue facility [networking must have been initialized already] */
+rtems_id
+rtems_taskqueue_initialize();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
diff --git a/rtemsbsd/src/rtems-bsd-taskqueue.c b/rtemsbsd/src/rtems-bsd-taskqueue.c
new file mode 100644
index 00000000..18da5431
--- /dev/null
+++ b/rtemsbsd/src/rtems-bsd-taskqueue.c
@@ -0,0 +1,325 @@
+/**
+ * @file
+ *
+ * @ingroup rtems_bsd_rtems
+ *
+ * @brief TODO.
+ */
+
+/*
+ *  COPYRIGHT (c) 1989-2012.
+ *  On-Line Applications Research Corporation (OAR).
+ *
+ * The license and distribution terms for this file may be
+ * found in the file LICENSE in this distribution or at
+ * http://www.rtems.com/license/LICENSE.
+ */
+
+#include <sys/cdefs.h>
+__FBSDID("$FreeBSD$");
+
+#include <rtems.h>
+#include <rtems/error.h>
+#include <freebsd/machine/rtems-bsd-taskqueue.h>
+
+/*
+#define STATIC static
+*/
+#undef  DEBUG
+
+#ifdef DEBUG
+#include <stdio.h>
+#ifndef STATIC
+#define STATIC
+#endif
+#else
+#ifndef STATIC
+#define STATIC static
+#endif
+#endif
+
+#define TQ_WAKE_EVENT RTEMS_EVENT_0
+
+/* This implementation is extremely simple; we assume
+ * that all taskqueues (and as a matter of fact there is
+ * only a single one) are manipulated with the rtems
+ * bsdnet semaphore held. I.e.,
+ *   taskqueue_enqueue()
+ *   taskqueue_drain()
+ *   etc.
+ * are called from an environment that holds the
+ * bsdnet semaphore.
+ * Likewise, the thread that works the taskqueue
+ * holds the semaphore while doing so.
+ *
+ */
+
+/* use single-linked list; 'drain' which would benefit from
+ * double-linked list is seldom used and performance doesn't
+ * matter much there. OTOH, the frequent case of working
+ * the list + enqueueing is more efficient for the single-linked
+ * list.
+struct task {
+	struct task *ta_next;
+	int		     ta_pending;
+	int		     ta_priority;
+	task_fn      ta_fn;
+	void        *ta_fn_arg;
+};
+ */
+
+struct taskqueue {
+	struct task anchor;
+	struct task *tail;
+	tq_enq_fn   enq_fn;
+	void       *enq_fn_arg;
+	rtems_id    tid;
+};
+
+
+STATIC struct taskqueue the_taskqueue = {
+	{ 0, 0, 0, 0, 0 },
+	&the_taskqueue.anchor,
+	taskqueue_thread_enqueue,
+	&taskqueue_fast,
+	0
+};
+
+struct taskqueue *taskqueue_fast = &the_taskqueue;
+struct taskqueue *taskqueue_swi = NULL;
+
+struct taskqueue *
+taskqueue_create(const char *name, int mflags, tq_enq_fn enq_fn, void *arg)
+{
+	if ( enq_fn != taskqueue_thread_enqueue )
+		rtems_panic("rtems_taskqueue: attempt to create non-standard TQ; implementation needs to be modified\n");
+	return &the_taskqueue;
+}
+
+struct taskqueue *
+taskqueue_create_fast(const char *name, int mflags, tq_enq_fn enq_fn, void *arg)
+{
+	return taskqueue_create(name, mflags, enq_fn, arg);
+}
+
+/* taskqueue_enqueue must be allowed from an ISR;
+ * hence, all critical list manipulation must lock out
+ * interrupts...
+ */
+int
+taskqueue_enqueue(struct taskqueue *tq, struct task *ta)
+{
+rtems_interrupt_level l;
+
+rtems_interrupt_disable(l);
+	if ( 0 == ta->ta_pending ++ ) {
+		/* hook into list */
+		ta->ta_next       = 0;
+		tq->tail->ta_next = ta;
+		tq->tail          = ta;
+	}
+	tq->enq_fn(tq->enq_fn_arg);
+rtems_interrupt_enable(l);
+	return 0;
+}
+
+void
+taskqueue_thread_enqueue(void *ctxt)
+{
+int                   dopost;
+/* pointer-to-pointer is what bsd provides; we currently
+ * follow the scheme even we don't directly use the argument
+ * passed to taskqueue_create...
+ */
+struct taskqueue *tq = *(struct taskqueue **)ctxt;
+	/* If this is the first entry on the list then the
+	 * task needs to be notified...
+	 */
+	dopost = ( tq->anchor.ta_next == tq->tail && 1 == tq->tail->ta_pending );
+
+	if ( dopost )
+		rtems_event_send(tq->tid, TQ_WAKE_EVENT);
+}
+
+/* Returns 0 on success */
+int
+taskqueue_start_threads(struct taskqueue **ptq, int count, int prio, const char *fmt, ...)
+{
+	if ( count != 1 )
+		rtems_panic("rtems_taskqueue: taskqueue_start_threads cannot currently deal with count != 1\n");
+	
+	/* Do (non thread-safe) lazy init as a fallback */
+	if ( ! the_taskqueue.tid )
+		rtems_taskqueue_initialize();
+	return 0;
+}
+
+void
+taskqueue_drain(struct taskqueue *tq, struct task *ta)
+{
+rtems_interrupt_level l;
+struct task *p, *q;
+int    i;
+
+	/* find predecessor; searching the list should be
+	 * safe; an ISR might append a new record to the tail
+	 * while we are working but that should be OK.
+	 */
+	for ( p = &tq->anchor; (q = p->ta_next); p=q ) {
+		if ( q == ta ) {
+		rtems_interrupt_disable(l);
+			/* found; do work */
+			/* remember 'pending' count and extract */
+			i              = ta->ta_pending;
+			ta->ta_pending = 0;
+			p->ta_next     = ta->ta_next;
+			ta->ta_next    = 0;
+			/* adjust tail */
+			if ( tq->tail == q )
+				tq->tail = p;
+		rtems_interrupt_enable(l);
+			for ( ; i>0; i-- ) {
+				ta->ta_fn(ta->ta_fn_arg, i);
+			}
+			return;
+		}
+	}
+}
+
+/* work the task queue and return
+ * nonzero if the list is not empty
+ * (which means that some callback has
+ * rescheduled itself)
+ */
+static void *
+taskqueue_work(struct taskqueue *tq)
+{
+rtems_interrupt_level l;
+struct task   *p, *q;
+task_fn        f;
+void        *arg;
+int            i;
+
+/* work off a temporary list in case any callback reschedules
+ * itself or if new tasks are queued from an ISR.
+ */
+rtems_interrupt_disable(l);
+	p = tq->anchor.ta_next;
+
+	tq->anchor.ta_next = 0;
+	tq->tail           = &tq->anchor;
+rtems_interrupt_enable(l);
+
+	while ( (q=p) ) {
+	rtems_interrupt_disable(l);
+		i = q->ta_pending;
+		q->ta_pending = 0;
+		/* extract */
+		p          = q->ta_next;
+		q->ta_next = 0;
+		f          = q->ta_fn;
+		arg        = q->ta_fn_arg;
+	rtems_interrupt_enable(l);
+		for ( ; i>0; i-- ) {
+			f(arg, i);
+		}
+	}
+	return tq->anchor.ta_next;
+}
+
+void
+taskqueue_free(struct taskqueue *tq)
+{
+	taskqueue_work(tq);
+}
+
+static void
+taskqueueDoWork(void *arg)
+{
+struct taskqueue *tq = arg;
+rtems_event_set  evs;
+rtems_status_code sc;
+	while ( 1 ) {
+		sc = rtems_bsdnet_event_receive(TQ_WAKE_EVENT, RTEMS_EVENT_ANY | RTEMS_WAIT, RTEMS_NO_TIMEOUT, &evs);
+		if ( RTEMS_SUCCESSFUL != sc ) {
+			rtems_error(sc,"rtems_taskqueue: taskqueueDoWork() unable to receive wakup event\n");
+			rtems_panic("Can't proceed\n");
+		}
+		if ( taskqueue_work(tq) ) {
+#if 0
+			/* chance to reschedule */
+			rtems_bsdnet_semaphore_release();
+			rtems_task_wake_after(0);
+			rtems_bsdnet_semaphore_obtain();
+#else
+			/* hopefully, releasing the semaphore (as part of bsdnet_event_receive)
+			 * and obtaining the event (which has been posted already)
+			 * yields the CPU if necessary...
+			 */
+#endif
+		}
+	}
+}
+
+#ifdef DEBUG
+struct task_dbg {
+	struct task t;
+	char        *nm;
+};
+
+struct task_dbg taskA = {
+	{0},
+	"taskA"
+};
+
+struct task_dbg taskB = {
+	{0},
+	"taskB"
+};
+
+struct task_dbg taskC = {
+	{0},
+	"taskC"
+};
+
+static void the_task_fn(void *arg, int pending)
+{
+struct task_dbg *td = arg;
+	printf("%s (pending: %i)\n", td->nm, pending);
+	/* Test rescheduling */
+	if ( pending > 3 )
+		taskqueue_enqueue(&the_taskqueue,&td->t);
+}
+
+void taskqueue_dump()
+{
+struct task *p;
+	printf("Anchor %p, Tail %p\n", &the_taskqueue.anchor, the_taskqueue.tail);
+	for ( p = the_taskqueue.anchor.ta_next; p; p=p->ta_next ) {
+		printf("%p: (pending %2i, next %p)\n",
+			p, p->ta_pending, p->ta_next);
+	}
+}
+#endif
+
+rtems_id
+rtems_taskqueue_initialize()
+{
+#ifdef DEBUG
+	TASK_INIT( &taskA.t, 0, the_task_fn, &taskA );
+	TASK_INIT( &taskB.t, 0, the_task_fn, &taskB );
+	TASK_INIT( &taskC.t, 0, the_task_fn, &taskC );
+#endif
+	if ( ! the_taskqueue.tid )
+		the_taskqueue.tid = rtems_bsdnet_newproc("tskq", 10000, taskqueueDoWork, &the_taskqueue);
+	return the_taskqueue.tid;
+}
+
+#ifdef DEBUG
+void
+_cexpModuleInitialize(void *u)
+{
+	rtems_bsdnet_initialize_network();
+	the_taskqueue.tid = rtems_taskqueue_initialize();
+}
+#endif