diff --git a/firmware/rust/src/main.rs b/firmware/rust/src/main.rs
index 9e9eb49..505fc8b 100644
--- a/firmware/rust/src/main.rs
+++ b/firmware/rust/src/main.rs
@@ -58,9 +58,6 @@ impl ClockGenerator {
         let dp = Peripherals::take().unwrap();
         let pins = pins!(dp);
 
-        // Set display PWM backlight pin as output
-        pins.pd5.into_output();
-
         // Init SPI
         let (spi, _) = Spi::new(
             dp.SPI,
@@ -79,7 +76,13 @@ impl ClockGenerator {
         );
 
         Self {
-            screen: Screen::new(dp.TC0, spi, pins.pb0.into_output(), pins.pb1.into_output()),
+            screen: Screen::new(
+                dp.TC0,
+                spi,
+                pins.pd5.into_output(),
+                pins.pb0.into_output(),
+                pins.pb1.into_output(),
+            ),
             tc1: dp.TC1,
             exint: dp.EXINT,
             delay: Delay::<MHz8>::new(),
diff --git a/firmware/rust/src/screen/mod.rs b/firmware/rust/src/screen/mod.rs
index fabedcc..621ca51 100644
--- a/firmware/rust/src/screen/mod.rs
+++ b/firmware/rust/src/screen/mod.rs
@@ -1,6 +1,6 @@
 use atmega_hal::{
     pac::TC0,
-    port::{mode::Output, Pin, PB0, PB1},
+    port::{mode::Output, Pin, PB0, PB1, PD5},
     Spi,
 };
 
@@ -45,34 +45,34 @@ impl Screens {
 pub struct Screen {
     lcd: Lcd,
     tc0: TC0,
+    pwm: Pin<Output, PD5>,
     screen: Screens,
 }
 
 impl Screen {
-    pub fn new(tc0: TC0, spi: Spi, cd: Pin<Output, PB0>, rst: Pin<Output, PB1>) -> Self {
+    pub fn new(
+        tc0: TC0,
+        spi: Spi,
+        pwm: Pin<Output, PD5>,
+        cd: Pin<Output, PB0>,
+        rst: Pin<Output, PB1>,
+    ) -> Self {
         Self {
             lcd: Lcd::new(spi, cd, rst),
             tc0,
+            pwm,
             screen: Screens::Splash(Splash),
         }
     }
 
     pub fn init(&mut self) {
         // Init display backlight
+        self.tc0.ocr0a.write(|w| unsafe { w.bits(255) });
         self.tc0.tccr0a.write(|w| {
             w.wgm0().pwm_fast();
-            w.com0b().match_clear();
-            w
+            w.com0b().match_clear()
         });
-        self.tc0.tccr0b.write(|w| {
-            w.wgm02().set_bit();
-            w.cs0().prescale_64();
-            w
-        });
-        self.tc0.ocr0a.write(|w| unsafe { w.bits(255) });
-        self.tc0
-            .ocr0b
-            .write(|w| unsafe { w.bits(nb::block!(eeprom::read_byte(&BACKLIGHT)).unwrap()) });
+        self.set_backlight(nb::block!(eeprom::read_byte(&BACKLIGHT)).unwrap());
 
         // Init lcd display
         self.lcd.init();
@@ -80,6 +80,29 @@ impl Screen {
         self.draw();
     }
 
+    fn set_backlight(&mut self, backlight: u8) {
+        match backlight {
+            0 => {
+                self.tc0.tccr0b.write(|w| w.cs0().no_clock());
+                self.pwm.set_low();
+            }
+            1..=5 => {
+                self.tc0.tccr0b.write(|w| {
+                    w.wgm02().set_bit();
+                    w.cs0().prescale_256()
+                });
+                self.tc0.ocr0b.write(|w| unsafe { w.bits(backlight - 1) });
+            }
+            _ => {
+                self.tc0.tccr0b.write(|w| {
+                    w.wgm02().set_bit();
+                    w.cs0().prescale_64()
+                });
+                self.tc0.ocr0b.write(|w| unsafe { w.bits(backlight - 6) });
+            }
+        }
+    }
+
     pub fn draw(&mut self) {
         self.lcd.fill_area(0, 0, 102, 8, 0x00);
 
@@ -93,7 +116,7 @@ impl Screen {
     pub fn input(&mut self, input: &Input) {
         match self.screen.input(input) {
             Event::Screen(screen) => self.screen = screen,
-            Event::Backlight(backlight) => self.tc0.ocr0b.write(|w| unsafe { w.bits(backlight) }),
+            Event::Backlight(backlight) => self.set_backlight(backlight),
             Event::Contrast(contrast) => self.lcd.set_contrast(contrast),
             Event::None => {}
         }