[Xenomai-core] xenomai-solo vxWorks: know whether a task is safe or not

[Xenomai-core] xenomai-solo vxWorks: know whether a task is safe or not

[Niklaus Giger]

[-- Attachment #1: Type: text/plain, Size: 431 bytes --]

Hi

For various reasons we were forced to examine under vxWorks 5.5 the private
WIND_TCB safeCnt field to determine whether a task was "safe" or not.

With the attached patch I try to add a taskIsSafe procedure to xenomai-solo.
It is probably not free from race condition, as I do not know how to access
a pthread_mutex.

Would such (may be improved) patch find its way into the "official" trunk?

Best regards

-- 
NIklaus Giger


[-- Attachment #2: taskIsSafe.diff --]
[-- Type: text/x-patch, Size: 1369 bytes --]

diff --git a/include/vxworks/taskInfo.h b/include/vxworks/taskInfo.h
index 61315d5..27b3cc6 100644
--- a/include/vxworks/taskInfo.h
+++ b/include/vxworks/taskInfo.h
@@ -50,6 +50,8 @@ TASK_ID taskNameToId(const char *name);
 
 TASK_ID taskIdDefault(TASK_ID task_id);
     
+BOOL taskIsSafe(TASK_ID task_id);
+
 BOOL taskIsReady(TASK_ID task_id);
 
 BOOL taskIsSuspended (TASK_ID task_id);
diff --git a/vxworks/taskLib.c b/vxworks/taskLib.c
index f6253b2..7dd52ae 100644
--- a/vxworks/taskLib.c
+++ b/vxworks/taskLib.c
@@ -595,7 +595,7 @@ STATUS taskSafe(void)
 	}
 
 	pthread_mutex_lock(&current->safelock);
-
+	current->safeCnt = 1;
 	return OK;
 }
 
@@ -615,11 +615,24 @@ STATUS taskUnsafe(void)
 		return ERROR;
 	}
 
+	current->safeCnt = 0;
 	pthread_mutex_unlock(&current->safelock);
 
 	return OK;
 }
 
