diff --git a/src/flashtest.c b/src/flashtest.c
index 2e57513..b0c6f74 100644
--- a/src/flashtest.c
+++ b/src/flashtest.c
@@ -443,7 +443,6 @@ int main (void)
 	int result, i, j, cnt, size;
 	uint32_t volatile *pUART_baud = (uint32_t*)SYS_UART_BAUD;
 	uint32_t volatile *pUART_stat = (uint32_t*)SYS_UART_STAT;
-	uint32_t volatile *pGPIO = (uint32_t*)SYS_GPIO_0_DATA;
 	uint32_t volatile *pReg_usec = (uint32_t*)SYS_TIMER_USEC;
 	uint32_t volatile *pReg_sec = (uint32_t*)SYS_TIMER_SEC;
 	uint32_t volatile *pSSRAM = (uint32_t*)SYS_SSRAM_IO;
@@ -474,15 +473,11 @@ int main (void)
 	struct timeval time_sec;
 	uint32_t *pPic;
 
-	gpio_if_t gpio_if;
-
 	pPic = (uint32_t*)malloc(800*600*sizeof(uint32_t));
 	memcpy(pPic, pFlashPicture, 800*600*sizeof(uint32_t));
 
 	size_t g_cnt = 1;
 
-	gpio_init(&gpio_if, SYS_GPIO_0_BASE);
-
 	*pVGA_cgcol = 0x00FFFFFF;
 
 	*pTim0_cnt = 0;
@@ -497,10 +492,6 @@ int main (void)
 	setbuf(stdout, NULL);
 	PrintCPUinfo();
 
-	gpio_write(&gpio_if, GPIO_0_MDIO_RST, GPIO_0_ALIGN_MDIO, GPIO_DATA_OFFSET, 1);
-	sleep(100);
-	gpio_write(&gpio_if, GPIO_0_MDIO_RST, GPIO_0_ALIGN_MDIO, GPIO_DATA_OFFSET, 0);
-
 	flash_t flash;
 	flash_find(&flash, SYS_FLASH_IO);
 	flash_print_info(&flash);
@@ -511,8 +502,6 @@ int main (void)
 	cnt = 0;
 	do
 	{
-		gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, cnt);
-
 //		printf("-------------------------------------------\n");
 //		printf("-- Mem Test -------------------------------\n");
 //		printf("-------------------------------------------\n");
diff --git a/src/hpi.c b/src/hpi.c
index a1fd5d7..86188f9 100644
--- a/src/hpi.c
+++ b/src/hpi.c
@@ -10,7 +10,7 @@ static volatile uint32_t _g_int_active;
 static volatile uint32_t _g_mbx_return;
 static volatile uint32_t _g_mbx_in_flag;
 
-extern gpio_if_t _g_gpio;
+extern gpio_if_t _g_gpio_usb_data;
 
 // ---------------------------------------------------------
 // Globals
@@ -299,7 +299,7 @@ void cy67k3_reset(void)
 {
 	int i;
 
-	gpio_write(&_g_gpio, GPIO_0_MASK_USB, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, GPIO_0_USB_RST | GPIO_0_USB_INTEN);
+	gpio_set(&_g_gpio_usb_data, GPIO_0_USB_RST | GPIO_0_USB_INTEN);
 
 	for (i=0; i <160; i++)
 	{
@@ -314,7 +314,7 @@ void cy67k3_reset(void)
 		);
 	}
 
-	gpio_write(&_g_gpio, GPIO_0_USB_RST, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, ~GPIO_0_USB_RST);
+	gpio_clr(&_g_gpio_usb_data, GPIO_0_USB_RST);
 
 }
 
@@ -419,7 +419,7 @@ uint32_t hpi_init(void)
 
 	interrupt_register(4, cy67k3_isr);
 	interrupt_enable(4);
-	gpio_write(&_g_gpio, GPIO_0_USB_INTEN, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, GPIO_0_USB_INTEN);
+	gpio_set(&_g_gpio_usb_data, GPIO_0_USB_INTEN);
 
 	return HPI_NOERROR;
 }
diff --git a/src/libsys/gpio.c b/src/libsys/gpio.c
index 5a75e20..1db863b 100644
--- a/src/libsys/gpio.c
+++ b/src/libsys/gpio.c
@@ -2,27 +2,33 @@
 #include "gpio.h"
 
 // ---------------------------------------------------------
-uint32_t gpio_init(gpio_if_t *pGPIO, uint32_t base_addr)
+void gpio_init(gpio_if_t *pObj, uint32_t base_addr, uint32_t mask, uint32_t shift)
 {
-	pGPIO->pBase = (uint32_t*)base_addr;
-	return 0;
+	pObj->pBase = (uint32_t*)base_addr;
+	pObj->mask = mask;
+	pObj->mask_shifted = mask << shift;
+	pObj->shift = shift;
 }
 
-uint32_t gpio_clean(gpio_if_t *pGPIO)
+void gpio_write(gpio_if_t *pObj, uint32_t value)
 {
-	return 0;
+	*pObj->pBase = (value & pObj->mask) << pObj->shift;
 }
 
