System clock is sort of working.

It appears the frequency divider does not work. I've followed the data
sheet, but no matter what I set the frequency divider to it appears to
not work. It's possible maybe the GPIO is using an un-divided clock, but
I'm not sure.

Also the 32khz clock does not work I think. It might be an issue with
the board. The waveform is jagged and looks awful.

But I can switch from the HSE to the PLL.

4 files changed, 473 insertions, 4 deletions

diff --git a/src/clock.c b/src/clock.c
new file mode 100644
index 0000000..8bf152f
--- /dev/null
+++ b/src/clock.c
@@ -0,0 +1,321 @@
+
+#include "clock.h"
+#include <stdio.h>
+
+#include "system.h"
+#include "ch573/clock.h"
+
+#define SAFE_MODE_CLOCK enter_safe_mode(); \
+  CH573_CLOCK__CLOCK_CFG_T_INTF
+#define CLOCK CH573_CLOCK__CLOCK_CFG_T_INTF
+#define CLOCK_CFG ch573_clock__clock_cfg
+
+/* Interface to a clock. */
+struct clock_intf {
+  int (*enable_clock)(const clock_cfg_t*);
+  int (*switch_clock)(const clock_cfg_t*);
+  int (*disable_other_clocks)(const clock_cfg_t*);
+};
+
+static inline int enable_pll_clock(const clock_cfg_t*);
+static inline int switch_to_pll(const clock_cfg_t*);
+static inline int disable_for_pll(const clock_cfg_t*);
+
+static inline int enable_lse_clock(const clock_cfg_t*);
+static inline int switch_to_lse(const clock_cfg_t*);
+static inline int disable_for_lse(const clock_cfg_t*);
+
+static inline int enable_lsi_clock(const clock_cfg_t*);
+static inline int switch_to_lsi(const clock_cfg_t*);
+static inline int disable_for_lsi(const clock_cfg_t*);
+
+static inline int enable_hse_clock(const clock_cfg_t*);
+static inline int switch_to_hse(const clock_cfg_t*);
+static inline int disable_for_hse(const clock_cfg_t*);
+
+#define pll_clock                              \
+  ((struct clock_intf){                        \
+      .enable_clock = enable_pll_clock,        \
+      .switch_clock = switch_to_pll,           \
+      .disable_other_clocks = disable_for_pll, \
+  })
+
+#define lse_clock                              \
+  ((struct clock_intf){                        \
+      .enable_clock = enable_lse_clock,        \
+      .switch_clock = switch_to_lse,           \
+      .disable_other_clocks = disable_for_lse, \
+  })
+
+#define lsi_clock                              \
+  ((struct clock_intf){                        \
+      .enable_clock = enable_lsi_clock,        \
+      .switch_clock = switch_to_lsi,           \
+      .disable_other_clocks = disable_for_lsi, \
+  })
+
+#define hse_clock                              \
+  ((struct clock_intf){                        \
+      .enable_clock = enable_hse_clock,        \
+      .switch_clock = switch_to_hse,           \
+      .disable_other_clocks = disable_for_hse, \
+  })
+
+static void fast_delay()
+{
+  for (int i = 0; i < 10; ++i) {
+    asm volatile("");
+  }
+}
+
+static inline int disable_pll(void)
+{
+  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_pll_pon.set(CLOCK_CFG, OFF);
+  return 0;
+}
+
+static inline int disable_hse(void)
+{
+  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_xt32m_pon.set(CLOCK_CFG, OFF);
+  return 0;
+}
+
+static inline int disable_lse(void)
+{
+  SAFE_MODE_CLOCK.ck32k_config.clk_xt32k_pon.set(CLOCK_CFG, DISABLED);
+  return 0;
+}
+
+static inline int disable_lsi(void)
+{
+  SAFE_MODE_CLOCK.ck32k_config.clk_int32k_pon.set(CLOCK_CFG, DISABLED);
+  return 0;
+}
+
+static inline int enable_pll(void)
+{
+  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_pll_pon.set(CLOCK_CFG, ON);
+  return 0;
+}
+
+static inline int enable_hse(void)
+{
+  SAFE_MODE_CLOCK.hfck_pwr_ctrl.clk_xt32m_pon.set(CLOCK_CFG, ON);
+  return 0;
+}
+
+static inline int enable_lse(void)
+{
+  SAFE_MODE_CLOCK.ck32k_config.clk_xt32k_pon.set(CLOCK_CFG, ENABLED);
+  fast_delay();
+  SAFE_MODE_CLOCK.xt32k_tune.xt32k_c_load.set(CLOCK_CFG, 0xf);
+  fast_delay();
+  return 0;
+}
+
+static inline int enable_lsi(void)
+{
+  SAFE_MODE_CLOCK.ck32k_config.clk_int32k_pon.set(CLOCK_CFG, ENABLED);
+  return 0;
+}
+
+static inline int enable_pll_clock(const clock_cfg_t* cfg)
+{
+  printf("Turning on PLL\n");
+  int ec = hse_clock.enable_clock(cfg);  // The PLL uses the HSE.
+  if (ec) {
+    return ec;
+  };
+  printf("Do enable.\n");
+  enable_pll();
+  printf("Fast delay.\n");
+  fast_delay();  // Delay for a few microseconds to wait for the PLL to stablize
+  printf("Done\n");
+  return 0;
+}
+
+static inline int enable_lse_clock(const clock_cfg_t* cfg)
+{
+  enable_lse();
+  fast_delay();
+  return 0;
+}
+
+static inline int enable_lsi_clock(const clock_cfg_t* cfg)
+{
+  enable_lsi();
+  fast_delay();
+  return 0;
+}
+
+static inline int enable_hse_clock(const clock_cfg_t* cfg)
+{
+  uint8_t divisor = cfg->hse_clock_divisor;
+  // Divisor should only be 5 bits in size with a minimum value of 2.
+  if (divisor > 0x1f || divisor == 1) {
+    return 1;
+  }
+
+  // divisor = 0 => use the default value.
+  if (divisor == 0) {
+    divisor = 5;  // Default value
+  }
+
+  enable_hse();
+  printf("Setting Divisor: %d\n", divisor);
+  SAFE_MODE_CLOCK.clk_sys_cfg.clk_pll_div.set(CLOCK_CFG, divisor);
+  fast_delay();
+  return 0;
+}
+
+static inline int switch_to_pll(const clock_cfg_t* cfg)
+{
+  printf("Switch to PLL.\n");
+  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_PLL);
+  return 0;
+}
+
+static inline int switch_to_lse(const clock_cfg_t* cfg)
+{
+  fast_delay();
+  SAFE_MODE_CLOCK.ck32k_config.clk_osc32k_xt.set(CLOCK_CFG, EXTERNAL_32K_OSC);
+  fast_delay();
+  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_CK32K);
+  return 0;
+}
+
+static inline int switch_to_lsi(const clock_cfg_t* cfg)
+{
+  SAFE_MODE_CLOCK.ck32k_config.clk_osc32k_xt.set(CLOCK_CFG, INTERNAL_32K_OSC);
+  fast_delay();
+  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_CK32K);
+  fast_delay();
+  return 0;
+}
+
+static inline int switch_to_hse(const clock_cfg_t* cfg)
+{
+  SAFE_MODE_CLOCK.clk_sys_cfg.clk_sys_mod.set(CLOCK_CFG, HCLK_SRC_CK32M);
+  return 0;
+}
+
+static inline int disable_for_pll(const clock_cfg_t* cfg)
+{
+  printf("Disable other clocks.\n");
+  // disable_lsi();
+  // disable_lse();
+  // Need to keep HSE on
+  return 0;
+}
+
+static inline int disable_for_lse(const clock_cfg_t* cfg)
+{
+  // disable_lsi();
+  disable_pll();
+  disable_hse();
+  return 0;
+}
+
+static inline int disable_for_lsi(const clock_cfg_t* cfg)
+{
+  // disable_lse();
+  disable_pll();
+  disable_hse();
+  return 0;
+}
+
+static inline int disable_for_hse(const clock_cfg_t* cfg)
+{
+  // disable_lse();
+  // disable_lsi();
+  disable_pll();
+  return 0;
+}
+
+int get_system_clock(clock_cfg_t* out)
+{
+  int ck32k = CLOCK.ck32k_config.clk_osc32k_xt.get(CLOCK_CFG);
+  int ckSel = CLOCK.clk_sys_cfg.clk_sys_mod.get(CLOCK_CFG);
+
+  if (ckSel == HCLK_SRC_PLL) {
+    out->sel = CLOCK_SELECTION_PLL;
+  } else if (ckSel == HCLK_SRC_CK32M) {
+    out->sel = CLOCK_SELECTION_HSE;
+  } else if (ckSel == HCLK_SRC_CK32K) {
+    if (ck32k == INTERNAL_32K_OSC) {
+      out->sel = CLOCK_SELECTION_LSI;
+    } else if (ck32k == EXTERNAL_32K_OSC) {
+      out->sel = CLOCK_SELECTION_LSE;
+    } else {
+      return 1;
+    }
+  } else {
+    return 1;
+  }
+
+  out->hse_clock_divisor = 0;
+  if (out->sel == CLOCK_SELECTION_HSE || out->sel == CLOCK_SELECTION_PLL) {
+    out->hse_clock_divisor = CLOCK.clk_sys_cfg.clk_pll_div.get(CLOCK_CFG);
+  }
+
+  return 0;
+}
+
+uint32_t get_clock_freq(const clock_cfg_t* cfg)
+{
+  uint32_t freq = 0;
+
+  switch (cfg->sel) {
+    case CLOCK_SELECTION_PLL:
+      freq = 480000000 / cfg->pll_clock_divisor;
+      break;
+    case CLOCK_SELECTION_HSE:
+      freq = 32000000 / cfg->hse_clock_divisor;
+      break;
+    case CLOCK_SELECTION_LSE:
+    case CLOCK_SELECTION_LSI:
+      freq = 32000;
+      break;
+  }
+
+  return freq;
+}
+
+extern clock_chg_evt_listener_t CLOCK_CHANGE_LISTENERS_START;
+extern clock_chg_evt_listener_t CLOCK_CHANGE_LISTENERS_END;
+
+static void fire_clock_change_listeners(const clock_cfg_t* clk_cfg)
+{
+  clock_chg_evt_listener_t* cur = &CLOCK_CHANGE_LISTENERS_START;
+  while (cur < &CLOCK_CHANGE_LISTENERS_END) {
+    cur->on_clk_change(clk_cfg);
+    ++ cur;
+  }
+}
+
+int set_system_clock(const clock_cfg_t* clk_cfg)
+{
+#define clocks \
+  ((struct clock_intf[]){pll_clock, lse_clock, lsi_clock, hse_clock})
+  int ec = clocks[clk_cfg->sel].enable_clock(clk_cfg);
+
+  if (ec) {
+    return ec;
+  }
+
+  ec = clocks[clk_cfg->sel].switch_clock(clk_cfg);
+
+  if (ec) {
+    return ec;
+  }
+
+  ec = clocks[clk_cfg->sel].disable_other_clocks(clk_cfg);
+
+  if (ec) {
+    return ec;
+  }
+
+  fire_clock_change_listeners(clk_cfg);
+#undef clocks
+  return 0;
+}
diff --git a/src/init.c b/src/init.c
index c402f17..5a0a7c3 100644
--- a/src/init.c
+++ b/src/init.c
@@ -1,6 +1,7 @@
 #include <stddef.h>
 #include <stdint.h>
 
