diff --git a/.cargo/config b/.cargo/config
new file mode 100644
index 0000000..86b3292
--- /dev/null
+++ b/.cargo/config
@@ -0,0 +1,8 @@
+[target.thumbv7em-none-eabihf]
+runner = "gdb -q -x gdb_config/openocd.gdb"
+rustflags = [
+  "-C", "link-arg=-Tlink.x",
+]
+
+[build]
+target = "thumbv7em-none-eabihf"
diff --git a/Cargo.toml b/Cargo.toml
new file mode 100644
index 0000000..3990888
--- /dev/null
+++ b/Cargo.toml
@@ -0,0 +1,44 @@
+[package]
+authors = ["occheung"]
+edition = "2018"
+readme = "README.md"
+name = "firmware-dev"
+version = "0.1.0"
+
+[dependencies]
+cortex-m-semihosting = "0.3.3"
+panic-halt = "0.2.0"
+cortex-m = "0.6.2"
+cortex-m-rt = "0.6.12"
+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" ] }
+xca9548a = "0.2.0"
+lm75 = "0.1.1"
+nb = "1.0.0"
+
+# 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", features = [ "itm" ] }
+
+[[example]]
+name = "ethernet"
+
+[[example]]
+name = "fpga_config"
+
+# Uncomment for the allocator example.
+# alloc-cortex-m = "0.3.5"
+
+# this lets you use `cargo fix`!
+[[bin]]
+name = "firmware-dev"
+test = false
+bench = false
+
+[profile.release]
+codegen-units = 1 # better optimizations
+debug = true # symbols are nice and they don't increase the size on Flash
+lto = true # better optimizations
diff --git a/examples/ethernet.rs b/examples/ethernet.rs
new file mode 100644
index 0000000..b2b2cb4
--- /dev/null
+++ b/examples/ethernet.rs
@@ -0,0 +1,325 @@
+#![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;
+extern crate stm32h7_ethernet as ethernet;
+
+use stm32h7xx_hal::gpio::Speed;
+use stm32h7xx_hal::hal::digital::v2::OutputPin;
+use stm32h7xx_hal::rcc::CoreClocks;
+use stm32h7xx_hal::{pac, prelude::*, stm32, stm32::interrupt};
+use Speed::*;
+
+/*
+#[cfg(feature = "itm")]
+use cortex_m_log::log::{trick_init, Logger};
+
+#[cfg(feature = "itm")]
+use cortex_m_log::{
+    destination::Itm, printer::itm::InterruptSync as InterruptSyncItm,
+};
+*/
+
+use core::{
+    str,
+    fmt::Write
+};
+use core::mem::uninitialized;
+
+// Exception: no phy::wait
+//use smoltcp::phy::wait as phy_wait;
+use smoltcp::wire::{EthernetAddress, IpAddress, IpCidr};
+use smoltcp::iface::{NeighborCache, EthernetInterfaceBuilder};
+use smoltcp::socket::SocketSet;
+//use smoltcp::socket::{UdpSocket, UdpSocketBuffer, UdpPacketMetadata};
+use smoltcp::socket::{SocketHandle, TcpSocket, TcpSocketBuffer};
+use smoltcp::time::{Duration, Instant};
+
+
+/// 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 TCP_RX_BUFFER_SIZE: usize = 2048;
+const TCP_TX_BUFFER_SIZE: usize = 2048;
+
+// the program entry point
+#[entry]
+fn main() -> ! {
+
+    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
+        .sys_ck(200.mhz())
+        .hclk(200.mhz())
+        .pll1_r_ck(100.mhz()) // for TRACECK
+        .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 gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
+    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();
+
+    let _rmii_ref_clk = gpioa.pa1.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_mdio = gpioa.pa2.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_mdc = gpioc.pc1.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_crs_dv = gpioa.pa7.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_rxd0 = gpioc.pc4.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_rxd1 = gpioc.pc5.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_tx_en = gpiog.pg11.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_txd0 = gpiog.pg13.into_alternate_af11().set_speed(VeryHigh);
+    let _rmii_txd1 = gpiob.pb13.into_alternate_af11().set_speed(VeryHigh);
+
+    // 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::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);
+    }
+
+    // ----------------------------------------------------------
+    // 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();
+
+    // TODO: Need Iinitialize TCP socket storage?
+    // Yes cannot into vectors
+    let mut rx_storage = [0; TCP_RX_BUFFER_SIZE];
+    let mut tx_storage = [0; TCP_TX_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; TCP_RX_BUFFER_SIZE];
+    let mut silent_tx_storage = [0; TCP_TX_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;
+    let mut receive_and_not_send = true;
+    let mut counter = 0;
+
+    // Record activeness of silent socket, init as false
+    let mut silent_socket_active = 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;
+                link_led.set_high().unwrap();
+            },
+            Err(e) => {
+                eth_up = false;
+                link_led.set_low().unwrap();
+            },
+        };
+
+	// Counting socket (:6969)
+        {
+            let mut socket = sockets.get::<TcpSocket>(tcp1_handle);
+            if !socket.is_open() {
+                socket.listen(6969).unwrap();
+                socket.set_timeout(Some(Duration::from_millis(5000)));
+            }
+
+            match socket.is_listening() {
+                false => listen_led.set_low().unwrap(),
+                _ => listen_led.set_high().unwrap(),
+            };
+
+            match socket.is_active() {
+                true => status_led.set_high().unwrap(),
+                _ => status_led.set_low().unwrap(),
+            };
+
+            if socket.can_recv() && receive_and_not_send {
+				hprintln!("recv 6969");
+                let data = socket.recv(|buffer| {
+                    counter += buffer.len();
+                    (buffer.len(), buffer)
+                });
+				hprintln!("{:?}", data);
+       	        receive_and_not_send = false;
+            }
+		    else if socket.can_recv() {
+				hprintln!("{:?}", socket.can_recv());
+		    }
+
+            if socket.can_send() && !receive_and_not_send {
+                writeln!(socket, "{}", counter);
+                receive_and_not_send = true;
+            }
+        }
+		// Silent socket (:7000)
+		{
+		    let mut socket = sockets.get::<TcpSocket>(silent_handle);
+		    if !socket.is_open() {
+				socket.listen(7000).unwrap();
+				socket.set_timeout(Some(Duration::from_millis(1000000)));
+		    }
+
+		    if socket.is_active() && !silent_socket_active {
+				hprintln!("tcp:7000 connected").unwrap();
+		    }
+		    else if !socket.is_active() && silent_socket_active {
+				hprintln!("tcp:7000 disconnected").unwrap();
+				socket.close();
+		    }
+		    // Update socket activeness
+		    silent_socket_active = socket.is_active();
+		
+
+		    if socket.can_recv() {
+		//		hprintln!("About to recv").unwrap();
+				hprintln!("{:?}", socket.recv(|buffer| {
+				    (buffer.len(), str::from_utf8(buffer).unwrap())
+				})).unwrap();
+		    }
+		}
+    }
+}
+
+#[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
new file mode 100644
index 0000000..a114174
--- /dev/null
+++ b/examples/fpga_config.rs
@@ -0,0 +1,132 @@
+#![no_main]
+#![no_std]
+
+use panic_semihosting as _;
+
+use stm32h7xx_hal::hal::digital::v2::{
+	InputPin,
+	OutputPin,
+};
+use stm32h7xx_hal::{pac, prelude::*, spi};
+
+use cortex_m;
+use cortex_m::asm::nop;
+use cortex_m_rt::entry;
+use cortex_m_semihosting::hprintln;
+
+use core::ptr;
+use nb::block;
+
+
+#[entry]
+fn main() -> ! {
+
+	hprintln!("Flashing configuration bitstream to iCE40 HX8K on Humpback.").unwrap();
+
+	let 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())
+		.freeze(vos, &dp.SYSCFG);
+	
+	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 gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
+	let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
+
+	// 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 mut fpga_ss = gpioa.pa4.into_push_pull_output();
+	let mut fpga_creset = gpiof.pf3.into_open_drain_output();
+
+	// Setup CDONE
+	let fpga_cdone = gpiod.pd15.into_pull_up_input();
+
+	// Setup SPI interface
+	let mut fpga_cfg_spi = dp.SPI1.spi(
+		(fpga_sck, fpga_sdo, fpga_sdi),
+		spi::MODE_3,
+		12.mhz(),
+		ccdr.peripheral.SPI1,
+		&ccdr.clocks,
+	);
+
+	// Data buffer setup
+	let mut dummy_byte :[u8; 1] = [0x00];
+	let mut dummy_13_bytes :[u8; 13] = [0x00; 13];
+
+	// Drive CRESET_B low
+	fpga_creset.set_low().unwrap();
+
+	// Drive SPI_SS_B low
+	fpga_ss.set_low().unwrap();
+
+	// Wait at least 200ns
+	delay.delay_us(1_u16);
+
+	// Drive CRESET_B high
+	fpga_creset.set_high().unwrap();
+
+	// Wait at least another 1200us to clear internal config memory
+	delay.delay_us(1200_u16);
+
+	// Before data transmission starts, check if C_DONE is truly dine
+	match fpga_cdone.is_high() {
+		Ok(false)	=> hprintln!("Reset successful!"),
+		Ok(_) 		=> hprintln!("Reset unsuccessful!"),
+		Err(_) 		=> hprintln!("Reset error!"),
+	}.unwrap();
+
+	// Set SPI_SS_B high
+	fpga_ss.set_high().unwrap();
+
+	// Send 8 dummy clock, effectively 1 byte of 0x00
+	fpga_cfg_spi.transfer(&mut dummy_byte).unwrap();
+
+	// Drive SPI_SS_B low
+	fpga_ss.set_low().unwrap();
+
+	// Send the whole image without interruption
+	let base_address = 0x08100000;
+	let size = 135100;
+	for index in 0..size {
+		unsafe {
+			let data :u8 = ptr::read_volatile((base_address + index) as *const u8);
+			block!(fpga_cfg_spi.send(data)).unwrap();
+			block!(fpga_cfg_spi.read()).unwrap();
+		}
+	}
+
+	// Drive SPI_SS_B high
+	fpga_ss.set_high().unwrap();
+
+	// Send at another 100 dummy clocks (choosing 13 bytes)
+	fpga_cfg_spi.transfer(&mut dummy_13_bytes).unwrap();
+
+	// Check the CDONE output from FPGA
+	if !(fpga_cdone.is_high().unwrap()) {
+		hprintln!("ERROR!").unwrap();
+	}
+	else {
+		hprintln!("Configuration successful!").unwrap();
+		// Send at least another 49 clock cycles to activate IO pins (choosing same 13 bytes)
+		fpga_cfg_spi.transfer(&mut dummy_13_bytes).unwrap();
+		hprintln!("User I/O pins activated.").unwrap();
+	}
+
+	loop {
+		nop();
+	}
+}
diff --git a/gdb_config/debug.gdb b/gdb_config/debug.gdb
new file mode 100644
index 0000000..f16f872
--- /dev/null
+++ b/gdb_config/debug.gdb
@@ -0,0 +1,18 @@
+target remote :3333
+
+# print demangled symbols
+set print asm-demangle on
+
+# set backtrace limit to not have infinite backtrace loops
+set backtrace limit 32
+
+# detect unhandled exceptions, hard faults and panics
+break DefaultHandler
+break HardFault
+break rust_begin_unwind
+
+# print using semihosting, slow af
+monitor arm semihosting enable
+
+# flash the program to STM32
+load
diff --git a/gdb_config/fpga_config.gdb b/gdb_config/fpga_config.gdb
new file mode 100644
index 0000000..65a21ef
--- /dev/null
+++ b/gdb_config/fpga_config.gdb
@@ -0,0 +1,31 @@
+target remote :3333
+
+# print demangled symbols
+set print asm-demangle on
+
+# set backtrace limit to not have infinite backtrace loops
+set backtrace limit 32
+
+# detect unhandled exceptions, hard faults and panics
+break DefaultHandler
+break HardFault
+break rust_begin_unwind
+
+# break at line 130 to auto quit
+break examples/fpga_config.rs:130
+
+# print using semihosting, slow af
+monitor arm semihosting enable
+
+# flash the program to bank 0
+load
+
+# flash the bitstream to bank 1
+mon flash write_bank 1 build/top.bin 0
+
+# just run immediately
+continue
+
+# auto quit when hanged
+detach
+quit
diff --git a/gdb_config/fpga_verify.gdb b/gdb_config/fpga_verify.gdb
new file mode 100644
index 0000000..b4d509d
--- /dev/null
+++ b/gdb_config/fpga_verify.gdb
@@ -0,0 +1,4 @@
+target remote :3333
+dump binary memory mem.bin 0x8100000 0x8120fbc
+detach
+quit
diff --git a/gdb_config/openocd.gdb b/gdb_config/openocd.gdb
new file mode 100644
index 0000000..da33af1
--- /dev/null
+++ b/gdb_config/openocd.gdb
@@ -0,0 +1,27 @@
+target remote :3333
+
+# print demangled symbols
+set print asm-demangle on
+
+# set backtrace limit to not have infinite backtrace loops
+set backtrace limit 32
+
+# detect unhandled exceptions, hard faults and panics
+break DefaultHandler
+break HardFault
+break rust_begin_unwind
+
+# situational break points: only enable for fpga_config example
+# break examples/fpga_config.rs:126
+
+# print using semihosting, slow af
+monitor arm semihosting enable
+
+# flash the program to STM32
+load
+
+# run the code immediately
+continue
+
+# situational exit gdb
+# quit
diff --git a/gdb_config/reset.gdb b/gdb_config/reset.gdb
new file mode 100644
index 0000000..440e441
--- /dev/null
+++ b/gdb_config/reset.gdb
@@ -0,0 +1,2 @@
+target remote :3333
+flash
diff --git a/memory.x b/memory.x
new file mode 100644
index 0000000..ae8742e
--- /dev/null
+++ b/memory.x
@@ -0,0 +1,54 @@
+MEMORY
+{
+  /* FLASH and RAM are mandatory memory regions */
+  FLASH  : ORIGIN = 0x08000000, LENGTH = 1024K
+  FLASH1 : ORIGIN = 0x08100000, LENGTH = 1024K
+  RAM    : ORIGIN = 0x20000000, LENGTH = 128K
+
+  /* AXISRAM */
+  AXISRAM : ORIGIN = 0x24000000, LENGTH = 512K
+
+  /* SRAM */
+  SRAM1 : ORIGIN = 0x30000000, LENGTH = 128K
+  SRAM2 : ORIGIN = 0x30020000, LENGTH = 128K
+  SRAM3 : ORIGIN = 0x30040000, LENGTH = 32K
+  SRAM4 : ORIGIN = 0x38000000, LENGTH = 64K
+
+  /* Backup SRAM */
+  BSRAM : ORIGIN = 0x38800000, LENGTH = 4K
+
+  /* Instruction TCM */
+  ITCM  : ORIGIN = 0x00000000, LENGTH = 64K
+}
+
+
+/* The location of the stack can be overridden using the
+   `_stack_start` symbol.  Place the stack at the end of RAM */
+_stack_start = ORIGIN(RAM) + LENGTH(RAM);
+
+/* The location of the .text section can be overridden using the
+   `_stext` symbol.  By default it will place after .vector_table */
+/* _stext = ORIGIN(FLASH) + 0x40c; */
+
+SECTIONS {
+  .itcm : ALIGN(8) {
+    *(.itcm .itcm.*);
+    . = ALIGN(8);
+    } > ITCM
+  .axisram : ALIGN(8) {
+    *(.axisram .axisram.*);
+    . = ALIGN(8);
+    } > AXISRAM
+  .sram1 (NOLOAD) : ALIGN(4) {
+    *(.sram1 .sram1.*);
+    . = ALIGN(4);
+    } > SRAM1
+  .sram2 (NOLOAD) : ALIGN(4) {
+    *(.sram2 .sram2.*);
+    . = ALIGN(4);
+    } > SRAM2
+  .sram3 (NOLOAD) : ALIGN(4) {
+    *(.sram3 .sram3.*);
+    . = ALIGN(4);
+    } > SRAM3
+} INSERT AFTER .bss;
diff --git a/src/TCA9548ARGER.rs b/src/TCA9548ARGER.rs
new file mode 100644
index 0000000..107e271
--- /dev/null
+++ b/src/TCA9548ARGER.rs
@@ -0,0 +1,112 @@
+#![no_main]
+#![no_std]
+
+extern crate panic_itm;
+
+use stm32h7xx_hal::hal::digital::v2::OutputPin;
+use stm32h7xx_hal::{pac, prelude::*};
+
+use cortex_m_rt::entry;
+
+use cortex_m_log::println;
+use cortex_m_log::{
+    destination::Itm, printer::itm::InterruptSync as InterruptSyncItm,
+};
+
+/*
+ * I2C Address of the I2C switch (TCA9548ARGER)
+ */
+const TCA9548ARGER_ADDR     :u8 = 0x72;
+
+/*
+ * Control register bit masks
+ */
+const CHANNEL_0             :u8 = 0x01;
+const CHANNEL_1             :u8 = 0x02;
+const CHANNEL_2             :u8 = 0x04;
+const CHANNEL_3             :u8 = 0x08;
+const CHANNEL_4             :u8 = 0x10;
+const CHANNEL_5             :u8 = 0x20;
+const CHANNEL_6             :u8 = 0x40;
+const CHANNEL_7             :u8 = 0x80;
+
+/*
+ * I2C Address of slaves at different channels
+ */
+const TEMP_1_ADDR           :u8 = 0x48;
+const TEMP_PRODUCT_ID_REG   :u8 = 0x07;
+
+#[entry]
+fn main() -> ! {
+    let cp = cortex_m::Peripherals::take().unwrap();
+    let dp = pac::Peripherals::take().unwrap();
+    let mut log = InterruptSyncItm::new(Itm::new(cp.ITM));
+
+    // Constrain and Freeze power
+    // println!(log, "Setup PWR...                  ");
+    let pwr = dp.PWR.constrain();
+    let vos = pwr.freeze();
+
+    // Constrain and Freeze clock
+    // println!(log, "Setup RCC...                  ");
+    let rcc = dp.RCC.constrain();
+    let ccdr = rcc.sys_ck(100.mhz()).freeze(vos, &dp.SYSCFG);
+    let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
+    let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
+
+    // Configure the SCL and the SDA pin for our I2C bus
+    let scl = gpiob.pb8.into_alternate_af4().set_open_drain();
+    let sda = gpiob.pb9.into_alternate_af4().set_open_drain();
+
+    let mut i2c =
+        dp.I2C1
+            .i2c((scl, sda), 100.khz(), ccdr.peripheral.I2C1, &ccdr.clocks);
+
+    // Setup delay
+    let mut delay = cp.SYST.delay(ccdr.clocks);
+    
+    // Configure led
+    let mut green = gpiob.pb0.into_push_pull_output();
+    let mut yellow = gpioe.pe1.into_push_pull_output();
+    let mut red = gpiob.pb14.into_push_pull_output();
+
+    // I2C switch (TCA9548ARGER_ADDR): Only enable channel 5
+    let mut tx1 :[u8, 1] = [CHANNEL_5];
+    let mut rx1 :[u8; 1] = [0];
+
+    loop {
+        i2c.write(TCA9548ARGER_ADDR, &tx1);
+        delay.delay_ms(10_u16);
+    }
+
+    // Read back the control value
+    i2c.read(TCA9548ARGER_ADDR, &mut rx1).unwrap();
+
+    // Match the control register content with the CHANNEL_5 mask
+    match rx1[0] {
+        CHANNEL_5 => yellow.set_high(),
+        _ => yellow.set_low(),
+    }.unwrap();
+
+    loop {
+        red.set_high().unwrap();
+    }
+
+    // delay.delay_ms(100_u16);
+
+    // Temperature sensor (TEMP_1_ADDR): write TEMP_PRODUCT_ID_REG, and read its content (1 byte)
+    let mut tx2 :[u8, 1] = TEMP_PRODUCT_ID_REG;
+    let mut rx2 :[u8; 1] = [0];
+    // i2c.write_read(TEMP_1_ADDR, &tx1.clone(), &mut rx).unwrap();
+    i2c.write_read(TCA9548ARGER_ADDR, &tx2.clone(), &mut rx2).unwrap();
+
+    // The ID should be 0xA1.
+    match rx2[0] {
+        0xA1 => green.set_high(),
+        _ => green.set_low(),
+    }.unwrap();
+
+    loop {
+        red.set_high().unwrap();
+    }
+}
diff --git a/src/blinky.rs b/src/blinky.rs
new file mode 100644
index 0000000..c7c1157
--- /dev/null
+++ b/src/blinky.rs
@@ -0,0 +1,76 @@
+#![deny(warnings)]
+#![deny(unsafe_code)]
+#![no_main]
+#![no_std]
+
+extern crate panic_itm;
+
+use cortex_m;
+use cortex_m_rt::entry;
+use stm32h7xx_hal::hal::digital::v2::OutputPin;
+use stm32h7xx_hal::{pac, prelude::*};
+
+use cortex_m_log::println;
+use cortex_m_log::{
+    destination::Itm, printer::itm::InterruptSync as InterruptSyncItm,
+};
+
+#[entry]
+fn main() -> ! {
+    let cp = cortex_m::Peripherals::take().unwrap();
+    let dp = pac::Peripherals::take().unwrap();
+    let mut log = InterruptSyncItm::new(Itm::new(cp.ITM));
+
+    // Constrain and Freeze power
+    println!(log, "Setup PWR...                  ");
+    let pwr = dp.PWR.constrain();
+    let vos = pwr.freeze();
+
+    // Constrain and Freeze clock
+    println!(log, "Setup RCC...                  ");
+    let rcc = dp.RCC.constrain();
+    let ccdr = rcc.sys_ck(100.mhz()).freeze(vos, &dp.SYSCFG);
+
+    println!(log, "");
+    println!(log, "stm32h7xx-hal example - Blinky");
+    println!(log, "");
+
+    let gpiob = dp.GPIOB.split(ccdr.peripheral.GPIOB);
+    let gpioe = dp.GPIOE.split(ccdr.peripheral.GPIOE);
+
+    // Configure PE1 as output.
+    let mut green = gpiob.pb0.into_push_pull_output();
+    let mut yellow = gpioe.pe1.into_push_pull_output();
+    let mut red = gpiob.pb14.into_push_pull_output();
+
+    // Get the delay provider.
+    let mut delay = cp.SYST.delay(ccdr.clocks);
+
+    let mut num = 0;
+
+    loop {
+        delay.delay_ms(500_u16);
+
+        if num & 4 != 0 {
+            green.set_high().unwrap();
+        }
+        else {
+            green.set_low().unwrap();
+        }
+
+        if num & 2 != 0 {
+            yellow.set_high().unwrap();
+        }
+        else {
+            yellow.set_low().unwrap();
+        }
+
+        if num & 1 != 0 {
+            red.set_high().unwrap();
+        }
+        else {
+            red.set_low().unwrap();
+        }
+        num = (num + 1) % 8;
+    }
+}
diff --git a/src/main.rs b/src/main.rs
new file mode 100644
index 0000000..4199063
--- /dev/null
+++ b/src/main.rs
@@ -0,0 +1,92 @@
+#![no_main]
+#![no_std]
+
+use panic_semihosting as _;
+
+use stm32h7xx_hal::hal::digital::v2::{
+	InputPin,
+	OutputPin,
+};
+use stm32h7xx_hal::{pac, prelude::*, spi};
+
+use cortex_m;
+use cortex_m::asm::nop;
+use cortex_m_rt::entry;
+use cortex_m_semihosting::hprintln;
+
+use core::ptr;
+use nb::block;
+
+
+#[entry]
+fn main() -> ! {
+
+	let 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())
+		.freeze(vos, &dp.SYSCFG);
+	
+	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 gpiod = dp.GPIOD.split(ccdr.peripheral.GPIOD);
+	let gpiof = dp.GPIOF.split(ccdr.peripheral.GPIOF);
+
+	// Setup CDONE for checking
+	let fpga_cdone = gpiod.pd15.into_pull_up_input();
+
+	match fpga_cdone.is_high() {
+		Ok(true)	=> hprintln!("FPGA is ready."),
+		Ok(_)		=> hprintln!("FPGA is in reset state."),
+		Err(_)		=> hprintln!("Error: Cannot read C_DONE"),
+	}.unwrap();
+
+	hprintln!("Start reading pin output...").unwrap();
+	delay.delay_ms(200_u16);
+
+
+	let pin = gpioa.pa0.into_pull_up_input();
+	let mut state = pin.is_high().unwrap();
+
+	hprintln!("Initial reading...");
+
+	match state {
+		true 	=> hprintln!("High."),
+		false 	=> hprintln!("Low."),
+	}.unwrap();
+
+	hprintln!("Polling...");
+
+	loop {
+		if pin.is_high().unwrap() != state {
+			match !state {
+				true 	=> hprintln!("High."),
+				false 	=> hprintln!("Low."),
+			}.unwrap();
+			state = !state;
+		}
+
+		if fpga_cdone.is_low().unwrap() {
+			hprintln!("FPGA is in reset state.");
+		}
+	}
+
+}
+
+
+
+
+
+
+
+
+
+