diff --git a/src/gpio.rs b/src/gpio.rs
index 0dcb4f7..dc0e1e7 100644
--- a/src/gpio.rs
+++ b/src/gpio.rs
@@ -24,7 +24,7 @@ pub enum GPIOState {
     Alternative5 = 0b010,
 }
 
-pub unsafe fn set_gpio_state(gpio: u8, state: GPIOState) -> Result<(), &'static str> {
+pub fn set_gpio_state(gpio: u8, state: GPIOState) -> Result<(), &'static str> {
     if gpio > 53 {
         return Err("GPIO out of range");
     }
@@ -32,15 +32,16 @@ pub unsafe fn set_gpio_state(gpio: u8, state: GPIOState) -> Result<(), &'static
     let register_index = gpio / 10;
     let register_offset = (gpio % 10) * 3;
     let register_addr = GPFSEL_BASE + (register_index as u32 * 4);
+    unsafe {
+        let current = core::ptr::read_volatile(register_addr as *const u32);
 
-    let current = core::ptr::read_volatile(register_addr as *const u32);
+        let mask = !(0b111 << register_offset);
+        let cleared = current & mask;
 
-    let mask = !(0b111 << register_offset);
-    let cleared = current & mask;
+        let new_val = cleared | ((state as u32) << register_offset);
 
-    let new_val = cleared | ((state as u32) << register_offset);
-
-    core::ptr::write_volatile(register_addr as *mut u32, new_val);
+        core::ptr::write_volatile(register_addr as *mut u32, new_val);
+    }
     Ok(())
 }
 
@@ -88,7 +89,7 @@ pub fn gpio_pull_down(gpio: u8) {
 fn gpio_pull_up_down(gpio: u8, val: u32) {
     unsafe {
         // Determine GPPUDCLK Register
-        let register_addr = gpio / 32;
+        let register_addr = GPPUDCLK_BASE + 4 * (gpio as u32 / 32);
         let register_offset = gpio % 32;
 
         // 1. Write Pull up
diff --git a/src/main.rs b/src/main.rs
index 77a69c5..2441f62 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,8 +4,9 @@
 
 use core::{arch::asm, panic::PanicInfo};
 
-use gpio::{gpio_high, gpio_low, set_gpio_state};
+use gpio::{gpio_get_state, gpio_high, gpio_low, gpio_pull_up, set_gpio_state};
 use timer::{delay_nops, sleep};
+use uart::print;
 
 mod gpio;
 mod timer;
@@ -29,18 +30,20 @@ pub unsafe extern "C" fn _start() {
 #[no_mangle]
 extern "C" fn main() {
     uart::configure_uart();
-    unsafe {
-        // Set ACT Led to Outout
-        let _ = set_gpio_state(29, gpio::GPIOState::Output);
+    // Set ACT Led to Outout
+    let _ = set_gpio_state(21, gpio::GPIOState::Output);
 
-        // Set GPIO Pins to UART
-        let _ = set_gpio_state(14, gpio::GPIOState::Alternative0);
-        let _ = set_gpio_state(15, gpio::GPIOState::Alternative0);
-    }
+    // Set GPIO Pins to UART
+    let _ = set_gpio_state(14, gpio::GPIOState::Alternative0);
+    let _ = set_gpio_state(15, gpio::GPIOState::Alternative0);
+
+    // Set GPIO 21 to Input
+    let _ = set_gpio_state(21, gpio::GPIOState::Input);
+    gpio_pull_up(21);
 
     // Delay so clock speed can stabilize
     delay_nops(50000);
-    uart::print("Hello World!\n");
+    uart::print("Hello World!\r\n");
 
     sleep(500_000);
 
@@ -49,6 +52,18 @@ extern "C" fn main() {
 
         sleep(500_000); // 0.5s
         let _ = gpio_low(29);
-        sleep(500_000) // 0.5s
+        sleep(500_000); // 0.5s
+        print_gpio_state();
     }
 }
+
+fn print_gpio_state() {
+    let state = gpio_get_state(21);
+
+    let ascii_byte = b'0' + state;
+    let data = [ascii_byte];
+
+    let s = str::from_utf8(&data).unwrap();
+    print(s);
+    print("\r\n");
+}
diff --git a/tools/deply_to_hw.sh b/tools/deply_to_hw.sh
index c439333..8e067e4 100755
--- a/tools/deply_to_hw.sh
+++ b/tools/deply_to_hw.sh
@@ -15,7 +15,7 @@ REMOTE_DIR="$TFTP_PATH"
 
 # === BUILD ===
 echo "[*] Building kernel..."
-eval $BUILD_COMMAND
+cargo build --release
 
 # === CONVERT TO IMG ===
 echo "[*] Convert kernel elf to img..."