+BOOL taskIsSafe(TASK_ID tid)
+{
+	struct wind_task *task = find_wind_task(tid);
+
+	if (task == NULL) {
+		errno = S_objLib_OBJ_ID_ERROR;
+		return ERROR;
+	}
+
+	return (task->safeCnt > 0);
+}
+
 STATUS taskIdVerify(TASK_ID tid)
 {
 	struct wind_task *task = find_wind_task(tid);
diff --git a/vxworks/taskLib.h b/vxworks/taskLib.h
index 299d059..f743270 100644
--- a/vxworks/taskLib.h
+++ b/vxworks/taskLib.h
@@ -42,6 +42,7 @@ struct wind_task_args {
 struct wind_task {
 
 	pthread_mutex_t safelock;
+	int	safeCnt;
 	sem_t barrier;
 
 	struct WIND_TCB *tcb;

Re: [Xenomai-core] xenomai-solo vxWorks: know whether a task is safe or not

[Philippe Gerum]

Niklaus Giger wrote:
> Hi
> 
> For various reasons we were forced to examine under vxWorks 5.5 the private
> WIND_TCB safeCnt field to determine whether a task was "safe" or not.
> 
> With the attached patch I try to add a taskIsSafe procedure to xenomai-solo.
> It is probably not free from race condition, as I do not know how to access
> a pthread_mutex.
> 
> Would such (may be improved) patch find its way into the "official" trunk?
> 

Assuming that VxWorks tracks the locking depth in safeCnt, that implementation
would not properly account for nested locking (safelock is a recursive mutex).
Additionally, since WIND_TCB is something of a semi-public structure in 5.x and
earlier versions, we could just track the safe counter there as well, instead of
adding a non-standard taskIsSafe() call; we already do that for the status field
anyway. Here is a possible implementation:

diff --git a/include/vxworks/taskLib.h b/include/vxworks/taskLib.h
index 510dabb..6334396 100644
--- a/include/vxworks/taskLib.h
+++ b/include/vxworks/taskLib.h
@@ -49,6 +49,7 @@ typedef struct WIND_TCB {
 	void *opaque;
 	int magic;
 	int status;
+	int safeCnt;
 	int flags;
 	FUNCPTR entry;
 } WIND_TCB;
diff --git a/vxworks/taskLib.c b/vxworks/taskLib.c
index f6253b2..a9e12e7 100644
--- a/vxworks/taskLib.c
+++ b/vxworks/taskLib.c
@@ -283,7 +283,6 @@ static STATUS __taskInit(struct wind_task *task,
 	int ret;

 	ret = check_task_priority(prio);
-
 	if (ret) {
 		errno = ret;
 		return ERROR;
@@ -307,6 +306,7 @@ static STATUS __taskInit(struct wind_task *task,
 	tcb->magic = task_magic;
 	tcb->opaque = task;
 	tcb->status = WIND_SUSPEND;
+	tcb->safeCnt = 0;
 	tcb->flags = flags;
 	tcb->entry = entry;

@@ -588,13 +588,13 @@ STATUS taskSafe(void)
 	}

 	current = wind_task_current();
-
 	if (current == NULL) {
 		errno = S_objLib_OBJ_NO_METHOD;
 		return ERROR;
 	}

 	pthread_mutex_lock(&current->safelock);
+	current->tcb->safeCnt++;

 	return OK;
 }
@@ -602,6 +602,7 @@ STATUS taskSafe(void)
 STATUS taskUnsafe(void)
 {
 	struct wind_task *current;
+	int ret;

 	if (threadobj_async_p()) {
 		errno = S_intLib_NOT_ISR_CALLABLE;
@@ -609,13 +610,14 @@ STATUS taskUnsafe(void)
 	}

 	current = wind_task_current();
-
 	if (current == NULL) {
 		errno = S_objLib_OBJ_NO_METHOD;
 		return ERROR;
 	}

-	pthread_mutex_unlock(&current->safelock);
+	ret = pthread_mutex_unlock(&current->safelock);
+	if (ret == 0)
+		current->tcb->safeCnt--;

 	return OK;
 }

-- 
Philippe.

Re: [Xenomai-core] xenomai-solo vxWorks: know whether a task is safe or not

[Niklaus Giger]

Am Montag 22 September 2008 18.17:10 schrieb Philippe Gerum:
> Niklaus Giger wrote:
> > Hi
> >
> > For various reasons we were forced to examine under vxWorks 5.5 the
> > private WIND_TCB safeCnt field to determine whether a task was "safe" or
> > not.
> >
> > With the attached patch I try to add a taskIsSafe procedure to
> > xenomai-solo. It is probably not free from race condition, as I do not
> > know how to access a pthread_mutex.
> >
> > Would such (may be improved) patch find its way into the "official"
> > trunk?
>
> Assuming that VxWorks tracks the locking depth in safeCnt, that
> implementation would not properly account for nested locking (safelock is a
> recursive mutex). Additionally, since WIND_TCB is something of a
> semi-public structure in 5.x and earlier versions, we could just track the
> safe counter there as well, instead of adding a non-standard taskIsSafe()
> call; we already do that for the status field anyway. Here is a possible
> implementation:

This implementation is okay for me, too. It seems to work, as far I could
test it.

I however still have some issues because we used sometimes (for historical 
reasons in order to emulate a pre-vxWorks home grown cooperative mulit-tasking 
operation system) a taskInit/taskActivate/taskRestart combination. 

Would it be a lot of work for you to add taskRestart to xenomai-solo?

Best regards

Niklaus 

> diff --git a/include/vxworks/taskLib.h b/include/vxworks/taskLib.h
> index 510dabb..6334396 100644
> --- a/include/vxworks/taskLib.h
> +++ b/include/vxworks/taskLib.h
> @@ -49,6 +49,7 @@ typedef struct WIND_TCB {
>  	void *opaque;
>  	int magic;
>  	int status;
> +	int safeCnt;
>  	int flags;
>  	FUNCPTR entry;
>  } WIND_TCB;
> diff --git a/vxworks/taskLib.c b/vxworks/taskLib.c
> index f6253b2..a9e12e7 100644
> --- a/vxworks/taskLib.c
> +++ b/vxworks/taskLib.c
> @@ -283,7 +283,6 @@ static STATUS __taskInit(struct wind_task *task,
>  	int ret;
>
>  	ret = check_task_priority(prio);
> -
>  	if (ret) {
>  		errno = ret;
>  		return ERROR;
> @@ -307,6 +306,7 @@ static STATUS __taskInit(struct wind_task *task,
>  	tcb->magic = task_magic;
>  	tcb->opaque = task;
>  	tcb->status = WIND_SUSPEND;
> +	tcb->safeCnt = 0;
>  	tcb->flags = flags;
>  	tcb->entry = entry;
>
> @@ -588,13 +588,13 @@ STATUS taskSafe(void)
>  	}
>
>  	current = wind_task_current();
> -
>  	if (current == NULL) {
>  		errno = S_objLib_OBJ_NO_METHOD;
>  		return ERROR;
>  	}
>
>  	pthread_mutex_lock(&current->safelock);
> +	current->tcb->safeCnt++;
>
>  	return OK;
>  }
> @@ -602,6 +602,7 @@ STATUS taskSafe(void)
>  STATUS taskUnsafe(void)
>  {
>  	struct wind_task *current;
> +	int ret;
>
>  	if (threadobj_async_p()) {
>  		errno = S_intLib_NOT_ISR_CALLABLE;
> @@ -609,13 +610,14 @@ STATUS taskUnsafe(void)
>  	}
>
>  	current = wind_task_current();
> -
>  	if (current == NULL) {
>  		errno = S_objLib_OBJ_NO_METHOD;
>  		return ERROR;
>  	}
>
> -	pthread_mutex_unlock(&current->safelock);
> +	ret = pthread_mutex_unlock(&current->safelock);
> +	if (ret == 0)
> +		current->tcb->safeCnt--;
>
>  	return OK;
>  }

-- 
NIklaus Giger
Wieshoschet 6
CH-8753 Mollis
+41  (0)55 612 20 54 P
+41  (0)55 618 64 68 G

Re: [Xenomai-core] xenomai-solo vxWorks: know whether a task is safe or not

[Philippe Gerum]

[-- Attachment #1: Type: text/plain, Size: 1978 bytes --]

Niklaus Giger wrote:
> Am Montag 22 September 2008 18.17:10 schrieb Philippe Gerum:
>> Niklaus Giger wrote:
>>> Hi
>>>
>>> For various reasons we were forced to examine under vxWorks 5.5 the
>>> private WIND_TCB safeCnt field to determine whether a task was "safe" or
>>> not.
>>>
>>> With the attached patch I try to add a taskIsSafe procedure to
>>> xenomai-solo. It is probably not free from race condition, as I do not
>>> know how to access a pthread_mutex.
>>>
>>> Would such (may be improved) patch find its way into the "official"
>>> trunk?
>> Assuming that VxWorks tracks the locking depth in safeCnt, that
>> implementation would not properly account for nested locking (safelock is a
>> recursive mutex). Additionally, since WIND_TCB is something of a
>> semi-public structure in 5.x and earlier versions, we could just track the
>> safe counter there as well, instead of adding a non-standard taskIsSafe()
>> call; we already do that for the status field anyway. Here is a possible
>> implementation:
> 
> This implementation is okay for me, too. It seems to work, as far I could
> test it.
> 
> I however still have some issues because we used sometimes (for historical 
> reasons in order to emulate a pre-vxWorks home grown cooperative mulit-tasking 
> operation system) a taskInit/taskActivate/taskRestart combination. 
> 
> Would it be a lot of work for you to add taskRestart to xenomai-solo?
> 

I must admit that I always considered the task restart interface as a pure
manifestation of the Evil, so it absence in SOLO might not be totally
fortuitous. Trashing your system with that stuff is way to easy.

Anyway, here is a possible implementation based on - cough, cough... -
sigsetjmp(); I don't like it since it adds code to various hot paths just to
provide a rarely used feature, so I may make this optional using a configuration
knob at build time if/when I actually merge that, mmmh, thing.

Please let me know how that works for you.

-- 
Philippe.

[-- Attachment #2: solo-vxworks-add-task-restart.patch --]
[-- Type: text/x-diff, Size: 6062 bytes --]

diff --git a/include/vxworks/taskLib.h b/include/vxworks/taskLib.h
index 6334396..2f09de6 100644
--- a/include/vxworks/taskLib.h
+++ b/include/vxworks/taskLib.h
@@ -39,6 +39,7 @@
 #define WIND_DELAY	0x4
 #define WIND_DEAD	0x8
 #define WIND_STOP	0x10	/* Never reported. */
+#define WIND_RESTART	0x20	/* Internal. */
 
 typedef intptr_t TASK_ID;
 
@@ -78,6 +79,8 @@ STATUS taskInit(WIND_TCB *pTcb,
 
 STATUS taskActivate(TASK_ID tid);
 
+STATUS taskRestart(TASK_ID tid);
+
 STATUS taskDelete(TASK_ID tid);
 
 STATUS taskDeleteForce(TASK_ID tid);
diff --git a/vxworks/msgQLib.c b/vxworks/msgQLib.c
index d591a7b..e3d071d 100644
--- a/vxworks/msgQLib.c
+++ b/vxworks/msgQLib.c
@@ -149,6 +149,8 @@ int msgQReceive(MSG_Q_ID msgQId, char *buffer, UINT maxNBytes, int timeout)
 		return ERROR;
 	}
 
+	check_task_restart(wind_task_current());
+
 	mq = get_mq_from_id(msgQId);
 	if (mq == NULL) {
 	objid_error:
@@ -217,6 +219,8 @@ STATUS msgQSend(MSG_Q_ID msgQId, const char *buffer, UINT bytes,
 	UINT maxbytes;
 	int ret;
 	
+	check_task_restart(wind_task_current());
+
 	mq = get_mq_from_id(msgQId);
 	if (mq == NULL) {
 	objid_error:
diff --git a/vxworks/semLib.c b/vxworks/semLib.c
index 6467354..dfc37cc 100644
--- a/vxworks/semLib.c
+++ b/vxworks/semLib.c
@@ -61,7 +61,9 @@ static STATUS xsem_take(struct wind_sem *sem, int timeout)
 
 	if (threadobj_async_p())
 		return S_intLib_NOT_ISR_CALLABLE;
-	  
+
+	check_task_restart(wind_task_current());
+
 	if (syncobj_lock(&sem->u.xsem.sobj, &syns))
 		return S_objLib_OBJ_ID_ERROR;
 
@@ -196,8 +198,11 @@ static STATUS msem_take(struct wind_sem *sem, int timeout)
 		return S_intLib_NOT_ISR_CALLABLE;
 	  
 	current = wind_task_current();
-	if (current != NULL && (sem->options & SEM_DELETE_SAFE))
-		pthread_mutex_lock(&current->safelock);
+	if (current != NULL) {
+		check_task_restart(current);
+		if (sem->options & SEM_DELETE_SAFE)
+			pthread_mutex_lock(&current->safelock);
+	}
 
 	if (timeout == NO_WAIT) {
 		ret = pthread_mutex_trylock(&sem->u.msem.lock);
diff --git a/vxworks/taskLib.c b/vxworks/taskLib.c
index a9e12e7..7e74cf0 100644
--- a/vxworks/taskLib.c
+++ b/vxworks/taskLib.c
@@ -97,6 +97,8 @@ struct wind_task *get_wind_task_or_self(TASK_ID tid)
 	if (current == NULL)
 		return NULL;
 
+	check_task_restart(current);
+
 	/* This one might block but can't fail, it is ours. */
 	threadobj_lock(&current->thobj);
 
@@ -114,6 +116,12 @@ void put_wind_task(struct wind_task *task)
 	threadobj_unlock(&task->thobj);
 }
 
+void check_task_restart(struct wind_task *current)
+{
+	if (current && (current->tcb->status & WIND_RESTART) != 0)
+		siglongjmp(current->ienv, 1);
+}
+
 static void task_finalizer(struct threadobj *thobj)
 {
 	struct wind_task *task = container_of(thobj, struct wind_task, thobj);
@@ -253,6 +261,11 @@ static void *task_trampoline(void *arg)
 	/* Wait for someone to run taskActivate() upon us. */
 	sem_wait(&task->barrier);
 
+	threadobj_lock(&task->thobj);
+	sigsetjmp(task->ienv, 1); /* For taskRestart(). */
+	task->tcb->status &= ~WIND_RESTART;
+	threadobj_unlock(&task->thobj);
+
 	args->entry(args->arg0, args->arg1, args->arg2, args->arg3,
 		    args->arg4, args->arg5, args->arg6, args->arg7,
 		    args->arg8, args->arg9);
@@ -356,7 +369,7 @@ STATUS taskInit(WIND_TCB *pTcb,
 		errno = S_intLib_NOT_ISR_CALLABLE;
 		return ERROR;
 	}
-	  
+
 	task = alloc_task();
 	if (task == NULL) {
 		errno = S_memLib_NOT_ENOUGH_MEMORY;
@@ -396,6 +409,30 @@ STATUS taskActivate(TASK_ID tid)
 	return OK;
 }
 
+STATUS taskRestart(TASK_ID tid)
+{
+	struct wind_task *task;
+
+	if (threadobj_async_p()) {
+		errno = S_intLib_NOT_ISR_CALLABLE;
+		return ERROR;
+	}
+	  
+	/* The thread lock will be released at jump site. */
+	task = get_wind_task_or_self(tid);
+	if (task == NULL) {
+		errno = S_objLib_OBJ_ID_ERROR;
+		return ERROR;
+	}
+
+	task->tcb->status |= WIND_RESTART;
+
+	if (task == wind_task_current())
+		siglongjmp(task->ienv, 1); /* Does not return. */
+
+	return OK;
+}
+
 TASK_ID taskSpawn(const char *name,
 		  int prio,
 		  int flags,
@@ -454,8 +491,10 @@ static STATUS __taskDelete(TASK_ID tid, int force)
 	if (task == NULL)
 		goto objid_error;
 
-	if (task == wind_task_current()) /* Self-deletion. */
+	if (task == wind_task_current()) { /* Self-deletion. */
+		check_task_restart(task);
 		pthread_exit(NULL);
+	}
 
 	/*
 	 * We always attempt to grab the thread safe lock first, then
@@ -517,11 +556,12 @@ TASK_ID taskIdSelf(void)
 	}
 	  
 	current = wind_task_current();
-
 	if (current == NULL) {
 		errno = S_objLib_OBJ_NO_METHOD;
 		return ERROR;
 	}
+
+	check_task_restart(current);
 	  
 	return (TASK_ID)current->tcb;
 }
@@ -546,6 +586,7 @@ STATUS taskSuspend(TASK_ID tid)
 	if (task == NULL)
 		goto objid_error;
 
+	check_task_restart(wind_task_current());
 	ret = threadobj_suspend(&task->thobj);
 	put_wind_task(task);
 
@@ -648,7 +689,6 @@ STATUS taskPrioritySet(TASK_ID tid, int prio)
 		goto objid_error;
 
 	ret = check_task_priority(prio);
-
 	if (ret) {
 		put_wind_task(task);
 		errno = ret;
@@ -745,12 +785,13 @@ STATUS taskDelay(int ticks)
 	}
 
 	current = wind_task_current();
-
 	if (current == NULL) {
 		errno = S_objLib_OBJ_NO_METHOD;
 		return ERROR;
 	}
 
+	check_task_restart(current);
+
 	if (ticks == 0) {
 		sched_yield();	/* Manual round-robin. */
 		return OK;
diff --git a/vxworks/taskLib.h b/vxworks/taskLib.h
index 299d059..46718e1 100644
--- a/vxworks/taskLib.h
+++ b/vxworks/taskLib.h
@@ -20,6 +20,7 @@
 #define _VXWORKS_TASKLIB_H
 
 #include <semaphore.h>
+#include <setjmp.h>
 #include <xenomai/threadobj.h>
 #include <xenomai/registry.h>
 #include <vxworks/taskLib.h>
@@ -53,6 +54,8 @@ struct wind_task {
 	struct threadobj thobj;
 	struct fsobj fsobj;
 	struct hashobj obj;
+
+	jmp_buf ienv;
 };
 
 int wind_task_get_priority(struct wind_task *task);
@@ -97,6 +100,8 @@ struct wind_task *get_wind_task_or_self(TASK_ID tid);
 
 void put_wind_task(struct wind_task *task);
 
+void check_task_restart(struct wind_task *current);
+
 extern struct hash_table wind_task_table;
 
 #endif /* _VXWORKS_TASKLIB_H */

Re: [Xenomai-core] xenomai-solo vxWorks: know whether a task is safe or not

[Philippe Gerum]

[-- Attachment #1: Type: text/plain, Size: 1982 bytes --]

Niklaus Giger wrote:
> Am Montag 22 September 2008 18.17:10 schrieb Philippe Gerum:
>> Niklaus Giger wrote:
>>> Hi
>>>
>>> For various reasons we were forced to examine under vxWorks 5.5 the
>>> private WIND_TCB safeCnt field to determine whether a task was "safe" or
>>> not.
>>>
>>> With the attached patch I try to add a taskIsSafe procedure to
>>> xenomai-solo. It is probably not free from race condition, as I do not
>>> know how to access a pthread_mutex.
>>>
>>> Would such (may be improved) patch find its way into the "official"
>>> trunk?
>> Assuming that VxWorks tracks the locking depth in safeCnt, that
>> implementation would not properly account for nested locking (safelock is a
>> recursive mutex). Additionally, since WIND_TCB is something of a
>> semi-public structure in 5.x and earlier versions, we could just track the
>> safe counter there as well, instead of adding a non-standard taskIsSafe()
>> call; we already do that for the status field anyway. Here is a possible
>> implementation:
> 
> This implementation is okay for me, too. It seems to work, as far I could
> test it.
> 
> I however still have some issues because we used sometimes (for historical 
> reasons in order to emulate a pre-vxWorks home grown cooperative mulit-tasking 
> operation system) a taskInit/taskActivate/taskRestart combination. 
> 
> Would it be a lot of work for you to add taskRestart to xenomai-solo?
> 

I must admit that I always considered the task restart interface as a pure
manifestation of the Evil, so its absence from SOLO might not be totally
fortuitous. Trashing your system with that stuff is way to easy.

Anyway, here is a possible implementation based on - cough, cough... -
sigsetjmp(); I don't like it since it adds code to various hot paths just to
provide a rarely used feature, so I may make this optional using a configuration
knob at build time if/when I actually merge that, mmmh, thing.

Please let me know how that works for you.

-- 
Philippe.


[-- Attachment #2: solo-vxworks-add-task-restart.patch --]
[-- Type: text/x-diff, Size: 6063 bytes --]

diff --git a/include/vxworks/taskLib.h b/include/vxworks/taskLib.h
index 6334396..2f09de6 100644
--- a/include/vxworks/taskLib.h
+++ b/include/vxworks/taskLib.h
@@ -39,6 +39,7 @@
 #define WIND_DELAY	0x4
 #define WIND_DEAD	0x8
 #define WIND_STOP	0x10	/* Never reported. */
+#define WIND_RESTART	0x20	/* Internal. */
 
 typedef intptr_t TASK_ID;
 
@@ -78,6 +79,8 @@ STATUS taskInit(WIND_TCB *pTcb,
 
 STATUS taskActivate(TASK_ID tid);
 
+STATUS taskRestart(TASK_ID tid);
+
 STATUS taskDelete(TASK_ID tid);
 
 STATUS taskDeleteForce(TASK_ID tid);
diff --git a/vxworks/msgQLib.c b/vxworks/msgQLib.c
index d591a7b..e3d071d 100644
--- a/vxworks/msgQLib.c
+++ b/vxworks/msgQLib.c
@@ -149,6 +149,8 @@ int msgQReceive(MSG_Q_ID msgQId, char *buffer, UINT maxNBytes, int timeout)
 		return ERROR;
 	}
 
+	check_task_restart(wind_task_current());
+
 	mq = get_mq_from_id(msgQId);
 	if (mq == NULL) {
 	objid_error:
@@ -217,6 +219,8 @@ STATUS msgQSend(MSG_Q_ID msgQId, const char *buffer, UINT bytes,
 	UINT maxbytes;
 	int ret;
 	
+	check_task_restart(wind_task_current());
+
 	mq = get_mq_from_id(msgQId);
 	if (mq == NULL) {
 	objid_error:
diff --git a/vxworks/semLib.c b/vxworks/semLib.c
index 6467354..dfc37cc 100644
--- a/vxworks/semLib.c
+++ b/vxworks/semLib.c
@@ -61,7 +61,9 @@ static STATUS xsem_take(struct wind_sem *sem, int timeout)
 
 	if (threadobj_async_p())
 		return S_intLib_NOT_ISR_CALLABLE;
-	  
+
+	check_task_restart(wind_task_current());
+
 	if (syncobj_lock(&sem->u.xsem.sobj, &syns))
 		return S_objLib_OBJ_ID_ERROR;
 
@@ -196,8 +198,11 @@ static STATUS msem_take(struct wind_sem *sem, int timeout)
 		return S_intLib_NOT_ISR_CALLABLE;
 	  
 	current = wind_task_current();
-	if (current != NULL && (sem->options & SEM_DELETE_SAFE))
-		pthread_mutex_lock(&current->safelock);
+	if (current != NULL) {
+		check_task_restart(current);
+		if (sem->options & SEM_DELETE_SAFE)
+			pthread_mutex_lock(&current->safelock);
+	}
 
 	if (timeout == NO_WAIT) {
 		ret = pthread_mutex_trylock(&sem->u.msem.lock);
diff --git a/vxworks/taskLib.c b/vxworks/taskLib.c
index a9e12e7..7e74cf0 100644
--- a/vxworks/taskLib.c
+++ b/vxworks/taskLib.c
@@ -97,6 +97,8 @@ struct wind_task *get_wind_task_or_self(TASK_ID tid)
 	if (current == NULL)
 		return NULL;
 
+	check_task_restart(current);
+
 	/* This one might block but can't fail, it is ours. */
 	threadobj_lock(&current->thobj);
 
@@ -114,6 +116,12 @@ void put_wind_task(struct wind_task *task)
 	threadobj_unlock(&task->thobj);
 }
 
+void check_task_restart(struct wind_task *current)
+{
+	if (current && (current->tcb->status & WIND_RESTART) != 0)
+		siglongjmp(current->ienv, 1);
+}
+
 static void task_finalizer(struct threadobj *thobj)
 {
 	struct wind_task *task = container_of(thobj, struct wind_task, thobj);
@@ -253,6 +261,11 @@ static void *task_trampoline(void *arg)
 	/* Wait for someone to run taskActivate() upon us. */
 	sem_wait(&task->barrier);
 
+	threadobj_lock(&task->thobj);
+	sigsetjmp(task->ienv, 1); /* For taskRestart(). */
+	task->tcb->status &= ~WIND_RESTART;
+	threadobj_unlock(&task->thobj);
+
 	args->entry(args->arg0, args->arg1, args->arg2, args->arg3,
 		    args->arg4, args->arg5, args->arg6, args->arg7,
 		    args->arg8, args->arg9);
@@ -356,7 +369,7 @@ STATUS taskInit(WIND_TCB *pTcb,
 		errno = S_intLib_NOT_ISR_CALLABLE;
 		return ERROR;
 	}
-	  
+
 	task = alloc_task();
 	if (task == NULL) {
 		errno = S_memLib_NOT_ENOUGH_MEMORY;
@@ -396,6 +409,30 @@ STATUS taskActivate(TASK_ID tid)
 	return OK;
 }
 
+STATUS taskRestart(TASK_ID tid)
+{
+	struct wind_task *task;
+
+	if (threadobj_async_p()) {
+		errno = S_intLib_NOT_ISR_CALLABLE;
+		return ERROR;
+	}
+	  
+	/* The thread lock will be released at jump site. */
+	task = get_wind_task_or_self(tid);
+	if (task == NULL) {
+		errno = S_objLib_OBJ_ID_ERROR;
+		return ERROR;
+	}
+
+	task->tcb->status |= WIND_RESTART;
+
+	if (task == wind_task_current())
+		siglongjmp(task->ienv, 1); /* Does not return. */
+
+	return OK;
+}
+
 TASK_ID taskSpawn(const char *name,
 		  int prio,
 		  int flags,
@@ -454,8 +491,10 @@ static STATUS __taskDelete(TASK_ID tid, int force)
 	if (task == NULL)
 		goto objid_error;
 
-	if (task == wind_task_current()) /* Self-deletion. */
+	if (task == wind_task_current()) { /* Self-deletion. */
+		check_task_restart(task);
 		pthread_exit(NULL);
+	}
 
 	/*
 	 * We always attempt to grab the thread safe lock first, then
@@ -517,11 +556,12 @@ TASK_ID taskIdSelf(void)
 	}
 	  
 	current = wind_task_current();
-
 	if (current == NULL) {
 		errno = S_objLib_OBJ_NO_METHOD;
 		return ERROR;
 	}
+
+	check_task_restart(current);
 	  
 	return (TASK_ID)current->tcb;
 }
@@ -546,6 +586,7 @@ STATUS taskSuspend(TASK_ID tid)
 	if (task == NULL)
 		goto objid_error;
 
+	check_task_restart(wind_task_current());
 	ret = threadobj_suspend(&task->thobj);
 	put_wind_task(task);
 
@@ -648,7 +689,6 @@ STATUS taskPrioritySet(TASK_ID tid, int prio)
 		goto objid_error;
 
 	ret = check_task_priority(prio);
-
 	if (ret) {
 		put_wind_task(task);
 		errno = ret;
@@ -745,12 +785,13 @@ STATUS taskDelay(int ticks)
 	}
 
 	current = wind_task_current();
-
 	if (current == NULL) {
 		errno = S_objLib_OBJ_NO_METHOD;
 		return ERROR;
 	}
 
+	check_task_restart(current);
+
 	if (ticks == 0) {
 		sched_yield();	/* Manual round-robin. */
 		return OK;
diff --git a/vxworks/taskLib.h b/vxworks/taskLib.h
index 299d059..46718e1 100644
--- a/vxworks/taskLib.h
+++ b/vxworks/taskLib.h
@@ -20,6 +20,7 @@
 #define _VXWORKS_TASKLIB_H
 
 #include <semaphore.h>
+#include <setjmp.h>
 #include <xenomai/threadobj.h>
 #include <xenomai/registry.h>
 #include <vxworks/taskLib.h>
@@ -53,6 +54,8 @@ struct wind_task {
 	struct threadobj thobj;
 	struct fsobj fsobj;
 	struct hashobj obj;
+
+	jmp_buf ienv;
 };
 
 int wind_task_get_priority(struct wind_task *task);
@@ -97,6 +100,8 @@ struct wind_task *get_wind_task_or_self(TASK_ID tid);
 
 void put_wind_task(struct wind_task *task);
 
+void check_task_restart(struct wind_task *current);
+
 extern struct hash_table wind_task_table;
 
 #endif /* _VXWORKS_TASKLIB_H */