diff --git a/arch/arm/include/asm/arch-at91/at91_pio.h b/arch/arm/include/asm/arch-at91/at91_pio.h
index 416cabf..0ec2dca 100644
--- a/arch/arm/include/asm/arch-at91/at91_pio.h
+++ b/arch/arm/include/asm/arch-at91/at91_pio.h
@@ -66,14 +66,50 @@ typedef struct at91_port {
 	u32	puer;		/* 0x64 Pull-up Enable Register */
 	u32	pusr;		/* 0x68 Pad Pull-up Status Register */
 	u32	reserved4;
+#if defined(CPU_HAS_PIO3)
+	u32	abcdsr1;	/* 0x70 Peripheral ABCD Select Register 1 */
+	u32	abcdsr2;	/* 0x74 Peripheral ABCD Select Register 2 */
+	u32	reserved5[2];
+	u32	ifscdr;		/* 0x80 Input Filter SCLK Disable Register */
+	u32	ifscer;		/* 0x84 Input Filter SCLK Enable Register */
+	u32	ifscsr;		/* 0x88 Input Filter SCLK Status Register */
+	u32	scdr;		/* 0x8C SCLK Divider Debouncing Register */
+	u32	ppddr;		/* 0x90 Pad Pull-down Disable Register */
+	u32	ppder;		/* 0x94 Pad Pull-down Enable Register */
+	u32	ppdsr;		/* 0x98 Pad Pull-down Status Register */
+	u32	reserved6;	/*  */
+#else
 	u32	asr;		/* 0x70 Select A Register */
 	u32	bsr;		/* 0x74 Select B Register */
 	u32	absr;		/* 0x78 AB Select Status Register */
 	u32	reserved5[9];	/*  */
+#endif
 	u32	ower;		/* 0xA0 Output Write Enable Register */
 	u32	owdr;		/* 0xA4 Output Write Disable Register */
-	u32	owsr;		/* OxA8 utput Write Status Register */
+	u32	owsr;		/* OxA8 Output Write Status Register */
+#if defined(CPU_HAS_PIO3)
+	u32	reserved7;	/*  */
+	u32	aimer;		/* 0xB0 Additional INT Modes Enable Register */
+	u32	aimdr;		/* 0xB4 Additional INT Modes Disable Register */
+	u32	aimmr;		/* 0xB8 Additional INT Modes Mask Register */
+	u32	reserved8;	/* */
+	u32	esr;		/* 0xC0 Edge Select Register */
+	u32	lsr;		/* 0xC4 Level Select Register */
+	u32	elsr;		/* 0xC8 Edge/Level Status Register */
+	u32	reserved9;	/* 0xCC */
+	u32	fellsr;		/* 0xD0 Falling /Low Level Select Register */
+	u32	rehlsr;		/* 0xD4 Rising /High Level Select Register */
+	u32	frlhsr;		/* 0xD8 Fall/Rise - Low/High Status Register */
+	u32	reserved10;	/* */
+	u32	locksr;		/* 0xE0 Lock Status */
+	u32	wpmr;		/* 0xE4 Write Protect Mode Register */
+	u32	wpsr;		/* 0xE8 Write Protect Status Register */
+	u32	reserved11[5];	/* */
+	u32	schmitt;	/* 0x100 Schmitt Trigger Register */
+	u32	reserved12[63];
+#else
 	u32	reserved6[85];
+#endif
 } at91_port_t;
 
 typedef union at91_pio {
@@ -94,6 +130,13 @@ typedef union at91_pio {
 #ifdef CONFIG_AT91_GPIO
 int at91_set_a_periph(unsigned port, unsigned pin, int use_pullup);
 int at91_set_b_periph(unsigned port, unsigned pin, int use_pullup);
+#if defined(CPU_HAS_PIO3)
+int at91_set_c_periph(unsigned port, unsigned pin, int use_pullup);
+int at91_set_d_periph(unsigned port, unsigned pin, int use_pullup);
+int at91_set_pio_debounce(unsigned port, unsigned pin, int is_on, int div);
+int at91_set_pio_pulldown(unsigned port, unsigned pin, int is_on);
+int at91_set_pio_disable_schmitt_trig(unsigned port, unsigned pin);
+#endif
 int at91_set_pio_input(unsigned port, unsigned pin, int use_pullup);
 int at91_set_pio_multi_drive(unsigned port, unsigned pin, int is_on);
 int at91_set_pio_output(unsigned port, unsigned pin, int value);
@@ -111,37 +154,4 @@ int at91_get_pio_value(unsigned port, unsigned pin);
 #define	AT91_PIO_PORTD		0x3
 #define	AT91_PIO_PORTE		0x4
 
-#ifdef CONFIG_AT91_LEGACY
-
-#define PIO_PER		0x00	/* Enable Register */
-#define PIO_PDR		0x04	/* Disable Register */
-#define PIO_PSR		0x08	/* Status Register */
-#define PIO_OER		0x10	/* Output Enable Register */
-#define PIO_ODR		0x14	/* Output Disable Register */
-#define PIO_OSR		0x18	/* Output Status Register */
-#define PIO_IFER	0x20	/* Glitch Input Filter Enable */
-#define PIO_IFDR	0x24	/* Glitch Input Filter Disable */
-#define PIO_IFSR	0x28	/* Glitch Input Filter Status */
-#define PIO_SODR	0x30	/* Set Output Data Register */
-#define PIO_CODR	0x34	/* Clear Output Data Register */
-#define PIO_ODSR	0x38	/* Output Data Status Register */
-#define PIO_PDSR	0x3c	/* Pin Data Status Register */
-#define PIO_IER		0x40	/* Interrupt Enable Register */
-#define PIO_IDR		0x44	/* Interrupt Disable Register */
-#define PIO_IMR		0x48	/* Interrupt Mask Register */
-#define PIO_ISR		0x4c	/* Interrupt Status Register */
-#define PIO_MDER	0x50	/* Multi-driver Enable Register */
-#define PIO_MDDR	0x54	/* Multi-driver Disable Register */
-#define PIO_MDSR	0x58	/* Multi-driver Status Register */
-#define PIO_PUDR	0x60	/* Pull-up Disable Register */
-#define PIO_PUER	0x64	/* Pull-up Enable Register */
-#define PIO_PUSR	0x68	/* Pull-up Status Register */
-#define PIO_ASR		0x70	/* Peripheral A Select Register */
-#define PIO_BSR		0x74	/* Peripheral B Select Register */
-#define PIO_ABSR	0x78	/* AB Status Register */
-#define PIO_OWER	0xa0	/* Output Write Enable Register */
-#define PIO_OWDR	0xa4	/* Output Write Disable Register */
-#define PIO_OWSR	0xa8	/* Output Write Status Register */
-#endif
-
 #endif
diff --git a/drivers/gpio/at91_gpio.c b/drivers/gpio/at91_gpio.c
index be2a026..ac3b322 100644
--- a/drivers/gpio/at91_gpio.c
+++ b/drivers/gpio/at91_gpio.c
@@ -86,7 +86,14 @@ int at91_set_a_periph(unsigned port, unsigned pin, int use_pullup)
 		mask = 1 << pin;
 		writel(mask, &pio->port[port].idr);
 		at91_set_pio_pullup(port, pin, use_pullup);
+#if defined(CPU_HAS_PIO3)
+		writel(readl(&pio->port[port].abcdsr1) & ~mask,
+			&pio->port[port].abcdsr1);
+		writel(readl(&pio->port[port].abcdsr2) & ~mask,
+			&pio->port[port].abcdsr2);
+#else
 		writel(mask, &pio->port[port].asr);
+#endif
 		writel(mask, &pio->port[port].pdr);
 	}
 	return 0;
@@ -104,12 +111,63 @@ int at91_set_b_periph(unsigned port, unsigned pin, int use_pullup)
 		mask = 1 << pin;
 		writel(mask, &pio->port[port].idr);
 		at91_set_pio_pullup(port, pin, use_pullup);
+#if defined(CPU_HAS_PIO3)
+		writel(readl(&pio->port[port].abcdsr1) | mask,
+			&pio->port[port].abcdsr1);
+		writel(readl(&pio->port[port].abcdsr2) & ~mask,
+			&pio->port[port].abcdsr2);
+#else
 		writel(mask, &pio->port[port].bsr);
+#endif
 		writel(mask, &pio->port[port].pdr);
 	}
 	return 0;
 }
 
