diff --git a/arch/arm/include/asm/arch-mx31/mx31-regs.h b/arch/arm/include/asm/arch-mx31/mx31-regs.h
index d72585c8a01e570f5fbe95514fadef4c1f46e588..f05e743d7614ca7cc5b9461b61c89ad7dbc31c88 100644
--- a/arch/arm/include/asm/arch-mx31/mx31-regs.h
+++ b/arch/arm/include/asm/arch-mx31/mx31-regs.h
@@ -57,6 +57,14 @@ struct clock_control_regs {
 	u32 pdr2;
 };
 
+/* GPIO Registers */
+struct gpio_regs {
+	u32	gpio_dr;
+	u32	gpio_dir;
+	u32	gpio_psr;
+};
+
+
 /* Bit definitions for RCSR register in CCM */
 #define CCM_RCSR_NF16B	(1 << 31)
 #define CCM_RCSR_NFMS	(1 << 30)
@@ -153,9 +161,9 @@ struct clock_control_regs {
 /*
  * GPIO
  */
-#define GPIO1_BASE	0x53FCC000
-#define GPIO2_BASE	0x53FD0000
-#define GPIO3_BASE	0x53FA4000
+#define GPIO1_BASE_ADDR	0x53FCC000
+#define GPIO2_BASE_ADDR	0x53FD0000
+#define GPIO3_BASE_ADDR	0x53FA4000
 #define GPIO_DR		0x00000000	/* data register */
 #define GPIO_GDIR	0x00000004	/* direction register */
 #define GPIO_PSR	0x00000008	/* pad status register */
diff --git a/arch/arm/include/asm/arch-mx31/mx31.h b/arch/arm/include/asm/arch-mx31/mx31.h
index f702d260f1157ec9e40f4484f0b99645312f51c5..5a5aa114a7471b8a513891bf9f5a5fb85fdbc7b0 100644
--- a/arch/arm/include/asm/arch-mx31/mx31.h
+++ b/arch/arm/include/asm/arch-mx31/mx31.h
@@ -28,31 +28,6 @@ extern u32 mx31_get_ipg_clk(void);
 #define imx_get_uartclk mx31_get_ipg_clk
 extern void mx31_gpio_mux(unsigned long mode);
 
-enum mx31_gpio_direction {
-	MX31_GPIO_DIRECTION_IN,
-	MX31_GPIO_DIRECTION_OUT,
-};
-
-#ifdef CONFIG_MX31_GPIO
-extern int mx31_gpio_direction(unsigned int gpio,
-			       enum mx31_gpio_direction direction);
-extern void mx31_gpio_set(unsigned int gpio, unsigned int value);
-extern int mx31_gpio_get(unsigned int gpio);
-#else
-static inline int mx31_gpio_direction(unsigned int gpio,
-				      enum mx31_gpio_direction direction)
-{
-	return 1;
-}
-static inline int mx31_gpio_get(unsigned int gpio)
-{
-	return 1;
-}
-static inline void mx31_gpio_set(unsigned int gpio, unsigned int value)
-{
-}
-#endif
-
 void mx31_uart1_hw_init(void);
 void mx31_spi2_hw_init(void);
 
diff --git a/arch/arm/include/asm/arch-mx51/imx-regs.h b/arch/arm/include/asm/arch-mx51/imx-regs.h
index 3887d3cec487c6e0d70ac789e6ade782eadf6fbf..0e3bc2a4fb28b93ce0a8766c611290fb773bc18f 100644
--- a/arch/arm/include/asm/arch-mx51/imx-regs.h
+++ b/arch/arm/include/asm/arch-mx51/imx-regs.h
@@ -256,6 +256,12 @@ struct weim {
 	u32	cswcr2;
 };
 
+/* GPIO Registers */
+struct gpio_regs {
+	u32	gpio_dr;
+	u32	gpio_dir;
+	u32	gpio_psr;
+};
 #endif /* __ASSEMBLER__*/
 
 #endif				/*  __ASM_ARCH_MXC_MX51_H__ */
diff --git a/board/davedenx/qong/qong.c b/board/davedenx/qong/qong.c
index e509383e8fd0df54988840401c0f1dbc4804da23..9abc29c5f70bdcac147b0825ba078f552cdacf1a 100644
--- a/board/davedenx/qong/qong.c
+++ b/board/davedenx/qong/qong.c
@@ -27,6 +27,7 @@
 #include <asm/arch/mx31-regs.h>
 #include <nand.h>
 #include <fsl_pmic.h>
+#include <mxc_gpio.h>
 #include "qong_fpga.h"
 
 DECLARE_GLOBAL_DATA_PTR;
@@ -41,9 +42,9 @@ int dram_init (void)
 
 static void qong_fpga_reset(void)
 {
-	mx31_gpio_set(QONG_FPGA_RST_PIN, 0);
+	mxc_gpio_set(QONG_FPGA_RST_PIN, 0);
 	udelay(30);
-	mx31_gpio_set(QONG_FPGA_RST_PIN, 1);
+	mxc_gpio_set(QONG_FPGA_RST_PIN, 1);
 
 	udelay(300);
 }
@@ -66,11 +67,11 @@ int board_early_init_f (void)
 
 	/* FPGA reset  Pin */
 	/* rstn = 0 */
-	mx31_gpio_set(QONG_FPGA_RST_PIN, 0);
-	mx31_gpio_direction(QONG_FPGA_RST_PIN, MX31_GPIO_DIRECTION_OUT);
+	mxc_gpio_set(QONG_FPGA_RST_PIN, 0);
+	mxc_gpio_direction(QONG_FPGA_RST_PIN, MXC_GPIO_DIRECTION_OUT);
 
 	/* set interrupt pin as input */
-	mx31_gpio_direction(QONG_FPGA_IRQ_PIN, MX31_GPIO_DIRECTION_IN);
+	mxc_gpio_direction(QONG_FPGA_IRQ_PIN, MXC_GPIO_DIRECTION_IN);
 
 #endif
 
@@ -206,27 +207,27 @@ static void board_nand_setup(void)
 	qong_fpga_reset();
 
 	/* Enable NAND flash */
-	mx31_gpio_set(15, 1);
-	mx31_gpio_set(14, 1);
-	mx31_gpio_direction(15, MX31_GPIO_DIRECTION_OUT);
-	mx31_gpio_direction(16, MX31_GPIO_DIRECTION_IN);
-	mx31_gpio_direction(14, MX31_GPIO_DIRECTION_IN);
-	mx31_gpio_set(15, 0);
+	mxc_gpio_set(15, 1);
+	mxc_gpio_set(14, 1);
+	mxc_gpio_direction(15, MXC_GPIO_DIRECTION_OUT);
+	mxc_gpio_direction(16, MXC_GPIO_DIRECTION_IN);
+	mxc_gpio_direction(14, MXC_GPIO_DIRECTION_IN);
+	mxc_gpio_set(15, 0);
 
 }
 
 int qong_nand_rdy(void *chip)
 {
 	udelay(1);
-	return mx31_gpio_get(16);
+	return mxc_gpio_get(16);
 }
 
 void qong_nand_select_chip(struct mtd_info *mtd, int chip)
 {
 	if (chip >= 0)
-		mx31_gpio_set(15, 0);
+		mxc_gpio_set(15, 0);
 	else
-		mx31_gpio_set(15, 1);
+		mxc_gpio_set(15, 1);
 
 }
 
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 07d395d898f58636fe8eb702994ffda401c59d0b..a0f455223b379aee85410bf6e551fbdcee14e236 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -27,7 +27,7 @@ LIB 	:= $(obj)libgpio.a
 
 COBJS-$(CONFIG_AT91_GPIO)	+= at91_gpio.o
 COBJS-$(CONFIG_KIRKWOOD_GPIO)	+= kw_gpio.o
