1 files changed, 1160 insertions, 0 deletions
diff --git a/gnuradio-core/src/lib/omnithread/vxWorks.cc b/gnuradio-core/src/lib/omnithread/vxWorks.cc
new file mode 100644
index 0000000000..25634ce938
--- /dev/null
+++ b/gnuradio-core/src/lib/omnithread/vxWorks.cc
@@ -0,0 +1,1160 @@
+//////////////////////////////////////////////////////////////////////////////
+// Filename:	 vxWorks.cc
+// Author:		Tihomir Sokcevic
+//					Acterna, Eningen.
+// Description: vxWorks adaptation of the omnithread wrapper classes
+// Notes:		 Munching strategy is imperative
+//////////////////////////////////////////////////////////////////////////////
+// $Log$
+// Revision 1.1  2004/04/10 18:00:52  eb
+// Initial revision
+//
+// Revision 1.1.1.1  2004/03/01 00:20:27  eb
+// initial checkin
+//
+// Revision 1.1  2003/05/25 05:29:04  eb
+// see ChangeLog
+//
+// Revision 1.1.2.1  2003/02/17 02:03:11  dgrisby
+// vxWorks port. (Thanks Michael Sturm / Acterna Eningen GmbH).
+//
+// Revision 1.1.1.1  2002/11/19 14:58:04  sokcevti
+// OmniOrb4.0.0 VxWorks port
+//
+// Revision 1.4  2002/10/15 07:54:09  kuttlest
+// change semaphore from SEM_FIFO to SEM_PRIO
+// ---
+//
+// Revision 1.3  2002/07/05 07:38:52  engeln
+// made priority redefinable on load time by defining int variables
+// 	omni_thread_prio_low = 220;
+// 	omni_thread_prio_normal = 110;
+// 	omni_thread_prio_high = 55;
+// the default priority is prio_normal.
+// The normal priority default has been increased from 200 to 110 and the
+//     high priority from 100 to 55.
+// ---
+//
+// Revision 1.2  2002/06/14 12:44:57  engeln
+// replaced possibly unsafe wakeup procedure in broadcast.
+// ---
+//
+// Revision 1.1.1.1  2002/04/02 10:09:34  sokcevti
+// omniORB4 initial realease
+//
+// Revision 1.0	2001/10/23 14:22:45	sokcevti
+// Initial Version 4.00
+// ---
+//
+//////////////////////////////////////////////////////////////////////////////
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Include files
+//////////////////////////////////////////////////////////////////////////////
+#include <stdlib.h>
+#include <stdio.h>
+#include <errno.h>
+#include <time.h>
+#include <omnithread.h>
+#include <sysLib.h>
+
+#include <assert.h>		// assert
+#include <intLib.h>		// intContext
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Local defines
+//////////////////////////////////////////////////////////////////////////////
+#define ERRNO(x) (((x) != 0) ? (errno) : 0)
+#define THROW_ERRORS(x) { if((x) != OK) throw omni_thread_fatal(errno); }
+#define OMNI_THREAD_ID	0x7F7155AAl
+#define OMNI_STACK_SIZE 32768l
+
+#ifdef _DEBUG
+	#include <fstream>
+	#define DBG_TRACE(X) X
+#else // _DEBUG
+	#define DBG_TRACE(X)
+#endif // _DEBUG
+
+#define DBG_ASSERT(X)
+
+#define DBG_THROW(X) X
+
+int omni_thread_prio_low = 220;
+int omni_thread_prio_normal = 110;
+int omni_thread_prio_high = 55;
+///////////////////////////////////////////////////////////////////////////
+//
+// Mutex
+//
+///////////////////////////////////////////////////////////////////////////
+omni_mutex::omni_mutex(void):m_bConstructed(false)
+{
+	mutexID = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
+
+	DBG_ASSERT(assert(mutexID != NULL));
+
+	if(mutexID==NULL)
+	{
+		DBG_TRACE(cout<<"Exception: omni_mutex::omni_mutex()  tid: "<<(int)taskIdSelf()<<endl);
+		DBG_THROW(throw omni_thread_fatal(-1));
+	}
+
+	m_bConstructed = true;
+}
+
+omni_mutex::~omni_mutex(void)
+{
+	m_bConstructed = false;
+
+	STATUS status = semDelete(mutexID);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_mutex::~omni_mutex()  mutexID: "<<(int)mutexID<<" tid: "<<(int)taskIdSelf()<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+/*
+void omni_mutex::lock(void)
+{
+	DBG_ASSERT(assert(!intContext()));		// not in ISR context
+	DBG_ASSERT(assert(m_bConstructed));
+
+	STATUS status = semTake(mutexID, WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_mutex::lock()  mutexID: "<<(int)mutexID<<" tid: "<<(int)taskIdSelf()<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+void omni_mutex::unlock(void)
+{
+	DBG_ASSERT(assert(m_bConstructed));
+
+	STATUS status = semGive(mutexID);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_mutex::unlock()  mutexID: "<<(int)mutexID<<" tid: "<<(int)taskIdSelf()<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+*/
+
+///////////////////////////////////////////////////////////////////////////
+//
+// Condition variable
+//
+///////////////////////////////////////////////////////////////////////////
+omni_condition::omni_condition(omni_mutex* m) : mutex(m)
+{
+	DBG_TRACE(cout<<"omni_condition::omni_condition  mutexID: "<<(int)mutex->mutexID<<" tid:"<<(int)taskIdSelf()<<endl);
+
+	waiters_ = 0;
+
+	sema_ = semCCreate(SEM_Q_PRIORITY, 0);
+	if(sema_ == NULL)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::omni_condition()  tid: "<<(int)taskIdSelf()<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	waiters_lock_ = semMCreate(SEM_Q_PRIORITY | SEM_INVERSION_SAFE);
+	if(waiters_lock_ == NULL)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::omni_condition()  tid: "<<(int)taskIdSelf()<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+}
+
+omni_condition::~omni_condition(void)
+{
+	STATUS status = semDelete(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::~omni_condition"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	status = semDelete(sema_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::~omni_condition"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+void omni_condition::wait(void)
+{
+	DBG_TRACE(cout<<"omni_condition::wait            mutexID: "<<(int)mutex->mutexID<<" tid:"<<(int)taskIdSelf()<<endl);
+
+	// Prevent race conditions on the <waiters_> count.
+
+	STATUS status = semTake(waiters_lock_,WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::wait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	++waiters_;
+
+	status = semGive(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::wait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	// disable task lock to have an atomic unlock+semTake
+	taskLock();
+
+	// We keep the lock held just long enough to increment the count of
+	// waiters by one.	Note that we can't keep it held across the call
+	// to wait() since that will deadlock other calls to signal().
+	mutex->unlock();
+
+	// Wait to be awakened by a cond_signal() or cond_broadcast().
+	status = semTake(sema_,WAIT_FOREVER);
+
+	// reenable task rescheduling
+	taskUnlock();
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::wait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	// Reacquire lock to avoid race conditions on the <waiters_> count.
+	status = semTake(waiters_lock_,WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::wait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	// We're ready to return, so there's one less waiter.
+	--waiters_;
+
+	// Release the lock so that other collaborating threads can make
+	// progress.
+	status = semGive(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::wait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	// Bad things happened, so let's just return below.
+
+	// We must always regain the <external_mutex>, even when errors
+	// occur because that's the guarantee that we give to our callers.
+	mutex->lock();
+}
+
+
+// The time given is absolute. Return 0 is timeout
+int omni_condition::timedwait(unsigned long secs, unsigned long nanosecs)
+{
+	STATUS result = OK;
+	timespec now;
+	unsigned long timeout;
+	int ticks;
+
+	// Prevent race conditions on the <waiters_> count.
+	STATUS status = semTake(waiters_lock_, WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::timedwait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	++waiters_;
+
+	status = semGive(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::timedwait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	clock_gettime(CLOCK_REALTIME, &now);
+
+	if(((unsigned long)secs <= (unsigned long)now.tv_sec) &&
+		(((unsigned long)secs < (unsigned long)now.tv_sec) ||
+		(nanosecs < (unsigned long)now.tv_nsec)))
+		timeout = 0;
+	else
+		timeout = (secs-now.tv_sec) * 1000 + (nanosecs-now.tv_nsec) / 1000000l;
+
+	// disable task lock to have an atomic unlock+semTake
+	taskLock();
+
+	// We keep the lock held just long enough to increment the count
+	// of waiters by one.
+	mutex->unlock();
+
+	// Wait to be awakened by a signal() or broadcast().
+	ticks = (timeout * sysClkRateGet()) / 1000L;
+	result = semTake(sema_, ticks);
+
+	// reenable task rescheduling
+	taskUnlock();
+
+	// Reacquire lock to avoid race conditions.
+	status = semTake(waiters_lock_, WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::timedwait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	--waiters_;
+
+	status = semGive(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::timedwait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	// A timeout has occured - fires exception if the origin is other than timeout
+	if(result!=OK && !(errno == S_objLib_OBJ_TIMEOUT || errno == S_objLib_OBJ_UNAVAILABLE))
+	{
+		DBG_TRACE(cout<<"omni_condition::timedwait! - thread:"<<omni_thread::self()->id()<<" SemID:"<<(int)sema_<<" errno:"<<errno<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	// We must always regain the <external_mutex>, even when errors
+	// occur because that's the guarantee that we give to our callers.
+	mutex->lock();
+
+	if(result!=OK) // timeout
+		return 0;
+
+	return 1;
+}
+
+void omni_condition::signal(void)
+{
+	DBG_TRACE(cout<<"omni_condition::signal          mutexID: "<<(int)mutex->mutexID<<" tid:"<<(int)taskIdSelf()<<endl);
+
+	STATUS status = semTake(waiters_lock_, WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::signal"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	int have_waiters = waiters_ > 0;
+
+	status = semGive(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::signal"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	if(have_waiters != 0)
+	{
+		status = semGive(sema_);
+
+		DBG_ASSERT(assert(status == OK));
+
+		if(status != OK)
+		{
+			DBG_TRACE(cout<<"Exception: omni_condition::signal"<<endl);
+			DBG_THROW(throw omni_thread_fatal(errno));
+		}
+	}
+}
+
+void omni_condition::broadcast(void)
+{
+	DBG_TRACE(cout<<"omni_condition::broadcast       mutexID: "<<(int)mutex->mutexID<<" tid:"<<(int)taskIdSelf()<<endl);
+
+	int have_waiters = 0;
+
+	// The <external_mutex> must be locked before this call is made.
+	// This is needed to ensure that <waiters_> and <was_broadcast_> are
+	// consistent relative to each other.
+	STATUS status = semTake(waiters_lock_, WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::signal"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	if(waiters_ > 0)
+	{
+		// We are broadcasting, even if there is just one waiter...
+		// Record the fact that we are broadcasting.	This helps the
+		// cond_wait() method know how to optimize itself.	Be sure to
+		// set this with the <waiters_lock_> held.
+		have_waiters = 1;
+	}
+
+	status = semGive(waiters_lock_);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_condition::signal"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+
+	if(have_waiters)
+	{
+		// Wake up all the waiters.
+		status = semFlush(sema_);
+
+			DBG_ASSERT(assert(status == OK));
+
+			if(status != OK)
+			{
+				DBG_TRACE(cout<<"omni_condition::broadcast1! - thread:"<<omni_thread::self()->id()<<" SemID:"<<(int)sema_<<" errno:"<<errno<<endl);
+				DBG_THROW(throw omni_thread_fatal(errno));
+			}
+
+	}
+}
+
+
+///////////////////////////////////////////////////////////////////////////
+//
+// Counting semaphore
+//
+///////////////////////////////////////////////////////////////////////////
+omni_semaphore::omni_semaphore(unsigned int initial)
+{
+
+	DBG_ASSERT(assert(0 <= (int)initial));		// POSIX expects only unsigned init values
+
+	semID = semCCreate(SEM_Q_PRIORITY, (int)initial);
+
+	DBG_ASSERT(assert(semID!=NULL));
+
+	if(semID==NULL)
+	{
+		DBG_TRACE(cout<<"Exception: omni_semaphore::omni_semaphore"<<endl);
+		DBG_THROW(throw omni_thread_fatal(-1));
+	}
+}
+
+omni_semaphore::~omni_semaphore(void)
+{
+	STATUS status = semDelete(semID);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_semaphore::~omni_semaphore"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+void omni_semaphore::wait(void)
+{
+	DBG_ASSERT(assert(!intContext()));		// no wait in ISR
+
+	STATUS status = semTake(semID, WAIT_FOREVER);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_semaphore::wait"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+int omni_semaphore::trywait(void)
+{
+	STATUS status = semTake(semID, NO_WAIT);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		if(errno == S_objLib_OBJ_UNAVAILABLE)
+		{
+			return 0;
+		}
+		else
+		{
+			DBG_ASSERT(assert(false));
+
+			DBG_TRACE(cout<<"Exception: omni_semaphore::trywait"<<endl);
+			DBG_THROW(throw omni_thread_fatal(errno));
+		}
+	}
+
+	return 1;
+}
+
+void omni_semaphore::post(void)
+{
+	STATUS status = semGive(semID);
+
+	DBG_ASSERT(assert(status == OK));
+
+	if(status != OK)
+	{
+		DBG_TRACE(cout<<"Exception: omni_semaphore::post"<<endl);
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+
+
+///////////////////////////////////////////////////////////////////////////
+//
+// Thread
+//
+///////////////////////////////////////////////////////////////////////////
+
+
+//
+// static variables
+//
+omni_mutex* omni_thread::next_id_mutex = 0;
+int omni_thread::next_id = 0;
+
+// omniORB requires a larger stack size than the default (21120) on OSF/1
+static size_t stack_size = OMNI_STACK_SIZE;
+
+
+//
+// Initialisation function (gets called before any user code).
+//
+
+static int& count() {
+  static int the_count = 0;
+  return the_count;
+}
+
+omni_thread::init_t::init_t(void)
+{
+	// Only do it once however many objects get created.
+	if(count()++ != 0)
+		return;
+
+	attach();
+}
+
+omni_thread::init_t::~init_t(void)
+{
+    if (--count() != 0) return;
+
+    omni_thread* self = omni_thread::self();
+    if (!self) return;
+
+    taskTcb(taskIdSelf())->spare1 = 0;
+    delete self;
+
+    delete next_id_mutex;
+}
+
+
+//
+// Wrapper for thread creation.
+//
+extern "C" void omni_thread_wrapper(void* ptr)
+{
+	omni_thread* me = (omni_thread*)ptr;
+
+	DBG_TRACE(cout<<"omni_thread_wrapper: thread "<<me->id()<<" started\n");
+
+	//
+	// We can now tweaked the task info since the tcb exist now
+	//
+	me->mutex.lock();	// To ensure that start has had time to finish
+	taskTcb(me->tid)->spare1 = OMNI_THREAD_ID;
+	taskTcb(me->tid)->spare2 = (int)ptr;
+	me->mutex.unlock();
+
+	//
+	// Now invoke the thread function with the given argument.
+	//
+	if(me->fn_void != NULL)
+	{
+		(*me->fn_void)(me->thread_arg);
+		omni_thread::exit();
+	}
+
+	if(me->fn_ret != NULL)
+	{
+		void* return_value = (*me->fn_ret)(me->thread_arg);
+		omni_thread::exit(return_value);
+	}
+
+	if(me->detached)
+	{
+		me->run(me->thread_arg);
+		omni_thread::exit();
+	}
+	else
+	{
+		void* return_value = me->run_undetached(me->thread_arg);
+		omni_thread::exit(return_value);
+	}
+}
+
+
+//
+// Special functions for VxWorks only
+//
+void omni_thread::attach(void)
+{
+	DBG_TRACE(cout<<"omni_thread_attach: VxWorks mapping thread initialising\n");
+
+	int _tid = taskIdSelf();
+
+	// Check the task is not already attached
+	if(taskTcb(_tid)->spare1 == OMNI_THREAD_ID)
+		return;
+
+	// Create the mutex required to lock the threads debugging id (create before the thread!!!)
+	if(next_id_mutex == 0)
+		next_id_mutex = new omni_mutex;
+
+	// Create a thread object for THIS running process
+	omni_thread* t = new omni_thread;
+
+	// Lock its mutex straigh away!
+	omni_mutex_lock l(t->mutex);
+
+	// Adjust data members of this instance
+	t->_state = STATE_RUNNING;
+	t->tid = taskIdSelf();
+
+	// Set the thread values so it can be recongnised as a omni_thread
+	// Set the id last can possibly prevent race condition
+	taskTcb(t->tid)->spare2 = (int)t;
+	taskTcb(t->tid)->spare1 = OMNI_THREAD_ID;
+
+	// Create the running_mutex at this stage, but leave it empty. We are not running
+	//	in the task context HERE, so taking it would be disastrous.
+	t->running_cond = new omni_condition(&t->mutex);
+}
+
+
+void omni_thread::detach(void)
+{
+	DBG_TRACE(cout<<"omni_thread_detach: VxWorks detaching thread mapping\n");
+
+	int _tid = taskIdSelf();
+
+	// Check the task has a OMNI_THREAD attached
+	if(taskTcb(_tid)->spare1 != OMNI_THREAD_ID)
+		return;
+
+	// Invalidate the id NOW !
+	taskTcb(_tid)->spare1 = 0;
+
+	// Even if NULL, it is safe to delete the thread
+	omni_thread* t = (omni_thread*)taskTcb(_tid)->spare2;
+	// Fininsh cleaning the tcb structure
+	taskTcb(_tid)->spare2 = 0;
+
+	delete t;
+}
+
+
+//
+// Constructors for omni_thread - set up the thread object but don't
+// start it running.
+//
+
+// construct a detached thread running a given function.
+omni_thread::omni_thread(void (*fn)(void*), void* arg, priority_t pri)
+{
+	common_constructor(arg, pri, 1);
+	fn_void = fn;
+	fn_ret = NULL;
+}
+
+// construct an undetached thread running a given function.
+omni_thread::omni_thread(void* (*fn)(void*), void* arg, priority_t pri)
+{
+	common_constructor(arg, pri, 0);
+	fn_void = NULL;
+	fn_ret = fn;
+}
+
+// construct a thread which will run either run() or run_undetached().
+
+omni_thread::omni_thread(void* arg, priority_t pri)
+{
+	common_constructor(arg, pri, 1);
+	fn_void = NULL;
+	fn_ret = NULL;
+}
+
+// common part of all constructors.
+void omni_thread::common_constructor(void* arg, priority_t pri, int det)
+{
+	_state = STATE_NEW;
+	_priority = pri;
+
+	// Set the debugging id
+	next_id_mutex->lock();
+	_id = next_id++;
+	next_id_mutex->unlock();
+
+	// Note : tid can only be setup when the task is up and running
+	tid = 0;
+
+	thread_arg = arg;
+	detached = det;		// may be altered in start_undetached()
+
+    _dummy       = 0;
+    _values      = 0;
+    _value_alloc = 0;
+}
+
+//
+// Destructor for omni_thread.
+//
+omni_thread::~omni_thread(void)
+{
+	DBG_TRACE(cout<<"omni_thread::~omni_thread for thread "<<id()<<endl);
+
+    if (_values) {
+        for (key_t i=0; i < _value_alloc; i++) {
+	    if (_values[i]) {
+	        delete _values[i];
+	    }
+        }
+	delete [] _values;
+    }
+
+	delete running_cond;
+}
+
+
+//
+// Start the thread
+//
+void omni_thread::start(void)
+{
+	omni_mutex_lock l(mutex);
+
+	DBG_ASSERT(assert(_state == STATE_NEW));
+
+	if(_state != STATE_NEW)
+		DBG_THROW(throw omni_thread_invalid());
+
+	// Allocate memory for the task. (The returned id cannot be trusted by the task)
+	tid = taskSpawn(
+		NULL,								 // Task name
+		vxworks_priority(_priority),	// Priority
+		0,									 // Option
+		stack_size,						 // Stack size
+		(FUNCPTR)omni_thread_wrapper, // Priority
+		(int)this,							// First argument is this
+		0,0,0,0,0,0,0,0,0				 // Remaining unused args
+		);
+
+	DBG_ASSERT(assert(tid!=ERROR));
+
+	if(tid==ERROR)
+		DBG_THROW(throw omni_thread_invalid());
+
+	_state = STATE_RUNNING;
+
+	// Create the running_mutex at this stage, but leave it empty. We are not running
+	//	in the task context HERE, so taking it would be disastrous.
+	running_cond = new omni_condition(&mutex);
+}
+
+
+//
+// Start a thread which will run the member function run_undetached().
+//
+void omni_thread::start_undetached(void)
+{
+	DBG_ASSERT(assert(!((fn_void != NULL) || (fn_ret != NULL))));
+
+	if((fn_void != NULL) || (fn_ret != NULL))
+		DBG_THROW(throw omni_thread_invalid());
+
+	detached = 0;
+
+	start();
+}
+
+
+//
+// join - Wait for the task to complete before returning to the calling process
+//
+void omni_thread::join(void** status)
+{
+	mutex.lock();
+
+	if((_state != STATE_RUNNING) && (_state != STATE_TERMINATED))
+	{
+		mutex.unlock();
+
+		DBG_ASSERT(assert(false));
+
+		DBG_THROW(throw omni_thread_invalid());
+	}
+
+	mutex.unlock();
+
+	DBG_ASSERT(assert(this != self()));
+
+	if(this == self())
+		DBG_THROW(throw omni_thread_invalid());
+
+	DBG_ASSERT(assert(!detached));
+
+	if(detached)
+		DBG_THROW(throw omni_thread_invalid());
+
+	mutex.lock();
+	running_cond->wait();
+	mutex.unlock();
+
+	if(status)
+		*status = return_val;
+
+	delete this;
+}
+
+
+//
+// Change this thread's priority.
+//
+void omni_thread::set_priority(priority_t pri)
+{
+	omni_mutex_lock l(mutex);
+
+	DBG_ASSERT(assert(_state == STATE_RUNNING));
+
+	if(_state != STATE_RUNNING)
+	{
+		DBG_THROW(throw omni_thread_invalid());
+	}
+
+	_priority = pri;
+
+	if(taskPrioritySet(tid, vxworks_priority(pri))==ERROR)
+	{
+		DBG_ASSERT(assert(false));
+
+		DBG_THROW(throw omni_thread_fatal(errno));
+	}
+}
+
+
+//
+// create - construct a new thread object and start it running.	Returns thread
+// object if successful, null pointer if not.
+//
+
+// detached version (the entry point is a void)
+omni_thread* omni_thread::create(void (*fn)(void*), void* arg, priority_t pri)
+{
+	omni_thread* t = new omni_thread(fn, arg, pri);
+
+	t->start();
+
+	return t;
+}
+
+// undetached version (the entry point is a void*)
+omni_thread* omni_thread::create(void* (*fn)(void*), void* arg, priority_t pri)
+{
+	omni_thread* t = new omni_thread(fn, arg, pri);
+
+	t->start();
+
+	return t;
+}
+
+
+//
+// exit() _must_ lock the mutex even in the case of a detached thread.	This is
+// because a thread may run to completion before the thread that created it has
+// had a chance to get out of start().	By locking the mutex we ensure that the
+// creating thread must have reached the end of start() before we delete the
+// thread object.	Of course, once the call to start() returns, the user can
+// still incorrectly refer to the thread object, but that's their problem.
+//
+void omni_thread::exit(void* return_value)
+{
+	omni_thread* me = self();
+
+	if(me)
+	{
+		me->mutex.lock();
+
+		me->return_val = return_value;
+		me->_state = STATE_TERMINATED;
+		me->running_cond->signal();
+
+		me->mutex.unlock();
+
+		DBG_TRACE(cout<<"omni_thread::exit: thread "<<me->id()<<" detached "<<me->detached<<" return value "<<(int)return_value<<endl);
+
+		if(me->detached)
+			delete me;
+	}
+	else
+		DBG_TRACE(cout<<"omni_thread::exit: called with a non-omnithread. Exit quietly."<<endl);
+
+	taskDelete(taskIdSelf());
+}
+
+
+omni_thread* omni_thread::self(void)
+{
+	if(taskTcb(taskIdSelf())->spare1 != OMNI_THREAD_ID)
+		return NULL;
+
+	return (omni_thread*)taskTcb(taskIdSelf())->spare2;
+}
+
+
+void omni_thread::yield(void)
+{
+	taskDelay(NO_WAIT);
+}
+
+
+void omni_thread::sleep(unsigned long secs, unsigned long nanosecs)
+{
+	int tps = sysClkRateGet();
+
+	// Convert to us to avoid overflow in the multiplication
+	//	tps should always be less than 1000 !
+	nanosecs /= 1000;
+
+	taskDelay(secs*tps + (nanosecs*tps)/1000000l);
+}
+
+
+void omni_thread::get_time( unsigned long* abs_sec,
+							unsigned long* abs_nsec,
+							unsigned long rel_sec,
+							unsigned long rel_nsec)
+{
+	timespec abs;
+	clock_gettime(CLOCK_REALTIME, &abs);
+	abs.tv_nsec += rel_nsec;
+	abs.tv_sec += rel_sec + abs.tv_nsec / 1000000000;
+	abs.tv_nsec = abs.tv_nsec % 1000000000;
+	*abs_sec = abs.tv_sec;
+	*abs_nsec = abs.tv_nsec;
+}
+
+
+int omni_thread::vxworks_priority(priority_t pri)
+{
+	switch (pri)
+	{
+	case PRIORITY_LOW:
+		return omni_thread_prio_low;
+
+	case PRIORITY_NORMAL:
+		return omni_thread_prio_normal;
+
+	case PRIORITY_HIGH:
+		return omni_thread_prio_high;
+	}
+
+	DBG_ASSERT(assert(false));
+
+	DBG_THROW(throw omni_thread_invalid());
+}
+
+
+void omni_thread::stacksize(unsigned long sz)
+{
+	stack_size = sz;
+}
+
+
+unsigned long omni_thread::stacksize()
+{
+	return stack_size;
+}
+
+
+void omni_thread::show(void)
+{
+	omni_thread *pThread;
+	int s1, s2;
+	int tid = taskIdSelf();
+
+	printf("TaskId is %.8x\n", tid);
+
+	s1 = taskTcb(tid)->spare1;
+
+	if(s1 != OMNI_THREAD_ID)
+	{
+		printf("Spare 1 is %.8x, and not recongnized\n", s1);
+
+		return;
+	}
+	else
+	{
+		printf("Spare 1 indicate an omni_thread.\n");
+	}
+
+	s2 = taskTcb(tid)->spare2;
+
+	if(s2 == 0)
+	{
+		printf("Spare 2 is NULL! - No thread object attached !!\n");
+
+		return;
+	}
+	else
+	{
+		printf("Thread object at %.8x\n", s2);
+	}
+
+	pThread = (omni_thread *)s2;
+
+	state_t status = pThread->_state;
+
+	printf("	| Thread status is ");
+
+	switch (status)
+	{
+	case STATE_NEW:
+		printf("NEW\n");		break;
+	case STATE_RUNNING:
+		printf("STATE_RUNNING\n"); break;
+	case STATE_TERMINATED:
+		printf("TERMINATED\n");	break;
+	default:
+		printf("Illegal (=%.8x)\n", (unsigned int)status);
+
+		return;
+	}
+
+	if(pThread->tid != tid)
+	{
+		printf("	| Task ID in thread object is different!! (=%.8x)\n", pThread->tid);
+
+		return;
+	}
+	else
+	{
+		printf("	| Task ID in thread consistent\n");
+	}
+
+	printf("\n");
+}
+
+
+//
+// Dummy thread
+//
+
+class omni_thread_dummy : public omni_thread {
+public:
+  inline omni_thread_dummy() : omni_thread()
+  {
+    _dummy = 1;
+    _state = STATE_RUNNING;
+
+	// Adjust data members of this instance
+	tid = taskIdSelf();
+
+	// Set the thread values so it can be recongnised as a omni_thread
+	// Set the id last can possibly prevent race condition
+	taskTcb(tid)->spare2 = (int)this;
+	taskTcb(tid)->spare1 = OMNI_THREAD_ID;
+   }
+  inline ~omni_thread_dummy()
+  {
+	taskTcb(taskIdSelf())->spare1 = 0;
+  }
+};
+
+omni_thread*
+omni_thread::create_dummy()
+{
+  if (omni_thread::self())
+    throw omni_thread_invalid();
+
+  return new omni_thread_dummy;
+}
+
+void
+omni_thread::release_dummy()
+{
+  omni_thread* self = omni_thread::self();
+  if (!self || !self->_dummy)
+    throw omni_thread_invalid();
+
+  omni_thread_dummy* dummy = (omni_thread_dummy*)self;
+  delete dummy;
+}
+
+
+#define INSIDE_THREAD_IMPL_CC
+#include "threaddata.cc"
+#undef INSIDE_THREAD_IMPL_CC