diff --git a/examples/ethernet.rs b/examples/ethernet.rs
index 71000d3..3a9854f 100644
--- a/examples/ethernet.rs
+++ b/examples/ethernet.rs
@@ -140,7 +140,7 @@ const BUFFER_SIZE: usize = 2048;
 #[entry]
 fn main() -> ! {
 
-    logger::semihosting_init();
+    // logger::semihosting_init();
 
     let mut cp = cortex_m::Peripherals::take().unwrap();
     let dp = pac::Peripherals::take().unwrap();
@@ -155,12 +155,17 @@ fn main() -> ! {
     // Initialise clocks...
     let rcc = dp.RCC.constrain();
     let ccdr = rcc
+        .use_hse(8.mhz())
         .sys_ck(200.mhz())
         .hclk(200.mhz())
-        .pll1_r_ck(100.mhz())   // for TRACECK
+        .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);
 
@@ -178,6 +183,10 @@ fn main() -> ! {
 	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();
 
diff --git a/examples/fpga_config.rs b/examples/fpga_config.rs
index a114174..d844f0e 100644
--- a/examples/fpga_config.rs
+++ b/examples/fpga_config.rs
@@ -1,29 +1,31 @@
 #![no_main]
 #![no_std]
 
+#[macro_use]
+extern crate log;
+
 use panic_semihosting as _;
 
 use stm32h7xx_hal::hal::digital::v2::{
 	InputPin,
 	OutputPin,
 };
-use stm32h7xx_hal::{pac, prelude::*, spi};
+use stm32h7xx_hal::{gpio::Speed, 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;
 
+#[path = "util/logger.rs"]
+mod logger;
 
 #[entry]
 fn main() -> ! {
 
-	hprintln!("Flashing configuration bitstream to iCE40 HX8K on Humpback.").unwrap();
-
-	let cp = cortex_m::Peripherals::take().unwrap();
+	let mut cp = cortex_m::Peripherals::take().unwrap();
 	let dp = pac::Peripherals::take().unwrap();
 
 	let pwr = dp.PWR.constrain();
@@ -33,7 +35,12 @@ fn main() -> ! {
 	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 mut delay = cp.SYST.delay(ccdr.clocks);
 
@@ -42,6 +49,12 @@ fn main() -> ! {
 	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();
@@ -84,10 +97,10 @@ fn main() -> ! {
 
 	// 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();
+		Ok(false)	=> debug!("Reset successful!"),
+		Ok(_) 		=> debug!("Reset unsuccessful!"),
+		Err(_) 		=> debug!("Reset error!"),
+	};
 
 	// Set SPI_SS_B high
 	fpga_ss.set_high().unwrap();
@@ -117,13 +130,13 @@ fn main() -> ! {
 
 	// Check the CDONE output from FPGA
 	if !(fpga_cdone.is_high().unwrap()) {
-		hprintln!("ERROR!").unwrap();
+		debug!("ERROR!");
 	}
 	else {
-		hprintln!("Configuration successful!").unwrap();
+		debug!("Configuration successful!");
 		// 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();
+		debug!("User I/O pins activated.");
 	}
 
 	loop {
diff --git a/gdb_config/fpga_config.gdb b/gdb_config/fpga_config.gdb
index 65a21ef..94b8ca1 100644
--- a/gdb_config/fpga_config.gdb
+++ b/gdb_config/fpga_config.gdb
@@ -12,10 +12,12 @@ break HardFault
 break rust_begin_unwind
 
 # break at line 130 to auto quit
-break examples/fpga_config.rs:130
+break examples/fpga_config.rs:143
 
 # print using semihosting, slow af
-monitor arm semihosting enable
+# monitor arm semihosting enable
+monitor tpiu config internal itm.fifo uart off 400000000
+monitor itm port 0 on
 
 # flash the program to bank 0
 load