diff --git a/examples/ethernet.rs b/examples/ethernet.rs deleted file mode 100644 index 2d36136..0000000 --- a/examples/ethernet.rs +++ /dev/null @@ -1,398 +0,0 @@ -#![no_main] -#![no_std] - -use core::sync::atomic::{AtomicU32, Ordering}; - -#[macro_use] -extern crate log; - -use cortex_m; -use cortex_m_rt::{ - entry, - exception, -}; -extern crate smoltcp; - -use stm32h7xx_hal::ethernet; -use stm32h7xx_hal::hal::digital::v2::InputPin; -use stm32h7xx_hal::rcc::CoreClocks; -use stm32h7xx_hal::{pac, prelude::*, spi, stm32, stm32::interrupt}; - -use core::{ - str, - fmt::Write -}; -use core::mem::uninitialized; - -// Exception: no phy::wait -//use smoltcp::phy::wait as phy_wait; -use smoltcp::wire::{IpAddress, IpCidr}; -use smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder}; -use smoltcp::socket::SocketSet; -use smoltcp::socket::{SocketHandle, TcpSocket, TcpSocketBuffer}; -use smoltcp::time::{Duration, Instant}; -// use smoltcp::log; - -// Use embedded-nal to access smoltcp -use embedded_nal::TcpStack; - -use firmware; -use firmware::{ - cpld::{ - CPLD, - }, - scpi::{ - HelloWorldCommand, - Channel0SwitchCommand, - Channel1SwitchCommand, - Channel2SwitchCommand, - Channel3SwitchCommand, - Channel0SystemClockCommand, - Channel0AttenuationCommand, - Channel1AttenuationCommand, - Channel2AttenuationCommand, - Channel3AttenuationCommand, - ClockSourceCommand, - ClockDivisionCommand, - ProfileCommand, - Channel0Profile0SingletoneCommand, - Channel0Profile0SingletoneFrequencyCommand, - Channel0Profile0SingletonePhaseCommand, - Channel0Profile0SingletoneAmplitudeCommand - }, - Urukul, scpi_root, recursive_scpi_tree, scpi_tree -}; -use scpi::prelude::*; -use scpi::ieee488::commands::*; -use scpi::scpi::commands::*; -use scpi::{ - ieee488_cls, - ieee488_ese, - ieee488_esr, - ieee488_idn, - ieee488_opc, - ieee488_rst, - ieee488_sre, - ieee488_stb, - ieee488_tst, - ieee488_wai, - scpi_crate_version, - scpi_status, - scpi_system, -}; - -#[path = "util/logger.rs"] -mod logger; - -/// 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(); - -// Theoratical maximum number of socket that can be handled -const SOCKET_COUNT: usize = 2; - -// Give buffer sizes of transmitting and receiving TCP packets -const BUFFER_SIZE: usize = 2048; - -// the program entry point -#[entry] -fn main() -> ! { - - // logger::semihosting_init(); - - let mut cp = cortex_m::Peripherals::take().unwrap(); - let dp = pac::Peripherals::take().unwrap(); - - // Initialise power... - let pwr = dp.PWR.constrain(); - let vos = pwr.freeze(); - - // Initialise SRAM3 - dp.RCC.ahb2enr.modify(|_, w| w.sram3en().set_bit()); - - // Initialise clocks... - let rcc = dp.RCC.constrain(); - let ccdr = rcc - .use_hse(8.mhz()) - .sys_ck(200.mhz()) - .hclk(200.mhz()) - .pll1_r_ck(400.mhz()) // for TRACECK - .pll1_q_ck(48.mhz()) // for SPI - .freeze(vos, &dp.SYSCFG); - - unsafe { - logger::enable_itm(&dp.DBGMCU, &mut cp.DCB, &mut cp.ITM); - } - - // Get the delay provider. - let delay = cp.SYST.delay(ccdr.clocks); - - // Initialise system... - cp.SCB.invalidate_icache(); - cp.SCB.enable_icache(); - 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 gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE); - 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); - - logger::init(); - - // Setup CDONE for checking - let fpga_cdone = gpiod.pd15.into_pull_up_input(); - - match fpga_cdone.is_high() { - Ok(true) => debug!("FPGA is ready."), - Ok(_) => debug!("FPGA is in reset state."), - Err(_) => debug!("Error: Cannot read C_DONE"), - }; - - // Setup Urukul - /* - * Using SPI1, AF5 - * SCLK -> PA5 - * MOSI -> PB5 - * MISO -> PA6 - * CS -> 0: PB12, 1: PA15, 2: PC7 - */ - - 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( - (sclk, miso, mosi), - spi::MODE_0, - 3.mhz(), - ccdr.peripheral.SPI1, - &ccdr.clocks, - ); - - let switch = CPLD::new(spi, (cs0, cs1, cs2), io_update); - let parts = switch.split(); - let mut urukul = Urukul::new( - parts.spi1, parts.spi2, parts.spi3, parts.spi4, parts.spi5, parts.spi6, parts.spi7 - ); - - // Setup ethernet pins - setup_ethernet_pins( - gpioa.pa1, gpioa.pa2, gpioc.pc1, gpioa.pa7, gpioc.pc4, - gpioc.pc5, gpiog.pg11, gpiog.pg13, gpiob.pb13 - ); - - // Initialise ethernet... - 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 - - let mac_addr = smoltcp::wire::EthernetAddress::from_bytes(&MAC_ADDRESS); - let (_eth_dma, mut eth_mac) = unsafe { - ethernet::new_unchecked( - 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); - } - - // ---------------------------------------------------------- - // Begin periodic tasks - - systick_init(&mut delay.free(), ccdr.clocks); - unsafe { - cp.SCB.shpr[15 - 4].write(128); - } // systick exception priority - - // ---------------------------------------------------------- - // 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[..]); - - // 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[..]) - .finalize(); - - // SCPI configs: migrated to scpi.rs, not meant to be touched - let tree = scpi_tree!(); - - // 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 buffers - let mut rx_storage = [0; BUFFER_SIZE]; - let mut tx_storage = [0; BUFFER_SIZE]; - - // Setup TCP sockets - let tcp1_rx_buffer = TcpSocketBuffer::new(&mut rx_storage[..]); - let tcp1_tx_buffer = TcpSocketBuffer::new(&mut tx_storage[..]); - let mut tcp1_socket = TcpSocket::new(tcp1_rx_buffer, tcp1_tx_buffer); - - // Setup a silent socket - let mut silent_rx_storage = [0; BUFFER_SIZE]; - let mut silent_tx_storage = [0; BUFFER_SIZE]; - let silent_rx_buffer = TcpSocketBuffer::new(&mut silent_rx_storage[..]); - let silent_tx_buffer = TcpSocketBuffer::new(&mut silent_tx_storage[..]); - let mut silent_socket = TcpSocket::new(silent_rx_buffer, silent_tx_buffer); - - // Socket storage - let mut sockets_storage = [ None, None ]; - - let mut sockets = SocketSet::new(&mut sockets_storage[..]); - let tcp1_handle = sockets.add(tcp1_socket); - let silent_handle = sockets.add(silent_socket); - let mut handles: [SocketHandle; SOCKET_COUNT] = unsafe { - uninitialized() - }; - - let mut eth_up = false; - - loop { - let _time = TIME.load(Ordering::Relaxed); - let eth_last = eth_up; - match iface.poll(&mut sockets, Instant::from_millis(_time as i64)) { - Ok(_) => { - eth_up = true; - }, - Err(e) => { - eth_up = false; - }, - }; - - // SCPI interaction socket (:7000) - { - let mut socket = sockets.get::(silent_handle); - if !socket.is_open() { - socket.listen(7000).unwrap(); - socket.set_timeout(Some(Duration::from_millis(1000000))); - } - - if socket.can_recv() { - let data = socket.recv(|buffer| { - (buffer.len(), buffer) - }).unwrap(); - if str::from_utf8(data).unwrap().trim() == "quit" { - socket.close(); - socket.abort(); - continue; - } - let result = context.run(data, &mut buf); - if let Err(err) = result { - writeln!(socket, "{}", str::from_utf8(err.get_message()).unwrap()).unwrap(); - } else { - write!(socket, "{}", str::from_utf8(buf.as_slice()).unwrap()).unwrap(); - } - } - } - } -} - -use stm32h7xx_hal::gpio::{ - gpioa::{PA1, PA2, PA7}, - gpiob::{PB13}, - gpioc::{PC1, PC4, PC5}, - gpiog::{PG11, PG13}, - Speed::VeryHigh, -}; - -/* - * Migrated ethernet setup pins - */ -#[allow(non_camel_case_types)] -pub fn setup_ethernet_pins( - pa1: PA1, pa2: PA2, pc1: PC1, pa7: PA7, pc4: PC4, - pc5: PC5, pg11: PG11, pg13: PG13, pb13: PB13 -) { - 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); -} diff --git a/examples/fpga_config.rs b/examples/fpga_config.rs deleted file mode 100644 index 91b72d3..0000000 --- a/examples/fpga_config.rs +++ /dev/null @@ -1,79 +0,0 @@ -#![no_main] -#![no_std] - -extern crate log; -use log::debug; -use stm32h7xx_hal::{pac, prelude::*, spi}; - -use cortex_m; -use cortex_m::asm::nop; -use cortex_m_rt::entry; - -use firmware::flash::flash_ice40_fpga; - -#[path = "util/logger.rs"] -mod logger; - -#[entry] -fn main() -> ! { - - let mut cp = cortex_m::Peripherals::take().unwrap(); - let dp = pac::Peripherals::take().unwrap(); - - let pwr = dp.PWR.constrain(); - let vos = pwr.freeze(); - - let rcc = dp.RCC.constrain(); - let ccdr = rcc - .sys_ck(400.mhz()) - .pll1_q_ck(48.mhz()) - .pll1_r_ck(400.mhz()) // for TRACECK - .freeze(vos, &dp.SYSCFG); - - unsafe { - logger::enable_itm(&dp.DBGMCU, &mut cp.DCB, &mut cp.ITM); - } - - let delay = cp.SYST.delay(ccdr.clocks); - - let gpioa = dp.GPIOA.split(ccdr.peripheral.GPIOA); - let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB); - let gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD); - let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF); - - // gpiob.pb3.into_alternate_af0().set_speed(Speed::VeryHigh); - - logger::init(); - - debug!("Flashing configuration bitstream to iCE40 HX8K on Humpback."); - - // Using SPI_1 alternate functions (af5) - let fpga_sck = gpiob.pb3.into_alternate_af5(); - let fpga_sdo = gpiob.pb4.into_alternate_af5(); - let fpga_sdi = gpiob.pb5.into_alternate_af5(); - - // Setup SPI_SS_B and CRESET_B - let fpga_ss = gpioa.pa4.into_push_pull_output(); - let fpga_creset = gpiof.pf3.into_open_drain_output(); - - // Setup CDONE - let fpga_cdone = gpiod.pd15.into_pull_up_input(); - - // Setup SPI interface - let fpga_cfg_spi = dp.SPI1.spi( - (fpga_sck, fpga_sdo, fpga_sdi), - spi::MODE_3, - 12.mhz(), - ccdr.peripheral.SPI1, - &ccdr.clocks, - ); - - // Pre-load the configuration bytes - let config_data = include_bytes!("../build/top.bin"); - - flash_ice40_fpga(fpga_cfg_spi, fpga_ss, fpga_creset, fpga_cdone, delay, config_data).unwrap(); - - loop { - nop(); - } -} diff --git a/examples/mqtt_client.rs b/examples/mqtt_client.rs deleted file mode 100644 index 038ab67..0000000 --- a/examples/mqtt_client.rs +++ /dev/null @@ -1,249 +0,0 @@ -#![no_std] -#![no_main] - -use log::{info, warn}; - -use smoltcp as net; -use stm32h7xx_hal::ethernet; -use stm32h7xx_hal::{gpio::Speed, prelude::*, spi, pac}; - -use heapless::consts; - -use cortex_m; -use cortex_m_rt::entry; -use rtic::cyccnt::{Instant, U32Ext}; - -use minimq::{ - embedded_nal::{IpAddr, Ipv4Addr, TcpStack}, - MqttClient, QoS -}; - -use firmware::nal_tcp_client::{NetworkStack, NetStorage}; -use firmware::{ - cpld::{ - CPLD, - }, -}; -use firmware::Urukul; -use firmware::mqtt_mux::MqttMux; - -#[path = "util/logger.rs"] -mod logger; - -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()); - // Reset RCC clock - dp.RCC.rsr.write(|w| w.rmvf().set_bit()); - - let pwr = dp.PWR.constrain(); - let vos = pwr.freeze(); - - let rcc = dp.RCC.constrain(); - let ccdr = rcc - .use_hse(8.mhz()) - .sysclk(400.mhz()) - .hclk(200.mhz()) - .pll1_q_ck(48.mhz()) // for SPI - .pll1_r_ck(400.mhz()) // for TRACECK - .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); - 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, mut eth_mac) = unsafe { - ethernet::new_unchecked( - 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 mut routes = net::iface::Routes::new(&mut store.routes_cache[..]); - let default_v4_gw = net::wire::Ipv4Address::new(192, 168, 1, 1); - 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 - ); - - cp.SCB.invalidate_icache(); - cp.SCB.enable_icache(); - - let mut mqtt_mux = MqttMux::new(urukul); - - // Time unit in ms - let mut time: u32 = 0; - - // 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(); - 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); - - // Case dealt: Ethernet connection break down, neither side has timeout - // Limitation: Timeout inequality will cause TCP socket state to desync - // Probably fixed in latest smoltcp commit - let mut client = MqttClient::::new( - IpAddr::V4(Ipv4Addr::new(192, 168, 1, 125)), - "Nucleo", - tcp_stack, - ) - .unwrap(); - - let mut tick = false; - let mut has_subscribed = false; - - loop { - // Update time accumulator in ms - // Tick once every ms - if Instant::now() > next_ms { - tick = true; - time += 1; - next_ms += 400_000.cycles(); - } - - // eth Poll if necessary - // Do not poll if eth link is down - if tick && client.network_stack.update_delay(time) == 0 && eth_mac.phy_poll_link() { - client.network_stack.update(time); - } - - // Process MQTT messages about Urukul/Control - let connection = client - .poll(|_client, topic, message, _properties| { - info!("On {:?}, received: {:?}", topic, message); - // Why is topic a string while message is a slice? - mqtt_mux.process_mqtt(topic, message); - }).is_ok(); - - if connection && !has_subscribed && tick { - match client.subscribe("Urukul/Control/#", &[]) { - Ok(()) => has_subscribed = true, - Err(minimq::Error::NotReady) => {}, - e => warn!("{:?}", e), - }; - } - - if connection && tick && (time % 3000) == 0 { - info!("Feedback from print publish: {:?}", client - .publish("Channel1/Switch", "Hello, World!".as_bytes(), QoS::AtMostOnce, &[])); - } - - // Reset tick flag - tick = false; - } -} \ No newline at end of file diff --git a/examples/mqtt_hello_world.rs b/examples/mqtt_hello_world.rs deleted file mode 100644 index 2333af1..0000000 --- a/examples/mqtt_hello_world.rs +++ /dev/null @@ -1,266 +0,0 @@ - -#![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>> = 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>, - } - - #[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::::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 = - // 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() } - } -}; \ No newline at end of file diff --git a/examples/tcp_client.rs b/examples/tcp_client.rs deleted file mode 100644 index 66272ed..0000000 --- a/examples/tcp_client.rs +++ /dev/null @@ -1,292 +0,0 @@ -#![no_std] -#![no_main] - -#[macro_use] -extern crate log; - -#[macro_use] -extern crate lazy_static; - -use smoltcp as net; -use stm32h7xx_hal::ethernet; -use stm32h7xx_hal::{gpio::Speed, prelude::*, spi, pac}; -use embedded_hal::{ - blocking::spi::Transfer, - digital::v2::OutputPin, -}; - -use core::sync::atomic::{AtomicU32, Ordering}; -use core::fmt::Write; -use core::str; - -// use heapless::{consts, String}; - -use cortex_m; -use cortex_m::iprintln; -use cortex_m_rt::{ - entry, - exception, -}; -// use cortex_m_semihosting::hprintln; - -// use panic_halt as _; -use panic_itm as _; - -use rtic::cyccnt::{Instant, U32Ext}; - -use log::info; -use log::debug; -use log::trace; -use log::warn; - -use nb::block; - -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}; - -#[link_section = ".sram3.eth"] -static mut DES_RING: ethernet::DesRing = ethernet::DesRing::new(); - -// Logging setup -#[path = "util/logger.rs"] -mod logger; -#[path = "util/clock.rs"] -mod clock; -// End of logging setup - -// static TIME: AtomicU32 = AtomicU32::new(0); - -#[entry] -fn main() -> ! { - - // logger::semihosting_init(); - let clock = clock::Clock::new(); - - 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()); - // Reset RCC clock - dp.RCC.rsr.write(|w| w.rmvf().set_bit()); - - let pwr = dp.PWR.constrain(); - let vos = pwr.freeze(); - - let rcc = dp.RCC.constrain(); - let ccdr = rcc - .use_hse(8.mhz()) - .sysclk(400.mhz()) - .hclk(200.mhz()) - // .per_ck(100.mhz()) - .pll1_q_ck(48.mhz()) // for SPI - .pll1_r_ck(400.mhz()) // for TRACECK - // .pll2_p_ck(100.mhz()) - // .pll2_q_ck(100.mhz()) - .freeze(vos, &dp.SYSCFG); - - unsafe { - logger::enable_itm(&dp.DBGMCU, &mut cp.DCB, &mut cp.ITM); - } - - let mut delay = cp.SYST.delay(ccdr.clocks); - - 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); - - 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(); - - // gpiob.pb3.into_alternate_af0().set_speed(Speed::VeryHigh); - - logger::init(); - - // 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, mut eth_mac) = unsafe { - ethernet::new_unchecked( - dp.ETHERNET_MAC, - dp.ETHERNET_MTL, - dp.ETHERNET_DMA, - &mut DES_RING, - mac_addr.clone(), - ) - }; - - unsafe { ethernet::enable_interrupt() } - - let mut ip_addrs = [net::wire::IpCidr::new(net::wire::IpAddress::v4(192, 168, 1, 200), 24)]; - - let mut neighbor_cache_entries = [None; 8]; - let mut neighbor_cache = net::iface::NeighborCache::new(&mut neighbor_cache_entries[..]); - neighbor_cache.fill( - net::wire::IpAddress::v4(192, 168, 1, 125), - net::wire::EthernetAddress([0x2C, 0xF0, 0x5D, 0x26, 0xB8, 0x2F]), - clock.elapsed(), - ); - - let mut net_interface = net::iface::EthernetInterfaceBuilder::new(eth_dma) - .ethernet_addr(mac_addr) - .neighbor_cache(neighbor_cache) - .ip_addrs(&mut 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(), - ); - - 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.invalidate_icache(); - cp.SCB.enable_icache(); - - // let mut time: u32 = 0; - // let mut next_ms = Instant::now(); - - // next_ms += 400_000.cycles(); - - let mut socket_set_entries: [_; 8] = Default::default(); - let mut sockets = net::socket::SocketSet::new(&mut socket_set_entries[..]); - - 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); - - // delay.delay_ms(2000_u16); - green_led.set_high().unwrap(); - - { - let mut socket = sockets.get::(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!"); - } - - yellow_led.set_low().unwrap(); - red_led.set_high().unwrap(); - - let mut green = true; - let mut connected = false; - - debug!("Poll link status: {}", eth_mac.phy_poll_link()); - 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) => { - debug!("poll error: {}", e); - } - } - - { - let mut socket = sockets.get::(handle); - - info!("Socket state: {} at time {}", socket.state(), clock.elapsed()); - - if socket.may_recv() { - yellow_led.set_high().unwrap(); - red_led.set_low().unwrap(); - let data = socket.recv(|data| { - (data.len(), data) - }).unwrap(); - if socket.can_send() { - socket.send_slice("response".as_bytes()).unwrap(); - } - } - if socket.may_send() { - yellow_led.set_high().unwrap(); - red_led.set_low().unwrap(); - debug!("close"); - socket.close(); - } - } - - 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) => { - green_led.set_low().unwrap(); - delay.delay_ms(time_delay.total_millis() as u32); - green_led.set_high().unwrap(); - clock.advance(time_delay) - }, - None => { - // delay.delay_ms(1_u32); - clock.advance(net::time::Duration::from_millis(1)) - }, - } - } -} diff --git a/examples/tcp_stack.rs b/examples/tcp_stack.rs deleted file mode 100644 index 7fb1295..0000000 --- a/examples/tcp_stack.rs +++ /dev/null @@ -1,180 +0,0 @@ -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>, - next_port: RefCell, - unused_handles: RefCell>, -} - -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 = 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 { - // 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 { - // 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 { - 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 { - // 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 { - // 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(()) - } -} \ No newline at end of file diff --git a/examples/util/clock.rs b/examples/util/clock.rs deleted file mode 100644 index 8064950..0000000 --- a/examples/util/clock.rs +++ /dev/null @@ -1,19 +0,0 @@ -use smoltcp::time::{Duration, Instant}; -use core::cell::Cell; - -#[derive(Debug)] -pub struct Clock(Cell); - -impl Clock { - pub fn new() -> Clock { - Clock(Cell::new(Instant::from_millis(0))) - } - - pub fn advance(&self, duration: Duration) { - self.0.set(self.0.get() + duration) - } - - pub fn elapsed(&self) -> Instant { - self.0.get() - } -} \ No newline at end of file diff --git a/examples/util/logger.rs b/examples/util/logger.rs deleted file mode 100644 index 76e1784..0000000 --- a/examples/util/logger.rs +++ /dev/null @@ -1,78 +0,0 @@ -// Enables ITM -pub unsafe fn enable_itm( - dbgmcu: &stm32h7xx_hal::stm32::DBGMCU, - dcb: &mut cortex_m::peripheral::DCB, - itm: &mut cortex_m::peripheral::ITM -) { - // ARMv7-M DEMCR: Set TRCENA. Enables DWT and ITM units - //unsafe { *(0xE000_EDFC as *mut u32) |= 1 << 24 }; - dcb.enable_trace(); - - // Ensure debug blocks are clocked before interacting with them - dbgmcu.cr.modify(|_, w| { - w.d1dbgcken() - .set_bit() - .d3dbgcken() - .set_bit() - .traceclken() - .set_bit() - .dbgsleep_d1() - .set_bit() - }); - - // SWO: Unlock - *(0x5c00_3fb0 as *mut u32) = 0xC5ACCE55; - // SWTF: Unlock - *(0x5c00_4fb0 as *mut u32) = 0xC5ACCE55; - - // SWO CODR Register: Set SWO speed - *(0x5c00_3010 as *mut _) = 200; - - // SWO SPPR Register: - // 1 = Manchester - // 2 = NRZ - *(0x5c00_30f0 as *mut _) = 2; - - // SWTF Trace Funnel: Enable for CM7 - *(0x5c00_4000 as *mut u32) |= 1; - - // ITM: Unlock - itm.lar.write(0xC5ACCE55); - // ITM Trace Enable Register: Enable lower 8 stimulus ports - itm.ter[0].write(1); - // ITM Trace Control Register: Enable ITM - itm.tcr.write( - (0b000001 << 16) | // TraceBusID - (1 << 3) | // enable SWO output - (1 << 0), // enable the ITM - ); -} - -use panic_itm as _; - -use lazy_static::lazy_static; -use log::LevelFilter; - -pub use cortex_m_log::log::Logger; - -use cortex_m_log::{ - destination::Itm as ItmDest, - printer::itm::InterruptSync, - modes::InterruptFree, - printer::itm::ItmSync -}; - -lazy_static! { - static ref LOGGER: Logger> = Logger { - level: LevelFilter::Trace, - inner: unsafe { - InterruptSync::new( - ItmDest::new(cortex_m::Peripherals::steal().ITM) - ) - }, - }; -} - -pub fn init() { - cortex_m_log::log::init(&LOGGER).unwrap(); -} \ No newline at end of file