-COBJS-$(CONFIG_MX31_GPIO)	+= mx31_gpio.o
+COBJS-$(CONFIG_MXC_GPIO)	+= mxc_gpio.o
 COBJS-$(CONFIG_PCA953X)		+= pca953x.o
 COBJS-$(CONFIG_S5P)		+= s5p_gpio.o
 
diff --git a/drivers/gpio/mx31_gpio.c b/drivers/gpio/mxc_gpio.c
similarity index 63%
rename from drivers/gpio/mx31_gpio.c
rename to drivers/gpio/mxc_gpio.c
index b07f0381561cab545a1863ca96489fc87a88f480..663141f1b4690f79286f9b1904ae619139a91936 100644
--- a/drivers/gpio/mx31_gpio.c
+++ b/drivers/gpio/mxc_gpio.c
@@ -21,19 +21,29 @@
  * MA 02111-1307 USA
  */
 #include <common.h>
-#include <asm/arch/mx31.h>
+#ifdef CONFIG_MX31
 #include <asm/arch/mx31-regs.h>
+#endif
+#ifdef CONFIG_MX51
+#include <asm/arch/imx-regs.h>
+#endif
+#include <asm/io.h>
+#include <mxc_gpio.h>
 
 /* GPIO port description */
 static unsigned long gpio_ports[] = {
-	[0] = GPIO1_BASE,
-	[1] = GPIO2_BASE,
-	[2] = GPIO3_BASE,
+	[0] = GPIO1_BASE_ADDR,
+	[1] = GPIO2_BASE_ADDR,
+	[2] = GPIO3_BASE_ADDR,
+#ifdef CONFIG_MX51
+	[3] = GPIO4_BASE_ADDR,
+#endif
 };
 
