diff --git a/src/gpio.rs b/src/gpio.rs
index 483e812..f44e990 100644
--- a/src/gpio.rs
+++ b/src/gpio.rs
@@ -1,30 +1,52 @@
-use crate::{
-    delay,
-    uart::{self},
-};
+use crate::uart::{self};
 
-const GPFSEL2: u32 = 0x3F20_0008; //GPIO 20-29
-const GPFSEL4: u32 = 0x3F20_0010; //GPIO 40-49
-const GPSET0: u32 = 0x3F20_001C;
-const GPCLR0: u32 = 0x3F20_0028;
+const GPFSEL_BASE: u32 = 0x3F20_0000;
+const GPSET_BASE: u32 = 0x3F20_001C;
+const GPCLR_BASE: u32 = 0x3F20_0028;
 
-unsafe fn set_gpio29_to_output() {
-    let value: u32 = 0b001 << 27;
+unsafe fn set_gpio_to_output(gpio: u8) -> Result<(), &'static str> {
+    if gpio > 53 {
+        return Err("GPIO out of range");
+    }
 
-    core::ptr::write_volatile(GPFSEL2 as *mut u32, value);
+    let register_index = gpio / 10;
+    let register_offset = (gpio % 10) * 3;
+    let register_addr = GPFSEL_BASE + (register_index as u32 * 4);
+
+    let current = core::ptr::read_volatile(register_addr as *const u32);
+
+    let mask = !(0b111 << register_offset);
+    let cleared = current & mask;
+
+    let new_val = cleared | (0b001 << register_offset);
+
+    core::ptr::write_volatile(register_addr as *mut u32, new_val);
+    Ok(())
 }
 
-pub fn pull_up_gpio29() {
+pub fn pull_up_gpio(gpio: u8) -> Result<(), &'static str> {
     unsafe {
         uart::print("Pull Up\n");
-        set_gpio29_to_output();
-        core::ptr::write_volatile(GPSET0 as *mut u32, 1 << 29);
+        set_gpio_to_output(29)?;
+
+        let register_index = gpio / 32;
+        let register_offset = gpio % 32;
+        let register_addr = GPSET_BASE + (register_index as u32 * 4);
+
+        core::ptr::write_volatile(register_addr as *mut u32, 1 << register_offset);
     }
+    Ok(())
 }
 
-pub fn pull_down_gpio29() {
+pub fn pull_down_gpio(gpio: u8) -> Result<(), &'static str> {
     unsafe {
         uart::print("Pull Down\n");
-        core::ptr::write_volatile(GPCLR0 as *mut u32, 1 << 29);
+
+        let register_index = gpio / 32;
+        let register_offset = gpio % 32;
+        let register_addr = GPCLR_BASE + (register_index as u32 * 4);
+
+        core::ptr::write_volatile(register_addr as *mut u32, 1 << register_offset);
     }
+    Ok(())
 }
diff --git a/src/main.rs b/src/main.rs
index 51832bb..c8c7d17 100644
--- a/src/main.rs
+++ b/src/main.rs
@@ -4,7 +4,7 @@
 
 use core::{arch::asm, panic::PanicInfo};
 
-use gpio::{pull_down_gpio29, pull_up_gpio29};
+use gpio::{pull_down_gpio, pull_up_gpio};
 
 mod gpio;
 mod uart;
@@ -12,7 +12,6 @@ mod uart;
 #[panic_handler]
 fn panic(_panic: &PanicInfo) -> ! {
     loop {
-        pull_up_gpio29();
         uart::print("Panic");
     }
 }
@@ -33,9 +32,9 @@ extern "C" fn main() {
     uart::print("Hello World!\n");
 
     loop {
-        pull_up_gpio29();
+        let _ = pull_up_gpio(29);
         unsafe { delay(1_000_000) }
-        pull_down_gpio29();
+        let _ = pull_down_gpio(29);
         unsafe { delay(1_000_000) }
     }
 }