1 files changed, 31 insertions, 10 deletions

diff --git a/cores/arduino/wiring.c b/cores/arduino/wiring.c
index 73c26f2..72bc282 100755
--- a/cores/arduino/wiring.c
+++ b/cores/arduino/wiring.c
@@ -24,32 +24,53 @@
 
 #include "wiring_private.h"
 
+// the prescaler is set so that timer0 ticks every 64 clock cycles, and the
+// the overflow handler is called every 256 ticks.
+#define MICROSECONDS_PER_TIMER0_OVERFLOW (clockCyclesToMicroseconds(64 * 256))
+
+// the whole number of milliseconds per timer0 overflow
+#define MILLIS_INC (MICROSECONDS_PER_TIMER0_OVERFLOW / 1000)
+
+// the fractional number of milliseconds per timer0 overflow. we shift right
+// by three to fit these numbers into a byte. (for the clock speeds we care
+// about - 8 and 16 MHz - this doesn't lose precision.)
+#define FRACT_INC ((MICROSECONDS_PER_TIMER0_OVERFLOW % 1000) >> 3)
+#define FRACT_MAX (1000 >> 3)
+
 volatile unsigned long timer0_overflow_count = 0;
-volatile unsigned long timer0_clock_cycles = 0;
 volatile unsigned long timer0_millis = 0;
+static unsigned char timer0_fract = 0;
 
 SIGNAL(TIMER0_OVF_vect)
 {
-	timer0_overflow_count++;
-	// timer 0 prescale factor is 64 and the timer overflows at 256
-	timer0_clock_cycles += 64UL * 256UL;
-	while (timer0_clock_cycles > clockCyclesPerMicrosecond() * 1000UL) {
-		timer0_clock_cycles -= clockCyclesPerMicrosecond() * 1000UL;
-		timer0_millis++;
+	// copy these to local variables so they can be stored in registers
+	// (volatile variables must be read from memory on every access)
+	unsigned long m = timer0_millis;
+	unsigned char f = timer0_fract;
+
+	m += MILLIS_INC;
+	f += FRACT_INC;
+	if (f >= FRACT_MAX) {
+		f -= FRACT_MAX;
+		m += 1;
 	}
+
+	timer0_fract = f;
+	timer0_millis = m;
+	timer0_overflow_count++;
 }
 
 unsigned long millis()
 {
 	unsigned long m;
 	uint8_t oldSREG = SREG;
-	
+
 	// disable interrupts while we read timer0_millis or we might get an
-	// inconsistent value (e.g. in the middle of the timer0_millis++)
+	// inconsistent value (e.g. in the middle of a write to timer0_millis)
 	cli();
 	m = timer0_millis;
 	SREG = oldSREG;
-	
+
 	return m;
 }