tcp_client: rm print

Cargo.toml

@@ -15,8 +15,8 @@ stm32h7xx-hal = {version = "0.6.0", features = [ "stm32h743v", "rt", "unproven"
 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-raw" ] }
 nb = "1.0.0"
-scpi = { path = "../scpi-fork/scpi", version = "0.3.4" }
+# scpi = { path = "../scpi-fork/scpi", version = "0.3.4" }
-# scpi = { git = "https://github.com/occheung/scpi-rs", branch = "issue-4" }
+scpi = { git = "https://github.com/occheung/scpi-rs", branch = "issue-4" }
 
 lexical-core = { version="0.7.1", features=["radix"], default-features=false }
 libm = "0.2.0"

examples/ethernet.rs

@@ -76,7 +76,7 @@ use firmware::{
 	cpld::{
 		CPLD,
     },
-    scpi::{ HelloWorldCommand, SwitchOnCommand },
+    scpi::{ HelloWorldCommand, Channel1SwitchCommand },
     Urukul,
 };
 use scpi::prelude::*;
@@ -318,25 +318,6 @@ fn main() -> ! {
         //
         scpi_crate_version!(),
         //Test
-        Node {
-            name: b"ABORt",
-            handler: None,
-            optional: false,
-            sub: &[],
-        },
-        Node {
-            name: b"INITiate",
-            handler: None,
-            optional: false,
-            sub: &[
-                Node {
-                    name: b"IMMediate",
-                    handler: None,
-                    optional: true,
-                    sub: &[],
-                }
-            ],
-        },
         Node {
             name: b"EXAMple",
             optional: true,
@@ -358,10 +339,17 @@ fn main() -> ! {
             ],
         },
         Node {
-            name: b"SWitch",
+            name: b"CHANNEL1",
             optional: false,
-            handler: Some(&SwitchOnCommand {}),
+            handler: None,
-            sub: &[],
+            sub: &[
+                Node {
+                    name: b"SWitch",
+                    optional: false,
+                    handler: Some(&Channel1SwitchCommand {}),
+                    sub: &[],
+                }
+            ],
         }
     ];
 

examples/mqtt_client.rs

@@ -0,0 +1,216 @@
+#![no_std]
+#![no_main]
+
+use smoltcp as net;
+use stm32h7_ethernet as ethernet;
+use stm32h7xx_hal::{gpio::Speed, prelude::*, spi, pac};
+use embedded_hal::{blocking::spi::Transfer};
+
+use heapless::{consts, String};
+
+use cortex_m;
+use cortex_m_rt::{
+    entry,
+    exception,
+};
+use cortex_m_semihosting::hprintln;
+
+use panic_halt as _;
+
+use rtic::cyccnt::{Instant, U32Ext};
+
+use minimq::{
+    embedded_nal::{IpAddr, Ipv4Addr, TcpStack, SocketAddr, Mode},
+    MqttClient, QoS,
+};
+
+use firmware::nal_tcp_client::{NetworkStack, NetStorage, NetworkInterface};
+use firmware::{Urukul};
+use firmware::cpld::{CPLD};
+
+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],
+    routes_cache: [None; 8],
+};
+
+#[link_section = ".sram3.eth"]
+static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();
+
+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);
+    };
+}
+
+#[entry]
+fn main() -> ! {
+    let mut cp = cortex_m::Peripherals::take().unwrap();
+    let dp = pac::Peripherals::take().unwrap();
+
+    cp.DWT.enable_cycle_counter();
+
+    // Enable SRAM3 for the descriptor ring.
+    dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit());
+
+    let rcc = dp.RCC.constrain();
+    let pwr = dp.PWR.constrain();
+    let vos = pwr.freeze();
+
+    let ccdr = rcc
+    .sysclk(400.mhz())
+    .hclk(200.mhz())
+    .per_ck(100.mhz())
+    .pll1_q_ck(48.mhz())    // for SPI
+    .pll2_p_ck(100.mhz())
+    .pll2_q_ck(100.mhz())
+    .freeze(vos, &dp.SYSCFG);
+
+    let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
+    let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
+    let gpioc = dp.GPIOC.split(ccdr.peripheral.GPIOC);
+    let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
+    let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
+    let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
+    let gpiog = dp.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 mac_addr = net::wire::EthernetAddress([0xAC, 0x6F, 0x7A, 0xDE, 0xD6, 0xC8]);
+    let (eth_dma, _eth_mac) = unsafe {
+        ethernet::ethernet_init(
+            dp.ETHERNET_MAC,
+            dp.ETHERNET_MTL,
+            dp.ETHERNET_DMA,
+            &mut DES_RING,
+            mac_addr.clone(),
+        )
+    };
+
+    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[..]);
+
+    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();
+
+    /*
+     *	Using SPI1, AF5
+     *  SCLK -> PA5
+     *	MOSI -> PB5
+     *	MISO -> PA6
+     *	CS -> 0: PB12, 1: PA15, 2: PC7
+     *  I/O_Update -> PB15
+     */
+    let sclk = gpioa.pa5.into_alternate_af5();
+    let mosi = gpiob.pb5.into_alternate_af5();
+    let miso = gpioa.pa6.into_alternate_af5();
+
+    let (cs0, cs1, cs2) = (
+        gpiob.pb12.into_push_pull_output(),
+        gpioa.pa15.into_push_pull_output(),
+        gpioc.pc7.into_push_pull_output(),
+    );
+
+    let io_update = gpiob.pb15.into_push_pull_output();     
+
+    let spi = dp.SPI1.spi(
+        (sclk, miso, mosi),
+        spi::MODE_0,
+        3.mhz(),
+        ccdr.peripheral.SPI1,
+        &ccdr.clocks,
+    );
+
+    let cpld = CPLD::new(spi, (cs0, cs1, cs2), io_update);
+    let parts = cpld.split();
+
+    let urukul = Urukul::new(
+        parts.spi1, parts.spi2, parts.spi3, parts.spi4, parts.spi5, parts.spi6, parts.spi7,
+        [25_000_000, 25_000_000, 25_000_000, 25_000_000]
+    );
+
+    cp.SCB.enable_icache();
+
+    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(&mut 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();
+        }
+
+        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 {
+            //cortex_m::asm::wfi();
+            cortex_m::asm::nop();
+        }
+    }
+}

