diff --git a/firmware/rust/src/main.rs b/firmware/rust/src/main.rs
index 87042c3..af812a2 100644
--- a/firmware/rust/src/main.rs
+++ b/firmware/rust/src/main.rs
@@ -5,6 +5,7 @@ use atmega_hal::{
     clock::MHz8,
     delay::Delay,
     pins,
+    port::{mode::Output, Pin, PB0},
     spi::{DataOrder, SerialClockRate, Settings, Spi},
     Peripherals,
 };
@@ -67,6 +68,28 @@ static ONDERS_ORG: [[u8; 48]; 2] = [
     ],
 ];
 
+fn clear_screen(spi: &mut Spi, cd: &mut Pin<Output, PB0>) {
+    let mut delay = Delay::<MHz8>::new();
+
+    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);
+        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);
+        cd.set_low();
+    }
+}
+
 #[atmega_hal::entry]
 fn main() -> ! {
     interrupt::free(|_cs| {
@@ -154,23 +177,7 @@ fn main() -> ! {
         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();
-        }
+        clear_screen(&mut spi, &mut lcd_cd);
 
         // Draw sacred chao
         for (i, page) in SACRED_CHAO.iter().enumerate() {