2 files changed, 6 insertions, 3 deletions
diff --git a/src/chdelta.c b/src/chdelta.c
index f87aec5a9..8a8100e5f 100644
--- a/src/chdelta.c
+++ b/src/chdelta.c
@@ -50,10 +50,9 @@ void chVTInit(void) {
  */
 void chVTSetI(VirtualTimer *vtp, t_time time, t_vtfunc vtfunc, void *par) {
 
-  vtp->vt_func = vtfunc;
   vtp->vt_par = par;
-
   if (time) {
+    vtp->vt_func = vtfunc;
     VirtualTimer *p = dlist.dl_next;
     while (p->vt_dtime < time) {
       time -= p->vt_dtime;
@@ -66,6 +65,8 @@ void chVTSetI(VirtualTimer *vtp, t_time time, t_vtfunc vtfunc, void *par) {
     if (p != (VirtualTimer *)&dlist)
       p->vt_dtime -= time;
   }
+  else
+    vtp->vt_func = NULL;
 }
 
 /**
diff --git a/src/chevents.c b/src/chevents.c
index a38975216..f88412e11 100644
--- a/src/chevents.c
+++ b/src/chevents.c
@@ -205,6 +205,7 @@ t_eventid chEvtWaitTimeout(t_eventmask ewmask,
                            t_time time) {
   t_eventid i;
   t_eventmask m;
+  t_msg msg;
 
   chSysLock();
 
@@ -221,9 +222,10 @@ t_eventid chEvtWaitTimeout(t_eventmask ewmask,
     currp->p_ewmask = ewmask;
     chSchGoSleepS(PRWTEVENT);
     if (!vt.vt_func) {
+      t_msg msg = currp->p_rdymsg;
 
       chSysUnlock();
-      return RDY_TIMEOUT; // No need to read t_rdymsg value, it is a timeout.
+      return msg;
     }
     chVTResetI(&vt);
   }