+#include "clock.h"
 #include "io.h"
 #include "isr_vector.h"
 
diff --git a/src/io.c b/src/io.c
index 0da33d1..6233e86 100644
--- a/src/io.c
+++ b/src/io.c
@@ -1,10 +1,11 @@
+#include "io.h"
+
 #include <stdint.h>
 #include <stdio.h>
 
 #include "ch573/gpio.h"
 #include "ch573/uart.h"
-
-#include "io.h"
+#include "clock.h"
 
 #ifndef DEBUG_UART
 #define DEBUG_UART UART1
@@ -26,6 +27,7 @@ static int uart1_FILE_put(char ch, FILE* unused)
 
   while (!UART.lsr.thr_empty.get(UART1));
   UART.thr.set(UART1, ch);
+  while (!UART.lsr.thr_empty.get(UART1));
 
   return 1;
 }
@@ -49,6 +51,8 @@ FILE _uart1_FILE = (FILE){
 
 FILE* const stdout = &_uart1_FILE;
 
+static int uart1_init_for_stdout = 0;
+
 void init_uart1_for_stdout(void)
 {
   GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, gpio_usart1_tx_pin);
@@ -61,4 +65,22 @@ void init_uart1_for_stdout(void)
 
   volatile uint32_t dl = (10 * 6400000 / 8 / BAUD_RATE + 5) / 10;
   UART.dl.set(UART1, dl);
+  uart1_init_for_stdout = 1;
+}
+
+/* When the system clock changes, we need to update the UART baud rate. */
+on_system_clock_changed(uart_clock_change)(const clock_cfg_t* cfg)
+{
+  if (uart1_init_for_stdout) {
+    clock_cfg_t c;
+
+    if (get_system_clock(&c)) {
+      return;
+    }
+
+    uint32_t freq = get_clock_freq(&c);
+
+    uint32_t dl = (10 * freq / 8 / BAUD_RATE + 5) / 10;
+    UART.dl.set(UART1, dl);
+  }
 }
