diff --git a/src/hal/hal.zig b/src/hal/hal.zig
index c875b9f..5d2d754 100644
--- a/src/hal/hal.zig
+++ b/src/hal/hal.zig
@@ -1,8 +1,10 @@
 pub const registers = @import("registers.zig");
 pub const resets = @import("resets.zig");
-pub const uart = @import("peripherals/uart.zig");
 pub const clocks = @import("clocks.zig");
 
+pub const uart = @import("peripherals/uart.zig");
+pub const gpio = @import("peripherals/gpio.zig");
+
 // NOTE: Not really happy with this
 pub const mmio = struct {
     pub inline fn write(address: u32, value: u32) void {
@@ -31,7 +33,7 @@ inline fn MMIO(address: u32) *volatile u32 {
 }
 
 // TODO: Do proper waiting
-fn dummy_wait(max_cycles: u32) void {
+pub fn dummy_wait(max_cycles: u32) void {
     var current_cycle: u32 = 0;
     while (current_cycle < max_cycles) : (current_cycle += 1) {
         asm volatile ("add zero, zero, zero");
diff --git a/src/hal/peripherals/gpio.zig b/src/hal/peripherals/gpio.zig
index 8d6d5dc..ee8e7a0 100644
--- a/src/hal/peripherals/gpio.zig
+++ b/src/hal/peripherals/gpio.zig
@@ -1,3 +1,47 @@
-// TODO:
-// - This file
-// - Move flashing led functions to here from startup.zig
+// TODO: Probably this file should include more aside from SIO
+const hal = @import("../hal.zig");
+
+const PAD_ISOLATION_BIT = 8;
+const PAD_INPUT_ENABLE_BIT = 6;
+
+fn pinAddressBit(base_addr: u32, pin: u8) !struct { reg_addr: u32, pin_bit: u5 } {
+    return switch (pin) {
+        0...31 => .{ .reg_addr = base_addr, .pin_bit = @truncate(pin) },
+        32...47 => .{ .reg_addr = base_addr + 4, .pin_bit = @truncate(pin - 32) },
+        else => error.InvalidPin,
+    };
+}
+
+pub fn init(pin_number: u8, pinType: enum { output, input }) !void {
+    const pin: u32 = @intCast(pin_number);
+    hal.mmio.write(hal.registers.GPIO_CTRL + (pin * 8), 5);
+    hal.mmio.write_clr(hal.registers.PADS_BANK0_GPIO + (pin * 4), 1 << PAD_ISOLATION_BIT);
+
+    switch (pinType) {
+        .input => {
+            hal.mmio.write(hal.registers.PADS_BANK0_GPIO + (pin * 4), 1 << PAD_ISOLATION_BIT | 1 << PAD_INPUT_ENABLE_BIT);
+        },
+        .output => {
+            const addr = try pinAddressBit(hal.registers.SIO_GPIO_OE_SET, pin_number);
+            hal.mmio.write_set(addr.reg_addr, @as(u32, 1) << addr.pin_bit);
+        },
+    }
+}
+
+pub fn toggle(pin: u8) !void {
+    const addr = try pinAddressBit(hal.registers.SIO_GPIO_OUT_XOR, pin);
+    hal.mmio.write(addr.reg_addr, @as(u32, 1) << addr.pin_bit);
+}
+
+pub fn set(pin: u8, state: bool) !void {
+    const addr = try pinAddressBit(switch (state) {
+        true => hal.registers.SIO_GPIO_OUT_SET,
+        false => hal.registers.SIO_GPIO_OUT_CLR,
+    }, pin);
+    hal.mmio.write(addr.reg_addr, @as(u32, 1) << addr.pin_bit);
+}
+
+pub fn readDigital(pin: u8) bool {
+    const addr = pinAddressBit(hal.registers.SIO_GPIO_IN, pin);
+    return hal.mmio.read(addr.reg_addr) & (@as(u32, 1) << addr.pin_bit) == 1;
+}
diff --git a/src/startup.zig b/src/startup.zig
index d2e071d..858d7a9 100644
--- a/src/startup.zig
+++ b/src/startup.zig
@@ -3,40 +3,18 @@ const clocks = hal.clocks;
 
 const LED_PIN: u32 = 25;
 
-fn flash_led(comptime max_delay: u32, comptime inf: bool) void {
-    if (!inf) {
-        var times: u8 = 0;
-        while (times < 10) : (times += 1) {
-            hal.mmio.write(hal.registers.SIO_GPIO_OUT_XOR, 1 << LED_PIN);
-
-            var delay: u32 = 0;
-            while (delay < max_delay) : (delay += 1) {
-                asm volatile ("add zero, zero, zero");
-            }
-        }
-    } else {
-        while (true) {
-            hal.mmio.write(hal.registers.SIO_GPIO_OUT_XOR, 1 << LED_PIN);
-
-            var delay: u32 = 0;
-            while (delay < max_delay) : (delay += 1) {
-                asm volatile ("add zero, zero, zero");
-            }
-        }
+fn flash_led(max_delay: u32, times: i16) void {
+    var counter: i16 = 0;
+    while (times == -1 or counter < times) : (counter += 1) {
+        hal.gpio.toggle(LED_PIN) catch unreachable;
+        hal.dummy_wait(max_delay);
     }
 }
 
 export fn _main() void {
-    // Set GPIO function to 5, 5 = SIO
-    hal.mmio.write(hal.registers.GPIO_CTRL + (LED_PIN * 8), 5);
-
-    // Clear pad isolation bit
-    hal.mmio.write_clr(hal.registers.PADS_BANK0_GPIO + (LED_PIN * 4), 1 << 8);
-
-    // Set output enable for the led pin
-    hal.mmio.write_set(hal.registers.SIO_GPIO_OE_SET, 1 << LED_PIN);
+    hal.gpio.init(LED_PIN, .output) catch unreachable;
 
-    flash_led(1_000_000, false);
+    flash_led(1_000_000, 10);
 
     clocks.xoscInit(.pico2_default);
     clocks.switchCLKRef();
@@ -47,5 +25,5 @@ export fn _main() void {
     //hal.uart.init(.UART0, 12_000_000, 115200);
     //hal.uart.write(.UART0, "Hello!\n");
 
-    flash_led(1_000_000, true);
+    flash_led(1_000_000, -1);
 }