From 7e859169fe249832ddf04a44470e3d4aed34e8df Mon Sep 17 00:00:00 2001
From: Lephe <sebastien.michelland@protonmail.com>
Date: Thu, 5 Jan 2023 20:25:44 +0100
Subject: [PATCH] cpg: add overclock save/restore functions

---
 include/gint/clock.h | 41 ++++++++++++++++++++--------------------
 src/cpg/overclock.c  | 45 ++++++++++++++++++++++++--------------------
 2 files changed, 45 insertions(+), 41 deletions(-)

diff --git a/include/gint/clock.h b/include/gint/clock.h
index dc5e5ea..acb5479 100644
--- a/include/gint/clock.h
+++ b/include/gint/clock.h
@@ -135,6 +135,26 @@ int clock_get_speed(void);
    leaving the add-in. */
 void clock_set_speed(int speed);
 
+/* If you want to faithfully save and restore the clock state while properly
+   handling clock speeds that are not Ftune/PTune's defaults, you can get a
+   full copy of the settings.
+
+   WARNING: Applying random settings with cpg_set_overclock_setting() might
+   damage your calculator! */
+
+struct cpg_overclock_setting
+{
+    uint32_t FLLFRQ, FRQCR;
+    uint32_t CS0BCR, CS2BCR, CS3BCR, CS5aBCR;
+    uint32_t CS0WCR, CS2WCR, CS3WCR, CS5aWCR;
+};
+
+/* Queries the clock setting from the hardware. */
+void cpg_get_overclock_setting(struct cpg_overclock_setting *s);
+
+/* Applies the specified overclock setting. */
+void cpg_set_overclock_setting(struct cpg_overclock_setting const *s);
+
 //---
 //	Sleep functions
 //---
@@ -153,27 +173,6 @@ void sleep_us_spin(uint64_t delay_us);
 /* sleep_ms(): Sleep for a fixed duration in milliseconds */
 #define sleep_ms(delay_ms) sleep_us((delay_ms) * 1000ull)
 
-//---
-// Low-level overclock functions
-//
-// These low-level functions directly read or write registers involved in
-// setting the overclock level. Don't use them directly unless you understand
-// how their interactions with the environment; instead, use clock_set_speed().
-//---
-
-struct cpg_overclock_setting
-{
-    uint32_t FLLFRQ, FRQCR;
-    uint32_t CS0BCR, CS2BCR, CS3BCR, CS5aBCR;
-    uint32_t CS0WCR, CS2WCR, CS3WCR, CS5aWCR;
-};
-
-/* Queries the clock setting from the hardware. */
-void cpg_get_overclock_setting(struct cpg_overclock_setting *s);
-
-/* Applies the specified overclock setting. */
-void cpg_set_overclock_setting(struct cpg_overclock_setting const *s);
-
 #ifdef __cplusplus
 }
 #endif
diff --git a/src/cpg/overclock.c b/src/cpg/overclock.c
index fa097f3..a9fa138 100644
--- a/src/cpg/overclock.c
+++ b/src/cpg/overclock.c
@@ -77,7 +77,7 @@ void cpg_get_overclock_setting(struct cpg_overclock_setting *s)
     }
 }
 
-void cpg_set_overclock_setting(struct cpg_overclock_setting const *s)
+static void cpg_low_level_set_setting(struct cpg_overclock_setting const *s)
 {
     if(isSH3()) {
         SH7705_WDT.WTCNT.WRITE = 0;
@@ -117,6 +117,30 @@ void cpg_set_overclock_setting(struct cpg_overclock_setting const *s)
     }
 }
 
+void cpg_set_overclock_setting(struct cpg_overclock_setting const *s)
+{
+    uint32_t old_Pphi = clock_freq()->Pphi_f;
+
+    /* Wait for asynchronous tasks to complete */
+    gint_world_sync();
+
+    /* Disable interrupts during the change */
+    cpu_atomic_start();
+
+    /* Load the clock settings */
+    cpg_low_level_set_setting(s);
+
+    /* Determine the change in frequency for Pϕ and recompute CPG data */
+    cpg_compute_freq();
+    uint32_t new_Pphi = clock_freq()->Pphi_f;
+
+    /* Update timers' TCNT and TCOR to match the new clock speed */
+    void timer_rescale(uint32_t old_Pphi, uint32_t new_Pphi);
+    timer_rescale(old_Pphi, new_Pphi);
+
+    cpu_atomic_end();
+}
+
 #ifdef FX9860G
 
 static struct cpg_overclock_setting const settings_fx9860g_sh3[5] = {
@@ -486,24 +510,5 @@ void clock_set_speed(int level)
         return;
 
     struct cpg_overclock_setting const *s = &settings[level - CLOCK_SPEED_F1];
-    uint32_t old_Pphi = clock_freq()->Pphi_f;
-
-    /* Wait for asynchronous tasks to complete */
-    gint_world_sync();
-
-    /* Disable interrupts during the change */
-    cpu_atomic_start();
-
-    /* Set the clock settings */
     cpg_set_overclock_setting(s);
-
-    /* Determine the change in frequency for Pϕ and recompute CPG data */
-    cpg_compute_freq();
-    uint32_t new_Pphi = clock_freq()->Pphi_f;
-
-    /* Update timers' TCNT and TCOR to match the new clock speed */
-    void timer_rescale(uint32_t old_Pphi, uint32_t new_Pphi);
-    timer_rescale(old_Pphi, new_Pphi);
-
-    cpu_atomic_end();
 }