diff --git a/core/net/mac/phase.c b/core/net/mac/phase.c
index 6e29ed959..e9c075fce 100644
--- a/core/net/mac/phase.c
+++ b/core/net/mac/phase.c
@@ -28,7 +28,6 @@
  *
  * This file is part of the Contiki operating system.
  *
- * $Id: phase.c,v 1.17 2010/12/18 22:12:53 dak664 Exp $
  */
 
 /**
@@ -108,6 +107,9 @@ phase_update(const struct phase_list *list,
   e = find_neighbor(list, neighbor);
   if(e != NULL) {
     if(mac_status == MAC_TX_OK) {
+#if PHASE_DRIFT_CORRECT
+      e->drift = time-e->time;
+#endif
       e->time = time;
     }
     /* If the neighbor didn't reply to us, it may have switched
@@ -140,6 +142,9 @@ phase_update(const struct phase_list *list,
       }
       rimeaddr_copy(&e->neighbor, neighbor);
       e->time = time;
+#if PHASE_DRIFT_CORRECT
+      e->drift = 0;
+#endif
       e->noacks = 0;
       list_push(*list->list, e);
     }
@@ -197,8 +202,25 @@ phase_wait(struct phase_list *list,
     now = RTIMER_NOW();
 
     sync = (e == NULL) ? now : e->time;
-    wait = (rtimer_clock_t)((sync - now) &
-                            (cycle_time - 1));
+
+#if PHASE_DRIFT_CORRECT
+{ int32_t s;
+  if (e->drift > cycle_time) {
+    s = e->drift%cycle_time/(e->drift/cycle_time);  //drift per cycle
+    s = s*(now-sync)/cycle_time;                    //estimated drift to now
+    sync += s;                                      //add it in
+  }
+}
+#endif
+
+#if 1
+    /* Faster if cycle_time is a power of two */
+    wait = (rtimer_clock_t)((sync - now) & (cycle_time - 1));
+#else
+    /* Works generally */
+    wait = cycle_time - (rtimer_clock_t)((now - sync) % cycle_time);
+#endif
+
     if(wait < guard_time) {
       wait += cycle_time;
     }
@@ -226,6 +248,7 @@ phase_wait(struct phase_list *list,
     expected = now + wait - guard_time;
     if(!RTIMER_CLOCK_LT(expected, now)) {
       /* Wait until the receiver is expected to be awake */
+//    printf("%d ",expected%cycle_time);  //for spreadsheet export
       while(RTIMER_CLOCK_LT(RTIMER_NOW(), expected));
     }
     return PHASE_SEND_NOW;
diff --git a/core/net/mac/phase.h b/core/net/mac/phase.h
index d74431fcc..7ad07f9cf 100644
--- a/core/net/mac/phase.h
+++ b/core/net/mac/phase.h
@@ -28,7 +28,6 @@
  *
  * This file is part of the Contiki operating system.
  *
- * $Id: phase.h,v 1.5 2010/09/13 13:39:05 adamdunkels Exp $
  */
 
 /**
@@ -48,10 +47,19 @@
 #include "lib/memb.h"
 #include "net/netstack.h"
 
+#if PHASE_CONF_DRIFT_CORRECT
+#define PHASE_DRIFT_CORRECT PHASE_CONF_DRIFT_CORRECT
+#else
+#define PHASE_DRIFT_CORRECT 1
+#endif
+
 struct phase {
   struct phase *next;
   rimeaddr_t neighbor;
   rtimer_clock_t time;
+#if PHASE_DRIFT_CORRECT
+  rtimer_clock_t drift;
+#endif
   uint8_t noacks;
   struct timer noacks_timer;
 };