From 7a5900be258f93c021535e1e8787ffeb12003308 Mon Sep 17 00:00:00 2001
From: Martin Preuss <martin@aquamaniac.de>
Date: Fri, 13 Sep 2024 21:41:38 +0200
Subject: [PATCH] avr: added functions to change speed.

Main code might work at 8 MHz, but boot code is compiled for 1 MHz,
so we need to set speed accordingly when rebooting into boot loader.
---
 avr/devices/n00/n00_main.asm        | 24 +++++++++++++++++++++++-
 avr/devices/n06/n06_main.asm        | 24 +++++++++++++++++++++++-
 avr/devices/n11/n11_main.asm        | 27 ++++++++++++++++++++++++++-
 avr/devices/n12/n12_main.asm        | 24 +++++++++++++++++++++++-
 avr/main.asm                        | 29 +++++++++++++++--------------
 avr/modules/comproto/msg_reboot.asm |  1 +
 6 files changed, 111 insertions(+), 18 deletions(-)

diff --git a/avr/devices/n00/n00_main.asm b/avr/devices/n00/n00_main.asm
index 1e55888..fb4c322 100644
--- a/avr/devices/n00/n00_main.asm
+++ b/avr/devices/n00/n00_main.asm
@@ -100,7 +100,7 @@
 	reti ; OC1A
 	reti ; OC1B
 	reti ; OVF1
-        rjmp timerIrqOC0A ; OC0A
+        rjmp baseTimerIrqOC0A ; OC0A
 	reti ; OC0B
 	reti ; OVF0
 	reti ; ACI
@@ -129,6 +129,8 @@ firmwareStart:   rjmp main
 .include "common/utils_copy_sdram.asm"
 .include "common/crc8.asm"
 
+.include "modules/basetimer/main.asm"
+
 #ifdef MODULES_TIMER
   .include "modules/timer/main.asm"
 #endif
@@ -244,6 +246,26 @@ timerList:
 .include "main.asm"
 
 
+systemSetSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, 0
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
+systemSetBootSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, (1<<CLKPS1) | (1<<CLKPS0)
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
 ; ---------------------------------------------------------------------------
 ; Called on first time run, i.e. on system start. No arguments, no results.
 
diff --git a/avr/devices/n06/n06_main.asm b/avr/devices/n06/n06_main.asm
index d0aedc6..eb8fec3 100644
--- a/avr/devices/n06/n06_main.asm
+++ b/avr/devices/n06/n06_main.asm
@@ -100,7 +100,7 @@
 	reti ; OC1A
 	reti ; OC1B
 	reti ; OVF1
-        rjmp timerIrqOC0A ; OC0A
+        rjmp baseTimerIrqOC0A ; OC0A
 	reti ; OC0B
 	reti ; OVF0
 	reti ; ACI
@@ -129,6 +129,8 @@ firmwareStart:   rjmp main
 .include "common/utils_copy_sdram.asm"
 .include "common/crc8.asm"
 
+.include "modules/basetimer/main.asm"
+
 #ifdef MODULES_TIMER
   .include "modules/timer/main.asm"
 #endif
@@ -244,6 +246,26 @@ timerList:
 .include "main.asm"
 
 
+systemSetSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, 0
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
+systemSetBootSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, (1<<CLKPS1) | (1<<CLKPS0)
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
 ; ---------------------------------------------------------------------------
 ; Called on first time run, i.e. on system start. No arguments, no results.
 
diff --git a/avr/devices/n11/n11_main.asm b/avr/devices/n11/n11_main.asm
index 39fa60c..ae43814 100644
--- a/avr/devices/n11/n11_main.asm
+++ b/avr/devices/n11/n11_main.asm
@@ -22,6 +22,7 @@
 
 
 .equ clock=1000000 ; Define the clock frequency
+;.equ clock=8000000 ; Define the clock frequency
 
 
 
@@ -104,7 +105,7 @@
 	reti ; OC1A
 	reti ; OC1B
 	reti ; OVF1
-        rjmp timerIrqOC0A ; OC0A
+        rjmp baseTimerIrqOC0A ; OC0A
 	reti ; OC0B
 	reti ; OVF0
 	reti ; ACI
@@ -133,6 +134,8 @@ firmwareStart:   rjmp main
 .include "common/utils_copy_sdram.asm"
 .include "common/crc8.asm"
 
+.include "modules/basetimer/main.asm"
+
 #ifdef MODULES_TIMER
   .include "modules/timer/main.asm"
 #endif
@@ -263,6 +266,28 @@ timerList:
 .include "main.asm"
 
 
+
+systemSetSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, 0
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
+systemSetBootSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, (1<<CLKPS1) | (1<<CLKPS0)
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
+
 ; ---------------------------------------------------------------------------
 ; Called on first time run, i.e. on system start. No arguments, no results.
 
