From 45e3a7758046536754f073c42ac27dc737e69fab Mon Sep 17 00:00:00 2001
From: Selene ToyKeeper
Date: Tue, 26 Sep 2017 16:47:44 -0600
Subject: Added dynamic underclocking to FSM, instead of doing it manually in
 Anduril.

---
 spaghetti-monster/fsm-misc.c | 19 +++++++++++++++++++
 1 file changed, 19 insertions(+)

(limited to 'spaghetti-monster/fsm-misc.c')

diff --git a/spaghetti-monster/fsm-misc.c b/spaghetti-monster/fsm-misc.c
index 5d28002..1b8f864 100644
--- a/spaghetti-monster/fsm-misc.c
+++ b/spaghetti-monster/fsm-misc.c
@@ -20,6 +20,25 @@
 #ifndef FSM_MISC_C
 #define FSM_MISC_C
 
+#ifdef USE_DYNAMIC_UNDERCLOCKING
+void auto_clock_speed() {
+    uint8_t level = actual_level;  // volatile, avoid repeat access
+    if (level < QUARTERSPEED_LEVEL) {
+        // run at quarter speed
+        // note: this only works when executed as two consecutive instructions
+        // (don't try to combine them or put other stuff between)
+        CLKPR = 1<<CLKPCE; CLKPR = 2;
+    }
+    else if (level < HALFSPEED_LEVEL) {
+        // run at half speed
+        CLKPR = 1<<CLKPCE; CLKPR = 1;
+    } else {
+        // run at full speed
+        CLKPR = 1<<CLKPCE; CLKPR = 0;
+    }
+}
+#endif
+
 #if defined(USE_BLINK_NUM) || defined(USE_BLINK_DIGIT)
 uint8_t blink_digit(uint8_t num) {
     //StatePtr old_state = current_state;
-- 
cgit v1.2.3