diff --git a/src/mailbox.rs b/src/mailbox.rs
new file mode 100644
index 0000000..cac50fa
--- /dev/null
+++ b/src/mailbox.rs
@@ -0,0 +1,57 @@
+use crate::{mmio_read, mmio_write};
+
+const MBOX_BASE: u32 = 0x3F00_B880;
+
+// MB0
+const MBOX_READ: u32 = MBOX_BASE + 0x00;
+const MBOX_READ_STATUS: u32 = MBOX_BASE + 0x18;
+
+// MB1
+const MBOX_WRITE: u32 = MBOX_BASE + 0x20;
+const MBOX_WRITE_STATUS: u32 = MBOX_BASE + 0x38;
+
+// Status
+const MAIL_FULL: u32 = 0x80000000;
+const MAIL_EMPTY: u32 = 0x40000000;
+
+#[repr(align(16))]
+struct MailboxBuffer([u32; 36]);
+
+pub fn read_mailbox(channel: u32) -> u32 {
+    // Wait until mailbox is not empty
+    loop {
+        while (mmio_read(MBOX_READ_STATUS) & MAIL_EMPTY != 0) {}
+        let mut data = mmio_read(MBOX_READ);
+        let read_channel = data & 0xF;
+
+        data >>= 4;
+
+        if channel == read_channel {
+            return data;
+        }
+    }
+}
+
+pub fn write_mailbox(channel: u32, data: u32) {
+    while (mmio_read(MBOX_WRITE_STATUS) & MAIL_FULL != 0) {}
+    mmio_write(MBOX_WRITE, data << 4 | (channel & 0xF));
+}
+
+pub fn read_soc_temp() -> u32 {
+    let mut mbox = MailboxBuffer([0; 36]);
+
+    mbox.0[0] = 8 * 4; // Total size in bytes
+    mbox.0[1] = 0; // Request
+    mbox.0[2] = 0x00030006; // Tag: Get temperature
+    mbox.0[3] = 8; // Value buffer size (bytes)
+    mbox.0[4] = 4; // Request size (bytes)
+    mbox.0[5] = 0; // Temp ID: 0 = SoC
+    mbox.0[6] = 0; // Response will be written here
+    mbox.0[7] = 0; // End tag
+
+    let addr = &mbox.0 as *const u32 as u32;
+    write_mailbox(8, addr);
+    let _ = read_mailbox(8);
+    let raw_temp = mbox.0[6];
+    raw_temp / 1000
+}
diff --git a/src/main.rs b/src/main.rs
index 479f264..a5d19ad 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -9,6 +9,7 @@ use core::{
 
 use nova::{
     irq_interrupt::enable_irq_source,
+    mailbox::read_soc_temp,
     peripherals::{
         gpio::{
             gpio_get_state, gpio_high, gpio_low, gpio_pull_up, set_falling_edge_detect,
@@ -77,6 +78,9 @@ pub extern "C" fn kernel_main() -> ! {
     set_falling_edge_detect(26, true);
 
     loop {
+        //let temp = read_soc_temp();
+        //u32_to_ascii(temp as u8);
+
         let _ = gpio_high(29);
 
         sleep_ms(500); // 0.5s
@@ -97,6 +101,15 @@ fn print_gpio_state() {
     print("\r\n");
 }
 
+fn u32_to_ascii(val: u8) {
+    let ascii_byte = b'0' + val;
+    let data = [ascii_byte];
+
+    let s = str::from_utf8(&data).unwrap();
+    print(s);
+    print("C\r\n");
+}
+
 pub fn get_current_el() -> u64 {
     let el: u64;
     unsafe {
diff --git a/src/peripherals/gpio.rs b/src/peripherals/gpio.rs
index 709277f..c283148 100644
--- a/src/peripherals/gpio.rs
+++ b/src/peripherals/gpio.rs
@@ -90,30 +90,28 @@ pub fn gpio_pull_down(gpio: u8) {
 }
 
 fn gpio_pull_up_down(gpio: u8, val: u32) {
-    unsafe {
-        // Determine GPPUDCLK Register
-        let register_addr = GPPUDCLK_BASE + 4 * (gpio as u32 / 32);
-        let register_offset = gpio % 32;
+    // Determine GPPUDCLK Register
+    let register_addr = GPPUDCLK_BASE + 4 * (gpio as u32 / 32);
+    let register_offset = gpio % 32;
 
-        // 1. Write Pull up
-        mmio_write(GPPUD, val);
+    // 1. Write Pull up
+    mmio_write(GPPUD, val);
 
-        // 2. Delay 150 cycles
-        delay_nops(150);
+    // 2. Delay 150 cycles
+    delay_nops(150);
 
-        // 3. Write to clock
-        let new_val = 0b1 << register_offset;
-        mmio_write(register_addr, new_val);
+    // 3. Write to clock
+    let new_val = 0b1 << register_offset;
+    mmio_write(register_addr, new_val);
 
-        // 4. Delay 150 cycles
-        delay_nops(150);
+    // 4. Delay 150 cycles
+    delay_nops(150);
 
-        // 5. reset GPPUD
-        mmio_write(GPPUD, 0);
+    // 5. reset GPPUD
+    mmio_write(GPPUD, 0);
 
-        // 6. reset clock
-        mmio_write(register_addr, 0);
-    }
+    // 6. reset clock
+    mmio_write(register_addr, 0);
 }
 
 /// Get the current status if falling edge detection is set