4 files changed, 105 insertions, 5 deletions
diff --git a/sw/demo1/.gdbinit b/sw/demo1/.gdbinit
new file mode 100644
index 0000000..08b6398
--- /dev/null
+++ b/sw/demo1/.gdbinit
@@ -0,0 +1,18 @@
+target remote :3333
+
+monitor arm semihosting enable
+
+# # send captured ITM to the file itm.fifo
+# # (the microcontroller SWO pin must be connected to the programmer SWO pin)
+# # 8000000 must match the core clock frequency
+# monitor tpiu config internal itm.fifo uart off 8000000
+
+# # OR: make the microcontroller SWO pin output compatible with UART (8N1)
+# # 2000000 is the frequency of the SWO pin
+# monitor tpiu config external uart off 8000000 2000000
+
+# # enable ITM port 0
+# monitor itm port 0 on
+
+#load
+#step
diff --git a/sw/demo1/Cargo.lock b/sw/demo1/Cargo.lock
index f3bfa79..89d8829 100644
--- a/sw/demo1/Cargo.lock
+++ b/sw/demo1/Cargo.lock
@@ -238,7 +238,6 @@ dependencies = [
 [[package]]
 name = "si5351"
 version = "0.2.0"
-source = "git+https://github.com/ilya-epifanov/si5351?rev=e509c36#e509c36df9b7521ffb253079e3226d6b870f0b4d"
 dependencies = [
  "bitflags",
  "embedded-hal",
diff --git a/sw/demo1/Cargo.toml b/sw/demo1/Cargo.toml
index cf33da1..28c9e67 100644
--- a/sw/demo1/Cargo.toml
+++ b/sw/demo1/Cargo.toml
@@ -19,13 +19,15 @@ stm32f1xx-hal = { version = "0.5", features = ["rt", "stm32f103"] }
 embedded-hal = { version = "0.2", features = [] }
 shared-bus = { version = "0.1.4", features = ["cortexm"] }
 hd44780-driver = { path = "../deps/hd44780-driver"}
-si5351 = { git = "https://github.com/ilya-epifanov/si5351", rev = "e509c36" }
+si5351 = { path = "../deps/si5351" }
 
 [profile.dev]
 codegen-units = 1
 incremental = false
+opt-level = 'z'
 
 [profile.release]
 codegen-units = 1
 debug = true
 lto = true
+opt-level = 'z'
diff --git a/sw/demo1/src/main.rs b/sw/demo1/src/main.rs
index 4f8ce6e..0b9f965 100644
--- a/sw/demo1/src/main.rs
+++ b/sw/demo1/src/main.rs
@@ -1,6 +1,8 @@
 #![no_main]
 #![no_std]
 
+use core::convert::TryInto;
+
 use cortex_m_rt::ExceptionFrame;
 use cortex_m_semihosting::hio;
 use panic_semihosting as _;
@@ -18,6 +20,45 @@ use si5351::{Si5351, Si5351Device};
 
 use core::fmt::Write;
 
+fn gcd(x: u32, y: u32) -> u32 {
+    let mut x = x;
+    let mut y = y;
+    while y != 0 {
+        let t = y;
+        y = x % y;
+        x = t;
+    }
+    x
+}
+
+fn clock_settings_for_pll(freq: u32, pll: u32) -> (u16, u32, u32) {
+    let a = pll / freq;
+    let b = pll - (a * freq);
+    let gcd = gcd(b, freq);
+
+    let b = b / gcd;
+    let c = freq / gcd;
+    (a.try_into().unwrap(), b, c)
+}
+
+fn clock_settings_with_pll_calculation(freq: u32, ref_clock: u32) -> (u16, u8, u32, u32) {
+    let mut divider : u32 = 900_000_000 / freq; // Calculate the division ratio. 900,000,000 is the maximum internal
+
+    if (divider % 2) == 1 {
+        divider -= 1 // Ensure an even integer division ratio
+    }
+
+    let pll_freq = divider * freq;
+    // mult is an integer that must be in the range 15..90
+    let mult = pll_freq / ref_clock;
+    let l = pll_freq % ref_clock;
+
+    let denom = 1048575;
+    let num = f64::from(l) * f64::from(denom) / f64::from(ref_clock);
+
+    (divider.try_into().unwrap(), mult.try_into().unwrap(), num as u32, denom)
+}
+
 fn print(step: usize) -> Result<(), core::fmt::Error> {
     let mut stdout = match hio::hstdout() {
         Ok(fd) => fd,
@@ -112,9 +153,49 @@ fn main() -> ! {
     let ref_clock = 25_000_000;
     let mut siclock = Si5351Device::new(i2c_busmanager.acquire(), false, ref_clock);
     siclock.init(si5351::CrystalLoad::_10).unwrap();
-    siclock.set_frequency(si5351::PLL::B, si5351::ClockOutput::Clk0, 28_000_000 + 4_915_200).unwrap();
-    siclock.set_frequency(si5351::PLL::A, si5351::ClockOutput::Clk1, 144_000_000 + 28_000_000).unwrap();
-    siclock.set_frequency(si5351::PLL::A, si5351::ClockOutput::Clk2, 4_915_100).unwrap();
+
+    // See freqplan.py for Si5351 frequency plan
+    // CLK1 = 116MHz
+    let pll_a_mult = 32;
+    siclock.setup_pll_int(si5351::PLL::A, 32).unwrap();
+
+    {
+        let clk1 = 116_000_000;
+        let (a, b, c) = clock_settings_for_pll(clk1, pll_a_mult * ref_clock);
+        siclock.setup_multisynth(si5351::Multisynth::MS1, a, b, c, si5351::OutputDivider::Div1).unwrap();
+        siclock.select_clock_pll(si5351::ClockOutput::Clk1, si5351::PLL::A);
+        siclock.set_clock_enabled(si5351::ClockOutput::Clk1, true);
+        siclock.flush_clock_control(si5351::ClockOutput::Clk1).unwrap();
+    }
+
+    {
+        let clk2 = 4_195_210;
+        let (a, b, c) = clock_settings_for_pll(clk2, pll_a_mult * ref_clock);
+        siclock.setup_multisynth(si5351::Multisynth::MS2, a, b, c, si5351::OutputDivider::Div1).unwrap();
+        siclock.select_clock_pll(si5351::ClockOutput::Clk2, si5351::PLL::A);
+        siclock.set_clock_enabled(si5351::ClockOutput::Clk2, true);
+        siclock.flush_clock_control(si5351::ClockOutput::Clk2).unwrap();
+    }
+
+    siclock.reset_pll(si5351::PLL::A).unwrap();
+
+    {
+        let clk0 = 23_000_000;
+
+        let (div, mult, num, denom) = clock_settings_with_pll_calculation(clk0, ref_clock);
+
+        siclock.setup_pll(si5351::PLL::B, mult, num, denom).unwrap();
+        siclock.setup_multisynth_int(si5351::Multisynth::MS0, div, si5351::OutputDivider::Div1).unwrap();
+
+        siclock.select_clock_pll(si5351::ClockOutput::Clk0, si5351::PLL::B);
+        siclock.set_clock_enabled(si5351::ClockOutput::Clk0, true);
+        siclock.flush_clock_control(si5351::ClockOutput::Clk0).unwrap();
+    }
+
+    siclock.reset_pll(si5351::PLL::B).unwrap();
+
+    siclock.flush_output_enabled().unwrap();
+
 
     lcd.set_cursor_pos(0, &mut delay).unwrap();
     lcd.write_str("Clocks set.     ", &mut delay).unwrap();