examples/tcp_client.rs

@@ -1,194 +1,181 @@
-#![no_std]
 #![no_main]
+#![no_std]
 
-use smoltcp as net;
+use core::sync::atomic::{AtomicU32, Ordering};
-use stm32h7_ethernet as ethernet;
-use stm32h7xx_hal::{gpio::Speed, prelude::*, spi, pac};
-use embedded_hal::{blocking::spi::Transfer};
 
-use heapless::{consts, String};
+// 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;
 
-use panic_halt as _;
+extern crate smoltcp;
 
-use rtic::cyccnt::{Instant, U32Ext};
+// Ethernet crate for STM32H7 has been merged into HAL in the latest commit
+extern crate stm32h7_ethernet as ethernet;
 
-use minimq::{
+use stm32h7xx_hal::gpio::Speed;
-    embedded_nal::{IpAddr, Ipv4Addr},
+use stm32h7xx_hal::hal::digital::v2::{
-    MqttClient, QoS,
+    OutputPin,
+    InputPin,
 };
+use stm32h7xx_hal::rcc::CoreClocks;
+use stm32h7xx_hal::{pac, prelude::*, spi, stm32, stm32::interrupt};
+use Speed::*;
 
-use firmware::nal_tcp_client::{NetworkStack, NetStorage, NetworkInterface};
+use libm::round;
-use firmware::{Urukul};
-use firmware::cpld::{CPLD};
 
-static mut NET_STORE: NetStorage = NetStorage {
+use core::{
-    // Placeholder for the real IP address, which is initialized at runtime.
+    str,
-    ip_addrs: [net::wire::IpCidr::Ipv6(
+    fmt::Write
-        net::wire::Ipv6Cidr::SOLICITED_NODE_PREFIX,
-    )],
-    neighbor_cache: [None; 8],
 };
+use core::mem::uninitialized;
 
+use smoltcp::wire::{EthernetAddress, IpAddress, IpCidr, Ipv4Address};
+use smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder, Routes};
+use smoltcp::socket::SocketSet;
+use smoltcp::socket::{SocketHandle, TcpSocket, TcpSocketBuffer};
+use smoltcp::time::{Duration, Instant};
+
+use embedded_nal::TcpStack;
+
+use firmware;
+use firmware::{
+	attenuator::Attenuator,
+	config_register::{
+		ConfigRegister,
+		CFGMask,
+		StatusMask,
+	},
+	dds::{
+		DDS,
+		DDSCFRMask,
+	},
+	cpld::{
+		CPLD,
+    },
+    Urukul,
+};
+use scpi::prelude::*;
+
+/// Configure SYSTICK for 1ms timebase
+fn systick_init(syst: &mut stm32::SYST, clocks: CoreClocks) {
+    let c_ck_mhz = clocks.c_ck().0 / 1_000_000;
+
+    let syst_calib = 0x3E8;
+
+    syst.set_clock_source(cortex_m::peripheral::syst::SystClkSource::Core);
+    syst.set_reload((syst_calib * c_ck_mhz) - 1);
+    syst.enable_interrupt();
+    syst.enable_counter();
+}
+
+/// ======================================================================
+/// Entry point
+/// ======================================================================
+
+/// TIME is an atomic u32 that counts milliseconds. Although not used
+/// here, it is very useful to have for network protocols
+static TIME: AtomicU32 = AtomicU32::new(0);
+
+/// Locally administered MAC address
+const MAC_ADDRESS: [u8; 6] = [0x02, 0x00, 0x11, 0x22, 0x33, 0x44];
+
+/// Ethernet descriptor rings are a global singleton
 #[link_section = ".sram3.eth"]
 static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new();
 
-macro_rules! add_socket {
+// Theoratical maximum number of socket that can be handled
-    ($sockets:ident, $tx_storage:ident, $rx_storage:ident) => {
+const SOCKET_COUNT: usize = 2;
-        let mut $rx_storage = [0; 4096];
-        let mut $tx_storage = [0; 4096];
 
-        let tcp_socket = {
+// Give buffer sizes of transmitting and receiving TCP packets
-            let tx_buffer = net::socket::TcpSocketBuffer::new(&mut $tx_storage[..]);
+const BUFFER_SIZE: usize = 2048;
-            let rx_buffer = net::socket::TcpSocketBuffer::new(&mut $rx_storage[..]);
-
-            net::socket::TcpSocket::new(tx_buffer, rx_buffer)
-        };
-
-        let _handle = $sockets.add(tcp_socket);
-    };
-}
-
-pub type UrukulType = Urukul<
-    firmware::spi_slave::SPISlave<
-        'static,
-        firmware::cpld::CPLD<
-            stm32h7xx_hal::spi::Spi<stm32h7xx_hal::device::SPI1>,
-            stm32h7xx_hal::gpio::gpiob::PB12<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-            stm32h7xx_hal::gpio::gpioa::PA15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-            stm32h7xx_hal::gpio::gpioc::PC7<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-            stm32h7xx_hal::gpio::gpiob::PB15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>
-        >,
-        stm32h7xx_hal::spi::Spi<stm32h7xx_hal::device::SPI1>,
-        stm32h7xx_hal::gpio::gpiob::PB12<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-        stm32h7xx_hal::gpio::gpioa::PA15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-        stm32h7xx_hal::gpio::gpioc::PC7<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-        stm32h7xx_hal::gpio::gpiob::PB15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>
-    >
->;
-
-pub type CPLDType = CPLD<
-    stm32h7xx_hal::spi::Spi<stm32h7xx_hal::device::SPI1>,
-    stm32h7xx_hal::gpio::gpiob::PB12<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-    stm32h7xx_hal::gpio::gpioa::PA15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-    stm32h7xx_hal::gpio::gpioc::PC7<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-    stm32h7xx_hal::gpio::gpiob::PB15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>
->;
-
-pub type IoUpdateType = stm32h7xx_hal::gpio::gpiob::PB15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>;
-pub type SpiType = stm32h7xx_hal::spi::Spi<stm32h7xx_hal::device::SPI1, u8>;
-pub type PartsType = firmware::spi_slave::Parts<
-    'static,
-    firmware::cpld::CPLD<
-        stm32h7xx_hal::spi::Spi<stm32h7xx_hal::device::SPI1>,
-        stm32h7xx_hal::gpio::gpiob::PB12<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-        stm32h7xx_hal::gpio::gpioa::PA15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-        stm32h7xx_hal::gpio::gpioc::PC7<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-        stm32h7xx_hal::gpio::gpiob::PB15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>
-    >,
-    stm32h7xx_hal::spi::Spi<stm32h7xx_hal::device::SPI1>,
-    stm32h7xx_hal::gpio::gpiob::PB12<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-    stm32h7xx_hal::gpio::gpioa::PA15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-    stm32h7xx_hal::gpio::gpioc::PC7<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>,
-    stm32h7xx_hal::gpio::gpiob::PB15<stm32h7xx_hal::gpio::Output<stm32h7xx_hal::gpio::PushPull>>
->;
 
+// the program entry point
 #[entry]
 fn main() -> ! {
+
     let mut cp = cortex_m::Peripherals::take().unwrap();
     let dp = pac::Peripherals::take().unwrap();
 
-    cp.DWT.enable_cycle_counter();
+    // Initialise power...
-
-    // Enable SRAM3 for the descriptor ring.
-    dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit());
-
-    let rcc = dp.RCC.constrain();
     let pwr = dp.PWR.constrain();
     let vos = pwr.freeze();
 
-    let ccdr = rcc
+    // Initialise SRAM3
-    .sysclk(400.mhz())
+    dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit());
-    .hclk(200.mhz())
-    .per_ck(100.mhz())
-    .pll1_q_ck(48.mhz())    // for SPI
-    .pll2_p_ck(100.mhz())
-    .pll2_q_ck(100.mhz())
-    .freeze(vos, &dp.SYSCFG);
 
