scpi: adopt rust standard on result/option handling

Cargo.toml

@@ -20,10 +20,8 @@ heapless = "0.5.5"
 
 # Logging and Panicking
 panic-itm = "0.4.1"
-panic-semihosting = { version = "0.5.3", features = [ "exit" ] }
 cortex-m-rtic = "0.5.3"
-cortex-m-log = { version = "0.6.2", features = [ "itm", "log-integration", "semihosting" ] }
+cortex-m-log = { version = "0.6.2", features = [ "itm", "log-integration" ] }
-cortex-m-semihosting = "0.3.3"
 log = {version = "0.4.11"}
 lazy_static = { version = "1.4.0", features = ["spin_no_std"] }
 
@@ -45,6 +43,9 @@ name = "tcp_client"
 [[example]]
 name = "mqtt_client"
 
+[[example]]
+name = "mqtt_hello_world"
+
 # Uncomment for the allocator example.
 # alloc-cortex-m = "0.3.5"
 

examples/ethernet.rs

@@ -1,28 +1,19 @@
 #![no_main]
 #![no_std]
 
-// extern crate cortex_m_rt as rt;
 use core::sync::atomic::{AtomicU32, Ordering};
 
 #[macro_use]
 extern crate log;
 
-// extern crate cortex_m;
-use panic_semihosting as _;
-
 use cortex_m;
 use cortex_m::asm::nop;
 use cortex_m_rt::{
     entry,
     exception,
 };
-use cortex_m_semihosting::hprintln;
-
 extern crate smoltcp;
 