diff --git a/src/main.c b/src/main.c
index 4cd54de..f06a775 100644
--- a/src/main.c
+++ b/src/main.c
@@ -4,6 +4,8 @@
 #include "ch573/gpio.h"
 #include "ch573/pwr.h"
 #include "ch573/uart.h"
+#include "clock.h"
+#include "system.h"
 
 #define GPIO_PORT_A ch573_gpio__gpio_port_a
 #define GPIO_PORT CH573_GPIO__GPIO_PORT_T_INTF
@@ -30,6 +32,93 @@ uint32_t collatz(uint32_t n)
   return c;
 }
 
+static void print_reg(uint32_t reg)
+{
+  // 0x40001008
+  printf("register @0x%08x = 0x%08x\n", reg, *(uint32_t*)reg);
+}
+
+static void set_system_clock_6Mhz(void)
+{
+  print_reg(0x40001008);
+  printf("Setting system clock to 6Mhz...\n");
+  clock_cfg_t clk_cfg = {0};
+  clk_cfg.sel = CLOCK_SELECTION_HSE;
+  clk_cfg.pll_clock_divisor = 5;
+  if (set_system_clock(&clk_cfg)) {
+    printf("Failed to set system clock.\n");
+  }
+  print_reg(0x40001008);
+  printf("Done\n");
+}
+
+static void set_system_clock_3Mhz(void)
+{
+  print_reg(0x40001008);
+  printf("Setting system clock to 3Mhz...\n");
+  clock_cfg_t clk_cfg = {0};
+  clk_cfg.sel = CLOCK_SELECTION_HSE;
+  clk_cfg.pll_clock_divisor = 10;
+  if (set_system_clock(&clk_cfg)) {
+    printf("Failed to set system clock.\n");
+  }
+  print_reg(0x40001008);
+  printf("Done\n");
+}
+
+static void set_system_clock_60Mhz(void)
+{
+  print_reg(0x40001008);
+  printf("Setting system clock to 60Mhz...\n");
+  clock_cfg_t clk_cfg = {0};
+  clk_cfg.sel = CLOCK_SELECTION_PLL;
+  clk_cfg.pll_clock_divisor = 8;
+  if (set_system_clock(&clk_cfg)) {
+    printf("Failed to set system clock.\n");
+  }
+  print_reg(0x40001008);
+  printf("Done\n");
+}
+
+static void set_system_clock_30Mhz(void)
+{
+  print_reg(0x40001008);
+  printf("Setting system clock to 30Mhz...\n");
+  clock_cfg_t clk_cfg = {0};
+  clk_cfg.sel = CLOCK_SELECTION_PLL;
+  clk_cfg.pll_clock_divisor = 16;
+  if (set_system_clock(&clk_cfg)) {
+    printf("Failed to set system clock.\n");
+  }
+  print_reg(0x40001008);
+  printf("Done\n");
+}
+
+static void set_system_clock_32kHz()
+{
+  print_reg(0x40001008);
+  printf("Setting system clock to 32kHz...\n");
+  clock_cfg_t clk_cfg = {0};
+  clk_cfg.sel = CLOCK_SELECTION_LSE;
+  if (set_system_clock(&clk_cfg)) {
+    printf("Failed to set system clock.\n");
+  }
+}
+
+static void basic_delay()
+{
+  for (int i = 0; i < 100000; ++i) {
+    asm volatile("");
+  }
+}
+
+static void fast_delay()
+{
+  for (int i = 0; i < 1000; ++i) {
+    asm volatile("");
+  }
+}
+
 /*
  * Main routine. This is called on_reset once everything else has been set up.
  */
