diff --git a/src/framebuffer.rs b/src/framebuffer.rs
index 4a85e41..aa80896 100644
--- a/src/framebuffer.rs
+++ b/src/framebuffer.rs
@@ -1,7 +1,11 @@
+use core::ptr::write_volatile;
+
 use crate::{
     mailbox::{read_mailbox, write_mailbox},
-    peripherals::uart::{print, print_u32},
+    peripherals::uart::{print, print_u32, print_u32_hex},
 };
+#[repr(align(16))]
+struct Mailbox([u32; 36]);
 
 const ALLOCATE_BUFFER: u32 = 0x00040001;
 const GET_PHYSICAL_DISPLAY_WH: u32 = 0x00040003;
@@ -10,79 +14,129 @@ const SET_VIRTUAL_DISPLAY_WH: u32 = 0x00048004;
 const SET_PIXEL_DEPTH: u32 = 0x00048005;
 const SET_PIXEL_ORDER: u32 = 0x00048006;
 const SET_FB_OFFSET: u32 = 0x00048009;
+const GET_PITCH: u32 = 0x00040008;
 
-pub fn init_fb() {
-    let mut mailbox = [0; 32];
-    mailbox[0] = 32 * 4;
-    mailbox[1] = 0;
+pub struct FrameBuffer {
+    pixel_depth: u32, // Bits per pixel
+    pitch: u32,       // Pixel per row
+    rows: u32,        // Rows
+    start_addr: *mut u32,
+    size: u32, //Bytes
+}
 
-    mailbox[2] = SET_PHYSICAL_DISPLAY_WH;
-    mailbox[3] = 8;
-    mailbox[4] = 8;
-    mailbox[5] = 1920;
-    mailbox[6] = 1200;
+impl FrameBuffer {
+    pub fn new() -> Self {
+        let mut mailbox = Mailbox([0; 36]);
+        mailbox.0[0] = 35 * 4;
+        mailbox.0[1] = 0;
 
-    mailbox[7] = SET_VIRTUAL_DISPLAY_WH;
-    mailbox[8] = 8;
-    mailbox[9] = 8;
-    mailbox[10] = 1920;
-    mailbox[11] = 1200;
+        mailbox.0[2] = SET_PHYSICAL_DISPLAY_WH;
+        mailbox.0[3] = 8;
+        mailbox.0[4] = 8;
+        mailbox.0[5] = 1920;
+        mailbox.0[6] = 1080;
 
-    mailbox[12] = SET_PIXEL_DEPTH;
-    mailbox[13] = 4;
-    mailbox[14] = 4;
-    mailbox[15] = 32; // 32 bit per pixel
+        mailbox.0[7] = SET_VIRTUAL_DISPLAY_WH;
+        mailbox.0[8] = 8;
+        mailbox.0[9] = 8;
+        mailbox.0[10] = 1920;
+        mailbox.0[11] = 1080;
 
-    mailbox[16] = SET_PIXEL_ORDER;
-    mailbox[17] = 4;
-    mailbox[18] = 4;
-    mailbox[19] = 0x1; // RGB
+        mailbox.0[12] = SET_PIXEL_DEPTH;
+        mailbox.0[13] = 4;
+        mailbox.0[14] = 4;
+        mailbox.0[15] = 32; // 32 bit per pixel
 
-    mailbox[20] = SET_FB_OFFSET;
-    mailbox[21] = 8;
-    mailbox[22] = 8;
-    mailbox[24] = 0; // X in pixels
-    mailbox[25] = 0; // Y in pixels
+        mailbox.0[16] = SET_PIXEL_ORDER;
+        mailbox.0[17] = 4;
+        mailbox.0[18] = 4;
+        mailbox.0[19] = 0x1; // RGB
 
-    mailbox[26] = ALLOCATE_BUFFER;
-    mailbox[27] = 8;
-    mailbox[28] = 4;
-    mailbox[29] = 4096; // Alignment
-    mailbox[30] = 0;
+        mailbox.0[20] = SET_FB_OFFSET;
+        mailbox.0[21] = 8;
+        mailbox.0[22] = 8;
+        mailbox.0[23] = 0; // X in pixels
+        mailbox.0[24] = 0; // Y in pixels
 
-    mailbox[31] = 0; // End tag
+        mailbox.0[25] = ALLOCATE_BUFFER;
+        mailbox.0[26] = 8;
+        mailbox.0[27] = 4;
+        mailbox.0[28] = 4096; // Alignment
+        mailbox.0[29] = 0;
 
-    // TODO: validate responses
+        mailbox.0[30] = GET_PITCH;
+        mailbox.0[31] = 4;
+        mailbox.0[32] = 0;
+        mailbox.0[33] = 0;
 
-    let addr = core::ptr::addr_of!(mailbox[0]) as u32;
+        mailbox.0[34] = 0; // End tag
 
-    write_mailbox(8, addr);
+        // TODO: validate responses
 
-    let _ = read_mailbox(8);
-    if mailbox[1] == 0 {
-        print("Failed\r\n");
+        let addr = core::ptr::addr_of!(mailbox.0[0]) as u32;
+
+        write_mailbox(8, addr);
+
+        let _ = read_mailbox(8);
+        if mailbox.0[1] == 0 {
+            print("Failed\r\n");
+        }
+
+        print_u32_hex(mailbox.0[28]);
+        print("\r\n");
+
+        mailbox.0[28] &= 0x3FFFFFFF;
+
+        Self {
+            pixel_depth: mailbox.0[15],
+            pitch: mailbox.0[33] / (mailbox.0[15] / 8),
+            rows: mailbox.0[29] / mailbox.0[33],
+            start_addr: mailbox.0[28] as *mut u32,
+            size: mailbox.0[29],
+        }
     }
 
-    print_u32(mailbox[29]);
+    pub fn draw_pixel(&self, x: u32, y: u32) {
+        let offset = x + y * self.pitch;
+        unsafe {
+            write_volatile(self.start_addr.add(offset as usize), 0x00AAFFFF);
+        }
+    }
 
-    mailbox[29] = (mailbox[29] & 0x00FF_FFFF) | 0x3F00_0000;
+    /*Bresenham's line algorithm */
+    pub fn draw_line(&self, x1: u32, y1: u32, x2: u32, y2: u32) {
+        if x1 == x2 {
+            for y in y1..=y2 {
+                self.draw_pixel(x1, y);
+            }
+            return;
+        }
+        let dx = x2 as i32 - x1 as i32;
+        let mut dy = y2 as i32 - y1 as i32;
+        let mut yi = 1;
 
-    let mut fb: *mut u32 = mailbox[29] as *mut u32;
+        let mut d = 2 * dy - dx;
+        let mut y = y1 as i32;
 
-    fb = unsafe { fb.add(1920 * 500 + 500) };
+        if dy < 0 {
+            yi = -1;
+            dy = -dy;
+        }
 
-    for x in 0..500 {
-        for y in 0..10 {
-            unsafe {
-                *fb = 0xFFFFBB00;
-                fb = fb.add(1);
-            };
+        for x in x1..=x2 {
+            self.draw_pixel(x, y as u32);
+            if d > 0 {
+                y = y + yi;
+                d += 2 * (dy - dx);
+            } else {
+                d += 2 * dy;
+            }
         }
     }
 }
 
 pub fn print_display_resolution() {
-    let mut mailbox = [0; 8];
+    let mut mailbox: [u32; 8] = [0; 8];
     mailbox[0] = 8 * 4;
     mailbox[1] = 0;
     mailbox[2] = GET_PHYSICAL_DISPLAY_WH;
diff --git a/src/main.rs b/src/main.rs
index da47fc7..c072687 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -9,7 +9,7 @@ use core::{
 };
 
 use nova::{
-    framebuffer::{init_fb, print_display_resolution},
+    framebuffer::{print_display_resolution, FrameBuffer},
     irq_interrupt::enable_irq_source,
     mailbox::read_soc_temp,
     peripherals::{
@@ -94,7 +94,23 @@ pub extern "C" fn kernel_main() -> ! {
     set_falling_edge_detect(26, true);
 
     print_display_resolution();
-    init_fb();
+    let fb = FrameBuffer::new();
+    print_display_resolution();
+
+    fb.draw_line(10, 10, 1000, 10);
+    fb.draw_line(10, 10, 1000, 200);
+    fb.draw_line(10, 10, 1000, 300);
+    fb.draw_line(10, 10, 1000, 400);
+    fb.draw_line(10, 10, 1000, 500);
+    fb.draw_line(10, 10, 1000, 600);
+    fb.draw_line(10, 10, 1000, 700);
+    fb.draw_line(10, 10, 1000, 800);
+    fb.draw_line(10, 10, 1000, 900);
+    fb.draw_line(10, 10, 1000, 1000);
+    fb.draw_line(10, 10, 100, 1000);
+
+    fb.draw_line(1800, 10, 1000, 900);
+    fb.draw_line(1800, 500, 1000, 100);
 
     loop {
         let temp = read_soc_temp();
diff --git a/src/peripherals/uart.rs b/src/peripherals/uart.rs
index ebe45b4..92e76a2 100644
--- a/src/peripherals/uart.rs
+++ b/src/peripherals/uart.rs
@@ -59,6 +59,39 @@ pub fn print_u32(mut val: u32) {
     }
 }
 
+pub fn print_u32_hex(mut val: u32) {
+    let mut last_valid = 0;
+    let mut values = [0u32; 8];
+    for (i, c) in (&mut values).iter_mut().enumerate() {
+        if val == 0 {
+            break;
+        }
+
+        *c = val % 16;
+        val /= 16;
+        if *c != 0 {
+            last_valid = i;
+        }
+    }
+
+    for (i, c) in values.iter().enumerate().rev() {
+        if i > last_valid {
+            continue;
+        }
+
+        let ascii_byte = if *c < 10 {
+            b'0' + *c as u8
+        } else {
+            b'A' - 10 + *c as u8
+        };
+
+        let data = [ascii_byte];
+
+        let s = str::from_utf8(&data).unwrap();
+        print(s);
+    }
+}
+
 /// Initialize UART peripheral
 pub fn uart_init() {
     let baud_div_times_64 = (UART_CLK * 4) / BAUD;