diff --git a/core/sys/energest.h b/core/sys/energest.h
index 6d43aaa11..a8f445b65 100644
--- a/core/sys/energest.h
+++ b/core/sys/energest.h
@@ -105,6 +105,21 @@ extern energest_t energest_leveldevice_current_leveltime[ENERGEST_CONF_LEVELDEVI
 										energest_current_time[type]); \
 										energest_current_mode[type] = 0; \
                                        } while(0)
+
+#define ENERGEST_SWITCH(type_off, type_on) do { \
+                                             rtimer_clock_t energest_local_variable_now = RTIMER_NOW(); \
+                                             if(energest_current_mode[type_off] != 0) { \
+                                               if (energest_local_variable_now < energest_current_time[type_off]) { \
+                                                 energest_total_time[type_off].current += RTIMER_ARCH_SECOND; \
+                                               } \
+                                               energest_total_time[type_off].current += (rtimer_clock_t)(energest_local_variable_now - \
+                                                 energest_current_time[type_off]); \
+                                               energest_current_mode[type_off] = 0; \
+                                             } \
+                                             energest_current_time[type_on] = energest_local_variable_now; \
+                                             energest_current_mode[type_on] = 1; \
+                                           } while(0)
+
 #else
 #define ENERGEST_OFF(type) if(energest_current_mode[type] != 0) do {	\
                            energest_total_time[type].current += (rtimer_clock_t)(RTIMER_NOW() - \
@@ -117,13 +132,24 @@ extern energest_t energest_leveldevice_current_leveltime[ENERGEST_CONF_LEVELDEVI
 			                energest_current_time[type]); \
 			   energest_current_mode[type] = 0; \
                                         } while(0)
-#endif
 
+#define ENERGEST_SWITCH(type_off, type_on) do { \
+                                             rtimer_clock_t energest_local_variable_now = RTIMER_NOW(); \
+                                             if(energest_current_mode[type_off] != 0) { \
+                                               energest_total_time[type_off].current += (rtimer_clock_t)(energest_local_variable_now - \
+                                                 energest_current_time[type_off]); \
+                                               energest_current_mode[type_off] = 0; \
+                                             } \
+                                             energest_current_time[type_on] = energest_local_variable_now; \
+                                             energest_current_mode[type_on] = 1; \
+                                           } while(0)
+#endif
 
 #else /* ENERGEST_CONF_ON */
 #define ENERGEST_ON(type) do { } while(0)
 #define ENERGEST_OFF(type) do { } while(0)
 #define ENERGEST_OFF_LEVEL(type,level) do { } while(0)
+#define ENERGEST_SWITCH(type_off, type_on) do { } while(0)
 #endif /* ENERGEST_CONF_ON */
 
 #endif /* ENERGEST_H_ */
