--- a/timer.inc	Fri May 14 18:19:22 2010 +0300
+++ b/timer.inc	Fri May 14 18:19:59 2010 +0300
@@ -139,6 +139,14 @@
 		out			SREG, r0
 		reti
 
+;; Prime the timer and sleep for 1s
+Timer_Sleep_1s:
+	; Initialize counter to 16k cycles
+		ldi			XH, high(16 * 1024)
+		ldi			XL, low(16 * 1024)
+
+	;; Continue
+
 ;; Count to X
 Timer_Sleep:
 	; Set TOP
@@ -158,12 +166,31 @@
 	; Done
 		ret
 
-;; Prime the timer and sleep for 1s
-Timer_Sleep_1s:
-	; Initialize counter to 16k cycles
-		ldi			XH, high(16 * 1024)
-		ldi			XL, low(16 * 1024)
+;; Update timer for given timeout
+Timer_Update:
+	; Set TOP
+		sts			OCR1AH, XH
+		sts			OCR1AL, XL
+	
+	; Check timer
+		lds			YL, TCNT1L
+		lds			YH, TCNT1H
+		
+		cp			YL, XL
+		cpc			YH, XH
 
-	; Start timer
-		rjmp		Timer_Sleep
+		brlo		timer_up_out
 
+	; Update
+	; XXX: figure out a better way to do this...
+		ldi			r16, 0
+		subi		XL, 2
+		sbc			XH, r16
+
+		sts			TCNT1L, XL
+		sts			TCNT1H, XH
+
+timer_up_out:
+	; Done
+		ret
+