1 files changed, 41 insertions, 10 deletions

diff --git a/mdk-stage1/dietlibc/libpthread/pthread_cond_timedwait.c b/mdk-stage1/dietlibc/libpthread/pthread_cond_timedwait.c
index 6613f2ca9..71d80bdb5 100644
--- a/mdk-stage1/dietlibc/libpthread/pthread_cond_timedwait.c
+++ b/mdk-stage1/dietlibc/libpthread/pthread_cond_timedwait.c
@@ -9,13 +9,14 @@ int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex,
 {
   _pthread_descr tmp;
   _pthread_descr this;
-  int ret;
+  int ret=0;
 
   __THREAD_INIT();
 
   this=__thread_self();
 
   /* put in wait-chain */
+  __NO_ASYNC_CANCEL_BEGIN;
   __pthread_lock(&(cond->lock));
   this->waiting=1;
   if (cond->wait_chain) {
@@ -23,21 +24,51 @@ int pthread_cond_timedwait(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 */
+  /* Aeh yeah / wait till cond-signal OR timeout */
   pthread_mutex_unlock(mutex);
-  ret=nanosleep(abstime,0);
+  while (this->waiting) {
+    struct timeval tv;
+    gettimeofday(&tv,0);
+    if ((abstime->tv_sec <= tv.tv_sec) && (abstime->tv_nsec <= (tv.tv_usec*1000))) {
+      ret = 1;
+      break;
+    }
+    __thread_wait_some_time();
+    if (this->canceled) break;	/* a cancel */
+  }
   pthread_mutex_lock(mutex);
 
-  __TEST_CANCEL();
-
-  if (ret) {
-    if ((*__errno_location())!=EINTR) return -1;
-    return 0;
+  __NO_ASYNC_CANCEL_BEGIN;
+  __pthread_lock(&(cond->lock));
+  if (this->waiting) {	/* still waiting -> TIMEOUT or 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;
+
+  if (ret) return ETIMEDOUT;
 
-  (*__errno_location())=ETIMEDOUT;
-  return -1;
+  {
+    register int err = (*__errno_location());
+    if (err==EINTR) return err;
+  }
+  return 0;
 }