diff --git a/arch/microblaze/include/asm/gpio.h b/arch/microblaze/include/asm/gpio.h
index 883f4d4..f5cad56 100644
--- a/arch/microblaze/include/asm/gpio.h
+++ b/arch/microblaze/include/asm/gpio.h
@@ -1,41 +1,15 @@
 #ifndef _ASM_MICROBLAZE_GPIO_H_
 #define _ASM_MICROBLAZE_GPIO_H_

-#include <asm/io.h>
+#include <asm-generic/gpio.h>

-static inline int gpio_request(unsigned gpio, const char *label)
-{
-	return 0;
-}
+/* Allocation functions */
+extern int gpio_alloc_dual(u32 baseaddr, const char *name, u32 gpio_no0,
+			   u32 gpio_no1);
+extern int gpio_alloc(u32 baseaddr, const char *name, u32 gpio_no);

-static inline int gpio_free(unsigned gpio)
-{
-	return 0;
-}
+#define gpio_status()	gpio_info()
+extern void gpio_info(void);

-static inline int gpio_direction_input(unsigned gpio)
-{
-	return 0;
-}
-
-static inline int gpio_direction_output(unsigned gpio, int value)
-{
-	return 0;
-}
-
-static inline int gpio_get_value(unsigned gpio)
-{
-	return 0;
-}
-
-static inline int gpio_set_value(unsigned gpio, int value)
-{
-	return 0;
-}
-
-static inline int gpio_is_valid(int number)
-{
-	return 0;
-}
 #endif

diff --git a/board/xilinx/microblaze-generic/microblaze-generic.c b/board/xilinx/microblaze-generic/microblaze-generic.c
index befbb3a..2f5f20e 100644
--- a/board/xilinx/microblaze-generic/microblaze-generic.c
+++ b/board/xilinx/microblaze-generic/microblaze-generic.c
@@ -31,12 +31,17 @@
 #include <asm/processor.h>
 #include <asm/microblaze_intc.h>
 #include <asm/asm.h>
+#include <asm/gpio.h>
+
+#ifdef CONFIG_XILINX_GPIO
+static int reset_pin = -1;
+#endif

 int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
-#ifdef CONFIG_SYS_GPIO_0
-	*((unsigned long *)(CONFIG_SYS_GPIO_0_ADDR)) =
-	    ++(*((unsigned long *)(CONFIG_SYS_GPIO_0_ADDR)));
+#ifdef CONFIG_XILINX_GPIO
+	if (reset_pin != -1)
+		gpio_direction_output(reset_pin, 1);
 #endif

 #ifdef CONFIG_XILINX_TB_WATCHDOG
@@ -52,8 +57,10 @@ int do_reset(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])

 int gpio_init (void)
 {
-#ifdef CONFIG_SYS_GPIO_0
-	*((unsigned long *)(CONFIG_SYS_GPIO_0_ADDR)) = 0xFFFFFFFF;
+#ifdef CONFIG_XILINX_GPIO
+	reset_pin = gpio_alloc(CONFIG_SYS_GPIO_0_ADDR, "reset", 1);
+	if (reset_pin != -1)
+		gpio_request(reset_pin, "reset_pin");
 #endif
 	return 0;
 }
diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index 9df1e26..830e8e6 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -47,6 +47,7 @@ COBJS-$(CONFIG_OMAP_GPIO)	+= omap_gpio.o
 COBJS-$(CONFIG_DB8500_GPIO)	+= db8500_gpio.o
 COBJS-$(CONFIG_BCM2835_GPIO)	+= bcm2835_gpio.o
 COBJS-$(CONFIG_S3C2440_GPIO)	+= s3c2440_gpio.o
+COBJS-$(CONFIG_XILINX_GPIO)	+= xilinx_gpio.o

 COBJS	:= $(COBJS-y)
 SRCS 	:= $(COBJS:.o=.c)
