--- sys.c_cvs	2006-10-11 14:04:04.000000000 +0200
+++ sys.c	2007-02-28 19:34:12.000000000 +0100
@@ -44,27 +44,35 @@
 };
 
 
-
 void
 sys_mbox_fetch(sys_mbox_t mbox, void **msg)
 {
+  sys_arch_timestamp_t diff;
   u32_t time;
   struct sys_timeouts *timeouts;
   struct sys_timeo *tmptimeout;
   sys_timeout_handler h;
   void *arg;
 
-
- again:
   timeouts = sys_arch_timeouts();
+  if (timeouts != NULL) {
+    diff = SYS_ARCH_TIMESTAMP_DIFF(sys_arch_time_now(), timeouts->remember);
+  } else {
+    diff = 0;
+  }
+
+  again:
 
   if (!timeouts || !timeouts->next) {
     sys_arch_mbox_fetch(mbox, msg, 0);
   } else {
-    if (timeouts->next->time > 0) {
-      time = sys_arch_mbox_fetch(mbox, msg, timeouts->next->time);
-    } else {
+    /* ANDREAS -> execute old pending timeouts */
+    if (timeouts->next->time <= diff) {
+      diff -= timeouts->next->time;
       time = SYS_ARCH_TIMEOUT;
+    } else {
+      timeouts->next->time -= diff;
+      time = sys_arch_mbox_fetch(mbox, msg, timeouts->next->time);
     }
 
     if (time == SYS_ARCH_TIMEOUT) {
@@ -88,69 +96,21 @@
    occured. The time variable is set to the number of
    milliseconds we waited for the message. */
       if (time <= timeouts->next->time) {
-  timeouts->next->time -= time;
+        timeouts->next->time -= time;
       } else {
-  timeouts->next->time = 0;
+        timeouts->next->time = 0;
       }
-    }
 
+      timeouts->remember = sys_arch_time_now();	// Remember the start Time.
+    }
   }
 }
 
+
 void
 sys_sem_wait(sys_sem_t sem)
 {
-  u32_t time;
-  struct sys_timeouts *timeouts;
-  struct sys_timeo *tmptimeout;
-  sys_timeout_handler h;
-  void *arg;
-
-  /*  while (sys_arch_sem_wait(sem, 1000) == 0);
-      return;*/
-
- again:
-
-  timeouts = sys_arch_timeouts();
-
-  if (!timeouts || !timeouts->next) {
-    sys_arch_sem_wait(sem, 0);
-  } else {
-    if (timeouts->next->time > 0) {
-      time = sys_arch_sem_wait(sem, timeouts->next->time);
-    } else {
-      time = SYS_ARCH_TIMEOUT;
-    }
-
-    if (time == SYS_ARCH_TIMEOUT) {
-      /* If time == SYS_ARCH_TIMEOUT, a timeout occured before a message
-   could be fetched. We should now call the timeout handler and
-   deallocate the memory allocated for the timeout. */
-      tmptimeout = timeouts->next;
-      timeouts->next = tmptimeout->next;
-      h = tmptimeout->h;
-      arg = tmptimeout->arg;
-      memp_free(MEMP_SYS_TIMEOUT, tmptimeout);
-      if (h != NULL) {
-        LWIP_DEBUGF(SYS_DEBUG, ("ssw h=%p(%p)\n", (void *)h, (void *)arg));
-        h(arg);
-      }
-
-
-      /* We try again to fetch a message from the mbox. */
-      goto again;
-    } else {
-      /* If time != SYS_ARCH_TIMEOUT, a message was received before the timeout
-   occured. The time variable is set to the number of
-   milliseconds we waited for the message. */
-      if (time <= timeouts->next->time) {
-  timeouts->next->time -= time;
-      } else {
-  timeouts->next->time = 0;
-      }
-    }
-
-  }
+  sys_arch_sem_wait(sem, 0); // Block until free
 }
 
 void
@@ -236,19 +196,6 @@
 }
 
 
-
-
-
-static void
-sswt_handler(void *arg)
-{
-    struct sswt_cb *sswt_cb = (struct sswt_cb *) arg;
-
-    /* Timeout. Set flag to TRUE and signal semaphore */
-    sswt_cb->timeflag = 1;
-    sys_sem_signal(*(sswt_cb->psem));
-}
-
 /* Wait for a semaphore with timeout (specified in ms) */
 /* timeout = 0: wait forever */
 /* Returns 0 on timeout. 1 otherwise */
@@ -256,30 +203,19 @@
 int
 sys_sem_wait_timeout(sys_sem_t sem, u32_t timeout)
 {
-    struct sswt_cb sswt_cb;
+    u32_t ret;
 
-    sswt_cb.psem = &sem;
-    sswt_cb.timeflag = 0;
+    ret = sys_arch_sem_wait(sem, timeout);
 
-    /* If timeout is zero, then just wait forever */
-    if (timeout > 0)
-        /* Create a timer and pass it the address of our flag */
-        sys_timeout(timeout, sswt_handler, &sswt_cb);
-    sys_sem_wait(sem);
-    /* Was it a timeout? */
-    if (sswt_cb.timeflag)
-    {
-        /* timeout */
+    if (ret ==  SYS_ARCH_TIMEOUT) {
+	 /* timeout */
         return 0;
     } else {
-        /* Not a timeout. Remove timeout entry */
-        sys_untimeout(sswt_handler, &sswt_cb);
+        /* Not a timeout */
         return 1;
     }
-
 }
 
-
 void
 sys_msleep(u32_t ms)
 {
