diff --git a/common/scoped_cycle_counter.h b/common/scoped_cycle_counter.h
index ba482b731..2ae773b0d 100644
--- a/common/scoped_cycle_counter.h
+++ b/common/scoped_cycle_counter.h
@@ -3,33 +3,35 @@
 
 class ScopedCycleCounter {
 public:
-  ScopedCycleCounter(uint64_t& dest) :
-    dest_(dest) {
-#ifndef DISABLE_DIAGNOSTIC_COMMANDS      
+  static inline uint32_t getCycles() {
 #ifdef TEENSYDUINO
-    cycles_ = ARM_DWT_CYCCNT;
+    return ARM_DWT_CYCCNT - counted_cycles_;
 #else
-    cycles_ = DWT->CYCCNT;
+    return DWT->CYCCNT - counted_cycles_;
 #endif
+  }
+  ScopedCycleCounter(uint64_t& dest) :
+    dest_(dest) {
+#ifndef DISABLE_DIAGNOSTIC_COMMANDS
+    cycles_ = getCycles();
 #endif    
   }
   ~ScopedCycleCounter() {
 #ifndef DISABLE_DIAGNOSTIC_COMMANDS
     uint32_t cycles;
-#ifdef TEENSYDUINO
-    cycles = ARM_DWT_CYCCNT - cycles_;
-    ARM_DWT_CYCCNT = cycles_;
-#else
-    cycles = DWT->CYCCNT - cycles_;
-    DWT->CYCCNT = cycles_;
-#endif
+    cycles = getCycles() - cycles_;
+    noInterrupts();
+    counted_cycles_ += cycles;
+    interrupts();
     dest_ += cycles;
 #endif    
   }
 private:
+  static uint32_t counted_cycles_;
   uint32_t cycles_;
   uint64_t& dest_;
 };
 
+uint32_t ScopedCycleCounter::counted_cycles_ = 0;
 
 #endif