diff --git a/cpu/cc2538/lpm.c b/cpu/cc2538/lpm.c
index 91d67acc9..45c7756e6 100644
--- a/cpu/cc2538/lpm.c
+++ b/cpu/cc2538/lpm.c
@@ -128,8 +128,7 @@ periph_permit_pm1(void)
 static void
 enter_pm0(void)
 {
-  ENERGEST_OFF(ENERGEST_TYPE_CPU);
-  ENERGEST_ON(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
   /* We are only interested in IRQ energest while idle or in LPM */
   ENERGEST_IRQ_RESTORE(irq_energest);
@@ -147,8 +146,7 @@ enter_pm0(void)
   /* Remember IRQ energest for next pass */
   ENERGEST_IRQ_SAVE(irq_energest);
 
-  ENERGEST_ON(ENERGEST_TYPE_CPU);
-  ENERGEST_OFF(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 }
 /*---------------------------------------------------------------------------*/
 static void
@@ -238,8 +236,7 @@ lpm_exit()
   /* Remember IRQ energest for next pass */
   ENERGEST_IRQ_SAVE(irq_energest);
 
-  ENERGEST_ON(ENERGEST_TYPE_CPU);
-  ENERGEST_OFF(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 }
 /*---------------------------------------------------------------------------*/
 void
@@ -312,8 +309,7 @@ lpm_enter()
 
   /* We are only interested in IRQ energest while idle or in LPM */
   ENERGEST_IRQ_RESTORE(irq_energest);
-  ENERGEST_OFF(ENERGEST_TYPE_CPU);
-  ENERGEST_ON(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
   /* Remember the current time so we can keep stats when we wake up */
   if(LPM_CONF_STATS) {
@@ -339,8 +335,7 @@ lpm_enter()
 
     /* Remember IRQ energest for next pass */
     ENERGEST_IRQ_SAVE(irq_energest);
-    ENERGEST_ON(ENERGEST_TYPE_CPU);
-    ENERGEST_OFF(ENERGEST_TYPE_LPM);
+    ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
   } else {
     /* All clear. Assert WFI and drop to PM1/2. This is now un-interruptible */
     assert_wfi();
diff --git a/cpu/cc26xx-cc13xx/lpm.c b/cpu/cc26xx-cc13xx/lpm.c
index 2f7b964f8..3484b74bf 100644
--- a/cpu/cc26xx-cc13xx/lpm.c
+++ b/cpu/cc26xx-cc13xx/lpm.c
@@ -202,8 +202,7 @@ wake_up(void)
 
   /* Remember IRQ energest for next pass */
   ENERGEST_IRQ_SAVE(irq_energest);
-  ENERGEST_ON(ENERGEST_TYPE_CPU);
-  ENERGEST_OFF(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 
   /* Sync so that we get the latest values before adjusting recharge settings */
   ti_lib_sys_ctrl_aon_sync();
@@ -396,8 +395,7 @@ lpm_drop()
 
     /* We are only interested in IRQ energest while idle or in LPM */
     ENERGEST_IRQ_RESTORE(irq_energest);
-    ENERGEST_OFF(ENERGEST_TYPE_CPU);
-    ENERGEST_ON(ENERGEST_TYPE_LPM);
+    ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
     /* Sync the AON interface to ensure all writes have gone through. */
     ti_lib_sys_ctrl_aon_sync();
@@ -429,8 +427,7 @@ lpm_drop()
 void
 lpm_sleep(void)
 {
-  ENERGEST_OFF(ENERGEST_TYPE_CPU);
-  ENERGEST_ON(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
   /* We are only interested in IRQ energest while idle or in LPM */
   ENERGEST_IRQ_RESTORE(irq_energest);
@@ -443,8 +440,7 @@ lpm_sleep(void)
   /* Remember IRQ energest for next pass */
   ENERGEST_IRQ_SAVE(irq_energest);
 
-  ENERGEST_ON(ENERGEST_TYPE_CPU);
-  ENERGEST_OFF(ENERGEST_TYPE_LPM);
+  ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 }
 /*---------------------------------------------------------------------------*/
 void
diff --git a/platform/cc2530dk/contiki-main.c b/platform/cc2530dk/contiki-main.c
index 88c7763a4..164481512 100644
--- a/platform/cc2530dk/contiki-main.c
+++ b/platform/cc2530dk/contiki-main.c
@@ -335,8 +335,7 @@ main(void) CC_NON_BANKED
     if(SLEEPCMD & SLEEP_MODE0) {
 #endif /* LPM_MODE==LPM_MODE_PM2 */
 
-      ENERGEST_OFF(ENERGEST_TYPE_CPU);
-      ENERGEST_ON(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
       /* We are only interested in IRQ energest while idle or in LPM */
       ENERGEST_IRQ_RESTORE(irq_energest);
@@ -352,8 +351,7 @@ main(void) CC_NON_BANKED
       /* Remember energest IRQ for next pass */
       ENERGEST_IRQ_SAVE(irq_energest);
 
-      ENERGEST_ON(ENERGEST_TYPE_CPU);
-      ENERGEST_OFF(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 
 #if (LPM_MODE==LPM_MODE_PM2)
       SLEEPCMD &= ~SLEEP_OSC_PD;            /* Make sure both HS OSCs are on */
diff --git a/platform/exp5438/contiki-exp5438-main.c b/platform/exp5438/contiki-exp5438-main.c
index 589a9572d..2f85f6ba2 100644
--- a/platform/exp5438/contiki-exp5438-main.c
+++ b/platform/exp5438/contiki-exp5438-main.c
@@ -307,8 +307,7 @@ main(int argc, char **argv)
       static unsigned long irq_energest = 0;
 
       /* Re-enable interrupts and go to sleep atomically. */
-      ENERGEST_OFF(ENERGEST_TYPE_CPU);
-      ENERGEST_ON(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
       /* We only want to measure the processing done in IRQs when we
          are asleep, so we discard the processing time done when we
          were awake. */
@@ -327,8 +326,7 @@ main(int argc, char **argv)
       irq_energest = energest_type_time(ENERGEST_TYPE_IRQ);
       eint();
       watchdog_start();
-      ENERGEST_OFF(ENERGEST_TYPE_LPM);
-      ENERGEST_ON(ENERGEST_TYPE_CPU);
+      ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
     }
   }
 }
diff --git a/platform/mbxxx/contiki-main.c b/platform/mbxxx/contiki-main.c
index 020591aff..8a6867d74 100644
--- a/platform/mbxxx/contiki-main.c
+++ b/platform/mbxxx/contiki-main.c
@@ -251,15 +251,13 @@ main(void)
     
     
     
-    ENERGEST_OFF(ENERGEST_TYPE_CPU);
-    /* watchdog_stop(); */
-    ENERGEST_ON(ENERGEST_TYPE_LPM);
+    /* watchdog_stop(); */    
+    ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
     /* Go to idle mode. */
     halSleepWithOptions(SLEEPMODE_IDLE,0);
     /* We are awake. */
     /* watchdog_start(); */
-    ENERGEST_OFF(ENERGEST_TYPE_LPM);
-    ENERGEST_ON(ENERGEST_TYPE_CPU);  
+    ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
     
   }
   
diff --git a/platform/seedeye/contiki-seedeye-main.c b/platform/seedeye/contiki-seedeye-main.c
index dabb8f126..5394b8e02 100644
--- a/platform/seedeye/contiki-seedeye-main.c
+++ b/platform/seedeye/contiki-seedeye-main.c
@@ -165,15 +165,13 @@ main(int argc, char **argv)
       r = process_run();
     } while(r > 0);
 
-    ENERGEST_OFF(ENERGEST_TYPE_CPU);   
-    ENERGEST_ON(ENERGEST_TYPE_LPM);
+    ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
     watchdog_stop();
     asm volatile("wait");
     watchdog_start();
 
-    ENERGEST_OFF(ENERGEST_TYPE_LPM);
-    ENERGEST_ON(ENERGEST_TYPE_CPU);
+    ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 
   }
 
diff --git a/platform/sensinode/contiki-sensinode-main.c b/platform/sensinode/contiki-sensinode-main.c
index 9ba3766be..541b28d26 100644
--- a/platform/sensinode/contiki-sensinode-main.c
+++ b/platform/sensinode/contiki-sensinode-main.c
@@ -346,8 +346,7 @@ main(void)
     if(SLEEP & SLEEP_MODE0) {
 #endif /* LPM_MODE==LPM_MODE_PM2 */
 
-      ENERGEST_OFF(ENERGEST_TYPE_CPU);
-      ENERGEST_ON(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
 
       /* We are only interested in IRQ energest while idle or in LPM */
       ENERGEST_IRQ_RESTORE(irq_energest);
@@ -363,8 +362,7 @@ main(void)
       /* Remember energest IRQ for next pass */
       ENERGEST_IRQ_SAVE(irq_energest);
 
-      ENERGEST_ON(ENERGEST_TYPE_CPU);
-      ENERGEST_OFF(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
 
 #if (LPM_MODE==LPM_MODE_PM2)
       SLEEP &= ~OSC_PD;            /* Make sure both HS OSCs are on */
diff --git a/platform/sky/contiki-sky-main.c b/platform/sky/contiki-sky-main.c
index 46f6f99de..a099778c6 100644
--- a/platform/sky/contiki-sky-main.c
+++ b/platform/sky/contiki-sky-main.c
@@ -465,8 +465,7 @@ main(int argc, char **argv)
 #endif
       
       /* Re-enable interrupts and go to sleep atomically. */
-      ENERGEST_OFF(ENERGEST_TYPE_CPU);
-      ENERGEST_ON(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
       /* We only want to measure the processing done in IRQs when we
 	 are asleep, so we discard the processing time done when we
 	 were awake. */
@@ -489,8 +488,7 @@ main(int argc, char **argv)
       irq_energest = energest_type_time(ENERGEST_TYPE_IRQ);
       eint();
       watchdog_start();
-      ENERGEST_OFF(ENERGEST_TYPE_LPM);
-      ENERGEST_ON(ENERGEST_TYPE_CPU);
+      ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
     }
   }
 
diff --git a/platform/wismote/contiki-wismote-main.c b/platform/wismote/contiki-wismote-main.c
index 8a79811ac..df0162585 100644
--- a/platform/wismote/contiki-wismote-main.c
+++ b/platform/wismote/contiki-wismote-main.c
@@ -425,8 +425,7 @@ main(int argc, char **argv)
       static unsigned long irq_energest = 0;
 
       /* Re-enable interrupts and go to sleep atomically. */
-      ENERGEST_OFF(ENERGEST_TYPE_CPU);
-      ENERGEST_ON(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
       /* We only want to measure the processing done in IRQs when we
 	 are asleep, so we discard the processing time done when we
 	 were awake. */
@@ -445,8 +444,7 @@ main(int argc, char **argv)
       irq_energest = energest_type_time(ENERGEST_TYPE_IRQ);
       eint();
       watchdog_start();
-      ENERGEST_OFF(ENERGEST_TYPE_LPM);
-      ENERGEST_ON(ENERGEST_TYPE_CPU);
+      ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
     }
   }
 }
diff --git a/platform/z1/contiki-z1-main.c b/platform/z1/contiki-z1-main.c
index bab055d08..45e97bde9 100644
--- a/platform/z1/contiki-z1-main.c
+++ b/platform/z1/contiki-z1-main.c
@@ -452,8 +452,7 @@ main(int argc, char **argv)
 #endif
 
       /* Re-enable interrupts and go to sleep atomically. */
-      ENERGEST_OFF(ENERGEST_TYPE_CPU);
-      ENERGEST_ON(ENERGEST_TYPE_LPM);
+      ENERGEST_SWITCH(ENERGEST_TYPE_CPU, ENERGEST_TYPE_LPM);
       /* We only want to measure the processing done in IRQs when we
          are asleep, so we discard the processing time done when we
          were awake. */
@@ -472,8 +471,7 @@ main(int argc, char **argv)
       irq_energest = energest_type_time(ENERGEST_TYPE_IRQ);
       eint();
       watchdog_start();
-      ENERGEST_OFF(ENERGEST_TYPE_LPM);
-      ENERGEST_ON(ENERGEST_TYPE_CPU);
+      ENERGEST_SWITCH(ENERGEST_TYPE_LPM, ENERGEST_TYPE_CPU);
     }
   }