diff --git a/include/drivers/sys-gpio.h b/include/drivers/sys-gpio.h
index cd3d8d12..c571976c 100644
--- a/include/drivers/sys-gpio.h
+++ b/include/drivers/sys-gpio.h
@@ -29,35 +29,84 @@ enum {
 
 enum {
     GPIO_PORTA = 0,
-    GPIO_PORTB,
-    GPIO_PORTC,
-    GPIO_PORTD,
-    GPIO_PORTE,
-    GPIO_PORTF,
-    GPIO_PORTG,
-    GPIO_PORTH,
-    GPIO_PORTI,
-    GPIO_PORTJ,
-    GPIO_PORTK,
-    GPIO_PORTL,
+    GPIO_PORTB = 1,
+    GPIO_PORTC = 2,
+    GPIO_PORTD = 3,
+    GPIO_PORTE = 4,
+    GPIO_PORTF = 5,
+    GPIO_PORTG = 6,
+    GPIO_PORTH = 7,
+    GPIO_PORTI = 8,
+    GPIO_PORTJ = 9,
+    GPIO_PORTK = 10,
+    GPIO_PORTL = 11,
+    GPIO_PORTM = 12,
+    GPIO_PORTN = 13,
 };
 
+
 enum gpio_pull_t {
     GPIO_PULL_UP = 0,
     GPIO_PULL_DOWN = 1,
     GPIO_PULL_NONE = 2,
 };
 
-typedef uint32_t gpio_t;
+#ifndef CONFIG_CHIP_GPIO_V1
+struct sunxi_gpio {
+    uint32_t cfg[4];
+    uint32_t dat;
+    uint32_t drv[4];
+    uint32_t pull[3];
+};
+#else
+struct sunxi_gpio {
+    uint32_t cfg[4];
+    uint32_t dat;
+    uint32_t drv[2];
+    uint32_t pull[2];
+};
+#endif
+
+/* gpio interrupt control */
+struct sunxi_gpio_int {
+    uint32_t cfg[3];
+    uint32_t ctl;
+    uint32_t sta;
+    uint32_t deb; /* interrupt debounce */
+};
+
+#define SUNXI_GPIO_BANKS 10
+#define SUNXI_GPIO_BANK_SIZE 32
 #define PIO_NUM_IO_BITS 5
 
-#define GPIO_PIN(x, y) (((uint32_t) (x << PIO_NUM_IO_BITS)) | y)
+struct sunxi_gpio_reg {
+    struct sunxi_gpio gpio_bank[SUNXI_GPIO_BANKS];
+    uint8_t res[0xbc];
+    struct sunxi_gpio_int gpio_int;
+};
+
+#define BANK_TO_GPIO(bank) (((bank) < GPIO_PORTL) ? &((struct sunxi_gpio_reg *) SUNXI_PIO_BASE)->gpio_bank[bank] : &((struct sunxi_gpio_reg *) SUNXI_R_PIO_BASE)->gpio_bank[(bank) -GPIO_PORTL])
+
+#define GPIO_BANK(pin) ((pin) >> PIO_NUM_IO_BITS)
+
+#define GPIO_CFG_INDEX(pin) (((pin) &0x1f) >> 3)
+#define GPIO_CFG_OFFSET(pin) ((((pin) &0x1f) & 0x7) << 2)
+
+#define GPIO_DRV_INDEX(pin) (((pin) &0x1f) >> 4)
+#define GPIO_DRV_OFFSET(pin) ((((pin) &0x1f) & 0xf) << 1)
+
+#define GPIO_PULL_INDEX(pin) (((pin) &0x1f) >> 4)
+#define GPIO_PULL_OFFSET(pin) ((((pin) &0x1f) & 0xf) << 1)
+
+typedef uint32_t gpio_t;
 
 typedef struct {
     gpio_t pin;
     uint8_t mux;
 } gpio_mux_t;
 
+#define GPIO_PIN(x, y) ((x) * SUNXI_GPIO_BANK_SIZE + (y))
+
 extern void sunxi_gpio_init(gpio_t pin, int cfg);
 
 extern void sunxi_gpio_set_value(gpio_t pin, int value);
diff --git a/src/drivers/sys-gpio.c b/src/drivers/sys-gpio.c
index 771278e6..e7a7809d 100644
--- a/src/drivers/sys-gpio.c
+++ b/src/drivers/sys-gpio.c
@@ -9,96 +9,75 @@
 
 #include <log.h>
 
-#include <sys-gpio.h>
 #include <reg-ncat.h>
+#include <sys-gpio.h>
 
-enum {
-    GPIO_CFG0 = 0x00,
-    GPIO_CFG1 = 0x04,
-    GPIO_CFG2 = 0x08,
-    GPIO_CFG3 = 0x0c,
-    GPIO_DAT = 0x10,
-    GPIO_DRV0 = 0x14,
-    GPIO_DRV1 = 0x18,
-    GPIO_DRV2 = 0x1c,
-    GPIO_DRV3 = 0x20,
-    GPIO_PUL0 = 0x24,
-    GPIO_PUL1 = 0x28,
-};
-
-static inline uint32_t _port_num(gpio_t pin) {
-    return pin >> PIO_NUM_IO_BITS;
+static void sunxi_gpio_bank_init(struct sunxi_gpio *pio, int bank_offset, uint32_t val) {
+    uint32_t index = GPIO_CFG_INDEX(bank_offset);
+    uint32_t offset = GPIO_CFG_OFFSET(bank_offset);
+    clrsetbits_le32((uint32_t) &pio->cfg[0] + index, 0xf << offset, val << offset);
+    printk(LOG_LEVEL_DEBUG, "GPIO: index = 0x%08x, offset = 0x%08x, pio->cfg[0] = 0x%08x\n", index, offset, read32((uint32_t) &pio->cfg[0]));
 }
 
-static uint32_t _port_base_get(gpio_t pin) {
-    uint32_t port = pin >> PIO_NUM_IO_BITS;
-    return SUNXI_PIO_BASE + port * 0x30;
-}
+void sunxi_gpio_init(gpio_t pin, int cfg) {
+    uint32_t bank = GPIO_BANK(pin);
+    struct sunxi_gpio *pio = BANK_TO_GPIO(bank);
 
-static inline uint32_t _pin_num(gpio_t pin) {
-    return (pin & ((1 << PIO_NUM_IO_BITS) - 1));
+    printk(LOG_LEVEL_DEBUG, "GPIO: pin = %d, bank = %d, &pio->cfg[0] = 0x%08x\n", pin, bank, &pio->cfg[0]);
+
+    sunxi_gpio_bank_init(pio, pin, cfg);
 }
 
-void sunxi_gpio_init(gpio_t pin, int cfg) {
-    uint32_t port_addr = _port_base_get(pin);
-    uint32_t pin_num = _pin_num(pin);
-    uint32_t addr;
-    uint32_t val;
+void sunxi_gpio_set_value(gpio_t pin, int value) {
+    u32 bank = GPIO_BANK(pin);
+    u32 index = GPIO_DRV_INDEX(pin);
+    u32 offset = GPIO_DRV_OFFSET(pin);
+    struct sunxi_gpio *pio = BANK_TO_GPIO(bank);
 
-    addr = port_addr + GPIO_CFG0 + ((pin_num >> 3) << 2);
-    val = read32(addr);
-    val &= ~(0xf << ((pin_num & 0x7) << 2));
-    val |= ((cfg & 0xf) << ((pin_num & 0x7) << 2));
-    write32(addr, val);
+    clrsetbits_le32((uint32_t) &pio->drv[0] + index, 0x3 << offset, value << offset);
 }
 
-void sunxi_gpio_set_value(gpio_t pin, int value) {
-    uint32_t port_addr = _port_base_get(pin);
-    uint32_t pin_num = _pin_num(pin);
-    uint32_t val;
+static int sunxi_gpio_get_bank_cfg(struct sunxi_gpio *pio, int bank_offset) {
+    uint32_t index = GPIO_CFG_INDEX(bank_offset);
+    uint32_t offset = GPIO_CFG_OFFSET(bank_offset);
+    uint32_t cfg;
+
+    cfg = readl((uint32_t) &pio->cfg[0] + index);
+    cfg >>= offset;
 
-    val = read32(port_addr + GPIO_DAT);
-    val &= ~(1 << pin_num);
-    val |= (!!value) << pin_num;
-    write32(port_addr + GPIO_DAT, val);
+    return cfg & 0xf;
 }
 
 int sunxi_gpio_read(gpio_t pin) {
-    uint32_t port_addr = _port_base_get(pin);
-    uint32_t pin_num = _pin_num(pin);
-    uint32_t val;
+    uint32_t bank = GPIO_BANK(pin);
+    struct sunxi_gpio *pio = BANK_TO_GPIO(bank);
 
-    val = read32(port_addr + GPIO_DAT);
-    return !!(val & (1 << pin_num));
+    return sunxi_gpio_get_bank_cfg(pio, pin);
 }
 
 void sunxi_gpio_set_pull(gpio_t pin, enum gpio_pull_t pull) {
-    uint32_t port_addr = _port_base_get(pin);
-    uint32_t pin_num = _pin_num(pin);
-    uint32_t addr;
-    uint32_t val, v;
+    uint32_t bank = GPIO_BANK(pin);
+    uint32_t index = GPIO_PULL_INDEX(pin);
+    uint32_t offset = GPIO_PULL_OFFSET(pin);
+    struct sunxi_gpio *pio = BANK_TO_GPIO(bank);
+    uint32_t val;
 
     switch (pull) {
         case GPIO_PULL_UP:
-            v = 0x1;
+            val = 0x1;
             break;
 
         case GPIO_PULL_DOWN:
-            v = 0x2;
+            val = 0x2;
             break;
 
         case GPIO_PULL_NONE:
-            v = 0x0;
+            val = 0x0;
             break;
 
         default:
-            v = 0x0;
+            val = 0x0;
             break;
     }
-
-    addr = port_addr + GPIO_PUL0 + ((pin_num >> 4) << 2);
-    val = read32(addr);
-    val &= ~(v << ((pin_num & 0xf) << 1));
-    val |= (v << ((pin_num & 0xf) << 1));
-    write32(addr, val);
+    clrsetbits_le32((uint32_t) &pio->pull[0] + index, 0x3 << offset, val << offset);
 }
diff --git a/src/drivers/sys-uart.c b/src/drivers/sys-uart.c
index 6953307c..58aa35fe 100644
--- a/src/drivers/sys-uart.c
+++ b/src/drivers/sys-uart.c
@@ -15,10 +15,6 @@ void sunxi_serial_init(sunxi_serial_t *uart) {
     uint32_t addr;
     uint32_t val;
 
-    /* Config uart TXD and RXD pins */
-    sunxi_gpio_init(uart->gpio_tx.pin, uart->gpio_tx.mux);
-    sunxi_gpio_init(uart->gpio_rx.pin, uart->gpio_rx.mux);
-
     /* Open the clock gate for uart */
     addr = CCU_BASE + CCU_UART_BGR_REG;
     val = read32(addr);
@@ -48,6 +44,10 @@ void sunxi_serial_init(sunxi_serial_t *uart) {
     val &= ~0x1f;
     val |= (0x3 << 0) | (0 << 2) | (0x0 << 3);
     write32(addr + 0x0c, val);
+
+    /* Config uart TXD and RXD pins */
+    sunxi_gpio_init(uart->gpio_tx.pin, uart->gpio_tx.mux);
+    sunxi_gpio_init(uart->gpio_rx.pin, uart->gpio_rx.mux);
 }
 
 void sunxi_serial_putc(void *arg, char c) {