+    // Initialise clocks...
+    let rcc = dp.RCC.constrain();
+    let ccdr = rcc
+        .sys_ck(200.mhz())
+        .hclk(200.mhz())
+        .pll1_r_ck(100.mhz())   // for TRACECK
+        .pll1_q_ck(48.mhz())    // for SPI
+        .freeze(vos, &dp.SYSCFG);
+
+    // Get the delay provider.
+    let delay = cp.SYST.delay(ccdr.clocks);
+
+    // Initialise system...
+    cp.SCB.invalidate_icache();
+    cp.SCB.enable_icache();
+    // TODO: ETH DMA coherence issues
+    // cp.SCB.enable_dcache(&mut cp.CPUID);
+    cp.DWT.enable_cycle_counter();
+
+    // Initialise IO...
     let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA);
     let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
     let gpioc = dp.GPIOC.split(ccdr.peripheral.GPIOC);
-    let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
+	let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
-    let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
+	let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
-    let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
+	let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
     let gpiog = dp.GPIOG.split(ccdr.peripheral.GPIOG);
+    // let mut link_led = gpiob.pb0.into_push_pull_output(); // LED1, green
+    // let mut status_led = gpioe.pe1.into_push_pull_output(); // LD2, yellow
+    // let mut listen_led = gpiob.pb14.into_push_pull_output(); // LD3, red
+    // link_led.set_low().ok();
+    // status_led.set_low().ok();
+    // listen_led.set_low().ok();
 