+#if defined(CPU_HAS_PIO3)
+/*
+ * mux the pin to the "C" internal peripheral role.
+ */
+int at91_set_c_periph(unsigned port, unsigned pin, int use_pullup)
+{
+	at91_pio_t	*pio = (at91_pio_t *) ATMEL_BASE_PIOA;
+	u32		mask;
+
+	if ((port < ATMEL_PIO_PORTS) && (pin < 32)) {
+		mask = 1 << pin;
+		writel(mask, &pio->port[port].idr);
+		at91_set_pio_pullup(port, pin, use_pullup);
+		writel(readl(&pio->port[port].abcdsr1) & ~mask,
+			&pio->port[port].abcdsr1);
+		writel(readl(&pio->port[port].abcdsr2) | mask,
+			&pio->port[port].abcdsr2);
+		writel(mask, &pio->port[port].pdr);
+	}
+	return 0;
+}
+
+/*
+ * mux the pin to the "D" internal peripheral role.
+ */
+int at91_set_d_periph(unsigned port, unsigned pin, int use_pullup)
+{
+	at91_pio_t	*pio = (at91_pio_t *) ATMEL_BASE_PIOA;
+	u32		mask;
+
+	if ((port < ATMEL_PIO_PORTS) && (pin < 32)) {
+		mask = 1 << pin;
+		writel(mask, &pio->port[port].idr);
+		at91_set_pio_pullup(port, pin, use_pullup);
+		writel(readl(&pio->port[port].abcdsr1) | mask,
+			&pio->port[port].abcdsr1);
+		writel(readl(&pio->port[port].abcdsr2) | mask,
+			&pio->port[port].abcdsr2);
+		writel(mask, &pio->port[port].pdr);
+	}
+	return 0;
+}
+#endif
+
 /*
  * mux the pin to the gpio controller (instead of "A" or "B" peripheral), and
  * configure it for an input.
@@ -162,13 +220,76 @@ int at91_set_pio_deglitch(unsigned port, unsigned pin, int is_on)
 
 	if ((port < ATMEL_PIO_PORTS) && (pin < 32)) {
 		mask = 1 << pin;
-		if (is_on)
+		if (is_on) {
+#if defined(CPU_HAS_PIO3)
+			writel(mask, &pio->port[port].ifscdr);
+#endif
 			writel(mask, &pio->port[port].ifer);
-		else
+		} else {
 			writel(mask, &pio->port[port].ifdr);
+		}
+	}
+	return 0;
+}
+
+#if defined(CPU_HAS_PIO3)
+/*
+ * enable/disable the debounce filter.
+ */
+int at91_set_pio_debounce(unsigned port, unsigned pin, int is_on, int div)
+{
+	at91_pio_t	*pio = (at91_pio_t *) ATMEL_BASE_PIOA;
+	u32		mask;
+
+	if ((port < ATMEL_PIO_PORTS) && (pin < 32)) {
+		mask = 1 << pin;
+		if (is_on) {
+			writel(mask, &pio->port[port].ifscer);
+			writel(div & PIO_SCDR_DIV, &pio->port[port].scdr);
+			writel(mask, &pio->port[port].ifer);
+		} else {
+			writel(mask, &pio->port[port].ifdr);
+		}
+	}
+	return 0;
+}
+
+/*
+ * enable/disable the pull-down.
+ * If pull-up already enabled while calling the function, we disable it.
+ */
+int at91_set_pio_pulldown(unsigned port, unsigned pin, int is_on)
+{
+	at91_pio_t	*pio = (at91_pio_t *) ATMEL_BASE_PIOA;
+	u32		mask;
+
+	if ((port < ATMEL_PIO_PORTS) && (pin < 32)) {
+		mask = 1 << pin;
+		writel(mask, &pio->port[port].pudr);
+		if (is_on)
+			writel(mask, &pio->port[port].ppder);
+		else
+			writel(mask, &pio->port[port].ppddr);
+	}
+	return 0;
+}
+
+/*
+ * disable Schmitt trigger
+ */
+int at91_set_pio_disable_schmitt_trig(unsigned port, unsigned pin)
+{
+	at91_pio_t	*pio = (at91_pio_t *) ATMEL_BASE_PIOA;
+	u32		mask;
+
+	if ((port < ATMEL_PIO_PORTS) && (pin < 32)) {
+		mask = 1 << pin;
+		writel(readl(&pio->port[port].schmitt) | mask,
+			&pio->port[port].schmitt);
 	}
 	return 0;
 }
+#endif
 
 /*
  * enable/disable the multi-driver. This is only valid for output and
