1 files changed, 24 insertions, 2 deletions

diff --git a/mdk-stage1/dietlibc/libpthread/pthread_cond_wait.c b/mdk-stage1/dietlibc/libpthread/pthread_cond_wait.c
index 0c6c0ae13..8db49e840 100644
--- a/mdk-stage1/dietlibc/libpthread/pthread_cond_wait.c
+++ b/mdk-stage1/dietlibc/libpthread/pthread_cond_wait.c
@@ -14,6 +14,7 @@ int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex)
   this=__thread_self();
 
   /* put in wait-chain */
+  __NO_ASYNC_CANCEL_BEGIN;
   __pthread_lock(&(cond->lock));
   this->waiting=1;
   if (cond->wait_chain) {
@@ -21,16 +22,37 @@ int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex)
     tmp->waitnext=this;
   } else cond->wait_chain=this;
   __pthread_unlock(&(cond->lock));
+  __NO_ASYNC_CANCEL_STOP;
 
   /* Aeh yeah / wait till signal */
   pthread_mutex_unlock(mutex);
   while (this->waiting) {
     __thread_wait_some_time();
-    if (this->canceled) this->waiting=0;	/* we got a cancel signal */
+    if (this->canceled) break;	/* we got a cancel signal */
   }
   pthread_mutex_lock(mutex);
 
-  __TEST_CANCEL();
+  __NO_ASYNC_CANCEL_BEGIN;
+  __pthread_lock(&(cond->lock));
+  if (this->waiting) {	/* still waiting -> SIGNAL */
+    _pthread_descr prev;
+    /* remove from wait-chain */
+    prev=cond->wait_chain;
+    if ((prev=cond->wait_chain)==this) {
+      cond->wait_chain=this->waitnext;
+    } else {
+      for (tmp=prev->waitnext;tmp;prev=tmp,tmp=prev->waitnext) {
+	if (tmp==this) {
+	  prev->waitnext=this->waitnext;
+	  break;
+	}
+      }
+    }
+    this->waiting=0;
+    this->waitnext=0;
+  }
+  __pthread_unlock(&(cond->lock));
+  __NO_ASYNC_CANCEL_END;
   return 0;
 }