diff --git a/Cargo.toml b/Cargo.toml
index 8588ca1..cf5d13e 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -12,12 +12,8 @@ cortex-m = "0.6.2"
 cortex-m-rt = "0.6.12"
 embedded-hal = "0.2.4"
 stm32h7xx-hal = {version = "0.7.1", features = [ "stm32h743v", "rt", "unproven", "ethernet", "phy_lan8742a" ] }
-# stm32h7-ethernet = { version = "0.2.0", features = [ "phy_lan8742a", "stm32h743v" ] }
 smoltcp = { version = "0.6.0", default-features = false, features = [ "ethernet", "proto-ipv4", "proto-ipv6", "socket-tcp" ] }
 nb = "1.0.0"
-# scpi = { path = "../scpi-fork/scpi", version = "0.3.4" }
-scpi = { git = "https://github.com/occheung/scpi-rs", branch = "issue-4" }
-mashup = "0.1.12"
 
 lexical-core = { version="0.7.1", features=["radix"], default-features=false }
 libm = "0.2.0"
@@ -33,6 +29,12 @@ panic-semihosting = { version = "0.5.3", features = [ "exit" ] }
 cortex-m-rtic = "0.5.3"
 cortex-m-log = { version = "~0.6", features = [ "itm" ] }
 
+[dependencies.scpi]
+git = "https://github.com/occheung/scpi-rs"
+branch = "issue-4"
+default-features = false
+features = [ "build-info", "unit-frequency" ]
+
 [[example]]
 name = "ethernet"
 
diff --git a/examples/mqtt_client.rs b/examples/mqtt_client.rs
index b382e90..465b235 100644
--- a/examples/mqtt_client.rs
+++ b/examples/mqtt_client.rs
@@ -4,7 +4,10 @@
 use smoltcp as net;
 use stm32h7xx_hal::ethernet;
 use stm32h7xx_hal::{gpio::Speed, prelude::*, spi, pac};
-use embedded_hal::{blocking::spi::Transfer};
+use embedded_hal::{
+    blocking::spi::Transfer,
+    digital::v2::OutputPin,
+};
 
 use heapless::{consts, String};
 
@@ -87,6 +90,11 @@ fn main() -> ! {
     let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
     let gpiog = dp.GPIOG.split(ccdr.peripheral.GPIOG);
 
+    let mut yellow_led = gpioe.pe1.into_push_pull_output();
+    yellow_led.set_low().unwrap();
+    let mut red_led = gpiob.pb14.into_push_pull_output();
+    red_led.set_high().unwrap();
+
     // Configure ethernet IO
     {
         let _rmii_refclk = gpioa.pa1.into_alternate_af11().set_speed(Speed::VeryHigh);
@@ -167,12 +175,13 @@ fn main() -> ! {
         [25_000_000, 25_000_000, 25_000_000, 25_000_000]
     );
 
+    cp.SCB.invalidate_icache();
     cp.SCB.enable_icache();
 
     let mut time: u32 = 0;
     let mut next_ms = Instant::now();
 
-    next_ms += 400_00.cycles();
+    next_ms += 400_000.cycles();
 
     let mut socket_set_entries: [_; 8] = Default::default();
     let mut sockets = net::socket::SocketSet::new(&mut socket_set_entries[..]);
@@ -187,6 +196,23 @@ fn main() -> ! {
     .unwrap();
 
     loop {
+        match client.is_connected() {
+            true => {
+                yellow_led.set_high().unwrap();
+                red_led.set_low().unwrap();
+            },
+            _ => {
+                yellow_led.set_low().unwrap();
+                red_led.set_high().unwrap();
+            },
+        };
+
+        client
+            .poll(|_client, topic, message, _properties| match topic {
+                _ => hprintln!("On '{:?}', received: {:?}", topic, message).unwrap(),
+            })
+            .unwrap();
+
         let tick = Instant::now() > next_ms;
 
         if tick {
@@ -200,12 +226,6 @@ fn main() -> ! {
                 .unwrap();
         }
 
-        client
-            .poll(|_client, topic, message, _properties| match topic {
-                _ => hprintln!("On '{:?}', received: {:?}", topic, message).unwrap(),
-            })
-            .unwrap();
-
         // Update the TCP stack.
         let sleep = client.network_stack.update(time);
         if sleep {
diff --git a/src/lib.rs b/src/lib.rs
index 2ace1b6..1a80ce1 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -12,9 +12,6 @@ use core::{
 use cortex_m;
 use cortex_m_semihosting::hprintln;
 
-#[macro_use]
-extern crate mashup;
-
 #[macro_use]
 pub mod bitmask_macro;
 
diff --git a/src/scpi.rs b/src/scpi.rs
index 698634b..1a05587 100644
--- a/src/scpi.rs
+++ b/src/scpi.rs
@@ -28,6 +28,7 @@ use scpi::{
     scpi_status,
     scpi_system,
 };
+// use scpi::suffix::{Frequency};
 
 use embedded_hal::blocking::spi::Transfer;
 use cortex_m_semihosting::hprintln;
@@ -236,19 +237,29 @@ impl<T:Device + UrukulTraits> Command<T> for ClockSourceCommand {
             Some(Token::CharacterProgramData(s)) => s,
             _ => return Err(ErrorCode::IllegalParameterValue.into()),
         };
-        let result = match str::from_utf8(data) {
-            Ok(str_param) => match str_param {
+        // let max_value = f64::Frequency::new::<hertz>(1_000_000_000_000.0);
+        // let min_value = f64::Frequency::new::<hertz>(0.0);
+
+        // 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 Ok(());
             }
-            _ => return Err(ErrorCode::IllegalParameterValue.into()),
         };
-        match result {
-            Ok(_) => Ok(()),
-            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-        }
+        Err(ErrorCode::IllegalParameterValue.into())
     }
 }