diff --git a/firmware/rust/src/main.rs b/firmware/rust/src/main.rs
index a5abd86..431bdd9 100644
--- a/firmware/rust/src/main.rs
+++ b/firmware/rust/src/main.rs
@@ -63,127 +63,123 @@ static ONDERS_ORG: [[u8; 48]; 2] = [
 
 #[atmega_hal::entry]
 fn main() -> ! {
-    // Disable interrupts when initializing
-    interrupt::disable();
+    interrupt::free(|_cs| {
+        // Get peripherals, pins and delay
+        let dp = Peripherals::take().unwrap();
+        let pins = pins!(dp);
+        let mut delay = Delay::<MHz8>::new();
 
-    // Get peripherals, pins and delay
-    let dp = Peripherals::take().unwrap();
-    let pins = pins!(dp);
-    let mut delay = Delay::<MHz8>::new();
+        // Init display backlight
+        let tc0 = dp.TC0;
+        tc0.tccr0a.write(|w| {
+            w.wgm0().pwm_fast();
+            w.com0b().match_clear();
+            w
+        });
+        tc0.tccr0b.write(|w| {
+            w.wgm02().set_bit();
+            w.cs0().prescale_64();
+            w
+        });
+        tc0.ocr0a.write(|w| unsafe { w.bits(255) });
+        // TODO: Use EEPROM
+        tc0.ocr0b.write(|w| unsafe { w.bits(0) });
+        pins.pd5.into_output();
 
-    // Init display backlight
-    let tc0 = dp.TC0;
-    tc0.tccr0a.write(|w| {
-        w.wgm0().pwm_fast();
-        w.com0b().match_clear();
-        w
-    });
-    tc0.tccr0b.write(|w| {
-        w.wgm02().set_bit();
-        w.cs0().prescale_64();
-        w
-    });
-    tc0.ocr0a.write(|w| unsafe { w.bits(255) });
-    // TODO: Use EEPROM
-    tc0.ocr0b.write(|w| unsafe { w.bits(0) });
-    pins.pd5.into_output();
-
-    // Init SPI
-    let (mut spi, _) = Spi::new(
-        dp.SPI,
-        pins.pb5.into_output(),
-        pins.pb3.into_output(),
-        pins.pb4.into_pull_up_input(),
-        pins.pb2.into_output(),
-        Settings {
-            data_order: DataOrder::MostSignificantFirst,
-            clock: SerialClockRate::OscfOver2,
-            mode: Mode {
-                polarity: Polarity::IdleLow,
-                phase: Phase::CaptureOnFirstTransition,
+        // Init SPI
+        let (mut spi, _) = Spi::new(
+            dp.SPI,
+            pins.pb5.into_output(),
+            pins.pb3.into_output(),
+            pins.pb4.into_pull_up_input(),
+            pins.pb2.into_output(),
+            Settings {
+                data_order: DataOrder::MostSignificantFirst,
+                clock: SerialClockRate::OscfOver2,
+                mode: Mode {
+                    polarity: Polarity::IdleLow,
+                    phase: Phase::CaptureOnFirstTransition,
+                },
             },
-        },
-    );
+        );
 
-    // Init LCD
-    let mut lcd_cd = pins.pb0.into_output();
-    let mut lcd_rst = pins.pb1.into_output();
-    // TODO: Test if delay is really needed
-    delay.delay_ms(1_u8);
-    lcd_rst.set_high();
-    // TODO: Try to reduce delay to a minimum
-    delay.delay_ms(5_u8);
-
-    block!(spi.send(0x40)).unwrap(); // (6)  Set Scroll Line: Display start line 0
-    block!(spi.send(0xA1)).unwrap(); // (13) Set SEG direction: SEG reverse
-    block!(spi.send(0xC0)).unwrap(); // (14) Set COM direction: Normal COM0 - COM63
-    block!(spi.send(0xA6)).unwrap(); // (11) Set Inverse Display: Display inverse off
-    block!(spi.send(0xA2)).unwrap(); // (17) Set LCD Bias Ratio: Set Bias 1/9 (Duty 1/65)
-    block!(spi.send(0x2F)).unwrap(); // (5)  Set Power Control: Booster, Regulator and Follower on
-    block!(spi.send(0x27)).unwrap(); // (8)  Set VLCD Resistor Ratio: Set Contrast
-    block!(spi.send(0x81)).unwrap(); // (9)  Set Electronic Volume: Set Contrast
-    block!(spi.send(0x08)).unwrap(); // (9)  Set Electronic Volume: Set Contrast // TODO: Use EEPROM
-    block!(spi.send(0xAF)).unwrap(); // (12) Set Display Enable: Display on
-
-    // Clear screen
-    for page in 0..8 {
-        block!(spi.send(0x00)).unwrap();
-        block!(spi.send(0x10)).unwrap();
-        block!(spi.send(0xB0 + page)).unwrap();
-
-        // TODO: This delay fixes issues, try find a better solution
+        // Init LCD
+        let mut lcd_cd = pins.pb0.into_output();
+        let mut lcd_rst = pins.pb1.into_output();
+        // TODO: Test if delay is really needed
         delay.delay_ms(1_u8);
-        lcd_cd.set_high();
+        lcd_rst.set_high();
+        // TODO: Try to reduce delay to a minimum
+        delay.delay_ms(5_u8);
 
-        for _ in 0..102 {
+        block!(spi.send(0x40)).unwrap(); // (6)  Set Scroll Line: Display start line 0
+        block!(spi.send(0xA1)).unwrap(); // (13) Set SEG direction: SEG reverse
+        block!(spi.send(0xC0)).unwrap(); // (14) Set COM direction: Normal COM0 - COM63
+        block!(spi.send(0xA6)).unwrap(); // (11) Set Inverse Display: Display inverse off
+        block!(spi.send(0xA2)).unwrap(); // (17) Set LCD Bias Ratio: Set Bias 1/9 (Duty 1/65)
+        block!(spi.send(0x2F)).unwrap(); // (5)  Set Power Control: Booster, Regulator and Follower on
+        block!(spi.send(0x27)).unwrap(); // (8)  Set VLCD Resistor Ratio: Set Contrast
+        block!(spi.send(0x81)).unwrap(); // (9)  Set Electronic Volume: Set Contrast
+        block!(spi.send(0x08)).unwrap(); // (9)  Set Electronic Volume: Set Contrast // TODO: Use EEPROM
+        block!(spi.send(0xAF)).unwrap(); // (12) Set Display Enable: Display on
+
+        // Clear screen
+        for page in 0..8 {
             block!(spi.send(0x00)).unwrap();
+            block!(spi.send(0x10)).unwrap();
+            block!(spi.send(0xB0 + page)).unwrap();
+
+            // TODO: This delay fixes issues, try find a better solution
+            delay.delay_ms(1_u8);
+            lcd_cd.set_high();
+
+            for _ in 0..102 {
+                block!(spi.send(0x00)).unwrap();
+            }
+
+            // TODO: This delay fixes issues, try find a better solution
+            delay.delay_ms(1_u8);
+            lcd_cd.set_low();
         }
 
-        // TODO: This delay fixes issues, try find a better solution
-        delay.delay_ms(1_u8);
-        lcd_cd.set_low();
-    }
+        // Draw sacred chao
+        for (i, page) in SACRED_CHAO.iter().enumerate() {
+            block!(spi.send(0x0F)).unwrap();
+            block!(spi.send(0x11)).unwrap();
+            block!(spi.send(0xB1 + i as u8)).unwrap();
 
-    // Draw sacred chao
-    for (i, page) in SACRED_CHAO.iter().enumerate() {
-        block!(spi.send(0x0F)).unwrap();
-        block!(spi.send(0x11)).unwrap();
-        block!(spi.send(0xB1 + i as u8)).unwrap();
+            // TODO: This delay fixes issues, try find a better solution
+            delay.delay_ms(1_u8);
+            lcd_cd.set_high();
 
-        // TODO: This delay fixes issues, try find a better solution
-        delay.delay_ms(1_u8);
-        lcd_cd.set_high();
+            for segment in page {
+                block!(spi.send(*segment)).unwrap();
+            }
 
-        for segment in page {
-            block!(spi.send(*segment)).unwrap();
+            // TODO: This delay fixes issues, try find a better solution
+            delay.delay_ms(1_u8);
+            lcd_cd.set_low();
         }
 
-        // TODO: This delay fixes issues, try find a better solution
-        delay.delay_ms(1_u8);
-        lcd_cd.set_low();
-    }
+        // Draw onders.org
+        for (i, page) in ONDERS_ORG.iter().enumerate() {
+            block!(spi.send(0x0A)).unwrap();
+            block!(spi.send(0x11)).unwrap();
+            block!(spi.send(0xB6 + i as u8)).unwrap();
 
-    // Draw onders.org
-    for (i, page) in ONDERS_ORG.iter().enumerate() {
-        block!(spi.send(0x0A)).unwrap();
-        block!(spi.send(0x11)).unwrap();
-        block!(spi.send(0xB6 + i as u8)).unwrap();
+            // TODO: This delay fixes issues, try find a better solution
+            delay.delay_ms(1_u8);
+            lcd_cd.set_high();
 
-        // TODO: This delay fixes issues, try find a better solution
-        delay.delay_ms(1_u8);
-        lcd_cd.set_high();
+            for segment in page {
+                block!(spi.send(*segment)).unwrap();
+            }
 
-        for segment in page {
-            block!(spi.send(*segment)).unwrap();
+            // TODO: This delay fixes issues, try find a better solution
+            delay.delay_ms(1_u8);
+            lcd_cd.set_low();
         }
-
-        // TODO: This delay fixes issues, try find a better solution
-        delay.delay_ms(1_u8);
-        lcd_cd.set_low();
-    }
-
-    // Enable interrupts
-    unsafe { interrupt::enable() };
+    });
 
     #[allow(clippy::empty_loop)]
     loop {}