From a4aa45d80c1b0787b18b56abf133bccf3b015c45 Mon Sep 17 00:00:00 2001
From: Lucas Schumacher <technolucas@protonmail.com>
Date: Tue, 26 Sep 2023 12:06:22 -0400
Subject: [PATCH] added generic clock control

---
 src/main.rs | 111 +++++++++++++++++++++++++++++++++-------------------
 1 file changed, 70 insertions(+), 41 deletions(-)

diff --git a/src/main.rs b/src/main.rs
index a4cae35..35aa487 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -7,8 +7,8 @@ use esp_backtrace as _;
 //use esp_println::println;
 
 use embedded_hal::serial::Read;
-use hal::{IO, uart, Uart, /*Delay*/};
-use hal::{clock::ClockControl, peripherals::Peripherals, prelude::*, i2c::I2C};
+use hal::{clock::ClockControl, i2c::I2C, peripherals::Peripherals, prelude::*};
+use hal::{uart, Uart /*Delay*/, IO};
 
 use si5351;
 use si5351::{Si5351, Si5351Device};
@@ -34,12 +34,7 @@ fn main() -> ! {
     let clocks = ClockControl::max(system.clock_control).freeze();
     //let mut delay = Delay::new(&clocks);
 
-    let mut serial = {
-        uart::Uart::new(
-            peripherals.UART0,
-            &mut system.peripheral_clock_control,
-        )
-    };
+    let serial = { uart::Uart::new(peripherals.UART0, &mut system.peripheral_clock_control) };
 
     // setup logger
     // To change the log_level change the env section in .cargo/config.toml
@@ -63,39 +58,73 @@ fn main() -> ! {
 
     let mut clock = Si5351Device::new_adafruit_module(i2c);
     match clock.init_adafruit_module() {
-        Ok(_) => log::info!("Si5351 Initialized succesfully"),
-        Err(si5351::Error::CommunicationError) => log::info!("Could not initialize Si5351: CommunicationError"),
-        Err(si5351::Error::InvalidParameter) => log::info!("Could not initialize Si5351: InvalidParameter"),
-    }
-
-    // clock.set_frequency(si5351::PLL::A, si5351::ClockOutput::Clk0, 14_175_000).unwrap();
-
-    loop {
-        //delay.delay_ms(5000u32);
-        let freq = read_freq(&mut serial);
-        log::info!("Got Freq: {}", freq);
+        Ok(_) => {
+            log::info!("Si5351 Initialized succesfully");
+            main_loop(clock, serial);
+        }
+        Err(si5351::Error::CommunicationError) => {
+            log::info!("Could not initialize Si5351: CommunicationError");
+            main_loop(Dummy {}, serial);
+        }
+        Err(si5351::Error::InvalidParameter) => {
+            log::info!("Could not initialize Si5351: InvalidParameter");
+            main_loop(Dummy {}, serial);
+        }
     }
 }
 
-// bool inval = false;
-// char c = serial.read();
-// for (;;) {
-    // int freq = 0;
-    // for (;(c >= '0' && c <= '9') || c == '.' || c == ',' || c == '_'; c = serial.read()) 
-        // if (c >= '0' && c <= '9') 
-            // freq = (freq * 10) + (c - '0');
+fn main_loop<T: Clockable, U: _esp_hal_uart_Instance>(mut clock: T, mut serial: Uart<'_, U>) -> ! {
+    loop {
+        let freq = read_freq(&mut serial);
+        log::info!("Got Freq: {}", freq);
+        match clock.set_freq(freq) {
+            Ok(_) => log::info!("Set frequency to {}", freq),
+            Err(_) => log::warn!("Could not set frequency to {}", freq),
+        }
+    }
+}
 
-    // if (c == 'G' || c=='M' || c=='K') freq = freq * mult(c) 
-    // c = serial.read()
+trait Clockable {
+    fn set_freq(&mut self, freq: u32) -> Result<(), ()>;
+}
+impl<T> Clockable for T
+where
+    T: Si5351,
+{
+    fn set_freq(&mut self, freq: u32) -> Result<(), ()> {
+        match self.set_frequency(si5351::PLL::A, si5351::ClockOutput::Clk0, freq) {
+            Ok(_) => Ok(()),
+            Err(_) => Err(()),
+        }
+    }
+}
+struct Dummy;
+impl Clockable for Dummy {
+    fn set_freq(&mut self, freq: u32) -> Result<(), ()> {
+        log::info!("Dummy set freq: {}", freq);
+        Ok(())
+    }
+}
 
-    // for ( ; c != '\n' or '\r'; c = serial.read()) if (!inval && c != ' ' or '\t') inval = true;
+//  bool inval = false;
+//  char c = serial.read();
+//  for (;;) {
+//      int freq = 0;
+//      for (;(c >= '0' && c <= '9') || c == '.' || c == ',' || c == '_'; c = serial.read())
+//          if (c >= '0' && c <= '9') freq = (freq * 10) + (c - '0');
 
-    // if !inval && freq > 0 {break;}
-// }
-// return freq;
+//      if (c == 'G' || c=='M' || c=='K') freq = freq * mult(c)
+//      c = serial.read()
+
+//      for ( ; c != '\n' or '\r'; c = serial.read())
+//          if (!inval && c != ' ' or '\t') inval = true;
+
+//      if !inval && freq > 0 {break;}
+//  }
+//  return freq;
 
 // fn rf<T: embedded_hal::serial::Read<u8>>(serial: &mut T) -> u32 { 0 }
-fn read_freq<T: _esp_hal_uart_Instance>(serial: &mut Uart<'_,T>) -> u32 {
+fn read_freq<T: _esp_hal_uart_Instance>(serial: &mut Uart<'_, T>) -> u32 {
     let mut freq = 0;
     let mut invalid = false;
     loop {
@@ -103,29 +132,29 @@ fn read_freq<T: _esp_hal_uart_Instance>(serial: &mut Uart<'_,T>) -> u32 {
             Ok(input) if input >= b'0' && input <= b'9' => {
                 log::info!("Got char: {}", char::from(input));
                 freq = (freq * 10) + (input - b'0') as u32;
-            },
-            Ok(input) if  invalid && (input == b'\n' || input == b'\r') => {
+            }
+            Ok(input) if invalid && (input == b'\n' || input == b'\r') => {
                 log::info!("Got bad end: {}", char::from(input));
                 freq = 0;
                 invalid = false;
-            },
+            }
             Ok(input) if !invalid && (input == b'\n' || input == b'\r') => {
                 log::info!("Got end: {}", char::from(input));
                 break;
-            },
+            }
             Ok(input) if input == b'.' || input == b',' || input == b'_' => {
                 log::info!("Got ignor: {}", char::from(input));
                 continue;
-            },
+            }
             Ok(input) => {
                 log::info!("Got inval: {}", char::from(input));
                 invalid = true;
-            },
+            }
             Err(e) => {
                 log::info!("Got error: {:?}", e);
-            },
+            }
         }
     }
 
     freq
-}
\ No newline at end of file
+}