diff --git a/Cargo.toml b/Cargo.toml
index 59a01ba..1bf83c5 100644
--- a/Cargo.toml
+++ b/Cargo.toml
@@ -7,7 +7,7 @@ edition = "2018"
 [dependencies]
 cortex-m = "0.6"
 cortex-m-rt = "0.6"
-stm32f1xx-hal = { version = "0.5", features = ["stm32f103", "stm32-usbd", "rt"] }
+stm32f1xx-hal = { version = "0.5.3", features = ["stm32f103", "stm32-usbd", "rt"] }
 panic-semihosting = "0.5"
 usb-device = "0.2.4"
 usbd-serial = "0.1"
diff --git a/src/main.rs b/src/main.rs
index 7e52706..c8f7e10 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -3,16 +3,30 @@
 
 extern crate panic_semihosting;
 
-use cortex_m::asm::delay;
+use core::cell::RefCell;
+use core::ops::DerefMut;
+
+use cortex_m::asm::wfi;
+use cortex_m::peripheral::NVIC;
+use cortex_m::interrupt::{free as interrupt_free, Mutex};
 use cortex_m_rt::entry;
 use embedded_hal::digital::v2::OutputPin;
-use stm32f1xx_hal::usb::{Peripheral, UsbBus};
-use stm32f1xx_hal::{prelude::*, stm32};
+use stm32f1xx_hal::stm32::{self, interrupt, Interrupt};
+use stm32f1xx_hal::{delay::Delay, prelude::*};
+use stm32f1xx_hal::usb::{Peripheral, UsbBusType};
+use usb_device::bus::UsbBusAllocator;
 use usb_device::prelude::*;
 use usbd_serial::{SerialPort, USB_CLASS_CDC};
 
+static mut USB_BUS: Option<UsbBusAllocator<UsbBusType>> = None;
+static USB_DEVICE: Mutex<RefCell<Option<UsbDevice<'static, UsbBusType>>>> =
+    Mutex::new(RefCell::new(None));
+static SERIAL: Mutex<RefCell<Option<SerialPort<'static, UsbBusType>>>> =
+    Mutex::new(RefCell::new(None));
+
 #[entry]
 fn main() -> ! {
+    let cp = cortex_m::Peripherals::take().unwrap();
     let dp = stm32::Peripherals::take().unwrap();
 
     let mut flash = dp.FLASH.constrain();
@@ -27,6 +41,8 @@ fn main() -> ! {
 
     assert!(clocks.usbclk_valid());
 
+    let mut delay = Delay::new(cp.SYST, clocks);
+
     // Configure the on-board LED (PC13, green)
     let mut gpioc = dp.GPIOC.split(&mut rcc.apb2);
     let mut led = gpioc.pc13.into_push_pull_output(&mut gpioc.crh);
@@ -34,61 +50,83 @@ fn main() -> ! {
 
     let mut gpioa = dp.GPIOA.split(&mut rcc.apb2);
 
-    // BluePill board has a pull-up resistor on the D+ line.
-    // Pull the D+ pin down to send a RESET condition to the USB bus.
-    // This forced reset is needed only for development, without it host
-    // will not reset your device when you upload new firmware.
     let mut usb_dp = gpioa.pa12.into_push_pull_output(&mut gpioa.crh);
-    usb_dp.set_low().ok();
-    delay(clocks.sysclk().0 / 100);
+    usb_dp.set_low().unwrap();
+    delay.delay_ms(10u16);
 
-    let usb = Peripheral {
-        usb: dp.USB,
-        pin_dm: gpioa.pa11,
-        pin_dp: usb_dp.into_floating_input(&mut gpioa.crh),
-    };
-    let usb_bus = UsbBus::new(usb);
+    let usb_bus;
+    unsafe {
+        let usb_peripherial = Peripheral {
+            usb: dp.USB,
+            pin_dm: gpioa.pa11,
+            pin_dp: usb_dp.into_floating_input(&mut gpioa.crh),
+        };
+        USB_BUS = Some(UsbBusType::new(usb_peripherial));
+        usb_bus = USB_BUS.as_ref().unwrap();
+    }
 
-    let mut serial = SerialPort::new(&usb_bus);
+    interrupt_free(|cs| {
+        SERIAL.borrow(cs).replace(Some(SerialPort::new(usb_bus)));
+        USB_DEVICE.borrow(cs).replace(Some(
+            UsbDeviceBuilder::new(usb_bus, UsbVidPid(0x16c0, 0x27dd))
+                .manufacturer("Ar3itrary Embedded")
+                .product("usart-demo")
+                .serial_number("2342")
+                .device_class(USB_CLASS_CDC)
+                .build(),
+        ));
+    });
 
-    let mut usb_dev = UsbDeviceBuilder::new(&usb_bus, UsbVidPid(0x16c0, 0x27dd))
-        .manufacturer("Fake company")
-        .product("Serial port")
-        .serial_number("TEST")
-        .device_class(USB_CLASS_CDC)
-        .build();
+    unsafe {
+        NVIC::unmask(Interrupt::USB_HP_CAN_TX);
+        NVIC::unmask(Interrupt::USB_LP_CAN_RX0);
+    }
 
     loop {
-        if !usb_dev.poll(&mut [&mut serial]) {
-            continue;
-        }
-
-        let mut buf = [0u8; 64];
-
-        match serial.read(&mut buf) {
-            Ok(count) if count > 0 => {
-                led.set_low().ok(); // Turn on
-
-                // Echo back in upper case
-                for c in buf[0..count].iter_mut() {
-                    if 0x61 <= *c && *c <= 0x7a {
-                        *c &= !0x20;
-                    }
-                }
-
-                let mut write_offset = 0;
-                while write_offset < count {
-                    match serial.write(&buf[write_offset..count]) {
-                        Ok(len) if len > 0 => {
-                            write_offset += len;
-                        }
-                        _ => {}
-                    }
-                }
-            }
-            _ => {}
-        }
-
-        led.set_high().ok(); // Turn off
+        wfi();
     }
 }
+
+#[interrupt]
+fn USB_HP_CAN_TX() {
+    usb_interrupt();
+}
+
+#[interrupt]
+fn USB_LP_CAN_RX0() {
+    usb_interrupt();
+}
+
+fn usb_interrupt() {
+    interrupt_free(|cs| {
+        match USB_DEVICE.borrow(cs).borrow_mut().deref_mut() {
+            Some(usb_dev) => {
+                match SERIAL.borrow(cs).borrow_mut().deref_mut() {
+                    Some(serial) => {
+                        if !usb_dev.poll(&mut [serial]) {
+                            return;
+                        }
+
+                        let mut buf = [0u8; 8];
+
+                        match serial.read(&mut buf) {
+                            Ok(count) if count > 0 => {
+                                // Echo back in upper case
+                                for c in buf[0..count].iter_mut() {
+                                    if 0x61 <= *c && *c <= 0x7a {
+                                        *c &= !0x20;
+                                    }
+                                }
+
+                                serial.write(&buf[0..count]).ok();
+                            }
+                            _ => {}
+                        };
+                    }
+                    _ => {}
+                };
+            }
+            _ => {}
+        };
+    });
+}