diff --git a/drivers/gpio/xilinx_gpio.c b/drivers/gpio/xilinx_gpio.c
new file mode 100644
index 0000000..37fb0c5
--- /dev/null
+++ b/drivers/gpio/xilinx_gpio.c
@@ -0,0 +1,364 @@
+/*
+ * Copyright (c) 2013 Xilinx, Michal Simek
+ *
+ * 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
+ */
+
+#include <common.h>
+#include <errno.h>
+#include <malloc.h>
+#include <linux/list.h>
+#include <asm/io.h>
+#include <asm/gpio.h>
+
+static LIST_HEAD(gpio_list);
+
+enum gpio_direction {
+	GPIO_DIRECTION_OUT = 0,
+	GPIO_DIRECTION_IN = 1,
+};
+
+/* Gpio simple map */
+struct gpio_regs {
+	u32 gpiodata;
+	u32 gpiodir;
+};
+
+#define GPIO_NAME_SIZE	10
+
+struct gpio_names {
+	char name[GPIO_NAME_SIZE];
+};
+
+/* Initialized, rxbd_current, rx_first_buf must be 0 after init */
+struct xilinx_gpio_priv {
+	struct gpio_regs *regs;
+	u32 gpio_min;
+	u32 gpio_max;
+	u32 gpiodata_store;
+	char name[GPIO_NAME_SIZE];
+	struct list_head list;
+	struct gpio_names *gpio_name;
+};
+
+/* Store number of allocated gpio pins */
+static u32 xilinx_gpio_max;
+
+/* Get associated gpio controller */
+static struct xilinx_gpio_priv *gpio_get_controller(unsigned gpio)
+{
+	struct list_head *entry;
+	struct xilinx_gpio_priv *priv = NULL;
+
+	list_for_each(entry, &gpio_list) {
+		priv = list_entry(entry, struct xilinx_gpio_priv, list);
+		if (gpio >= priv->gpio_min && gpio <= priv->gpio_max) {
+			debug("%s: reg: %x, min-max: %d-%d\n", __func__,
+			      (u32)priv->regs, priv->gpio_min, priv->gpio_max);
+			return priv;
+		}
+	}
+	puts("!!!Can't get gpio controller!!!\n");
+	return NULL;
+}
+
+/* Get gpio pin name if used/setup */
+static char *get_name(unsigned gpio)
+{
+	u32 gpio_priv;
+	struct xilinx_gpio_priv *priv;
+
+	debug("%s\n", __func__);
+
+	priv = gpio_get_controller(gpio);
+	if (priv) {
+		gpio_priv = gpio - priv->gpio_min;
+
+		return *priv->gpio_name[gpio_priv].name ?
+			priv->gpio_name[gpio_priv].name : "UNKNOWN";
+	}
+	return "UNKNOWN";
+}
+
+/* Get output value */
+static int gpio_get_output_value(unsigned gpio)
+{
+	u32 val, gpio_priv;
+	struct xilinx_gpio_priv *priv = gpio_get_controller(gpio);
+
+	if (priv) {
+		gpio_priv = gpio - priv->gpio_min;
+		val = !!(priv->gpiodata_store & (1 << gpio_priv));
+		debug("%s: reg: %x, gpio_no: %d, dir: %d\n", __func__,
+		      (u32)priv->regs, gpio_priv, val);
+
+		return val;
+	}
+	return -1;
+}
+
+/* Get input value */
+static int gpio_get_input_value(unsigned gpio)
+{
+	u32 val, gpio_priv;
+	struct gpio_regs *regs;
+	struct xilinx_gpio_priv *priv = gpio_get_controller(gpio);
+
+	if (priv) {
+		regs = priv->regs;
+		gpio_priv = gpio - priv->gpio_min;
+		val = readl(&regs->gpiodata);
+		val = !!(val & (1 << gpio_priv));
+		debug("%s: reg: %x, gpio_no: %d, dir: %d\n", __func__,
+		      (u32)priv->regs, gpio_priv, val);
+
+		return val;
+	}
+	return -1;
+}
+
+/* Set gpio direction */
+static int gpio_set_direction(unsigned gpio, enum gpio_direction direction)
+{
+	u32 val, gpio_priv;
+	struct gpio_regs *regs;
+	struct xilinx_gpio_priv *priv = gpio_get_controller(gpio);
+
+	if (priv) {
+		regs = priv->regs;
+		val = readl(&regs->gpiodir);
+
+		gpio_priv = gpio - priv->gpio_min;
+		if (direction == GPIO_DIRECTION_OUT)
+			val &= ~(1 << gpio_priv);
+		else
+			val |= 1 << gpio_priv;
+
+		writel(val, &regs->gpiodir);
+		debug("%s: reg: %x, gpio_no: %d, dir: %d\n", __func__,
+		      (u32)priv->regs, gpio_priv, val);
+
+		return 0;
+	}
+
+	return -1;
+}
+
+/* Get gpio direction */
+static int gpio_get_direction(unsigned gpio)
+{
+	u32 val, gpio_priv;
+	struct gpio_regs *regs;
+	struct xilinx_gpio_priv *priv = gpio_get_controller(gpio);
+
+	if (priv) {
+		regs = priv->regs;
+		gpio_priv = gpio - priv->gpio_min;
+		val = readl(&regs->gpiodir);
+		val = !!(val & (1 << gpio_priv));
+		debug("%s: reg: %x, gpio_no: %d, dir: %d\n", __func__,
+		      (u32)priv->regs, gpio_priv, val);
+
+		return val;
+	}
+
+	return -1;
+}
+
+/*
+ * Get input value
+ * for example gpio setup to output only can't get input value
+ * which is breaking gpio toggle command
+ */
+int gpio_get_value(unsigned gpio)
+{
+	u32 val;
+
+	if (gpio_get_direction(gpio) == GPIO_DIRECTION_OUT)
+		val = gpio_get_output_value(gpio);
+	else
+		val = gpio_get_input_value(gpio);
+
+	return val;
+}
+
+/* Set output value */
+static int gpio_set_output_value(unsigned gpio, int value)
+{
+	u32 val, gpio_priv;
+	struct gpio_regs *regs;
+	struct xilinx_gpio_priv *priv = gpio_get_controller(gpio);
+
+	if (priv) {
+		regs = priv->regs;
+		gpio_priv = gpio - priv->gpio_min;
+		val = priv->gpiodata_store;
+		if (value)
+			val |= 1 << gpio_priv;
+		else
+			val &= ~(1 << gpio_priv);
+
+		writel(val, &regs->gpiodata);
+		debug("%s: reg: %x, gpio_no: %d, output_val: %d\n", __func__,
+		      (u32)priv->regs, gpio_priv, val);
+		priv->gpiodata_store = val;
+
+		return 0;
+	}
+
+	return -1;
+}
+
+int gpio_set_value(unsigned gpio, int value)
+{
+	if (gpio_get_direction(gpio) == GPIO_DIRECTION_OUT)
+		return gpio_set_output_value(gpio, value);
+
+	return -1;
+}
+
+/* Set GPIO as input */
+int gpio_direction_input(unsigned gpio)
+{
+	debug("%s\n", __func__);
+	return gpio_set_direction(gpio, GPIO_DIRECTION_IN);
+}
+
+/* Setup GPIO as output and set output value */
+int gpio_direction_output(unsigned gpio, int value)
+{
+	int ret = gpio_set_direction(gpio, GPIO_DIRECTION_OUT);
+
+	debug("%s\n", __func__);
+
+	if (ret < 0)
+		return ret;
+
+	return gpio_set_output_value(gpio, value);
+}
+
+/* Show gpio status */
+void gpio_info(void)
+{
+	unsigned gpio;
+
+	struct list_head *entry;
+	struct xilinx_gpio_priv *priv = NULL;
+
+	list_for_each(entry, &gpio_list) {
+		priv = list_entry(entry, struct xilinx_gpio_priv, list);
+		printf("\n%s: %s/%x (%d-%d)\n", __func__, priv->name,
+		       (u32)priv->regs, priv->gpio_min, priv->gpio_max);
+
+		for (gpio = priv->gpio_min; gpio <= priv->gpio_max; gpio++) {
+			printf("GPIO_%d:\t%s is an ", gpio, get_name(gpio));
+			if (gpio_get_direction(gpio) == GPIO_DIRECTION_OUT)
+				printf("OUTPUT value = %d\n",
+				       gpio_get_output_value(gpio));
+			else
+				printf("INPUT value = %d\n",
+				       gpio_get_input_value(gpio));
+		}
+	}
+}
+
+int gpio_request(unsigned gpio, const char *label)
+{
+	u32 gpio_priv;
+	struct xilinx_gpio_priv *priv;
+
+	if (gpio >= xilinx_gpio_max)
+		return -EINVAL;
+
+	priv = gpio_get_controller(gpio);
+	if (priv) {
+		gpio_priv = gpio - priv->gpio_min;
+
+		if (label != NULL) {
+			strncpy(priv->gpio_name[gpio_priv].name, label,
+				GPIO_NAME_SIZE);
+			priv->gpio_name[gpio_priv].name[GPIO_NAME_SIZE - 1] =
+					'\0';
+		}
+		return 0;
+	}
+
+	return -1;
+}
+
+int gpio_free(unsigned gpio)
+{
+	u32 gpio_priv;
+	struct xilinx_gpio_priv *priv;
+
+	if (gpio >= xilinx_gpio_max)
+		return -EINVAL;
+
+	priv = gpio_get_controller(gpio);
+	if (priv) {
+		gpio_priv = gpio - priv->gpio_min;
+		priv->gpio_name[gpio_priv].name[0] = '\0';
+
+		/* Do nothing here */
+		return 0;
+	}
+
+	return -1;
+}
+
+int gpio_alloc(u32 baseaddr, const char *name, u32 gpio_no)
+{
+	struct xilinx_gpio_priv *priv;
+
+	priv = calloc(1, sizeof(struct xilinx_gpio_priv));
+
+	/* Setup gpio name */
+	if (name != NULL) {
+		strncpy(priv->name, name, GPIO_NAME_SIZE);
+		priv->name[GPIO_NAME_SIZE - 1] = '\0';
+	}
+	priv->regs = (struct gpio_regs *)baseaddr;
+
+	priv->gpio_min = xilinx_gpio_max;
+	xilinx_gpio_max = priv->gpio_min + gpio_no;
+	priv->gpio_max = xilinx_gpio_max - 1;
+
+	priv->gpio_name = calloc(gpio_no, sizeof(struct gpio_names));
+
+	INIT_LIST_HEAD(&priv->list);
+	list_add_tail(&priv->list, &gpio_list);
+
+	printf("%s: Add %s (%d-%d)\n", __func__, name,
+	       priv->gpio_min, priv->gpio_max);
+
+	/* Return the first gpio allocated for this device */
+	return priv->gpio_min;
+}
+
+/* Dual channel gpio is one IP with two independent channels */
+int gpio_alloc_dual(u32 baseaddr, const char *name, u32 gpio_no0, u32 gpio_no1)
+{
+	int ret;
+
+	ret = gpio_alloc(baseaddr, name, gpio_no0);
+	gpio_alloc(baseaddr + 8, strcat((char *)name, "_1"), gpio_no1);
+
+	/* Return the first gpio allocated for this device */
+	return ret;
+}
diff --git a/include/configs/microblaze-generic.h b/include/configs/microblaze-generic.h
index 0c4e719..17f53ba 100644
--- a/include/configs/microblaze-generic.h
+++ b/include/configs/microblaze-generic.h
@@ -104,7 +104,7 @@

 /* gpio */
 #ifdef XILINX_GPIO_BASEADDR
-# define CONFIG_SYS_GPIO_0		1
+# define CONFIG_XILINX_GPIO
 # define CONFIG_SYS_GPIO_0_ADDR		XILINX_GPIO_BASEADDR
 #endif

@@ -312,6 +312,7 @@
 #define CONFIG_CMD_IRQ
 #define CONFIG_CMD_MFSL
 #define CONFIG_CMD_ECHO
+#define CONFIG_CMD_GPIO

 #if defined(CONFIG_DCACHE) || defined(CONFIG_ICACHE)
 # define CONFIG_CMD_CACHE