-uint32_t gpio_write(gpio_if_t *pGPIO, uint32_t mask, uint32_t shift, uint32_t offset, uint32_t value)
+uint32_t gpio_read(gpio_if_t *pObj)
 {
-	*(pGPIO->pBase + offset) = (((*(pGPIO->pBase + offset) >> shift) & ~mask) | (value & mask)) << shift;
-	return 0;
+	uint32_t result = (*pObj->pBase >> pObj->shift) & pObj->mask;
+	return result;
 }
 
-uint32_t gpio_read(gpio_if_t *pGPIO, uint32_t mask, uint32_t shift, uint32_t offset, uint32_t *pValue)
+void gpio_set(gpio_if_t *pObj, uint32_t value)
 {
-	*pValue = (*(pGPIO->pBase + offset) >> shift) & mask;
-	return 0;
+	*pObj->pBase |= (value & pObj->mask) << pObj->shift;
+}
+
+void gpio_clr(gpio_if_t *pObj, uint32_t value)
+{
+	*pObj->pBase &= ~((value | pObj->mask) << pObj->shift);
 }
 
 // ---------------------------------------------------------
diff --git a/src/libsys/gpio.h b/src/libsys/gpio.h
index 39575e7..bd2c9ea 100644
--- a/src/libsys/gpio.h
+++ b/src/libsys/gpio.h
@@ -1,41 +1,24 @@
 #ifndef GPIO_H
 #define GPIO_H
 
-#define GPIO_DATA_OFFSET 		0	/* Offset for data */
-#define GPIO_DIR_OFFSET			1	/* Offset from any channel */
-
 // ---------------------------------------------------------
 // Types
 // ---------------------------------------------------------
 typedef struct _sgpio_if_t
 {
 	volatile uint32_t *pBase;
+	volatile uint32_t mask;
+	volatile uint32_t mask_shifted;
+	volatile uint32_t shift;
 
 } gpio_if_t;
 
-/* Some quick macros */
+void gpio_init(gpio_if_t *pObj, uint32_t base_addr, uint32_t mask, uint32_t shift);
 
-/* Write/Read from the DATA register */
+void gpio_write(gpio_if_t *pObj, uint32_t value);
+uint32_t gpio_read(gpio_if_t *pObj);
 
-#define gpio_write_data(gpio,mask,shift,value) \
-        gpio_write(gpio, mask, shift, GPIO_DATA_OFFSET, value)
-
-#define gpio_read_data(gpio,mask,shift,ret) \
-        gpio_read(gpio, mask, shift, GPIO_DATA_OFFSET, ret)
-
-/* Write/Read from the TRISTATE register ( for bidirectional GPIOs) */
-
-#define gpio_set_dir(gpio,mask,shift,value) \
-        gpio_write(gpio, mask, shift, GPIO_DIR_OFFSET, value)
-
-#define gpio_get_dir(gpio,mask,shift,ret) \
-        gpio_read(gpio, mask, shift, GPIO_DIR_OFFSET, ret)
-
-
-uint32_t gpio_init(gpio_if_t *pGPIO, uint32_t base_addr);
-uint32_t gpio_clean(gpio_if_t *pGPIO);
-
-uint32_t gpio_write(gpio_if_t *pGPIO, uint32_t mask, uint32_t shift, uint32_t offset, uint32_t value);
-uint32_t gpio_read(gpio_if_t *pGPIO, uint32_t mask, uint32_t shift, uint32_t offset, uint32_t *pValue);
+void gpio_set(gpio_if_t *pObj, uint32_t value);
+void gpio_clr(gpio_if_t *pObj, uint32_t value);
 
 #endif // GPIO_H
diff --git a/src/test_hpi.c b/src/test_hpi.c
index 8032a65..492b0c2 100644
--- a/src/test_hpi.c
+++ b/src/test_hpi.c
@@ -13,7 +13,10 @@ static volatile uint32_t _g_uart_msg;
 static volatile uint32_t _g_rst;
 static volatile uint32_t _g_sus;
 static volatile uint32_t _g_cfg;
-gpio_if_t _g_gpio;
+gpio_if_t _g_gpio_usb_data;
+gpio_if_t _g_gpio_usb_dir;
+gpio_if_t _g_gpio_led_data;
+gpio_if_t _g_gpio_led_dir;
 
 #define CMD_SYNC_EB				0xC24755AA
 #define CMD_SYNC_EL				0x47C2AA55
@@ -619,15 +622,18 @@ int main(void)
 	_g_sus = 0;
 	_g_cfg = 0;
 
-	gpio_init(&_g_gpio, SYS_GPIO_0_BASE);
 
 	// USB
-	gpio_write(&_g_gpio, GPIO_0_MASK_USB, GPIO_0_ALIGN_USB, GPIO_DIR_OFFSET, GPIO_0_MASK_USB);
-	gpio_write(&_g_gpio, GPIO_0_MASK_USB, GPIO_0_ALIGN_USB, GPIO_DATA_OFFSET, 0);
+	gpio_init(&_g_gpio_usb_dir, SYS_GPIO_0_DIR, GPIO_0_MASK_USB, GPIO_0_ALIGN_USB);
+	gpio_init(&_g_gpio_usb_data, SYS_GPIO_0_DATA, GPIO_0_MASK_USB, GPIO_0_ALIGN_USB);
+	gpio_write(&_g_gpio_usb_dir, GPIO_0_MASK_USB);
+	gpio_write(&_g_gpio_usb_data, 0);
 
 	// LED