-int mx31_gpio_direction(unsigned int gpio, enum mx31_gpio_direction direction)
+int mxc_gpio_direction(unsigned int gpio, enum mxc_gpio_direction direction)
 {
 	unsigned int port = gpio >> 5;
+	struct gpio_regs *regs;
 	u32 l;
 
 	if (port >= ARRAY_SIZE(gpio_ports))
@@ -41,22 +51,26 @@ int mx31_gpio_direction(unsigned int gpio, enum mx31_gpio_direction direction)
 
 	gpio &= 0x1f;
 
-	l = __REG(gpio_ports[port] + GPIO_GDIR);
+	regs = (struct gpio_regs *)gpio_ports[port];
+
+	l = readl(&regs->gpio_dir);
+
 	switch (direction) {
-	case MX31_GPIO_DIRECTION_OUT:
+	case MXC_GPIO_DIRECTION_OUT:
 		l |= 1 << gpio;
 		break;
-	case MX31_GPIO_DIRECTION_IN:
+	case MXC_GPIO_DIRECTION_IN:
 		l &= ~(1 << gpio);
 	}
-	__REG(gpio_ports[port] + GPIO_GDIR) = l;
+	writel(l, &regs->gpio_dir);
 
 	return 0;
 }
 
-void mx31_gpio_set(unsigned int gpio, unsigned int value)
+void mxc_gpio_set(unsigned int gpio, unsigned int value)
 {
 	unsigned int port = gpio >> 5;
+	struct gpio_regs *regs;
 	u32 l;
 
 	if (port >= ARRAY_SIZE(gpio_ports))
@@ -64,17 +78,20 @@ void mx31_gpio_set(unsigned int gpio, unsigned int value)
 
 	gpio &= 0x1f;
 
-	l = __REG(gpio_ports[port] + GPIO_DR);
+	regs = (struct gpio_regs *)gpio_ports[port];
+
+	l = readl(&regs->gpio_dr);
 	if (value)
 		l |= 1 << gpio;
 	else
 		l &= ~(1 << gpio);
-	__REG(gpio_ports[port] + GPIO_DR) = l;
+	writel(l, &regs->gpio_dr);
 }
 
-int mx31_gpio_get(unsigned int gpio)
+int mxc_gpio_get(unsigned int gpio)
 {
 	unsigned int port = gpio >> 5;
+	struct gpio_regs *regs;
 	u32 l;
 
 	if (port >= ARRAY_SIZE(gpio_ports))
@@ -82,7 +99,9 @@ int mx31_gpio_get(unsigned int gpio)
 
 	gpio &= 0x1f;
 
-	l = (__REG(gpio_ports[port] + GPIO_DR) >> gpio) & 0x01;
+	regs = (struct gpio_regs *)gpio_ports[port];
+
+	l = (readl(&regs->gpio_dr) >> gpio) & 0x01;
 
 	return l;
 }
diff --git a/drivers/spi/mxc_spi.c b/drivers/spi/mxc_spi.c
index e15a63caca9f342741a72670b79ab4ca5c94b32f..802cd2efb0fbe4ab05214342def17f6d045bc2f6 100644
--- a/drivers/spi/mxc_spi.c
+++ b/drivers/spi/mxc_spi.c
@@ -23,6 +23,7 @@
 #include <spi.h>
 #include <asm/errno.h>
 #include <asm/io.h>
