avr: try calculating idle times.

avr/timer.asm

@@ -16,6 +16,8 @@
 
 .equ TIMER_FLAGS_IF_ADDR = 1
 
+.equ TIMER_STATES_MASK   = 7
+
 
 
 ; ***************************************************************************
@@ -27,7 +29,10 @@ timerModuleData:
 timerModuleTickCounter:      .byte 1
 timerTicksSinceLastRun:      .byte 2
 timerModuleCounterSecs:      .byte 4
-
+timerInterrupts:             .byte 2
+timerStateBeforeSleep:       .byte 1
+timerStatePos:               .byte 1
+timerStates:                 .byte TIMER_STATES_MASK+1
 timerModuleData_end:
 
 
@@ -163,6 +168,7 @@ timerInitTimers_loop:
     st X+, r20
     st X, r21
     inc r16
+    inc r16
   rjmp timerInitTimers_loop
 
 timerInitTimers_end:
@@ -242,6 +248,68 @@ timerCallR19R18:
 
 
 
+Timer_AfterSleep:
+  in r16, TCNT0
+  sts timerStateBeforeSleep, r16
+  ret
+
+
+
+Timer_BeforeSleep:
+  in r16, TCNT0   ; read counter value
+  lds r17, timerStateBeforeSleep
+  sub r16, r17
+  brcc Timer_BeforeSleep_l1
+  ldi r17, 98-1
+  add r16, r17
+Timer_BeforeSleep_l1:
+  ldi xl, LOW(timerStates)
+  ldi xh, HIGH(timerStates)
+  lds r18, timerStatePos
+  clr r17
+  add xl, r18
+  adc xh, r17
+  st X, r16
+  inc r18
+  andi r18, TIMER_STATES_MASK
+  sts timerStatePos, r18
+  ret
+
+
+
+; ---------------------------------------------------------------------------
+; Calculate aproximate idle percentage
+;
+; IN:
+; OUT:
+; - R16: approximate idle percentage (between 0 and 97)
+; REGS: r16, r17, r18, r24, r25, X
+
+Timer_CalcIdle:
+  ldi xl, LOW(timerStates)
+  ldi xh, HIGH(timerStates)
+  ldi r18, TIMER_STATES_MASK+1
+  clr r24
+  clr r25
+  clr r17
+Timer_CalcIdle_loop:
+    ld r16, X+
+    add r24, r16
+    adc r25, r17
+    dec r18
+  brne Timer_CalcIdle_loop
+  lsr r25
+  ror r24
+  lsr r25
+  ror r24
+  lsr r25
+  ror r24
+  mov r16, r24
+  ret
+
+
+
+
 ; ---------------------------------------------------------------------------
 ; OC0A interrupt handler
 ;
@@ -259,6 +327,13 @@ timerIrqOC0A:
         adiw r25:r24, 1
         sts timerTicksSinceLastRun, r24
         sts timerTicksSinceLastRun+1, r25
+        
+        lds r24, timerInterrupts
+        lds r25, timerInterrupts+1
+        adiw r25:r24, 1
+        sts timerInterrupts, r24
+        sts timerInterrupts+1, r25
+
       pop r25
     pop r24
     out SREG, r15