-	gpio_write(&_g_gpio, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DIR_OFFSET, GPIO_0_MASK_LED);
-	gpio_write(&_g_gpio, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, 0);
+	gpio_init(&_g_gpio_led_dir, SYS_GPIO_0_DIR, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED);
+	gpio_init(&_g_gpio_led_data, SYS_GPIO_0_DATA, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED);
+	gpio_write(&_g_gpio_led_dir, GPIO_0_MASK_LED);
+	gpio_write(&_g_gpio_led_data, 0);
 
 	usb_init();
 
diff --git a/src/testbench.c b/src/testbench.c
index 55767c2..7407f27 100644
--- a/src/testbench.c
+++ b/src/testbench.c
@@ -31,7 +31,11 @@
 extern int paranoia(int argc, char **argv);
 extern int calibrate_delay(void);
 
-gpio_if_t gpio_if;
+gpio_if_t gpio_led_data;
+gpio_if_t gpio_led_dir;
+gpio_if_t gpio_mdio_data;
+gpio_if_t gpio_mdio_dir;
+
 int g_cnt;
 box_t box;
 gfx_t gfx;
@@ -135,7 +139,7 @@ void handler7(EXCEPTION_CONTEXT *xcp)
 	if (*pTim_stat & 1)
 	{
 		*pTim_stat = 1;	// Acknowledge IRQ
-		gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, g_cnt++);
+		gpio_write(&gpio_led_data, g_cnt++);
 		GFX_frame(&gfx, GFX_FRAME_NOSYNC);
 		GFX_restore(&gfx, &box, posx, posy);
 		GFX_show(&gfx);
@@ -1066,7 +1070,6 @@ int main (void)
 
 	g_cnt = 1;
 	int screen_fd = open("VGA-0", 0, 0);
-	gpio_init(&gpio_if, SYS_GPIO_0_BASE);
 
 	*pVGA_cgcol = 0x00FFFFFF;
 
@@ -1082,10 +1085,6 @@ int main (void)
 	setbuf(stdout, NULL);
 	PrintCPUinfo();
 
-	gpio_write(&gpio_if, GPIO_0_MDIO_RST, GPIO_0_ALIGN_MDIO, GPIO_DATA_OFFSET, 1);
-	sleep(100);
-	gpio_write(&gpio_if, GPIO_0_MDIO_RST, GPIO_0_ALIGN_MDIO, GPIO_DATA_OFFSET, 0);
-
 	interrupt_register(3, handler3);
 	fprintf(stderr, "UART interrupt registered.\n");
 
@@ -1144,7 +1143,25 @@ int main (void)
 		}
 	}
 	srand(clock());
-	gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, uncached_cachemiss_bug());
+
+	// Init GPIO LED
+	gpio_init(&gpio_led_data, SYS_GPIO_0_DATA, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED);
+	gpio_init(&gpio_led_dir, SYS_GPIO_0_DIR, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED);
+
+	// Init GPIO MDIO
+	gpio_init(&gpio_mdio_data, SYS_GPIO_0_DATA, GPIO_0_MASK_MDIO, GPIO_0_ALIGN_MDIO);
+	gpio_init(&gpio_mdio_dir, SYS_GPIO_0_DIR, GPIO_0_MASK_MDIO, GPIO_0_ALIGN_MDIO);
+
+	// Set GPIO Dirs
+	gpio_set(&gpio_led_dir, GPIO_0_MASK_LED);
+	gpio_set(&gpio_mdio_dir, GPIO_0_MDIO_RST);
+
+	// Reset PHY
+	gpio_set(&gpio_mdio_data, GPIO_0_MDIO_RST);
+	sleep(100);
+	gpio_clr(&gpio_mdio_data, GPIO_0_MDIO_RST);
+	
+	gpio_write(&gpio_led_data, uncached_cachemiss_bug());
 
 	Test16_MemTest();
 
@@ -1170,8 +1187,11 @@ int main (void)
 	posx = posy = 0;
 	GFX_show(&gfx);
 
+	// Enable timer interrupt
 	interrupt_enable(7);
 	*pTim_ctrl |= 3;
+	
+	// Enable UART interrupt
 	interrupt_enable(3);
 	*pUART_stat |= (1 << 6);
 
@@ -1187,7 +1207,7 @@ int main (void)
 			interrupt_enable(7);
 		}
 
-		gpio_write(&gpio_if, GPIO_0_MASK_LED, GPIO_0_ALIGN_LED, GPIO_DATA_OFFSET, cnt);
+		gpio_write(&gpio_led_data, cnt);
 
 		printf("-------------------------------------------\n");
 		printf("-- LoadStore ------------------------------\n");