+#include <mxc_gpio.h>
 
 #ifdef CONFIG_MX27
 /* i.MX27 has a completely wrong register layout and register definitions in the
@@ -68,9 +69,6 @@ static unsigned long spi_bases[] = {
 	0x53f84000,
 };
 
-#define OUT	MX31_GPIO_DIRECTION_OUT
-#define mxc_gpio_direction	mx31_gpio_direction
-#define mxc_gpio_set		mx31_gpio_set
 #elif defined(CONFIG_MX51)
 #include <asm/arch/imx-regs.h>
 #include <asm/arch/clock.h>
@@ -111,13 +109,12 @@ static unsigned long spi_bases[] = {
 	CSPI2_BASE_ADDR,
 	CSPI3_BASE_ADDR,
 };
-#define mxc_gpio_direction(gpio, dir)	(0)
-#define mxc_gpio_set(gpio, value)	{}
-#define OUT	1
 #else
 #error "Unsupported architecture"
 #endif
 
+#define OUT	MXC_GPIO_DIRECTION_OUT
+
 struct mxc_spi_slave {
 	struct spi_slave slave;
 	unsigned long	base;
@@ -126,6 +123,7 @@ struct mxc_spi_slave {
 	u32		cfg_reg;
 #endif
 	int		gpio;
+	int		ss_pol;
 };
 
 static inline struct mxc_spi_slave *to_mxc_spi_slave(struct spi_slave *slave)
@@ -147,7 +145,7 @@ void spi_cs_activate(struct spi_slave *slave)
 {
 	struct mxc_spi_slave *mxcs = to_mxc_spi_slave(slave);
 	if (mxcs->gpio > 0)
-		mxc_gpio_set(mxcs->gpio, mxcs->ctrl_reg & MXC_CSPICTRL_SSPOL);
+		mxc_gpio_set(mxcs->gpio, mxcs->ss_pol);
 }
 
 void spi_cs_deactivate(struct spi_slave *slave)
@@ -155,7 +153,7 @@ void spi_cs_deactivate(struct spi_slave *slave)
 	struct mxc_spi_slave *mxcs = to_mxc_spi_slave(slave);
 	if (mxcs->gpio > 0)
 		mxc_gpio_set(mxcs->gpio,
-			      !(mxcs->ctrl_reg & MXC_CSPICTRL_SSPOL));
+			      !(mxcs->ss_pol));
 }
 
 #ifdef CONFIG_MX51
@@ -394,6 +392,7 @@ struct spi_slave *spi_setup_slave(unsigned int bus, unsigned int cs,
 	mxcs->slave.bus = bus;
 	mxcs->slave.cs = cs;
 	mxcs->base = spi_bases[bus];
+	mxcs->ss_pol = (mode & SPI_CS_HIGH) ? 1 : 0;
 
 #ifdef CONFIG_MX51
 	/* Can be used for i.MX31 too ? */
diff --git a/include/configs/imx31_phycore.h b/include/configs/imx31_phycore.h
index 1dbafa0521ce59c21148fe173169c9dc13a254f3..62944a93f6ee7301ea206aac50bdb3f6913cd9e4 100644
--- a/include/configs/imx31_phycore.h
+++ b/include/configs/imx31_phycore.h
@@ -183,7 +183,7 @@
 #ifdef CONFIG_IMX31_PHYCORE_EET
 #define BOARD_LATE_INIT
 
-#define CONFIG_MX31_GPIO			1
+#define CONFIG_MXC_GPIO
 
 #define CONFIG_HARD_SPI				1
 #define CONFIG_MXC_SPI				1
diff --git a/include/configs/qong.h b/include/configs/qong.h
index 7a68b7b97ead66b231f47a6de2eb736f5b5ee4f0..7cd955826df4ded9faff14bb16c502e0299740d2 100644
--- a/include/configs/qong.h
+++ b/include/configs/qong.h
@@ -52,7 +52,7 @@
 #define CONFIG_MXC_UART	1
 #define CONFIG_SYS_MX31_UART1	1
 
-#define CONFIG_MX31_GPIO
+#define CONFIG_MXC_GPIO
 
 #define CONFIG_MXC_SPI
 #define CONFIG_DEFAULT_SPI_BUS	1
diff --git a/include/mxc_gpio.h b/include/mxc_gpio.h
new file mode 100644
index 0000000000000000000000000000000000000000..002ba61e00679ab4417a4d73db11e6e5f6eb3e99
--- /dev/null
+++ b/include/mxc_gpio.h
@@ -0,0 +1,52 @@
+/*
+ *
+ * (c) 2007 Pengutronix, Sascha Hauer <s.hauer@pengutronix.de>
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation; either version 2 of
+ * the License, or (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#ifndef __MXC_GPIO_H
+#define __MXC_GPIO_H
+
+enum mxc_gpio_direction {
+	MXC_GPIO_DIRECTION_IN,
+	MXC_GPIO_DIRECTION_OUT,
+};
+
+#ifdef CONFIG_MXC_GPIO
+extern int mxc_gpio_direction(unsigned int gpio,
+			       enum mxc_gpio_direction direction);
+extern void mxc_gpio_set(unsigned int gpio, unsigned int value);
+extern int mxc_gpio_get(unsigned int gpio);
+#else
+static inline int mxc_gpio_direction(unsigned int gpio,
+				      enum mxc_gpio_direction direction)
+{
+	return 1;
+}
+static inline int mxc_gpio_get(unsigned int gpio)
+{
+	return 1;
+}
+static inline void mxc_gpio_set(unsigned int gpio, unsigned int value)
+{
+}
+#endif
+
+#endif