diff --git a/avr/devices/n12/n12_main.asm b/avr/devices/n12/n12_main.asm
index d3b2f1c..1ca1e97 100644
--- a/avr/devices/n12/n12_main.asm
+++ b/avr/devices/n12/n12_main.asm
@@ -90,7 +90,7 @@
 	reti ; OC1A
 	reti ; OC1B
 	reti ; OVF1
-        rjmp timerIrqOC0A ; OC0A
+        rjmp baseTimerIrqOC0A ; OC0A
 	reti ; OC0B
 	reti ; OVF0
 	reti ; ACI
@@ -119,6 +119,8 @@ firmwareStart:   rjmp main
 .include "common/utils_copy_sdram.asm"
 .include "common/crc8.asm"
 
+.include "modules/basetimer/main.asm"
+
 #ifdef MODULES_TIMER
   .include "modules/timer/main.asm"
 #endif
@@ -249,6 +251,26 @@ timerList:
 .include "main.asm"
 
 
+systemSetSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, 0
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
+systemSetBootSpeed:
+.if clock == 8000000
+  ldi r16, (1<<CLKPCE)
+  ldi r17, (1<<CLKPS1) | (1<<CLKPS0)
+  out CLKPR, r16
+  out CLKPR, r17
+.endif
+  ret
+
+
 ; ---------------------------------------------------------------------------
 ; Called on first time run, i.e. on system start. No arguments, no results.
 
diff --git a/avr/main.asm b/avr/main.asm
index 7c9fd9b..359d356 100644
--- a/avr/main.asm
+++ b/avr/main.asm
@@ -1,5 +1,5 @@
 ; ***************************************************************************
-; copyright   : (C) 2023 by Martin Preuss
+; copyright   : (C) 2024 by Martin Preuss
 ; email       : martin@libchipcard.de
 ;
 ; ***************************************************************************
@@ -21,6 +21,7 @@
 ;
 
 main:
+  cli
   ; setup stack
   .ifdef SPH               ; if SPH is defined
     ldi r16, High(RAMEND)
@@ -30,6 +31,8 @@ main:
   out SPL, r16             ; init LSB stack pointer
 
 ;  rcall watchdogOff        ; turn off watchdog timer (sometimes it stays on after reboot)
+
+  rcall systemSetSpeed
   
   rcall initModules
 
@@ -60,7 +63,7 @@ main:
 ;  sts twiMasterScanEnabled, r16
 
 main_loop:
-    rcall runModulesUntilIdle
+    rcall runModules
 
     sei ; make sure interrupts really are enabled
     ; only modify SE, SM1 and SM0
@@ -89,6 +92,7 @@ main_loop:
 
 initModules:
   rcall Utils_Init
+  rcall BaseTimer_Init ; unconditionally call this
 
 #ifdef MODULES_TIMER
   rcall Timer_Init
@@ -150,7 +154,7 @@ initModules:
 
 
 ; ---------------------------------------------------------------------------
-; runModulesUntilIdle
+; runModules
 ;
 ; Call run functions of the used modules. Add your routine calls here.
 ;
@@ -160,24 +164,20 @@ initModules:
 ; - nothing
 ; USED: depending on called routines
 
-runModulesUntilIdle:
-
-#ifdef MODULES_TIMER
-  ; TIMER module
-  rcall Timer_Run
-#endif
-
+runModules:
+  rcall BaseTimer_Run
+  
 #ifdef MODULES_COM
 ; COM module (call until carry flag cleared but at most 10 times to not starve other modules)
   ldi r16, 10
-runModulesUntilIdle_Com:
+runModules_Com:
     push r16
       rcall Com2_Run
     pop r16
-    brcc runModulesUntilIdle_ComEnd
+    brcc runModules_ComEnd
     dec r16
-  brne runModulesUntilIdle_Com
-runModulesUntilIdle_ComEnd:  
+  brne runModules_Com
+runModules_ComEnd:  
   
 #endif
 
@@ -238,6 +238,7 @@ initialWait_l2: ; wait for 10ms
 
 
 
+
 #ifdef MODULES_LCD
 
 printTimerMark:
diff --git a/avr/modules/comproto/msg_reboot.asm b/avr/modules/comproto/msg_reboot.asm
index 0eaa5e4..2efd539 100644
--- a/avr/modules/comproto/msg_reboot.asm
+++ b/avr/modules/comproto/msg_reboot.asm
@@ -46,6 +46,7 @@ cproHandleReboot_loop1:
 
   ; directly call bootloader
   cli
+  rcall systemSetBootSpeed    ; reset speed if necessary (TODO: let bootloader set its speed)
   rjmp BOOTLOADER_ADDR
 cproHandleReboot_notHandled:
   clc