@@ -37,8 +126,44 @@ int main(void)
 {
   GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, 8);
   GPIO_PORT.pd_drv.set(GPIO_PORT_A, 0, 8);
+  GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
+
+  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_OUT, 11);
+  GPIO_PORT.pd_drv.set(GPIO_PORT_A, 0, 11);
+
+  GPIO_PORT.dir.set(GPIO_PORT_A, DIR_IN, 10);
+  GPIO_PORT.pd_drv.set(GPIO_PORT_A, PD_DRV_OPEN_DRAIN, 11);
+
+  set_system_clock_6Mhz();
+  uint32_t reg = (*(uint32_t*)0x40001008);
+  reg &= ~0x1f;
+  reg |= 10;
+  enter_safe_mode();
+  (*(uint32_t*)0x40001008) = reg;
+
+
+  for (int i = 0;; ++i) {
+    GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
+    GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
+  }
+
+  // clock_cfg_t cfg;
+  // for (int i = 0;; ++i) {
+  //   if (i % 16 == 0) {
+  //     if (cur_clock) {
+  //       set_system_clock_60Mhz();
+  //     } else {
+  //       set_system_clock_6Mhz();
+  //     }
+  //     get_system_clock(&cfg);
+  //     printf("Cur Clock Mhz: %d\n", get_clock_freq(&cfg));
+  //     cur_clock = !cur_clock;
+  //   }
 
-  printf("Hello, %s!\n", "World");
-  for (;;);
+  //   basic_delay();
+  //   GPIO_PORT.out.set(GPIO_PORT_A, ON, 8);
+  //   basic_delay();
+  //   GPIO_PORT.out.set(GPIO_PORT_A, OFF, 8);
+  // }
   return 0;
 }