-// Ethernet crate for STM32H7 has been merged into HAL in the latest commit
-// extern crate stm32h7_ethernet as ethernet;
-
 use stm32h7xx_hal::ethernet;
 use stm32h7xx_hal::gpio::Speed;
 use stm32h7xx_hal::hal::digital::v2::{

examples/fpga_config.rs

@@ -3,9 +3,6 @@
 
 #[macro_use]
 extern crate log;
-
-use panic_semihosting as _;
-
 use stm32h7xx_hal::hal::digital::v2::{
 	InputPin,
 	OutputPin,

examples/mqtt_client.rs

@@ -19,10 +19,6 @@ use cortex_m_rt::{
     entry,
     exception,
 };
-use cortex_m_semihosting::hprintln;
-
-// use panic_halt as _;
-
 use rtic::cyccnt::{Instant, U32Ext};
 
 use minimq::{
@@ -67,8 +63,6 @@ macro_rules! add_socket {
 
 #[entry]
 fn main() -> ! {
-    // logger::semihosting_init();
-
     let mut cp = cortex_m::Peripherals::take().unwrap();
     let dp = pac::Peripherals::take().unwrap();
 
@@ -84,15 +78,17 @@ fn main() -> ! {
 
     let rcc = dp.RCC.constrain();
     let ccdr = rcc
-                .use_hse(16.mhz())
+                .use_hse(8.mhz())
                 .sysclk(400.mhz())
                 .hclk(200.mhz())
-                .per_ck(100.mhz())
                 .pll1_q_ck(48.mhz())    // for SPI
-                .pll2_p_ck(100.mhz())
+                .pll1_r_ck(400.mhz())   // for TRACECK
-                .pll2_q_ck(100.mhz())
                 .freeze(vos, &dp.SYSCFG);
 
+    unsafe {
+        logger::enable_itm(&dp.DBGMCU, &mut cp.DCB, &mut cp.ITM);
+    }
+    logger::init();
     let mut delay = cp.SYST.delay(ccdr.clocks);
 
     let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
@@ -103,13 +99,6 @@ 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();
-    let mut green_led = gpiob.pb0.into_push_pull_output();
-    green_led.set_low().unwrap();
-
     // Configure ethernet IO
     {
         let _rmii_refclk = gpioa.pa1.into_alternate_af11().set_speed(Speed::VeryHigh);
@@ -125,7 +114,7 @@ fn main() -> ! {
 
     // Configure ethernet
     let mac_addr = net::wire::EthernetAddress([0xAC, 0x6F, 0x7A, 0xDE, 0xD6, 0xC8]);
-    let (eth_dma, _eth_mac) = unsafe {
+    let (eth_dma, mut eth_mac) = unsafe {
         ethernet::new_unchecked(
             dp.ETHERNET_MAC,
             dp.ETHERNET_MTL,
@@ -143,15 +132,10 @@ fn main() -> ! {
 
     let neighbor_cache = net::iface::NeighborCache::new(&mut store.neighbor_cache[..]);
 
-    let default_v4_gw = net::wire::Ipv4Address::new(192, 168, 1, 1);
-    let mut routes = net::iface::Routes::new(&mut store.routes_cache[..]);
-    routes.add_default_ipv4_route(default_v4_gw).unwrap();
-
     let mut net_interface = net::iface::EthernetInterfaceBuilder::new(eth_dma)
         .ethernet_addr(mac_addr)
         .neighbor_cache(neighbor_cache)
         .ip_addrs(&mut store.ip_addrs[..])
-        // .routes(routes)
         .finalize();
 
     /*
@@ -192,13 +176,13 @@ fn main() -> ! {
 
     cp.SCB.invalidate_icache();
     cp.SCB.enable_icache();
-    // cp.SCB.clean_dcache(&mut cp.CPUID);
-    // cp.SCB.disable_dcache(&mut cp.CPUID);
-    // cp.SCB.enable_dcache(&mut cp.CPUID);
 
+    // Time unit in ms
     let mut time: u32 = 0;
-    let mut next_ms = Instant::now();
 
+    // Cycle counter for 1 ms
+    // This effectively provides a conversion from rtic unit to ms
+    let mut next_ms = Instant::now();
     next_ms += 400_000.cycles();
 
     let mut socket_set_entries: [_; 8] = Default::default();
@@ -212,50 +196,33 @@ fn main() -> ! {
         tcp_stack,
     )
     .unwrap();
-
+    let mut tick = false;
-    delay.delay_ms(1000_u16);
-
-    client.network_stack.update(time);
-    green_led.set_high().unwrap();
 
     loop {
-        let tick = Instant::now() > next_ms;
+        // Update time accumulator in ms
-
+        // Tick once every ms
-        if tick {
+        if Instant::now() > next_ms {
-            next_ms += 400_000.cycles();
+            tick = true;
             time += 1;
+            next_ms += 400_000.cycles();
         }
 
-        client.network_stack.update(time);
+        // Poll if necessary
+        if tick && client.network_stack.update_delay(time) == 0 && eth_mac.phy_poll_link() {
+            client.network_stack.update(time);
+        }
 
-        client
+        let connection = client
             .poll(|_client, topic, message, _properties| match topic {
                 _ => info!("On '{:?}', received: {:?}", topic, message),
-            })
+            }).is_ok();
-            .unwrap();
 
-        match client.is_connected() {
+        if connection && tick && (time % 3000) == 0 {
-            true => {
+            info!("Feedback from print publish: {:?}", client
-                yellow_led.set_high().unwrap();
+                .publish("Channel1/Switch", "Hello, World!".as_bytes(), QoS::AtMostOnce, &[]));
-                red_led.set_low().unwrap();
-            },
-            _ => {
-                yellow_led.set_low().unwrap();
-                red_led.set_high().unwrap();
-            },
-        };
-
-        if tick && (time % 1000) == 0 {
-            client
-                .publish("nucleo", "Hello, World!".as_bytes(), QoS::AtMostOnce, &[])
-                .unwrap();
         }
 
-        // Update the TCP stack.
+        // Reset tick flag
-        // let sleep = client.network_stack.update(time);
+        tick = false;
-        // if sleep {
-        //     //cortex_m::asm::wfi();
-        //     cortex_m::asm::nop();
-        // }
     }
-}
+}

examples/mqtt_hello_world.rs

@@ -0,0 +1,266 @@
+
+#![no_std]
+#![no_main]
+
+#[macro_use]
+extern crate log;
+
+use smoltcp as net;
+// use stm32h7_ethernet as ethernet;
+use stm32h7xx_hal::{gpio::Speed, prelude::*, ethernet};
+
+use heapless::{consts, String};
+// use si7021::Si7021;
+
+use cortex_m;
+
+use panic_halt as _;
+// use serde::{Deserialize, Serialize};
+
+use rtic::cyccnt::{Instant, U32Ext};
+
+mod tcp_stack;
+
+#[path = "util/logger.rs"]
+mod logger;
+
+use minimq::{
+    embedded_nal::{IpAddr, Ipv4Addr},
+    MqttClient, QoS,
+};
+use tcp_stack::NetworkStack;
+
+pub struct NetStorage {
+    ip_addrs: [net::wire::IpCidr; 1],
+    neighbor_cache: [Option<(net::wire::IpAddress, net::iface::Neighbor)>; 8],
+}
+
+static mut NET_STORE: NetStorage = NetStorage {
+    // Placeholder for the real IP address, which is initialized at runtime.
+    ip_addrs: [net::wire::IpCidr::Ipv6(
+        net::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX,
+    )],
+    neighbor_cache: [None; 8],
+};
+
+#[link_section = ".sram3.eth"]
+static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();
+
+// #[derive(Serialize, Deserialize)]
+// struct Temperature {
+//     temperature_c: f32,
+// }
+
+type NetworkInterface =
+    net::iface::EthernetInterface<'static, 'static, 'static, ethernet::EthernetDMA<'static>>;
+
+macro_rules! add_socket {
+    ($sockets:ident, $tx_storage:ident, $rx_storage:ident) => {
+        let mut $rx_storage = [0; 4096];
+        let mut $tx_storage = [0; 4096];
+
+        let tcp_socket = {
+            let tx_buffer = net::socket::TcpSocketBuffer::new(&mut $tx_storage[..]);
+            let rx_buffer = net::socket::TcpSocketBuffer::new(&mut $rx_storage[..]);
+
+            net::socket::TcpSocket::new(tx_buffer, rx_buffer)
+        };
+
+        let _handle = $sockets.add(tcp_socket);
+    };
+}
+
+// #[cfg(not(feature = "semihosting"))]
+// fn init_log() {}
+
+// #[cfg(feature = "semihosting")]
+// fn init_log() {
+//     use cortex_m_log::log::{init as init_log, Logger};
+//     use cortex_m_log::printer::semihosting::{hio::HStdout, InterruptOk};
+//     use log::LevelFilter;
+
+//     static mut LOGGER: Option<Logger<InterruptOk<HStdout>>> = None;
+
+//     let logger = Logger {
+//         inner: InterruptOk::<_>::stdout().unwrap(),
+//         level: LevelFilter::Info,
+//     };
+
+//     let logger = unsafe { LOGGER.get_or_insert(logger) };
+
+//     init_log(logger).unwrap();
+// }
+
+#[rtic::app(device = stm32h7xx_hal::stm32, peripherals = true, monotonic = rtic::cyccnt::CYCCNT)]
+const APP: () = {
+    struct Resources {
+        net_interface: NetworkInterface,
+        // si7021: Si7021<stm32h7xx_hal::i2c::I2c<stm32h7xx_hal::stm32::I2C2>>,
+    }
+
+    #[init]
+    fn init(c: init::Context) -> init::LateResources {
+        let mut cp = unsafe {
+            cortex_m::Peripherals::steal()
+        };
+        cp.DWT.enable_cycle_counter();
+
+        // Enable SRAM3 for the descriptor ring.
+        c.device.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit());
+
+        let rcc = c.device.RCC.constrain();
+        let pwr = c.device.PWR.constrain();
+        let vos = pwr.freeze();
+
+        let ccdr = rcc
+            .sysclk(400.mhz())
+            .hclk(200.mhz())
+            .per_ck(100.mhz())
+            .pll1_r_ck(400.mhz())   // for TRACECK
+            .pll2_p_ck(100.mhz())
+            .pll2_q_ck(100.mhz())
+            .freeze(vos, &c.device.SYSCFG);
+
+        unsafe {
+            logger::enable_itm(&c.device.DBGMCU, &mut cp.DCB, &mut cp.ITM);
+        }
+        logger::init();
+
+        let gpioa = c.device.GPIOA.split(ccdr.peripheral.GPIOA);
+        let gpiob = c.device.GPIOB.split(ccdr.peripheral.GPIOB);
+        let gpioc = c.device.GPIOC.split(ccdr.peripheral.GPIOC);
+        let gpiof = c.device.GPIOF.split(ccdr.peripheral.GPIOF);
+        let gpiog = c.device.GPIOG.split(ccdr.peripheral.GPIOG);
+
+        // Configure ethernet IO
+        {
+            let _rmii_refclk = gpioa.pa1.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_mdio = gpioa.pa2.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_mdc = gpioc.pc1.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_crs_dv = gpioa.pa7.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_rxd0 = gpioc.pc4.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_rxd1 = gpioc.pc5.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_tx_en = gpiog.pg11.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_txd0 = gpiog.pg13.into_alternate_af11().set_speed(Speed::VeryHigh);
+            let _rmii_txd1 = gpiob.pb13.into_alternate_af11().set_speed(Speed::VeryHigh);
+        }
+
+        // Configure ethernet
+        let net_interface = {
+            let mac_addr = net::wire::EthernetAddress([0xAC, 0x6F, 0x7A, 0xDE, 0xD6, 0xC8]);
+            let (eth_dma, mut _eth_mac) = unsafe {
+                ethernet::new_unchecked(
+                    c.device.ETHERNET_MAC,
+                    c.device.ETHERNET_MTL,
+                    c.device.ETHERNET_DMA,
+                    &mut DES_RING,
+                    mac_addr.clone(),
+                )
+            };
+            
+            while !_eth_mac.phy_poll_link() {}
+
+            unsafe { ethernet::enable_interrupt() }
+
+            let store = unsafe { &mut NET_STORE };
+
+            store.ip_addrs[0] = net::wire::IpCidr::new(net::wire::IpAddress::v4(192, 168, 1, 200), 24);
+
+            let neighbor_cache = net::iface::NeighborCache::new(&mut store.neighbor_cache[..]);
+
+            net::iface::EthernetInterfaceBuilder::new(eth_dma)
+                .ethernet_addr(mac_addr)
+                .neighbor_cache(neighbor_cache)
+                .ip_addrs(&mut store.ip_addrs[..])
+                .finalize()
+        };
+
+        // Configure I2C
+        // let si7021 = {
+        //     let i2c_sda = gpiof.pf0.into_open_drain_output().into_alternate_af4();
+        //     let i2c_scl = gpiof.pf1.into_open_drain_output().into_alternate_af4();
+        //     let i2c = c.device.I2C2.i2c(
+        //         (i2c_scl, i2c_sda),
+        //         100.khz(),
+        //         ccdr.peripheral.I2C2,
+        //         &ccdr.clocks,
+        //     );
+
+        //     Si7021::new(i2c)
+        // };
+
+        cp.SCB.enable_icache();
+
+        init::LateResources {
+            net_interface: net_interface,
+            // si7021: si7021,
+        }
+    }
+
+    #[idle(resources=[net_interface])]
+    fn idle(c: idle::Context) -> ! {
+        let mut time: u32 = 0;
+        let mut next_ms = Instant::now();
+
+        next_ms += 400_00.cycles();
+
+        let mut socket_set_entries: [_; 8] = Default::default();
+        let mut sockets = net::socket::SocketSet::new(&mut socket_set_entries[..]);
+        add_socket!(sockets, rx_storage, tx_storage);
+
+        let tcp_stack = NetworkStack::new(c.resources.net_interface, sockets);
+        let mut client = MqttClient::<consts::U256, _>::new(
+            IpAddr::V4(Ipv4Addr::new(192, 168, 1, 125)),
+            "nucleo",
+            tcp_stack,
+        )
+        .unwrap();
+
+        loop {
+            let tick = Instant::now() > next_ms;
+
+            if tick {
+                next_ms += 400_000.cycles();
+                time += 1;
+            }
+
+            if tick && (time % 1000) == 0 {
+                client
+                    .publish("nucleo", "Hello, World!".as_bytes(), QoS::AtMostOnce, &[])
+                    .unwrap();
+
+                // let temperature = Temperature {
+                //     temperature_c: c.resources.si7021.temperature_celsius().unwrap(),
+                // };
+                // let temperature: String<consts::U256> =
+                //     serde_json_core::to_string(&temperature).unwrap();
+                // client
+                //     .publish(
+                //         "temperature",
+                //         &temperature.into_bytes(),
+                //         QoS::AtMostOnce,
+                //         &[],
+                //     )
+                //     .unwrap();
+            }
+
+            client
+                .poll(|_client, topic, message, _properties| match topic {
+                    _ => info!("On '{:?}', received: {:?}", topic, message),
+                });
+                // .unwrap();
+
+            // Update the TCP stack.
+            let sleep = client.network_stack.update(time);
+            if sleep {
+                //cortex_m::asm::wfi();
+                cortex_m::asm::nop();
+            }
+        }
+    }
+
+    #[task(binds=ETH)]
+    fn eth(_: eth::Context) {
+        unsafe { ethernet::interrupt_handler() }
+    }
+};

examples/tcp_client.rs

@@ -85,7 +85,7 @@ fn main() -> ! {
     let ccdr = rcc
                 .use_hse(8.mhz())
                 .sysclk(400.mhz())
-                // .hclk(200.mhz())
+                .hclk(200.mhz())
                 // .per_ck(100.mhz())
                 .pll1_q_ck(48.mhz())    // for SPI
                 .pll1_r_ck(400.mhz())   // for TRACECK
@@ -133,7 +133,7 @@ fn main() -> ! {
 
     // Configure ethernet
     let mac_addr = net::wire::EthernetAddress([0xAC, 0x6F, 0x7A, 0xDE, 0xD6, 0xC8]);
-    let (eth_dma, _eth_mac) = unsafe {
+    let (eth_dma, mut eth_mac) = unsafe {
         ethernet::new_unchecked(
             dp.ETHERNET_MAC,
             dp.ETHERNET_MTL,
@@ -219,12 +219,13 @@ fn main() -> ! {
     };
     let handle = sockets.add(tcp_socket);
 
-    delay.delay_ms(2000_u16);
+    // delay.delay_ms(2000_u16);
     green_led.set_high().unwrap();
 
     {
         let mut socket = sockets.get::<net::socket::TcpSocket>(handle);
         socket.connect((net::wire::IpAddress::v4(192, 168, 1, 125), 1883), 49500).unwrap();
+        socket.set_timeout(Some(net::time::Duration::from_millis(2000)));
         debug!("connect!");
     }
 
@@ -232,14 +233,15 @@ fn main() -> ! {
     red_led.set_high().unwrap();
 
     let mut green = true;
-    // let mut eth_status = 0;
+    let mut connected = false;
-    // unsafe {
+
-    //     eth_status = (*pac::ETHERNET_MAC::ptr()).macdr.read().bits();
+    debug!("Poll link status: {}", eth_mac.phy_poll_link());
-    //     debug!("eth_status: {:X}", eth_status);
+    while !eth_mac.phy_poll_link() {}
-    // }
+    debug!("Poll link status: {}", eth_mac.phy_poll_link());
 
     loop {
         // let timestamp = net::time::Instant::from_millis(TIME.load(Ordering::Relaxed) as i64);
+        while !eth_mac.phy_poll_link() {}
         match net_interface.poll(&mut sockets, clock.elapsed()) {
             Ok(_) => {},
             Err(e) => {
@@ -247,20 +249,11 @@ fn main() -> ! {
             }
         }
 
-        // unsafe {
-        //     // let p = &DES_RING as *const _;
-        //     // let tdeses = core::ptr::read_volatile(p as *const [u32; 16]);
-        //     // debug!("{:X?}", core::ptr::read_volatile(tdeses[0] as *const [u32; 20]))
-        //     let new_eth_status = (*pac::ETHERNET_MAC::ptr()).macdr.read().bits();
-        //     if new_eth_status != eth_status {
-        //         eth_status = new_eth_status;
-        //         debug!("eth_status: {:X}", eth_status);
-        //     }
-        // }
-
         {
             let mut socket = sockets.get::<net::socket::TcpSocket>(handle);
 
+            info!("Socket state: {} at time {}", socket.state(), clock.elapsed());
+
             if socket.may_recv() {
                 yellow_led.set_high().unwrap();
                 red_led.set_low().unwrap();
@@ -281,6 +274,7 @@ fn main() -> ! {
 
         match net_interface.poll_delay(&sockets, clock.elapsed()) {
             Some(net::time::Duration {millis :0}) => {
+                clock.advance(net::time::Duration::from_millis(1));
                 continue;
             }
             Some(time_delay) => {

examples/tcp_stack.rs

@@ -0,0 +1,180 @@
+use core::cell::RefCell;
+use nb;
+
+use heapless::{consts, Vec};
+
+use super::{net, NetworkInterface};
+use minimq::embedded_nal;
+
+#[derive(Debug)]
+pub enum NetworkError {
+    NoSocket,
+    ConnectionFailure,
+    ReadFailure,
+    WriteFailure,
+}
+
+// TODO: The network stack likely needs a time-tracking mechanic here to support
+// blocking/nonblocking/timeout based operations.
+pub struct NetworkStack<'a, 'b, 'c, 'n> {
+    network_interface: &'n mut NetworkInterface,
+    sockets: RefCell<net::socket::SocketSet<'a, 'b, 'c>>,
+    next_port: RefCell<u16>,
+    unused_handles: RefCell<Vec<net::socket::SocketHandle, consts::U16>>,
+}
+
+impl<'a, 'b, 'c, 'n> NetworkStack<'a, 'b, 'c, 'n> {
+    pub fn new(
+        interface: &'n mut NetworkInterface,
+        sockets: net::socket::SocketSet<'a, 'b, 'c>,
+    ) -> Self {
+        let mut unused_handles: Vec<net::socket::SocketHandle, consts::U16> = Vec::new();
+        for socket in sockets.iter() {
+            unused_handles.push(socket.handle()).unwrap();
+        }
+
+        NetworkStack {
+            network_interface: interface,
+            sockets: RefCell::new(sockets),
+            next_port: RefCell::new(49152),
+            unused_handles: RefCell::new(unused_handles),
+        }
+    }
+
+    pub fn update(&mut self, time: u32) -> bool {
+        match self.network_interface.poll(
+            &mut self.sockets.borrow_mut(),
+            net::time::Instant::from_millis(time as i64),
+        ) {
+            Ok(changed) => changed == false,
+            Err(e) => {
+                info!("{:?}", e);
+                true
+            }
+        }
+    }
+
+    fn get_ephemeral_port(&self) -> u16 {
+        // Get the next ephemeral port
+        let current_port = self.next_port.borrow().clone();
+
+        let (next, wrap) = self.next_port.borrow().overflowing_add(1);
+        *self.next_port.borrow_mut() = if wrap { 49152 } else { next };
+
+        return current_port;
+    }
+}
+
+impl<'a, 'b, 'c, 'n> embedded_nal::TcpStack for NetworkStack<'a, 'b, 'c, 'n> {
+    type TcpSocket = net::socket::SocketHandle;
+    type Error = NetworkError;
+
+    fn open(&self, _mode: embedded_nal::Mode) -> Result<Self::TcpSocket, Self::Error> {
+        // TODO: Handle mode?
+        match self.unused_handles.borrow_mut().pop() {
+            Some(handle) => {
+                // Abort any active connections on the handle.
+                let mut sockets = self.sockets.borrow_mut();
+                let internal_socket: &mut net::socket::TcpSocket = &mut *sockets.get(handle);
+                internal_socket.abort();
+
+                Ok(handle)
+            }
+            None => Err(NetworkError::NoSocket),
+        }
+    }
+
+    fn connect(
+        &self,
+        socket: Self::TcpSocket,
+        remote: embedded_nal::SocketAddr,
+    ) -> Result<Self::TcpSocket, Self::Error> {
+        // TODO: Handle socket mode?
+
+        let mut sockets = self.sockets.borrow_mut();
+        let internal_socket: &mut net::socket::TcpSocket = &mut *sockets.get(socket);
+
+        // If we're already in the process of connecting, ignore the request silently.
+        if internal_socket.is_open() {
+            return Ok(socket);
+        }
+
+        match remote.ip() {
+            embedded_nal::IpAddr::V4(addr) => {
+                let address = {
+                    let octets = addr.octets();
+                    net::wire::Ipv4Address::new(octets[0], octets[1], octets[2], octets[3])
+                };
+                internal_socket
+                    .connect((address, remote.port()), self.get_ephemeral_port())
+                    .map_err(|_| NetworkError::ConnectionFailure)?;
+                // internal_socket.set_timeout(Some(net::time::Duration::from_millis(2000)));
+            }
+            embedded_nal::IpAddr::V6(addr) => {
+                let address = {
+                    let octets = addr.segments();
+                    net::wire::Ipv6Address::new(
+                        octets[0], octets[1], octets[2], octets[3], octets[4], octets[5],
+                        octets[6], octets[7],
+                    )
+                };
+                internal_socket
+                    .connect((address, remote.port()), self.get_ephemeral_port())
+                    .map_err(|_| NetworkError::ConnectionFailure)?;
+                // internal_socket.set_timeout(Some(net::time::Duration::from_millis(2000)));
+            }
+        };
+
+        Ok(socket)
+    }
+
+    fn is_connected(&self, socket: &Self::TcpSocket) -> Result<bool, Self::Error> {
+        let mut sockets = self.sockets.borrow_mut();
+        let socket: &mut net::socket::TcpSocket = &mut *sockets.get(*socket);
+
+        Ok(socket.may_send() && socket.may_recv())
+    }
+
+    fn write(&self, socket: &mut Self::TcpSocket, buffer: &[u8]) -> nb::Result<usize, Self::Error> {
+        // TODO: Handle the socket mode.
+
+        let mut sockets = self.sockets.borrow_mut();
+        let socket: &mut net::socket::TcpSocket = &mut *sockets.get(*socket);
+
+        let result = socket.send_slice(buffer);
+
+        match result {
+            Ok(num_bytes) => Ok(num_bytes),
+            Err(_) => Err(nb::Error::Other(NetworkError::WriteFailure)),
+        }
+    }
+
+    fn read(
+        &self,
+        socket: &mut Self::TcpSocket,
+        buffer: &mut [u8],
+    ) -> nb::Result<usize, Self::Error> {
+        // TODO: Handle the socket mode.
+
+        let mut sockets = self.sockets.borrow_mut();
+        let socket: &mut net::socket::TcpSocket = &mut *sockets.get(*socket);
+
+        let result = socket.recv_slice(buffer);
+
+        match result {
+            Ok(num_bytes) => Ok(num_bytes),
+            Err(_) => Err(nb::Error::Other(NetworkError::ReadFailure)),
+        }
+    }
+
+    fn close(&self, socket: Self::TcpSocket) -> Result<(), Self::Error> {
+        // TODO: Free the ephemeral port in use by the socket.
+
+        let mut sockets = self.sockets.borrow_mut();
+        let internal_socket: &mut net::socket::TcpSocket = &mut *sockets.get(socket);
+        internal_socket.close();
+
+        self.unused_handles.borrow_mut().push(socket).unwrap();
+        Ok(())
+    }
+}

examples/util/logger.rs

@@ -48,7 +48,7 @@ pub unsafe fn enable_itm(
     );
 }
 
-// use panic_itm as _;
+use panic_itm as _;
 
 use lazy_static::lazy_static;
 use log::LevelFilter;
@@ -75,22 +75,4 @@ lazy_static! {
 
 pub fn init() {
     cortex_m_log::log::init(&LOGGER).unwrap();
-}
-
-// use panic_semihosting as _;
-
-use cortex_m_log::printer::semihosting;
-use cortex_m_log::printer::semihosting::Semihosting;
-use cortex_m_log::modes::InterruptOk;
-use cortex_m_semihosting::hio::HStdout;
-
-lazy_static! {
-    static ref HLOGGER: Logger<Semihosting<InterruptOk, HStdout>> = Logger {
-        level: LevelFilter::Trace,
-        inner: semihosting::InterruptOk::<_>::stdout().expect("Get Semihosting stdout"),
-    };
-}
-
-pub fn semihosting_init() {
-    cortex_m_log::log::init(&HLOGGER).unwrap();
 }

gdb_config/fpga_config.gdb

@@ -12,7 +12,7 @@ break HardFault
 break rust_begin_unwind
 
 # break at line 130 to auto quit
-break examples/fpga_config.rs:143
+break examples/fpga_config.rs:140
 
 # print using semihosting, slow af
 # monitor arm semihosting enable

src/lib.rs

@@ -55,6 +55,7 @@ pub enum Error<E> {
 	ParameterError,
 }
 
+#[derive(Debug)]
 pub enum ClockSource {
 	OSC,
 	SMA,

src/nal_tcp_client.rs

@@ -52,6 +52,13 @@ impl<'a, 'b, 'c, 'n> NetworkStack<'a, 'b, 'c, 'n> {
         }
     }
 
+    pub fn update_delay(&mut self, time: u32) -> u32 {
+        self.network_interface.poll_delay(
+            &mut self.sockets.borrow_mut(),
+            net::time::Instant::from_millis(time as i64),
+        ).map_or(1000, |next_poll_time| next_poll_time.total_millis() as u32)
+    }
+
     pub fn update(&mut self, time: u32) -> bool {
         match self.network_interface.poll(
             &mut self.sockets.borrow_mut(),

src/scpi.rs

@@ -152,20 +152,14 @@ impl<T: Device + UrukulTraits> Command<T> for Channel0SwitchCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        match context.device.get_channel_switch_status(0) {
+        let next_state: bool = args.next_data(true)?
-            Ok(status) => {
+                                    .map_or(
-                let next_state: bool = match args.next_data(true) {
+                                        context.device.get_channel_switch_status(0)
-                    Ok(Some(token)) => token.try_into()?,
+                                                        .map(|current| !current)
-                    Ok(None) => !status,
+                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
-                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
+                                        |token| token.try_into()
-                };
+                                    )?;
-                match context.device.set_channel_switch(0, next_state) {
+        context.device.set_channel_switch(0, next_state).map_err(|_| Error::new(ErrorCode::HardwareError))
-                    Ok(()) => Ok(()),
-                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-                }
-            },
-            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-        }
     }
 }
 
@@ -173,20 +167,14 @@ impl<T: Device + UrukulTraits> Command<T> for Channel1SwitchCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        match context.device.get_channel_switch_status(1) {
+        let next_state: bool = args.next_data(true)?
-            Ok(status) => {
+                                    .map_or(
-                let next_state: bool = match args.next_data(true) {
+                                        context.device.get_channel_switch_status(1)
-                    Ok(Some(token)) => token.try_into()?,
+                                                        .map(|current| !current)
-                    Ok(None) => !status,
+                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
-                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
+                                        |token| token.try_into()
-                };
+                                    )?;
-                match context.device.set_channel_switch(1, next_state) {
+        context.device.set_channel_switch(1, next_state).map_err(|_| Error::new(ErrorCode::HardwareError))
-                    Ok(()) => Ok(()),
-                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-                }
-            },
-            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-        }
     }
 }
 
@@ -194,20 +182,14 @@ impl<T: Device + UrukulTraits> Command<T> for Channel2SwitchCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        match context.device.get_channel_switch_status(2) {
+        let next_state: bool = args.next_data(true)?
-            Ok(status) => {
+                                    .map_or(
-                let next_state: bool = match args.next_data(true) {
+                                        context.device.get_channel_switch_status(2)
-                    Ok(Some(token)) => token.try_into()?,
+                                                        .map(|current| !current)
-                    Ok(None) => !status,
+                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
-                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
+                                        |token| token.try_into()
-                };
+                                    )?;
-                match context.device.set_channel_switch(2, next_state) {
+        context.device.set_channel_switch(2, next_state).map_err(|_| Error::new(ErrorCode::HardwareError))
-                    Ok(()) => Ok(()),
-                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-                }
-            },
-            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-        }
     }
 }
 
@@ -215,74 +197,32 @@ impl<T: Device + UrukulTraits> Command<T> for Channel3SwitchCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        match context.device.get_channel_switch_status(3) {
+        let next_state: bool = args.next_data(true)?
-            Ok(status) => {
+                                    .map_or(
-                let next_state: bool = match args.next_data(true) {
+                                        context.device.get_channel_switch_status(3)
-                    Ok(Some(token)) => token.try_into()?,
+                                                        .map(|current| !current)
-                    Ok(None) => !status,
+                                                        .map_err(|_| Error::new(ErrorCode::HardwareError)),
-                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
+                                        |token| token.try_into()
-                };
+                                    )?;
-                match context.device.set_channel_switch(3, next_state) {
+        context.device.set_channel_switch(3, next_state).map_err(|_| Error::new(ErrorCode::HardwareError))
-                    Ok(()) => Ok(()),
-                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-                }
-            },
-            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-        }
     }
 }
 
+// Handle CLOCK:SOURCE command, setup the proper source for the system clock
+// Leave clock division to CLOCK:DIVision command
 impl<T:Device + UrukulTraits> Command<T> for ClockSourceCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
 
+        // next_data() fucntion call can never return CharacterProgramData, could be an oversight
         let s: &[u8] = match args.next_data(false)? {
             Some(Token::CharacterProgramData(s)) => s,
             _ => return Err(ErrorCode::IllegalParameterValue.into()),
         };
-	/*
-        debug!("Converted data: {:?}", data);
 
-        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());
-                }
-            } {
-                debug!("Finished parsing source");
-            }
-        } 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)?;
+                                .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| {
@@ -291,8 +231,34 @@ impl<T:Device + UrukulTraits> Command<T> for ClockSourceCommand {
 													_ => Err(ErrorCode::IllegalParameterValue.into()),
 												})
 											})?;
-		debug!("Received frequency: {:?}", frequency);
+        trace!("Received frequency: {:?}", frequency);
-		Ok(())
+
+        let clock_source = match s_str {
+			source if source.eq_ignore_ascii_case("OSC") => {
+                // If clock source is OSC, it must be 100MHz (not configurable)
+                if frequency.get::<megahertz>() != 100.0 {
+                    warn!("Clock selection failed! OSC must be 100 MHz");
+                    return Err(ErrorCode::IllegalParameterValue.into());
+                }
+                ClockSource::OSC
+			},
+			source if source.eq_ignore_ascii_case("MMCX") => {
+                // TODO: Implement frequency check for MMCX
+                ClockSource::MMCX
+			},
+			source if source.eq_ignore_ascii_case("SMA") => {
+                // TODO: Implement frequency check for SMA
+                ClockSource::SMA
+			},
+			_ => {
+				warn!("Clock selection failed! Argument error!");
+				return Err(ErrorCode::IllegalParameterValue.into());
+            },
+        };
+
+        trace!("Changing clock source to {:?} at {:?}", clock_source, frequency);
+        context.device.set_clock_source(clock_source)
+                        .map_err(|_| Error::new(ErrorCode::HardwareError))
     }
 }
 
@@ -300,26 +266,17 @@ impl<T:Device + UrukulTraits> Command<T> for ClockDivisionCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        let data :u8 = match args.next_data(false)? {
+        let div :f32 = args.next_data(false)?
-            Some(token) => {
+                                .map_or(Err(Error::new(ErrorCode::IllegalParameterValue)),
-                match f32::try_from(token) {
+                                        |token| token.try_into())?;
-                    Ok(val) => {
+        trace!("Received master clock division factor: {}", div);
-                        if val == 1.0 || val == 2.0 || val == 4.0 {
+        match div {
-                            val as u8
+            1.0 | 2.0 | 4.0 => {
-                        } else {
+                debug!("Set master clock division as {}", div);
-                            return Err(ErrorCode::IllegalParameterValue.into())
+                context.device.set_clock_division(div as u8)
-                        }
+                                .map_err(|_| Error::new(ErrorCode::HardwareError))
-                    },
+            }
-                    Err(_e) => {
+            _ => Err(Error::new(ErrorCode::IllegalParameterValue)),
-                        return Err(ErrorCode::IllegalParameterValue.into())
-                    },
-                }
-            },
-            _ => return Err(ErrorCode::IllegalParameterValue.into()),
-        };
-        match context.device.set_clock_division(data) {
-            Ok(()) => Ok(()),
-            Err(_) => Err(Error::new(ErrorCode::HardwareError)),
         }
     }
 }