diff --git a/firmware_files/bootcode.bin b/firmware_files/bootcode.bin
new file mode 100755
index 0000000..d8e8936
Binary files /dev/null and b/firmware_files/bootcode.bin differ
diff --git a/config.txt b/firmware_files/config.txt
similarity index 65%
rename from config.txt
rename to firmware_files/config.txt
index e79395d..2bfa2a7 100755
--- a/config.txt
+++ b/firmware_files/config.txt
@@ -1,4 +1,3 @@
 kernel=kernel8.img
 arm_64bit=1
 enable_uart=1
-program_usb_boot_mode=1
diff --git a/firmware_files/start.elf b/firmware_files/start.elf
new file mode 100755
index 0000000..2bca3c5
Binary files /dev/null and b/firmware_files/start.elf differ
diff --git a/src/gpio.rs b/src/gpio.rs
index 06a36ea..0dcb4f7 100644
--- a/src/gpio.rs
+++ b/src/gpio.rs
@@ -1,9 +1,16 @@
-use crate::uart::{self};
+use core::ptr::{read_volatile, write_volatile};
+
+use crate::{
+    timer::delay_nops,
+    uart::{self},
+};
 
 const GPFSEL_BASE: u32 = 0x3F20_0000;
 const GPSET_BASE: u32 = 0x3F20_001C;
 const GPCLR_BASE: u32 = 0x3F20_0028;
 const GPLEV_BASE: u32 = 0x3F20_0034;
+const GPPUD: u32 = 0x3F20_0094;
+const GPPUDCLK_BASE: u32 = 0x3F20_0098;
 
 #[repr(u32)]
 pub enum GPIOState {
@@ -39,8 +46,6 @@ pub unsafe fn set_gpio_state(gpio: u8, state: GPIOState) -> Result<(), &'static
 
 pub fn gpio_high(gpio: u8) -> Result<(), &'static str> {
     unsafe {
-        uart::print("Pull Up\n");
-
         let register_index = gpio / 32;
         let register_offset = gpio % 32;
         let register_addr = GPSET_BASE + (register_index as u32 * 4);
@@ -52,8 +57,6 @@ pub fn gpio_high(gpio: u8) -> Result<(), &'static str> {
 
 pub fn gpio_low(gpio: u8) -> Result<(), &'static str> {
     unsafe {
-        uart::print("Pull Down\n");
-
         let register_index = gpio / 32;
         let register_offset = gpio % 32;
         let register_addr = GPCLR_BASE + (register_index as u32 * 4);
@@ -73,3 +76,38 @@ pub fn gpio_get_state(gpio: u8) -> u8 {
         return ((state >> register_offset) & 0b1) as u8;
     }
 }
+
+pub fn gpio_pull_up(gpio: u8) {
+    gpio_pull_up_down(gpio, 0b10);
+}
+
+pub fn gpio_pull_down(gpio: u8) {
+    gpio_pull_up_down(gpio, 0b01);
+}
+
+fn gpio_pull_up_down(gpio: u8, val: u32) {
+    unsafe {
+        // Determine GPPUDCLK Register
+        let register_addr = gpio / 32;
+        let register_offset = gpio % 32;
+
+        // 1. Write Pull up
+        write_volatile(GPPUD as *mut u32, val);
+
+        // 2. Delay 150 cycles
+        delay_nops(150);
+
+        // 3. Write to clock
+        let new_val = 0b1 << register_offset;
+        write_volatile(register_addr as *mut u32, new_val);
+
+        // 4. Delay 150 cycles
+        delay_nops(150);
+
+        // 5. reset GPPUD
+        write_volatile(GPPUD as *mut u32, 0);
+
+        // 6. reset clock
+        write_volatile(register_addr as *mut u32, 0);
+    }
+}
diff --git a/src/main.rs b/src/main.rs
index c127110..77a69c5 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -21,6 +21,7 @@ fn panic(_panic: &PanicInfo) -> ! {
 #[no_mangle]
 #[link_section = ".text._start"]
 pub unsafe extern "C" fn _start() {
+    // Set the stack pointer
     asm!("ldr x0, =0x8004000", "mov sp, x0");
     main();
 }
@@ -29,19 +30,25 @@ pub unsafe extern "C" fn _start() {
 extern "C" fn main() {
     uart::configure_uart();
     unsafe {
+        // Set ACT Led to Outout
         let _ = set_gpio_state(29, gpio::GPIOState::Output);
+
+        // Set GPIO Pins to UART
         let _ = set_gpio_state(14, gpio::GPIOState::Alternative0);
         let _ = set_gpio_state(15, gpio::GPIOState::Alternative0);
     }
 
     // Delay so clock speed can stabilize
-    unsafe { delay_nops(50000) }
+    delay_nops(50000);
     uart::print("Hello World!\n");
 
+    sleep(500_000);
+
     loop {
         let _ = gpio_high(29);
-        unsafe { sleep(500_000) } // 0.5s
+
+        sleep(500_000); // 0.5s
         let _ = gpio_low(29);
-        unsafe { sleep(500_000) } // 0.5s
+        sleep(500_000) // 0.5s
     }
 }
diff --git a/src/timer.rs b/src/timer.rs
index 906907d..b7db428 100644
--- a/src/timer.rs
+++ b/src/timer.rs
@@ -4,15 +4,15 @@ fn read_clo() -> u32 {
     unsafe { return core::ptr::read_volatile(TIMER_CLO as *const u32) }
 }
 
-pub unsafe fn sleep(microseconds: u32) {
+pub fn sleep(microseconds: u32) {
     let start = read_clo();
     while read_clo() - start < microseconds {
-        core::arch::asm!("nop");
+        unsafe { core::arch::asm!("nop") }
     }
 }
 
-pub unsafe fn delay_nops(count: u32) {
+pub fn delay_nops(count: u32) {
     for _ in 0..count {
-        core::arch::asm!("nop");
+        unsafe { core::arch::asm!("nop") }
     }
 }