-    // Configure ethernet IO
+	// Setup CDONE for checking
-    {
+	let fpga_cdone = gpiod.pd15.into_pull_up_input();
-        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
+	match fpga_cdone.is_high() {
-    let mut net_interface = {
+		Ok(true)	=> hprintln!("FPGA is ready."),
-        let mac_addr = net::wire::EthernetAddress([0xAC, 0x6F, 0x7A, 0xDE, 0xD6, 0xC8]);
+		Ok(_)		=> hprintln!("FPGA is in reset state."),
-        let (eth_dma, _eth_mac) = unsafe {
+		Err(_)		=> hprintln!("Error: Cannot read C_DONE"),
-            ethernet::ethernet_init(
+    }.unwrap();
-                dp.ETHERNET_MAC,
-                dp.ETHERNET_MTL,
-                dp.ETHERNET_DMA,
-                &mut DES_RING,
-                mac_addr.clone(),
-            )
-        };
     
-        unsafe { ethernet::enable_interrupt() }
+    // Setup Urukul
+	/*
+	 *	Using SPI1, AF5
+	 *  SCLK -> PA5
+	 *	MOSI -> PB5
+	 *	MISO -> PA6
+	 *	CS -> 0: PB12, 1: PA15, 2: PC7
+	 */
 
-        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()
-    };
-
-    /*
-     *	Using SPI1, AF5
-     *  SCLK -> PA5
-     *	MOSI -> PB5
-     *	MISO -> PA6
-     *	CS -> 0: PB12, 1: PA15, 2: PC7
-     *  I/O_Update -> PB15
-     */
     let sclk = gpioa.pa5.into_alternate_af5();
     let mosi = gpiob.pb5.into_alternate_af5();
     let miso = gpioa.pa6.into_alternate_af5();
 
+
     let (cs0, cs1, cs2) = (
         gpiob.pb12.into_push_pull_output(),
         gpioa.pa15.into_push_pull_output(),
         gpioc.pc7.into_push_pull_output(),
     );
 
+    /*
+     *	I/O_Update -> PB15
+     */
     let io_update = gpiob.pb15.into_push_pull_output();
 
     let spi = dp.SPI1.spi(
@@ -199,63 +186,201 @@ fn main() -> ! {
         &ccdr.clocks,
     );
 
-    let cpld = CPLD::new(spi, (cs0, cs1, cs2), io_update);
+    let switch = CPLD::new(spi, (cs0, cs1, cs2), io_update);
-    let parts = cpld.split();
+    let parts = switch.split();
-
+    let mut urukul = Urukul::new(
-    let urukul = Urukul::new(
         parts.spi1, parts.spi2, parts.spi3, parts.spi4, parts.spi5, parts.spi6, parts.spi7,
         [25_000_000, 25_000_000, 25_000_000, 25_000_000]
     );
 
-    cp.SCB.enable_icache();
+    // Setup ethernet pins
+    setup_ethernet_pins(
+        gpioa.pa1, gpioa.pa2, gpioc.pc1, gpioa.pa7, gpioc.pc4,
+        gpioc.pc5, gpiog.pg11, gpiog.pg13, gpiob.pb13
+    );
 
-    let mut time: u32 = 0;
+    // Initialise ethernet...
-    let mut next_ms = Instant::now();
+    assert_eq!(ccdr.clocks.hclk().0, 200_000_000); // HCLK 200MHz
+    assert_eq!(ccdr.clocks.pclk1().0, 100_000_000); // PCLK 100MHz
+    assert_eq!(ccdr.clocks.pclk2().0, 100_000_000); // PCLK 100MHz
+    assert_eq!(ccdr.clocks.pclk4().0, 100_000_000); // PCLK 100MHz
 
-    next_ms += 400_00.cycles();
+    let mac_addr = smoltcp::wire::EthernetAddress::from_bytes(&MAC_ADDRESS);
+    let (_eth_dma, mut eth_mac) = unsafe {
+        ethernet::ethernet_init(
+            dp.ETHERNET_MAC,
+            dp.ETHERNET_MTL,
+            dp.ETHERNET_DMA,
+            &mut DES_RING,
+            mac_addr.clone(),
+        )
+    };
+    unsafe {
+        ethernet::enable_interrupt();
+        cp.NVIC.set_priority(stm32::Interrupt::ETH, 196); // Mid prio
+        cortex_m::peripheral::NVIC::unmask(stm32::Interrupt::ETH);
+    }
 
-    let mut socket_set_entries: [_; 8] = Default::default();
+    // ----------------------------------------------------------
-    let mut sockets = net::socket::SocketSet::new(&mut socket_set_entries[..]);
+    // Begin periodic tasks
-    add_socket!(sockets, rx_storage, tx_storage);
 
-    let tcp_stack = NetworkStack::new(&mut net_interface, sockets);
+    systick_init(&mut delay.free(), ccdr.clocks);
-    let mut client = MqttClient::<consts::U256, _>::new(
+    unsafe {
-        IpAddr::V4(Ipv4Addr::new(192, 168, 1, 125)),
+        cp.SCB.shpr[15 - 4].write(128);
-        "nucleo",
+    } // systick exception priority
-        tcp_stack,
+
-    )
+    // ----------------------------------------------------------
-    .unwrap();
+    // Main application loop
+
+    // Setup addresses, maybe not MAC?
+    // MAC is set up in prior
+    let local_addr = IpAddress::v4(192, 168, 1, 200);
+    let mut ip_addrs = [IpCidr::new(local_addr, 24)];
+
+    // let neighbor_cache = NeighborCache::new(BTreeMap::new());
+    let mut neighbor_storage = [None; 16];
+    let neighbor_cache = NeighborCache::new(&mut neighbor_storage[..]);
+
+    // Routes
+    let default_v4_gw = Ipv4Address::new(192, 168, 1, 1);
+    let mut routes_storage = [None; 8];
+    let mut routes = Routes::new(&mut routes_storage[..]);
+    routes.add_default_ipv4_route(default_v4_gw).unwrap();
+
+    // Device? _eth_dma, as it implements phy::device
+    let mut iface = EthernetInterfaceBuilder::new(_eth_dma)
+            .ethernet_addr(mac_addr)
+            .neighbor_cache(neighbor_cache)
+            .ip_addrs(&mut ip_addrs[..])
+            .routes(routes)
+            .finalize();
+
+    // SCPI configs
+    // Device was declared in prior
+
+    // let mut errors = ArrayErrorQueue::<[Error; 10]>::new();
+    // let mut context = Context::new(&mut urukul, &mut errors, TREE);
+
+    // //Response bytebuffer
+    // let mut buf = ArrayVecFormatter::<[u8; 256]>::new();
+
+    // SCPI configs END
+
+    // TCP socket
+    let server_socket = {
+        static mut server_rx_storage :[u8; BUFFER_SIZE] = [0; BUFFER_SIZE];
+        static mut server_tx_storage :[u8; BUFFER_SIZE] = [0; BUFFER_SIZE];
+        let server_rx_buffer = TcpSocketBuffer::new( unsafe { &mut server_rx_storage[..] } );
+        let server_tx_buffer = TcpSocketBuffer::new( unsafe { &mut server_tx_storage[..] } );
+        TcpSocket::new(server_rx_buffer, server_tx_buffer)
+    };
+
+    // Setup a silent socket
+    let client_socket = {
+        static mut client_rx_storage :[u8; BUFFER_SIZE] = [0; BUFFER_SIZE];
+        static mut client_tx_storage :[u8; BUFFER_SIZE] = [0; BUFFER_SIZE];
+        let client_rx_buffer = TcpSocketBuffer::new( unsafe { &mut client_rx_storage[..] } );
+        let client_tx_buffer = TcpSocketBuffer::new( unsafe { &mut client_tx_storage[..] } );
+        TcpSocket::new(client_rx_buffer, client_tx_buffer)
+    };
+
+    // Socket storage
+    let mut sockets_storage: [_; 2] = Default::default();
+
+    let mut socket_set = SocketSet::new(&mut sockets_storage[..]);
+    let server_handle = socket_set.add(server_socket);
+    let client_handle = socket_set.add(client_socket);
 
     loop {
-        let tick = Instant::now() > next_ms;
+        let _time = TIME.load(Ordering::Relaxed);
+        match iface.poll(&mut socket_set, Instant::from_millis(_time as i64)) {
+            Ok(_) => {
+                // hprintln!("Ethernet up").unwrap();
+            },
+            Err(e) => {
+                hprintln!("Ethernet down!").unwrap();
+            },
+        };
 
-        if tick {
+	    // Conenct to TCP server through port 49500
-            next_ms += 400_000.cycles();
+        {
-            time += 1;
+            let mut socket = socket_set.get::<TcpSocket>(server_handle);
+            if !socket.is_active() && !socket.is_listening(){
+                socket.listen(7777).unwrap();
+                hprintln!("Server listening").unwrap();
+            }
+
+            // hprintln!("listener state :{}", socket.state()).unwrap();
+
+            if socket.can_recv() {
+                hprintln!("{:?}", str::from_utf8(socket.recv(|data| {
+                    (data.len(), data)
+                }).unwrap()).unwrap()).unwrap();
+            }
         }
 
-        if tick && (time % 1000) == 0 {
+        {
-            client
+            let mut socket = socket_set.get::<TcpSocket>(client_handle);
-                .publish("nucleo", "Hello, World!".as_bytes(), QoS::AtMostOnce, &[])
+            if !socket.is_open() || !socket.can_send() {
-                .unwrap();
+                socket.abort();
+                socket.close();
+                // hprintln!("reset state: {}", socket.state()).unwrap();
+                socket.connect((IpAddress::v4(192, 168, 1, 200), 1883),
+                (IpAddress::Unspecified, 45000)).unwrap();
+                // hprintln!("post connect state: {}", socket.state()).unwrap();
+            }
+
+            // hprintln!("client state: {}", socket.state()).unwrap();
+
+            if socket.can_send() {
+                socket.send_slice(b"regards from socket").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 {
-            //cortex_m::asm::wfi();
-            cortex_m::asm::nop();
-        }
-
-        match client.is_connected() {
-            true => hprintln!("Connected"),
-            false => hprintln!("No Connection"),
-        }.unwrap();
     }
 }
+
+use stm32h7xx_hal::gpio::{
+    gpioa::{PA1, PA2, PA7},
+    gpiob::{PB13},
+    gpioc::{PC1, PC4, PC5},
+    gpiog::{PG11, PG13},
+    Speed::VeryHigh,
+};
+
+/*
+ *  Migrated ethernet setup pins
+ */
+pub fn setup_ethernet_pins<REF_CLK, MDIO, MDC, CRS_DV, RXD0, RXD1, TX_EN, TXD0, TXD1>(
+    pa1: PA1<REF_CLK>, pa2: PA2<MDIO>, pc1: PC1<MDC>, pa7: PA7<CRS_DV>, pc4: PC4<RXD0>,
+    pc5: PC5<RXD1>, pg11: PG11<TX_EN>, pg13: PG13<TXD0>, pb13: PB13<TXD1>
+) {
+    pa1.into_alternate_af11().set_speed(VeryHigh);
+    pa2.into_alternate_af11().set_speed(VeryHigh);
+    pc1.into_alternate_af11().set_speed(VeryHigh);
+    pa7.into_alternate_af11().set_speed(VeryHigh);
+    pc4.into_alternate_af11().set_speed(VeryHigh);
+    pc5.into_alternate_af11().set_speed(VeryHigh);
+    pg11.into_alternate_af11().set_speed(VeryHigh);
+    pg13.into_alternate_af11().set_speed(VeryHigh);
+    pb13.into_alternate_af11().set_speed(VeryHigh);
+}
+
+#[interrupt]
+fn ETH() {
+    unsafe { ethernet::interrupt_handler() }
+}
+
+#[exception]
+fn SysTick() {
+    TIME.fetch_add(1, Ordering::Relaxed);
+}
+
+#[exception]
+fn HardFault(ef: &cortex_m_rt::ExceptionFrame) -> ! {
+    panic!("HardFault at {:#?}", ef);
+}
+
+#[exception]
+fn DefaultHandler(irqn: i16) {
+    panic!("Unhandled exception (IRQn = {})", irqn);
+}

src/lib.rs

@@ -53,6 +53,7 @@ pub enum Error<E> {
 	DDSError,
 	ConfigRegisterError,
 	DDSCLKError,
+	ParameterError,
 }
 
 /*
@@ -142,7 +143,8 @@ where
 
 pub trait UrukulTraits {
 	type Error;
-	fn switch(&mut self, channel: u32) -> Result<(), Self::Error>;
+	fn get_channel_switch_status(&mut self, channel: u32) -> Result<bool, Self::Error>;
+	fn set_channel_switch(&mut self, channel: u32, status: bool) -> Result<(), Self::Error>;
 }
 
 impl<SPI, E> UrukulTraits for Urukul<SPI>
@@ -151,20 +153,52 @@ where
 {
 	type Error = Error<E>;
 
-	fn switch(&mut self, channel: u32) -> Result<(), Self::Error>{
+	fn get_channel_switch_status(&mut self, channel: u32) -> Result<bool, Self::Error> {
-		if channel < 16 {
+		if channel < 4 {
-			let switch_set = channel | u32::from(self.config_register.get_status(StatusMask::RF_SW)?);
+			match self.config_register.get_status(StatusMask::RF_SW) {
+				Ok(val) => Ok((val & (1 << channel)) != 0),
+				Err(_e) => Err(_e),
+			}
+		} else {
+			Err(Error::ParameterError)
+		}
+	}
 
+	fn set_channel_switch(&mut self, channel: u32, status: bool) -> Result<(), Self::Error> {
+		if channel < 4 {
+			let prev = u32::from(self.config_register.get_status(StatusMask::RF_SW)?);
+			let next = {
+				if status {
+					prev | (1 << channel)
+				} else {
+					prev & (!(1 << channel))
+				}
+			};
 			match self.config_register.set_configurations(&mut [
-				(CFGMask::RF_SW, switch_set),
+				(CFGMask::RF_SW, next),
 			]) {
 				Ok(_) => Ok(()),
 				Err(_e) => Err(_e),
 			}
 		} else {
-			Err(Error::ConfigRegisterError)
+			Err(Error::ParameterError)
 		}
 	}
+
+	// fn switch_on(&mut self, channel: u32) -> Result<(), Self::Error>{
+	// 	if channel < 16 {
+	// 		let switch_set = channel | u32::from(self.config_register.get_status(StatusMask::RF_SW)?);
+
+	// 		match self.config_register.set_configurations(&mut [
+	// 			(CFGMask::RF_SW, switch_set),
+	// 		]) {
+	// 			Ok(_) => Ok(()),
+	// 			Err(_e) => Err(_e),
+	// 		}
+	// 	} else {
+	// 		Err(Error::ParameterError)
+	// 	}
+	// }
 }
 
 // /*

src/nal_tcp_client.rs

@@ -21,6 +21,7 @@ pub enum NetworkError {
 pub struct NetStorage {
     pub ip_addrs: [net::wire::IpCidr; 1],
     pub neighbor_cache: [Option<(net::wire::IpAddress, net::iface::Neighbor)>; 8],
+    pub routes_cache: [Option<(net::wire::IpCidr, net::iface::Route)>; 8],
 }
 
 pub type NetworkInterface =

src/scpi.rs

@@ -31,7 +31,7 @@ use scpi::{
 use embedded_hal::blocking::spi::Transfer;
 use cortex_m_semihosting::hprintln;
 
-use crate::{Urukul, UrukulTraits};
+use crate::{Urukul, UrukulTraits, Error as UrukulError};
 
 // pub struct MyDevice;
 
@@ -49,39 +49,38 @@ impl<T: Device> Command<T> for HelloWorldCommand {
     }
 }
 
-pub struct SwitchOnCommand {}
+pub struct Channel1SwitchCommand {}
-impl<T: Device + UrukulTraits> Command<T> for SwitchOnCommand {
+impl<T: Device + UrukulTraits> Command<T> for Channel1SwitchCommand {
     nquery!();
 
     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
-        let mut channel :u32 = 0;
+        match context.device.get_channel_switch_status(1) {
-        let mut channel_index = 0;
+            Ok(status) => {
-        while true {
+                let next_state: bool = match args.next_data(true) {
-            match args.next_data(channel_index != 0) {
+                    Ok(Some(token)) => token.try_into()?,
-                Ok(Some(token)) => {
+                    Ok(None) => !status,
-                    match u32::try_from(token) {
+                    Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
-                        Ok(shift) => {
+                };
-                            if shift >= 4 {
+                match context.device.set_channel_switch(1, next_state) {
-                                return Err(ErrorCode::IllegalParameterValue.into());
+                    Ok(()) => Ok(()),
-                            }
+                    Err(T) => panic!("Switched an illegal channel!"),
-                            channel |= (1 << shift);
+                    Err(_) => Err(Error::new(ErrorCode::HardwareError)),
-                        },
+                }
-                        Err(_) => return Err(ErrorCode::IllegalParameterValue.into()),
+            },
-                    }
-                },
-                Ok(None) => break,
-                Ok(_) => return Err(ErrorCode::MissingParameter.into()),
-                Err(_e) => return Err(_e),
-            };
-            channel_index += 1;
-        }
-        match context.device.switch(channel) {
-            Ok(()) => Ok(()),
             Err(_) => Err(Error::new(ErrorCode::HardwareError)),
         }
     }
 }
 
+// pub struct SwitchCnannelCommand {}
+// impl<T: Device + UrukulTraits> Command<T> for SwitchChannelCommand {
+//     nquery!();
+
+//     fn event(&self, context: &mut Context<T>, args: &mut Tokenizer) -> Result<()> {
+        
+//     }
+// }
+
 /*
  *  Implement "Device" trait from SCPI
  *  TODO: Implement mandatory commands