diff --git a/src/main.rs b/src/main.rs
index b8d00da..36f91b3 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -1,33 +1,74 @@
 #![no_std]
 #![no_main]
 
+use core::ptr::read_volatile;
+
 // pick a panicking behavior
 use panic_halt as _; // you can put a breakpoint on `rust_begin_unwind` to catch panics
 // use panic_abort as _; // requires nightly
 // use panic_itm as _; // logs messages over ITM; requires ITM support
 // use panic_semihosting as _; // logs messages to the host stderr; requires a debugger
 
-use cortex_m::asm;
-use cortex_m_rt::entry;
+use cortex_m_rt::{entry, exception};
 use cortex_m_semihosting::hprintln;
 use stm32f1::stm32f103;
 
+static mut SYS_TICK: u64 = 0;
+
+fn get_sys_tick() -> u64 {
+    unsafe { read_volatile(&raw const SYS_TICK) }
+}
+
+#[exception]
+fn SysTick() {
+    unsafe { SYS_TICK += 1; }
+}
+
+fn toggle_mcu_led(peripherals: &mut stm32f103::Peripherals) {
+    peripherals.GPIOA.odr.modify(|r, w|  {
+        match r.odr3().bit() {
+            true => w.odr3().clear_bit(),
+            false => w.odr3().set_bit(),
+        }
+    });
+}
+
+
+
 #[entry]
 fn main() -> ! {
-    asm::nop(); // To not have main optimize to abort in release mode, remove when you add code
-    hprintln!("Hello, world!");
+    let mut peripherals = stm32f103::Peripherals::take().unwrap();
+    let mut core_peripherals = cortex_m::Peripherals::take().unwrap();
+
+    // Enable Cortex-M SysTick
+    peripherals.STK.ctrl.modify(|_,w| w.tickint().set_bit());   // Enable Systick Interrupt
+    core_peripherals.SYST.set_reload(1_000 - 1);                        // Set period to 1ms
+    core_peripherals.SYST.clear_current();                              // Clear the current value register
+    core_peripherals.SYST.enable_counter();                             // Enable SysTick
 
-    let peripherals = stm32f103::Peripherals::take().unwrap();
 
     // Enable clock for GPIOA by setting IOPAEN in RCC APB2ENR
     peripherals.RCC.apb2enr.modify(|_, w| w.iopaen().set_bit());
-
     peripherals.GPIOA.crl.modify(|_, w| w.mode3().output());
     peripherals.GPIOA.crl.modify(|_, w| w.cnf3().push_pull());
     peripherals.GPIOA.odr.modify(|_, w| w.odr3().high());
 
+    // Init i2c
+    // 8MHz SYSCLOCK / 16 = 500khz i2c bus
+    peripherals.RCC.cfgr.modify(|_, w| w.ppre1().div16());
+    peripherals.RCC.apb1enr.modify(|_, w| w.i2c1en().enabled());
+    peripherals.RCC.apb1enr.modify(|_, w| w.i2c2en().enabled());
+    peripherals.I2C1.cr1.modify(|_,w| w.pe().enabled());
+
+    let mut timer_ms = 500;
+
     loop {
         // your code goes here
-        asm::nop();
+        let current_value = get_sys_tick();
+        if  current_value >= timer_ms
+        {
+            toggle_mcu_led(&mut peripherals);
+            timer_ms = current_value + 500;
+        }
     }
 }