From b11891e57f3a2df1256e27cbf0dcfd8aaa3df660 Mon Sep 17 00:00:00 2001
From: occheung <dc@m-labs.hk>
Date: Sun, 13 Sep 2020 00:58:58 +0800
Subject: [PATCH] scpi: refine clock source cmd

---
 examples/ethernet.rs |  2 +-
 src/scpi.rs          | 68 +++++++++++++++++++++++++++++++++-----------
 2 files changed, 52 insertions(+), 18 deletions(-)

diff --git a/examples/ethernet.rs b/examples/ethernet.rs
index 3a9854f..c48e32d 100644
--- a/examples/ethernet.rs
+++ b/examples/ethernet.rs
@@ -183,7 +183,7 @@ fn main() -> ! {
 	let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
     let gpiog = dp.GPIOG.split(ccdr.peripheral.GPIOG);
 
-    gpiob.pb3.into_alternate_af0().set_speed(Speed::VeryHigh);
+//    gpiob.pb3.into_alternate_af0().set_speed(Speed::VeryHigh);
 
     logger::init();
 
diff --git a/src/scpi.rs b/src/scpi.rs
index 5df6eb5..7d0ee61 100644
--- a/src/scpi.rs
+++ b/src/scpi.rs
@@ -28,7 +28,9 @@ use scpi::{
     scpi_status,
     scpi_system,
 };
-// use scpi::suffix::{Frequency};
+use scpi::suffix::{Amplitude, Db};
+use uom::si::frequency::{gigahertz, hertz, kilohertz, megahertz, Frequency};
+use uom::si::{f32, f64};
 
 use embedded_hal::blocking::spi::Transfer;
 
@@ -39,6 +41,8 @@ use crate::{
     ClockSource,
 };
 
+use log::{trace, debug, info, warn};
+
 #[macro_export]
 macro_rules! recursive_scpi_tree {
     // Handle optional headers (end-node)
@@ -232,33 +236,63 @@ impl<T:Device + UrukulTraits> Command<T> for ClockSourceCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        let data: &[u8] = match args.next_data(false)? {
+
+        let s: &[u8] = match args.next_data(false)? {
             Some(Token::CharacterProgramData(s)) => s,
             _ => return Err(ErrorCode::IllegalParameterValue.into()),
         };
-        // let max_value = f64::Frequency::new::<hertz>(1_000_000_000_000.0);
-        // let min_value = f64::Frequency::new::<hertz>(0.0);
+	/*
+        debug!("Converted data: {:?}", data);
 
-        // let freq: f64::Frequency = args.next_data(true)?
-        //                             .map_or(Ok(f64::Frequency::new::<hertz>(0.0)),
-        //                             |t| {
-        //     t.numeric(|s| match s {
-        //         NumericValues::Maximum => Ok(f64::Frequency::new::<hertz>(max_value)),
-        //         NumericValues::Minimum => Ok(f64::Frequency::new::<hertz>(min_value)),
-        //         _ => Err(ErrorCode::IllegalParameterValue.into()),
-        //     })
-        // })?;
         if let Ok(str_param) = str::from_utf8(data) {
             if let Ok(cmd) = match str_param {
                 "OSC" => context.device.set_clock_source(ClockSource::OSC),
                 "MMCX" => context.device.set_clock_source(ClockSource::MMCX),
                 "SMA" => context.device.set_clock_source(ClockSource::SMA),
-                _ => return Err(ErrorCode::IllegalParameterValue.into()),
+                _ => {
+                	return Err(ErrorCode::IllegalParameterValue.into());
+                }
             } {
-                return Ok(());
+                debug!("Finished parsing source");
             }
-        };
-        Err(ErrorCode::IllegalParameterValue.into())
+        } else {
+            return Err(ErrorCode::IllegalParameterValue.into());
+        }
+    	let s: &[u8] = args
+    				.next_data(false)?
+    				.unwrap_or(Token::CharacterProgramData(b"default"))
+    				.try_into()?;
+    */
+    	let s_str: &str = str::from_utf8(s)
+    						.map_err(|_| ErrorCode::CharacterDataError)?;
+		match s_str {
+			source if source.eq_ignore_ascii_case("OSC") => {
+				trace!("Changing clock source to OSC");
+				context.device.set_clock_source(ClockSource::OSC);
+			},
+			source if source.eq_ignore_ascii_case("MMCX") => {
+				trace!("Changing clock source to MMCX");
+				context.device.set_clock_source(ClockSource::MMCX);
+			},
+			source if source.eq_ignore_ascii_case("SMA") => {
+				trace!("Changing clocksource to SMA");
+				context.device.set_clock_source(ClockSource::SMA);
+			},
+			_ => {
+				warn!("Clock selection failed! Argument error!");
+				return Err(ErrorCode::IllegalParameterValue.into());
+			},
+		};
+
+		let frequency: f64::Frequency = args.next_data(true)?
+											.map_or(Ok(f64::Frequency::new::<hertz>(0.0)), |t| {
+												t.numeric(|s| match s {
+													NumericValues::Default => Ok(f64::Frequency::new::<hertz>(0.0)),
+													_ => Err(ErrorCode::IllegalParameterValue.into()),
+												})
+											})?;
+		debug!("Received frequency: {:?}", frequency);
+		Ok(())
     }
 }