diff --git a/Demo/Drivers/gpio.c b/Demo/Drivers/gpio.c
index 485de89..15cec85 100644
--- a/Demo/Drivers/gpio.c
+++ b/Demo/Drivers/gpio.c
@@ -5,130 +5,170 @@
 
 #include "gpio.h"
 
-typedef struct {
-	unsigned long	GPFSEL[6];	///< Function selection registers.
-	unsigned long	Reserved_1;
-	unsigned long	GPSET[2];
-	unsigned long	Reserved_2;
-	unsigned long	GPCLR[2];
-	unsigned long	Reserved_3;
-	unsigned long	GPLEV[2];
-	unsigned long	Reserved_4;
-	unsigned long	GPEDS[2];
-	unsigned long	Reserved_5;
-	unsigned long	GPREN[2];
-	unsigned long	Reserved_6;
-	unsigned long	GPFEN[2];
-	unsigned long	Reserved_7;
-	unsigned long	GPHEN[2];
-	unsigned long	Reserved_8;
-	unsigned long	GPLEN[2];
-	unsigned long	Reserved_9;
-	unsigned long	GPAREN[2];
-	unsigned long	Reserved_A;
-	unsigned long	GPAFEN[2];
-	unsigned long	Reserved_B;
-	unsigned long	GPPUD[1];
-	unsigned long	GPPUDCLK[2];
-	//Ignoring the reserved and test bytes
-} BCM2835_GPIO_REGS;
-
-volatile BCM2835_GPIO_REGS * const pRegs = (BCM2835_GPIO_REGS *) (0x20200000);
-
-
-void SetGpioFunction(unsigned int pinNum, unsigned int funcNum) {
-
-	int offset = pinNum / 10;
-
-	unsigned long val = pRegs->GPFSEL[offset];	// Read in the original register value.
-
-	int item = pinNum % 10;
-	val &= ~(0x7 << (item * 3));
-	val |= ((funcNum & 0x7) << (item * 3));
-	pRegs->GPFSEL[offset] = val;
+#define BANK(pin)	((pin) >> 5)
+#define MASK(pin)	(1UL << ((pin) & 0x1F))
+
+// Base memory location for GPIO registers:
+#define GPIO_REG_BASE	0x20200000
+
+// Define a GPIO register:
+#define GPIO_REG(offs)	((volatile unsigned long *) (GPIO_REG_BASE + offs))
+
+// Function selection:
+#define GPFSEL		GPIO_REG (0x0000)
+
+// Set and clear pins:
+#define GPSET		GPIO_REG (0x001C)
+#define GPCLR		GPIO_REG (0x0028)
+
+// Pin level readout:
+#define GPLEV		GPIO_REG (0x0034)
+
+// Pin event detection status:
+#define GPEDS		GPIO_REG (0x0040)
+
+// Pin event detection:
+#define GPREN		GPIO_REG (0x004C)
+#define GPFEN		GPIO_REG (0x0058)
+#define GPHEN		GPIO_REG (0x0064)
+#define GPLEN		GPIO_REG (0x0070)
+#define GPAREN		GPIO_REG (0x007C)
+#define GPAFEN		GPIO_REG (0x0088)
+
+// Pull up/down/none:
+#define GPPUD		GPIO_REG (0x0094)
+#define GPPUDCLK	GPIO_REG (0x0098)
+
+void
+gpioFunctionSet (const unsigned int pin, const enum GpioFunc func)
+{
+	unsigned long bank = pin / 10;
+	unsigned long item = pin % 10;
+	unsigned long mask = 7UL << (item * 3);
+
+	// Get current register value:
+	unsigned long reg = GPFSEL[bank];
+
+	// Mask out the bits for this pin:
+	reg &= ~mask;
+
+	// Insert new bits for this pin:
+	reg |= (unsigned long)func << (item * 3);
+
+	// Store back:
+	GPFSEL[bank] = reg;
 }
 
-void SetGpioDirection(unsigned int pinNum, enum GPIO_DIR dir) {
-	SetGpioFunction(pinNum,dir);
+enum GpioFunc
+gpioFunctionGet (const unsigned int pin)
+{
+	unsigned long bank = pin / 10;
+	unsigned long item = pin % 10;
+
+	return (GPFSEL[bank] >> (item * 3)) & 7;
 }
 
-void SetGpio(unsigned int pinNum, unsigned int pinVal) {
-	unsigned long offset=pinNum/32;
-	unsigned long mask=(1<<(pinNum%32));
+void
+gpioWrite (const unsigned int pin, const unsigned int val)
+{
+	unsigned long bank = BANK(pin);
+	unsigned long mask = MASK(pin);
 
-	if(pinVal) {
-		pRegs->GPSET[offset]|=mask;
-	} else {
-		pRegs->GPCLR[offset]|=mask;
-	}
+	val ? (GPSET[bank] |= mask)
+	    : (GPCLR[bank] |= mask);
 }
 
-int ReadGpio(unsigned int pinNum) {
-	return ((pRegs->GPLEV[pinNum/32])>>(pinNum%32))&1;
+unsigned int
+gpioRead (const unsigned int pin)
+{
+	unsigned long bank = BANK(pin);
+	unsigned long mask = MASK(pin);
+
+	return (GPLEV[bank] & mask) ? 1 : 0;
 }
 
-void EnableGpioDetect(unsigned int pinNum, enum DETECT_TYPE type)
+static volatile unsigned long *detect_map[] = {
+	[GPIO_DETECT_RISING]        = GPREN,
+	[GPIO_DETECT_FALLING]       = GPFEN,
+	[GPIO_DETECT_HIGH]          = GPHEN,
+	[GPIO_DETECT_LOW]           = GPLEN,
+	[GPIO_DETECT_RISING_ASYNC]  = GPAREN,
+	[GPIO_DETECT_FALLING_ASYNC] = GPAFEN,
+};
+
+void
+gpioDetectEnable (const unsigned int pin, const enum GpioDetect type)
 {
-	unsigned long mask=(1<<pinNum);
-	unsigned long offset=pinNum/32;
-	
-	switch(type) {
-	case DETECT_RISING:
-		pRegs->GPREN[offset]|=mask;
-		break;
-	case DETECT_FALLING:
-		pRegs->GPFEN[offset]|=mask;
-		break;
-	case DETECT_HIGH:
-		pRegs->GPHEN[offset]|=mask;
-		break;
-	case DETECT_LOW:
-		pRegs->GPLEN[offset]|=mask;
-		break;
-	case DETECT_RISING_ASYNC:
-		pRegs->GPAREN[offset]|=mask;
-		break;
-	case DETECT_FALLING_ASYNC:
-		pRegs->GPAFEN[offset]|=mask;
-		break;
-	case DETECT_NONE:
-		break;
-	}
+	unsigned long bank = BANK(pin);
+	unsigned long mask = MASK(pin);
+
+	detect_map[type][bank] |= mask;
 }
 
-void DisableGpioDetect(unsigned int pinNum, enum DETECT_TYPE type)
+void
+gpioDetectDisable (const unsigned int pin, const enum GpioDetect type)
 {
-	unsigned long mask=~(1<<(pinNum%32));
-	unsigned long offset=pinNum/32;
-	
-	switch(type) {
-	case DETECT_RISING:
-		pRegs->GPREN[offset]&=mask;
-		break;
-	case DETECT_FALLING:
-		pRegs->GPFEN[offset]&=mask;
-		break;
-	case DETECT_HIGH:
-		pRegs->GPHEN[offset]&=mask;
-		break;
-	case DETECT_LOW:
-		pRegs->GPLEN[offset]&=mask;
-		break;
-	case DETECT_RISING_ASYNC:
-		pRegs->GPAREN[offset]&=mask;
-		break;
-	case DETECT_FALLING_ASYNC:
-		pRegs->GPAFEN[offset]&=mask;
-		break;
-	case DETECT_NONE:
-		break;
-	}
+	unsigned long bank = BANK(pin);
+	unsigned long mask = MASK(pin);
+
+	detect_map[type][bank] &= ~mask;
+}
+
+unsigned int
+gpioDetected (const unsigned int pin)
+{
+	unsigned long bank = BANK(pin);
+	unsigned long mask = MASK(pin);
+
+	// No event if bit not set:
+	if (!(GPEDS[bank] & mask))
+		return 0;
+
+	// Clear event by setting bit to 1:
+	GPEDS[bank] |= mask;
+
+	// Confirm event:
+	return 1;
 }
 
-void ClearGpioInterrupt(unsigned int pinNum)
+static inline void
+wait (unsigned long cycles)
 {
-	unsigned long mask=(1<<(pinNum%32));
-	unsigned long offset=pinNum/32;
+	// Each loop iteration takes three instructions:
+	cycles /= 3;
+
+	// Busy-wait by decrementing the cycle counter until it reaches zero:
+	asm volatile (
+		"1: sub %0, %0, #1  \n\t"
+		"   cmp %0, #0      \n\t"
+		"   bne 1b          \n\t"
+		:
+		: "r" (cycles)
+		: "cc"
+	);
+}
+
+void
+gpioPull (const unsigned int pin, const enum GpioPull type)
+{
+	unsigned long bank = BANK(pin);
+	unsigned long mask = MASK(pin);
+
+	// Setup direction register:
+	*GPPUD = type;
+
+	// Wait 150 cycles to set up the control signal:
+	wait(150);
+
+	// Activate clock signal for this pin:
+	GPPUDCLK[bank] |= mask;
+
+	// Wait 150 cycles to hold the control signal:
+	wait(150);
+
+	// Remove control signal:
+	*GPPUD = 0;
 
-	pRegs->GPEDS[offset]=mask;
+	// Remove clock signal:
+	GPPUDCLK[bank] = 0;
 }
diff --git a/Demo/Drivers/gpio.h b/Demo/Drivers/gpio.h
index fa45970..c10c44e 100644
--- a/Demo/Drivers/gpio.h
+++ b/Demo/Drivers/gpio.h
@@ -1,48 +1,54 @@
 #ifndef _GPIO_H_
 #define _GPIO_H_
 
-/* GPIO event detect types */
-enum DETECT_TYPE {
-	DETECT_NONE,
-	DETECT_RISING,
-	DETECT_FALLING,
-	DETECT_HIGH,
-	DETECT_LOW,
-	DETECT_RISING_ASYNC,
-	DETECT_FALLING_ASYNC
+// GPIO event detect types
+enum GpioDetect {
+	GPIO_DETECT_RISING,
+	GPIO_DETECT_FALLING,
+	GPIO_DETECT_HIGH,
+	GPIO_DETECT_LOW,
+	GPIO_DETECT_RISING_ASYNC,
+	GPIO_DETECT_FALLING_ASYNC,
 };
 
-/* GPIO pull up or down states */
-enum PULL_STATE {
-	PULL_DISABLE,
-	PULL_UP,
-	PULL_DOWN,
-	PULL_RESERVED
+// GPIO pull up or down states
+enum GpioPull {
+	GPIO_PULL_DISABLE	= 0,
+	GPIO_PULL_DOWN		= 1,
+	GPIO_PULL_UP		= 2,
+	GPIO_PULL_RESERVED	= 3,
 };
 
-/* Pin data direction */
-enum GPIO_DIR {
-	GPIO_IN,
-	GPIO_OUT
+// Pin functions
+enum GpioFunc {
+	GPIO_FUNC_INPUT		= 0,	// Pin is input
+	GPIO_FUNC_OUTPUT	= 1,	// Pin is output
+	GPIO_FUNC_ALT_0		= 4,	// Alternative function 0
+	GPIO_FUNC_ALT_1		= 5,	// Alternative function 1
+	GPIO_FUNC_ALT_2		= 6,	// Alternative function 2
+	GPIO_FUNC_ALT_3		= 7,	// Alternative function 3
+	GPIO_FUNC_ALT_4		= 3,	// Alternative function 4
+	GPIO_FUNC_ALT_5		= 2,	// Alternative function 5
 };
 
-/* GPIO pin setup */
-void SetGpioFunction	(unsigned int pinNum, unsigned int funcNum);
-/* A simple wrapper around SetGpioFunction */
-void SetGpioDirection	(unsigned int pinNum, enum GPIO_DIR dir);
+// Set GPIO pin function:
+void gpioFunctionSet (const unsigned int pin, const enum GpioFunc func);
 
-/* Set GPIO output level */
-void SetGpio			(unsigned int pinNum, unsigned int pinVal);
+// Get GPIO pin function:
+enum GpioFunc gpioFunctionGet (const unsigned int pin);
 
-/* Read GPIO pin level */
-int ReadGpio			(unsigned int pinNum);
+// Set GPIO output level:
+void gpioWrite (const unsigned int pin, const unsigned int val);
 
-/* GPIO pull up/down resistor control function (NOT YET IMPLEMENTED) */
-int PudGpio				(unsigned int pinNum, enum PULL_STATE state);
+// Read GPIO pin level:
+unsigned int gpioRead (const unsigned int pin);
 
-/* Interrupt related functions */
-void EnableGpioDetect	(unsigned int pinNum, enum DETECT_TYPE type);
-void DisableGpioDetect	(unsigned int pinNum, enum DETECT_TYPE type);
-void ClearGpioInterrupt	(unsigned int pinNum);
+// GPIO pull-up/down/none:
+void gpioPull (const unsigned int pin, const enum GpioPull type);
+
+// Event detection functions:
+void gpioDetectEnable     (const unsigned int pin, const enum GpioDetect type);
+void gpioDetectDisable    (const unsigned int pin, const enum GpioDetect type);
+unsigned int gpioDetected (const unsigned int pin);
 
 #endif
diff --git a/Demo/main.c b/Demo/main.c
index 183a259..a13c8b2 100644
--- a/Demo/main.c
+++ b/Demo/main.c
@@ -9,7 +9,7 @@ void task1(void *pParam) {
 	int i = 0;
 	while(1) {
 		i++;
-		SetGpio(16, 1);
+		gpioWrite(16, 1);
 		vTaskDelay(200);
 	}
 }
@@ -20,7 +20,7 @@ void task2(void *pParam) {
 	while(1) {
 		i++;
 		vTaskDelay(100);
-		SetGpio(16, 0);
+		gpioWrite(16, 0);
 		vTaskDelay(100);
 	}
 }
@@ -34,7 +34,7 @@ void task2(void *pParam) {
  **/
 void main (void)
 {
-	SetGpioFunction(16, 1);			// RDY led
+	gpioFunctionSet(16, GPIO_FUNC_OUTPUT);		// RDY led
 
 	xTaskCreate(task1, "LED_0", 128, NULL, 0, NULL);
 	xTaskCreate(task2, "LED_1", 128, NULL, 0, NULL);