[Feature]add MT2731_MP2_MR2_SVN388 baseline version

Change-Id: Ief04314834b31e27effab435d3ca8ba33b499059
diff --git a/src/bsp/lk/platform/mt2701/cpu_early_init.S b/src/bsp/lk/platform/mt2701/cpu_early_init.S
new file mode 100644
index 0000000..36db27b
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/cpu_early_init.S
@@ -0,0 +1,157 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+.equ INT_BIT           ,0xC0
+.equ SRAMROM_BOOT_ADDR ,0x10001800
+.equ GIC_CPU_BASE      ,0x10212000
+.equ SLAVE_JUMP_REG    ,0x10202034
+.equ SLAVE1_MAGIC_REG  ,0x10202038
+.equ SLAVE2_MAGIC_REG  ,0x1020203C
+.equ SLAVE3_MAGIC_REG  ,0x10202040
+.equ SLAVE1_MAGIC_NUM  ,0x534C4131
+.equ SLAVE2_MAGIC_NUM  ,0x4C415332
+.equ SLAVE3_MAGIC_NUM  ,0x41534C33
+/* TODO: need to define a address not be overwritten */
+.equ ramBase           ,0x0010FC00
+
+.globl  slave_core
+.globl  __cpu_early_init
+
+.type   __cpu_early_init, function
+__cpu_early_init:
+    /* set the cpu to SVC32 mode */
+    MRS r0,cpsr
+    BIC r0,r0,#0x1f
+    ORR r0,r0,#0xd3
+    MSR cpsr,r0
+
+    /* disable interrupt */
+    MRS r0, cpsr
+    MOV r1, #INT_BIT
+    ORR r0, r0, r1
+    MSR cpsr_cxsf, r0
+
+    /* set L2$ delay select */
+    LDR r0, =0x10200014
+    MOV r1, #0xf
+    STR r1, [r0]
+
+    /* set CNTFRQ = 13MHz = 0xC65D40 */
+    MOVW r0, #0x5D40
+    MOVT r0, #0xC6
+    MCR p15, 0, r0, c14, c0, 0
+
+    /* set CNTVOFF to zero */
+    /* 1. switch to monitor mode */
+    MRS r1,cpsr
+    BIC r0,r1,#0x1f
+    ORR r0,r0,#0x16
+    MSR cpsr,r0
+    /* 2. set SCR.NS=0 */
+    MRC p15, 0, r2, c1, c1, 0
+    ORR r0, r2, #0x1
+    MCR p15, 0, r0, c1, c1, 0
+    /* 3. set CNTVOFF=0 */
+    MOV r0, #0
+    MCRR p15, 4, r0, r0, c14
+    /* 4. set SCR.NS=1 */
+    MCR p15, 0, r2, c1, c1, 0
+    /* 5. switch to original mode */
+    MSR cpsr,r1
+
+    MRC p15, 0, r0, c0, c0, 5
+    MOVW r1, #0xf0f
+    TST r0, r1
+    BNE slave_core
+    bx lr
+
+.type   slave_core, function
+slave_core:
+    @ enable GIC
+    movw r2, #:lower16:GIC_CPU_BASE
+    movt r2, #:upper16:GIC_CPU_BASE
+    mov r1,#0xF0
+    str r1,[r2,#4]
+    mov r1,#1
+    str r1,[r2,#0]
+
+    ldr r1, [r2]
+    orr r1, #1
+    str r1, [r2]
+
+    ldr r9, =load_magic_num
+    ldm r9, {r1-r8}
+    ldr r9, =ramBase
+    stm r9, {r1-r8}
+
+    and r0, r0, #0x3
+    teq r0, #0x2
+    beq ca7_core2
+    teq r0, #0x3
+    beq ca7_core3
+
+    @ bootrom power dowm mode
+    movw r2, #:lower16:SRAMROM_BOOT_ADDR
+    movt r2, #:upper16:SRAMROM_BOOT_ADDR
+    ldr r4, =_start
+    str r4, [r2]
+
+ca7_core1:
+    @ ca7 core 1
+    movw r0, #:lower16:SLAVE1_MAGIC_REG
+    movt r0, #:upper16:SLAVE1_MAGIC_REG
+    movw r1, #:lower16:SLAVE1_MAGIC_NUM
+    movt r1, #:upper16:SLAVE1_MAGIC_NUM
+    sev
+    b ramBase
+ca7_core2:
+    @ ca7 core 2
+    movw r0, #:lower16:SLAVE2_MAGIC_REG
+    movt r0, #:upper16:SLAVE2_MAGIC_REG
+    movw r1, #:lower16:SLAVE2_MAGIC_NUM
+    movt r1, #:upper16:SLAVE2_MAGIC_NUM
+    wfe
+    b ramBase
+ca7_core3:    
+    @ ca7 core 3
+    movw r0, #:lower16:SLAVE3_MAGIC_REG
+    movt r0, #:upper16:SLAVE3_MAGIC_REG
+    movw r1, #:lower16:SLAVE3_MAGIC_NUM
+    movt r1, #:upper16:SLAVE3_MAGIC_NUM
+    wfe
+    b ramBase
+
+/* 
+ * Before waking cores up, it needs to set magic number 
+ * for each core and set jump address in SLAVE_JUMP_REG 
+ * for pc of core.
+ */
+load_magic_num:
+    wfi
+    ldr r2, [r0]
+    cmp r2, r1
+    bne load_magic_num
+    movw r0, #:lower16:SLAVE_JUMP_REG
+    movt r0, #:upper16:SLAVE_JUMP_REG
+    ldr r1, [r0]
+    mov pc, r1
diff --git a/src/bsp/lk/platform/mt2701/debug.c b/src/bsp/lk/platform/mt2701/debug.c
new file mode 100644
index 0000000..2ba6587
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/debug.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#include <stdarg.h>
+#include <reg.h>
+#include <stdio.h>
+#include <kernel/thread.h>
+#include <dev/uart.h>
+#include <platform/debug.h>
+#include <platform/mt2701.h>
+#include <target/debugconfig.h>
+#include <reg.h>
+
+/* DEBUG_UART must be defined to 0 or 1 */
+#if defined(DEBUG_UART) && DEBUG_UART == 0
+#define DEBUG_UART_BASE UART0_BASE
+#elif defined(DEBUG_UART) && DEBUG_UART == 1
+#define DEBUG_UART_BASE UART1_BASE
+#else
+#error define DEBUG_UART to something valid
+#endif
+
+void platform_dputc(char c)
+{
+    if (c == '\n')
+        uart_putc(DEBUG_UART, '\r');
+    uart_putc(DEBUG_UART, c);
+}
+
+int platform_dgetc(char *c, bool wait)
+{
+    int ret = uart_getc(DEBUG_UART, wait);
+    if (ret == -1)
+        return -1;
+    *c = ret;
+    return 0;
+}
+
+void platform_pputc(char c)
+{
+    if (c == '\n')
+        uart_pputc(DEBUG_UART, '\r');
+    uart_pputc(DEBUG_UART, c);
+}
+
+int platform_pgetc(char *c, bool wait)
+{
+    int ret = uart_pgetc(DEBUG_UART);
+    if (ret < 0)
+        return ret;
+    *c = ret;
+    return 0;
+}
+
diff --git a/src/bsp/lk/platform/mt2701/drivers/efuse/efuse.c b/src/bsp/lk/platform/mt2701/drivers/efuse/efuse.c
new file mode 100644
index 0000000..f536cdb
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/efuse/efuse.c
@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) 2017 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <bits.h>
+#include <debug.h>
+#include <errno.h>
+#include <platform/efuse.h>
+#include <platform/mt_reg_base.h>
+#include <reg.h>
+#include <stdio.h>
+#include <string.h>
+
+#define REG_SBC_PUBK0_HASH (EFUSE_BASE + 0x90)
+#define REG_SBC_PUBK1_HASH (EFUSE_BASE + 0x220)
+#define REG_SBC_PUBK2_HASH (EFUSE_BASE + 0x240)
+#define REG_SBC_PUBK3_HASH (EFUSE_BASE + 0x260)
+#define REG_SBC_PUBK0_DIS (EFUSE_BASE + 0x2a0)
+#define REG_SBC_PUBK1_DIS (EFUSE_BASE + 0x2a4)
+#define REG_SBC_PUBK2_DIS (EFUSE_BASE + 0x2a8)
+#define REG_SBC_PUBK3_DIS (EFUSE_BASE + 0x2ac)
+#define REG_HRID (EFUSE_BASE + 0x140)
+
+struct efuse {
+    u32 idx;
+    addr_t reg;
+    u32 bit_shift;
+    u32 bit_width;
+};
+
+#define EFUEE_SET(_idx, _reg, _bit_shift, _bit_width) {   \
+        .idx = _idx,                  \
+        .reg = _reg,                    \
+        .bit_shift = _bit_shift,                \
+        .bit_width = _bit_width,                \
+}
+
+static const struct efuse efuse_data[] = {
+    EFUEE_SET(SBC_PUBK0_HASH, REG_SBC_PUBK0_HASH, 0, 0),
+    EFUEE_SET(SBC_PUBK1_HASH, REG_SBC_PUBK1_HASH, 0, 0),
+    EFUEE_SET(SBC_PUBK2_HASH, REG_SBC_PUBK2_HASH, 0, 0),
+    EFUEE_SET(SBC_PUBK3_HASH, REG_SBC_PUBK3_HASH, 0, 0),
+    EFUEE_SET(SBC_PUBK0_DIS, REG_SBC_PUBK0_DIS, 0, 1),
+    EFUEE_SET(SBC_PUBK1_DIS, REG_SBC_PUBK1_DIS, 0, 1),
+    EFUEE_SET(SBC_PUBK2_DIS, REG_SBC_PUBK2_DIS, 0, 1),
+    EFUEE_SET(SBC_PUBK3_DIS, REG_SBC_PUBK3_DIS, 0, 1),
+    EFUEE_SET(HRID, REG_HRID, 0, 0),
+};
+
+int fuse_read(u32 fuse, u8 *data, size_t len)
+{
+    addr_t efuse_reg;
+    u32 i, bit_width, bit_shift, fuse_data[16] = {0};
+
+    if (!len || !data)
+        return -EINVAL;
+
+    if (fuse >= EFUSE_MAX)
+        return -ENXIO;
+
+    if (efuse_info[fuse].len != len)
+        return -EINVAL;
+
+    if (efuse_data[fuse].idx != fuse)
+        return -EFAULT;
+
+    bit_shift = efuse_data[fuse].bit_shift;
+    efuse_reg = efuse_data[fuse].reg;
+    bit_width = efuse_data[fuse].bit_width;
+
+    if (len < 4) {
+        fuse_data[0] = (readl(efuse_reg) >> bit_shift) & BIT_MASK(bit_width);
+        dprintf(SPEW, "[read] fuse_data[0] = 0x%08x\n", fuse_data[0]);
+    } else {
+        u32 reg_cnt;
+
+        reg_cnt = len >> 2;
+        for (i = 0; i < reg_cnt; i++) {
+            fuse_data[i] = readl(efuse_reg + (i << 2));
+            dprintf(SPEW, "[read] fuse_data[%d] = 0x%08x\n", i, fuse_data[i]);
+        }
+    }
+
+    memcpy((void *)data, (void *)&fuse_data, len);
+
+    return 0;
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/efuse/rules.mk b/src/bsp/lk/platform/mt2701/drivers/efuse/rules.mk
new file mode 100644
index 0000000..8f2a023
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/efuse/rules.mk
@@ -0,0 +1,7 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+    $(LOCAL_DIR)/efuse.c \
+
+include make/module.mk
diff --git a/src/bsp/lk/platform/mt2701/drivers/key/mtk_key.c b/src/bsp/lk/platform/mt2701/drivers/key/mtk_key.c
new file mode 100644
index 0000000..2f8485d
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/key/mtk_key.c
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <reg.h>
+#include <platform/mt_reg_base.h>
+#include <debug.h>
+#include <platform/mtk_key.h>
+#define GPIO_DIN18          (GPIO_BASE + 0x00000740)
+
+bool check_download_key(void)
+{
+    return (readl(GPIO_DIN18) & (1U << 6)) == 0;
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/mmc/mmc_core.c b/src/bsp/lk/platform/mt2701/drivers/mmc/mmc_core.c
new file mode 100644
index 0000000..b5d0971
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/mmc/mmc_core.c
@@ -0,0 +1,1062 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+/*=======================================================================*/
+/* HEADER FILES                                                          */
+/*=======================================================================*/
+#include <config.h>
+#include <platform/msdc.h>
+#include <platform/mmc_core.h>
+#include <lib/bio.h>
+#include <lib/heap.h>
+#include <lib/partition.h>
+#include <string.h>
+#include <err.h>
+#include <kernel/mutex.h>
+
+#define CMD_RETRIES        (5)
+#define CMD_TIMEOUT        (100)    /* 100ms */
+
+static int mmc_set_ext_csd(struct mmc_card *card, u8 addr, u8 value);
+
+typedef struct {
+    bdev_t bdev;
+    u32 part_id;
+    struct mmc_host *host;
+    struct mmc_card *card;
+} mmc_dev_t;
+
+static const unsigned int tran_exp[] = {
+    10000,      100000,     1000000,    10000000,
+    0,      0,      0,      0
+};
+
+static const unsigned char mmc_tran_mant[] = {
+    0,  10, 12, 13, 15, 20, 26, 30,
+    35, 40, 45, 52, 55, 60, 70, 80,
+};
+
+static u32 unstuff_bits(u32 *resp, u32 start, u32 size)
+{
+    const u32 __mask = (1 << (size)) - 1;
+    const int __off = 3 - ((start) / 32);
+    const int __shft = (start) & 31;
+    u32 __res;
+
+    __res = resp[__off] >> __shft;
+    if ((size) + __shft >= 32)
+        __res |= resp[__off-1] << (32 - __shft);
+    return __res & __mask;
+}
+
+#define UNSTUFF_BITS(r,s,sz)    unstuff_bits(r,s,sz)
+
+static int mmc_switch_part(mmc_dev_t *dev)
+{
+    int err = MMC_ERR_NONE;
+    struct mmc_card *card;
+    struct mmc_host *host;
+    u8 cfg;
+
+    host = dev->host;
+    if (host->curr_part == dev->part_id)
+        /* already set to specific partition */
+        return MMC_ERR_NONE;
+
+    if (dev->part_id > EXT_CSD_PART_CFG_GP_PART_4) {
+        dprintf(CRITICAL, "[MSDC] Unsupported partid: %u\n", dev->part_id);
+        return MMC_ERR_INVALID;
+    }
+
+    card = dev->card;
+    ASSERT(card);
+
+    cfg = card->ext_csd.part_cfg;
+    cfg = (cfg & ~0x7) | dev->part_id;
+    err = mmc_set_ext_csd(card, EXT_CSD_PART_CFG, cfg);
+    if (err)
+        dprintf(CRITICAL, "[MSDC] switch to part %u failed!\n", dev->part_id);
+    else
+        card->ext_csd.part_cfg = cfg;
+
+    return err;
+}
+
+static int mmc_cmd(struct mmc_host *host, struct mmc_command *cmd)
+{
+    int err;
+    int retry = cmd->retries;
+
+    do {
+        err = msdc_cmd(host, cmd);
+        if (err == MMC_ERR_NONE)
+            break;
+    } while (retry--);
+
+    return err;
+}
+
+static u32 mmc_select_voltage(struct mmc_host *host, u32 ocr)
+{
+    int bit;
+
+    ocr &= host->ocr_avail;
+
+    bit = __builtin_ffs(ocr);
+    if (bit) {
+        bit -= 1;
+        ocr &= 3 << bit;
+    } else {
+        ocr = 0;
+    }
+    return ocr;
+}
+
+static inline int mmc_go_idle(struct mmc_host *host)
+{
+    struct mmc_command cmd = {
+        MMC_CMD_GO_IDLE_STATE, 0, RESP_NONE, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+    return mmc_cmd(host, &cmd);
+}
+
+static int mmc_send_op_cond(struct mmc_host *host, u32 ocr, u32 *rocr)
+{
+    int i, err = 0;
+    struct mmc_command cmd = {
+        MMC_CMD_SEND_OP_COND, 0, RESP_R3, {0}, CMD_TIMEOUT, 0, 0
+    };
+
+    cmd.arg = ocr;
+
+    for (i = 100; i; i--) {
+        err = mmc_cmd(host, &cmd);
+        if (err)
+            break;
+
+        /* if we're just probing, do a single pass */
+        if (ocr == 0)
+            break;
+
+        if (cmd.resp[0] & MMC_CARD_BUSY)
+            break;
+
+        err = MMC_ERR_TIMEOUT;
+
+        spin(10000);
+    }
+
+    if (!err && rocr)
+        *rocr = cmd.resp[0];
+
+    return err;
+}
+
+static int mmc_all_send_cid(struct mmc_host *host)
+{
+    struct mmc_command cmd = {
+        MMC_CMD_ALL_SEND_CID, 0, RESP_R2, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+    return mmc_cmd(host, &cmd);
+}
+
+static int mmc_send_relative_addr(struct mmc_host *host,
+                                  struct mmc_card *card, unsigned int *rca)
+{
+    struct mmc_command cmd = {
+        MMC_CMD_SET_RELATIVE_ADDR, 0, RESP_R1, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+    cmd.arg = *rca << 16;
+    return mmc_cmd(host, &cmd);
+}
+
+static int mmc_select_card(struct mmc_host *host, struct mmc_card *card)
+{
+    struct mmc_command cmd = {
+        MMC_CMD_SELECT_CARD, 0, RESP_R1B, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+    cmd.arg = card->rca << 16;
+    return mmc_cmd(host, &cmd);
+}
+
+static int mmc_send_status(struct mmc_host *host, struct mmc_card *card,
+                           u32 *status)
+{
+    int err;
+    struct mmc_command cmd = {
+        MMC_CMD_SEND_STATUS, 0, RESP_R1, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+    cmd.arg = card->rca << 16;
+
+    err = mmc_cmd(host, &cmd);
+    if (err == MMC_ERR_NONE)
+        *status = cmd.resp[0];
+    return err;
+}
+
+static int mmc_switch(struct mmc_host *host, struct mmc_card *card,
+                      u8 set, u8 index, u8 value)
+{
+    int err;
+    u32 status = 0, count = 0;
+    struct mmc_command cmd = {
+        MMC_CMD_SWITCH, 0, RESP_R1B, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+
+    cmd.arg = (MMC_SWITCH_MODE_WRITE_BYTE << 24) | (index << 16) |
+              (value << 8) | set;
+
+    err = mmc_cmd(host, &cmd);
+    if (err != MMC_ERR_NONE)
+        return err;
+
+    do {
+        err = mmc_send_status(host, card, &status);
+        if (err) {
+            dprintf(CRITICAL, "[eMMC] Fail to send status %d\n", err);
+            break;
+        }
+        if (status & R1_SWITCH_ERROR) {
+            dprintf(CRITICAL, "[eMMC] switch error. arg(0x%x)\n", cmd.arg);
+            return MMC_ERR_FAILED;
+        }
+        if (count++ >= 600000) {
+            dprintf(CRITICAL, "[%s]: timeout happend, count=%d, status=0x%x\n",
+                    __func__, count, status);
+            break;
+        }
+    } while (!(status & R1_READY_FOR_DATA) || (R1_CURRENT_STATE(status) == 7));
+
+    if (!err && (index == EXT_CSD_PART_CFG))
+        host->curr_part = value & 0x7;
+
+    return err;
+}
+
+static int mmc_read_csds(struct mmc_host *host, struct mmc_card *card)
+{
+    int err;
+    struct mmc_command cmd = {
+        MMC_CMD_SEND_CSD, 0, RESP_R2, {0}, CMD_TIMEOUT * 100, CMD_RETRIES, 0
+    };
+
+    cmd.arg = card->rca << 16;
+
+    err = mmc_cmd(host, &cmd);
+    if (err == MMC_ERR_NONE) {
+        unsigned int e, m;
+        card->csd.mmca_vsn = UNSTUFF_BITS(&cmd.resp[0], 122, 4);
+        m = UNSTUFF_BITS(&cmd.resp[0], 99, 4);
+        e = UNSTUFF_BITS(&cmd.resp[0], 96, 3);
+        card->csd.max_dtr = tran_exp[e] * mmc_tran_mant[m];
+        e = UNSTUFF_BITS(&cmd.resp[0], 47, 3);
+        m = UNSTUFF_BITS(&cmd.resp[0], 62, 12);
+        card->csd.capacity = (1 + m) << (e + 2);
+        card->csd.read_blkbits = UNSTUFF_BITS(&cmd.resp[0], 80, 4);
+    }
+
+    return err;
+}
+
+static void mmc_decode_ext_csd(struct mmc_card *card, u8 *ext_csd)
+{
+    card->ext_csd.sectors =
+        ext_csd[EXT_CSD_SEC_CNT + 0] << 0 |
+        ext_csd[EXT_CSD_SEC_CNT + 1] << 8 |
+        ext_csd[EXT_CSD_SEC_CNT + 2] << 16 |
+        ext_csd[EXT_CSD_SEC_CNT + 3] << 24;
+
+    card->ext_csd.rev = ext_csd[EXT_CSD_REV];
+    card->ext_csd.boot_info   = ext_csd[EXT_CSD_BOOT_INFO];
+    card->ext_csd.boot_part_sz = ext_csd[EXT_CSD_BOOT_SIZE_MULT] * 128 * 1024;
+
+    if (card->ext_csd.sectors)
+        mmc_card_set_blockaddr(card);
+
+    if ((ext_csd[EXT_CSD_CARD_TYPE] & EXT_CSD_CARD_TYPE_DDR_52_1_2V) ||
+            (ext_csd[EXT_CSD_CARD_TYPE] & EXT_CSD_CARD_TYPE_DDR_52)) {
+        card->ext_csd.ddr_support = 1;
+        card->ext_csd.hs_max_dtr = 52000000;
+    } else if (ext_csd[EXT_CSD_CARD_TYPE] & EXT_CSD_CARD_TYPE_52) {
+        card->ext_csd.hs_max_dtr = 52000000;
+    } else if ((ext_csd[EXT_CSD_CARD_TYPE] & EXT_CSD_CARD_TYPE_26)) {
+        card->ext_csd.hs_max_dtr = 26000000;
+    } else {
+        /* MMC v4 spec says this cannot happen */
+        dprintf(CRITICAL, "[eMMC] MMCv4 but HS unsupported\n");
+    }
+
+    card->ext_csd.part_cfg = ext_csd[EXT_CSD_PART_CFG];
+    card->ext_csd.sec_support = ext_csd[EXT_CSD_SEC_FEATURE_SUPPORT];
+    card->ext_csd.reset_en = ext_csd[EXT_CSD_RST_N_FUNC];
+
+    return;
+}
+
+/* Read and decode extended CSD. */
+static int mmc_read_ext_csd(struct mmc_host *host, struct mmc_card *card)
+{
+    int err = MMC_ERR_NONE;
+    u8 *ext_csd;
+    int result = MMC_ERR_NONE;
+    struct mmc_data data;
+    u32 base = host->base;
+    struct mmc_command cmd = {
+        MMC_CMD_SEND_EXT_CSD, 0, RESP_R1, {0}, CMD_TIMEOUT, CMD_RETRIES, 0
+    };
+
+    if (card->csd.mmca_vsn < CSD_SPEC_VER_4) {
+        dprintf(INFO, "[eMMC] MMCA_VSN: %d. Skip EXT_CSD\n",
+                card->csd.mmca_vsn);
+        return MMC_ERR_NONE;
+    }
+
+    /*
+     * As the ext_csd is so large and mostly unused, we don't store the
+     * raw block in mmc_card.
+     */
+    ext_csd = malloc(512);
+    ASSERT(ext_csd);
+    memset(ext_csd, 0, 512);
+
+    msdc_reset_tune_counter(host);
+
+    do {
+        MSDC_DMA_ON;
+        MSDC_CLR_FIFO();
+        MSDC_WRITE32(SDC_BLK_NUM, 1);
+        host->blklen = 512;
+        msdc_set_timeout(host, 100000000, 0);
+        err = mmc_cmd(host, &cmd);
+        if (err != MMC_ERR_NONE)
+            goto out;
+
+        data.cmd = &cmd;
+        data.blks = 1;
+        data.buf = ext_csd;
+        data.timeout = 100;
+        err = msdc_dma_transfer(host, &data);
+        MSDC_DMA_OFF;
+        if (err != MMC_ERR_NONE) {
+            if (msdc_abort_handler(host, 1))
+                dprintf(CRITICAL, "[eMMC] data abort failed\n");
+            result = msdc_tune_read(host);
+        }
+    } while (err && result != MMC_ERR_READTUNEFAIL);
+    msdc_reset_tune_counter(host);
+    mmc_decode_ext_csd(card, ext_csd);
+
+out:
+    free(ext_csd);
+    return err;
+}
+
+static int mmc_erase_start(struct mmc_card *card, u32 blknr)
+{
+    struct mmc_command cmd = {
+        MMC_CMD_ERASE_GROUP_START, 0, RESP_R1, {0}, CMD_TIMEOUT, 3, 0
+    };
+    cmd.arg = blknr;
+    return mmc_cmd(card->host, &cmd);
+}
+
+static int mmc_erase_end(struct mmc_card *card, u32 blknr)
+{
+    struct mmc_command cmd = {
+        MMC_CMD_ERASE_GROUP_END, 0, RESP_R1, {0}, CMD_TIMEOUT, 3, 0
+    };
+    cmd.arg = blknr;
+    return mmc_cmd(card->host, &cmd);
+}
+
+static int mmc_erase(struct mmc_card *card, u32 arg)
+{
+    int err;
+    u32 status;
+    struct mmc_command cmd = {
+        MMC_CMD_ERASE, 0, RESP_R1B, {0}, CMD_TIMEOUT, 3, 0
+    };
+    cmd.arg = arg;
+
+    if (arg & MMC_ERASE_SECURE_REQ) {
+        if (!(card->ext_csd.sec_support & EXT_CSD_SEC_FEATURE_ER_EN))
+            return MMC_ERR_INVALID;
+    }
+    if ((arg & MMC_ERASE_GC_REQ) || (arg & MMC_ERASE_TRIM)) {
+        if (!(card->ext_csd.sec_support & EXT_CSD_SEC_FEATURE_GB_CL_EN))
+            return MMC_ERR_INVALID;
+    }
+
+    err = mmc_cmd(card->host, &cmd);
+    if (err)
+        return err;
+
+    do {
+        err = mmc_send_status(card->host, card, &status);
+        if (err)
+            break;
+        if (R1_STATUS(status) != 0)
+            break;
+    } while (R1_CURRENT_STATE(status) == 7);
+
+    return err;
+}
+
+/* start_addr & len must block-aligned */
+static int mmc_do_trim(struct mmc_card *card, off_t start_addr, size_t len)
+{
+    int err = MMC_ERR_NONE;
+    off_t end_addr;
+
+    if (len < MMC_BLOCK_SIZE)
+        goto error;
+
+    /* Calculate the start and end erase address */
+    end_addr = start_addr +
+               (((len >> MMC_BLOCK_BITS_SHFT) - 1) << MMC_BLOCK_BITS_SHFT);
+    if (mmc_card_blockaddr(card)) {
+        /* Capacity larger than 2GB */
+        start_addr >>= MMC_BLOCK_BITS_SHFT;
+        end_addr >>= MMC_BLOCK_BITS_SHFT;
+    }
+
+    err = mmc_erase_start(card, start_addr);
+    if (err)
+        goto error;
+
+    err = mmc_erase_end(card, end_addr);
+    if (err)
+        goto error;
+
+    err = mmc_erase(card, MMC_ERASE_TRIM);
+
+error:
+    if (err)
+        dprintf(CRITICAL, "%s: erase range (0x%llx~0x%llx) failed,Err<%d>\n",
+                __func__, start_addr, end_addr, err);
+
+    return err;
+}
+
+static void mmc_set_clock(struct mmc_host *host, int ddr, unsigned int hz)
+{
+    if (hz >= host->f_max) {
+        hz = host->f_max;
+    } else if (hz < host->f_min) {
+        hz = host->f_min;
+    }
+    msdc_config_clock(host, ddr > 0 ? 1 : 0, hz);
+}
+
+static int mmc_set_ext_csd(struct mmc_card *card, u8 addr, u8 value)
+{
+    int err;
+
+    /* can't write */
+    if (192 <= addr || !card)
+        return MMC_ERR_INVALID;
+
+    err = mmc_switch(card->host, card, EXT_CSD_CMD_SET_NORMAL, addr, value);
+
+    if (err == MMC_ERR_NONE)
+        err = mmc_read_ext_csd(card->host, card);
+
+    return err;
+}
+
+static int mmc_set_bus_width(struct mmc_host *host, struct mmc_card *card, int width)
+{
+    int err = MMC_ERR_NONE;
+    u32 arg = 0;
+
+    if (card->csd.mmca_vsn < CSD_SPEC_VER_4)
+        goto out;
+
+    if (width == HOST_BUS_WIDTH_8) {
+        if (host->caps & MMC_CAP_8_BIT_DATA) {
+            arg = ((host->caps & MMC_CAP_DDR) && card->ext_csd.ddr_support) ?
+                  EXT_CSD_BUS_WIDTH_8_DDR : EXT_CSD_BUS_WIDTH_8;
+        } else {
+            width = HOST_BUS_WIDTH_4;
+        }
+    }
+    if (width == HOST_BUS_WIDTH_4) {
+        if (host->caps & MMC_CAP_4_BIT_DATA) {
+            arg = ((host->caps & MMC_CAP_DDR) && card->ext_csd.ddr_support) ?
+                  EXT_CSD_BUS_WIDTH_4_DDR : EXT_CSD_BUS_WIDTH_4;
+        } else {
+            width = HOST_BUS_WIDTH_1;
+        }
+    }
+    if (width == HOST_BUS_WIDTH_1)
+        arg = EXT_CSD_BUS_WIDTH_1;
+
+    err = mmc_switch(host, card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_BUS_WIDTH, arg);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Switch to bus width(%d) failed\n", arg);
+        goto out;
+    }
+    if (arg == EXT_CSD_BUS_WIDTH_8_DDR || arg == EXT_CSD_BUS_WIDTH_4_DDR) {
+        mmc_card_set_ddr(card);
+    } else {
+        mmc_card_clr_ddr(card);
+    }
+    mmc_set_clock(host, card->state, host->sclk);
+
+    msdc_config_bus(host, width);
+
+out:
+    return err;
+}
+
+static int mmc_set_reset_func(struct mmc_card *card, u8 enable)
+{
+	int err = MMC_ERR_FAILED;
+
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4)
+		goto out;
+
+	if (card->ext_csd.reset_en == 0) {
+		err = mmc_set_ext_csd(card, EXT_CSD_RST_N_FUNC, enable);
+		if (err == MMC_ERR_NONE)
+			card->ext_csd.reset_en = enable;
+	} else {
+		/* no need set */
+		return MMC_ERR_NONE;
+	}
+out:
+	return err;
+}
+
+static int mmc_set_boot_bus(struct mmc_card *card, u8 rst_bwidth, u8 mode, u8 bwidth)
+{
+	int err = MMC_ERR_FAILED;
+	u8 arg;
+
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4)
+		goto out;
+
+	arg = mode | rst_bwidth | bwidth;
+
+	err = mmc_set_ext_csd(card, EXT_CSD_BOOT_BUS_WIDTH, arg);
+out:
+	return err;
+}
+
+static int mmc_set_part_config(struct mmc_card *card, u8 cfg)
+{
+	int err = MMC_ERR_FAILED;
+
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4)
+		goto out;
+
+	err = mmc_set_ext_csd(card, EXT_CSD_PART_CFG, cfg);
+out:
+	return err;
+}
+
+static int mmc_boot_config(struct mmc_card *card, u8 acken, u8 enpart, u8 buswidth, u8 busmode)
+{
+	int err = MMC_ERR_FAILED;
+	u8 val;
+	u8 rst_bwidth = 0;
+	u8 cfg;
+
+	if (card->csd.mmca_vsn < CSD_SPEC_VER_4 ||
+	    !card->ext_csd.boot_info || card->ext_csd.rev < 3)
+		goto out;
+
+	cfg = card->ext_csd.part_cfg;
+	/* configure boot partition */
+	val = acken | enpart | (cfg & 0x7);
+	err = mmc_set_part_config(card, val);
+	if (err != MMC_ERR_NONE)
+		goto out;
+	else
+		card->ext_csd.part_cfg = val;
+
+	/* configure boot bus mode and width */
+	rst_bwidth = (buswidth != EXT_CSD_BOOT_BUS_WIDTH_1 ? 1 : 0) << 2;
+	dprintf(INFO, "=====Set boot Bus Width<%d>=======\n", buswidth);
+	dprintf(INFO, "=====Set boot Bus mode<%d>=======\n", busmode);
+	err = mmc_set_boot_bus(card, rst_bwidth, busmode, buswidth);
+out:
+
+	return err;
+}
+
+static int emmc_boot_prepare(struct mmc_card *card)
+{
+	int err = MMC_ERR_NONE;
+
+	err = mmc_boot_config(card, EXT_CSD_PART_CFG_EN_ACK,
+				EXT_CSD_PART_CFG_EN_BOOT_PART_1,
+				EXT_CSD_BOOT_BUS_WIDTH_1,
+				EXT_CSD_BOOT_BUS_MODE_DEFT);
+	if (err)
+		goto exit;
+
+	err = mmc_set_reset_func(card, 1);
+exit:
+	return err;
+}
+
+static int mmc_dev_bread(struct mmc_card *card, unsigned long blknr, u32 blkcnt, u8 *dst)
+{
+    struct mmc_host *host = card->host;
+    u32 blksz = host->blklen;
+    int tune = 0;
+    int retry = 3;
+    int err;
+    unsigned long src;
+
+    src = mmc_card_highcaps(card) ? blknr : blknr * blksz;
+
+    do {
+        if (!tune) {
+            err = host->blk_read(host, (uchar *)dst, src, blkcnt);
+        } else {
+#ifdef FEATURE_MMC_RD_TUNING
+            err = msdc_tune_bread(host, (uchar *)dst, src, blkcnt);
+#endif
+            if (err && (host->sclk > (host->f_max >> 4)))
+                mmc_set_clock(host, card->state, host->sclk >> 1);
+        }
+        if (err == MMC_ERR_NONE) {
+            break;
+        }
+
+        if (err == MMC_ERR_BADCRC || err == MMC_ERR_ACMD_RSPCRC || err == MMC_ERR_CMD_RSPCRC) {
+            tune = 1;
+            retry++;
+        } else if (err == MMC_ERR_READTUNEFAIL || err == MMC_ERR_CMDTUNEFAIL) {
+            dprintf(CRITICAL, "[eMMC] Fail to tuning,%s",
+                    (err == MMC_ERR_CMDTUNEFAIL) ?
+                    "cmd tune failed!\n" : "read tune failed!\n");
+            break;
+        }
+    } while (retry--);
+
+    return err;
+}
+
+static int mmc_dev_bwrite(struct mmc_card *card, unsigned long blknr,
+                          u32 blkcnt, const u8 *src)
+{
+    struct mmc_host *host = card->host;
+    u32 blksz = host->blklen;
+    u32 status;
+    int tune = 0;
+    int retry = 3;
+    int err;
+    unsigned long dst;
+
+    dst = mmc_card_highcaps(card) ? blknr : blknr * blksz;
+
+    do {
+        if (!tune) {
+            err = host->blk_write(host, dst, (uchar *)src, blkcnt);
+        } else {
+#ifdef FEATURE_MMC_WR_TUNING
+            err = msdc_tune_bwrite(host, dst, (uchar *)src, blkcnt);
+#endif
+            if (err && (host->sclk > (host->f_max >> 4)))
+                mmc_set_clock(host, card->state, host->sclk >> 1);
+        }
+        if (err == MMC_ERR_NONE) {
+            do {
+                err = mmc_send_status(host, card, &status);
+                if (err) {
+                    dprintf(CRITICAL, "[eMMC] Fail to send status %d\n", err);
+                    break;
+                }
+            } while (!(status & R1_READY_FOR_DATA) ||
+                     (R1_CURRENT_STATE(status) == 7));
+            dprintf(INFO, "[eMMC] Write %d bytes (DONE)\n", blkcnt * blksz);
+            break;
+        }
+
+        if (err == MMC_ERR_BADCRC || err == MMC_ERR_ACMD_RSPCRC || err == MMC_ERR_CMD_RSPCRC) {
+            tune = 1;
+            retry++;
+        } else if (err == MMC_ERR_WRITETUNEFAIL || err == MMC_ERR_CMDTUNEFAIL) {
+            dprintf(CRITICAL, "[eMMC] Fail to tuning,%s",
+                    (err == MMC_ERR_CMDTUNEFAIL) ?
+                    "cmd tune failed!\n" : "write tune failed!\n");
+            break;
+        }
+    } while (retry--);
+
+    return err;
+}
+
+static ssize_t mmc_block_read(struct bdev *dev, void *buf, bnum_t block,
+                              uint count)
+{
+    mmc_dev_t *__dev = (mmc_dev_t *)dev;
+    struct mmc_host *host = __dev->host;
+    struct mmc_card *card = __dev->card;
+    u32 maxblks = host->max_phys_segs;
+    u32 leftblks, totalblks = count;
+    ssize_t ret = 0;
+
+    mutex_acquire(&host->lock);
+    if (mmc_switch_part(__dev)) {
+	    ret = ERR_IO;
+	    goto done;
+    }
+
+    do {
+        leftblks = ((count > maxblks) ? maxblks : count);
+        if (mmc_dev_bread(card, (unsigned long)block, leftblks, buf)){
+            ret = ERR_IO;
+            goto done;
+        }
+        block += leftblks;
+        buf += maxblks * dev->block_size;
+        count -= leftblks;
+    } while (count);
+
+    if (dev->block_size * totalblks > 0x7fffffffU)
+        /* ssize_t is defined as signed, should take a look here */
+        dprintf(CRITICAL, "[MSDC] %s: WARN! The return size is overflow! 0x%x\n",
+                __func__, dev->block_size * totalblks);
+
+done:
+    mutex_release(&host->lock);
+    return ret ? ret :(ssize_t)(dev->block_size * totalblks);
+}
+
+static ssize_t mmc_block_write(struct bdev *dev, const void *buf, bnum_t block,
+                               uint count)
+{
+    mmc_dev_t *__dev = (mmc_dev_t *)dev;
+    struct mmc_host *host = __dev->host;
+    struct mmc_card *card = __dev->card;
+    u32 maxblks = host->max_phys_segs;
+    u32 leftblks, totalblks = count;
+    ssize_t ret = 0;
+
+    mutex_acquire(&host->lock);
+    if (mmc_switch_part(__dev)) {
+        ret = ERR_IO;
+        goto done;
+    }
+
+    do {
+        leftblks = ((count > maxblks) ? maxblks : count);
+        if (mmc_dev_bwrite(card, (unsigned long)block, leftblks, buf)) {
+            ret = ERR_IO;
+            goto done;
+        }
+        block += leftblks;
+        buf = (u8 *)buf + maxblks * dev->block_size;
+        count -= leftblks;
+    } while (count);
+
+    if (dev->block_size * totalblks > 0x7fffffffU)
+        /* ssize_t is defined as signed, should take a look here */
+        dprintf(CRITICAL, "[MSDC] %s: WARN! The return size is overflow! 0x%x\n",
+                __func__, dev->block_size * totalblks);
+
+ done:
+    mutex_release(&host->lock);
+    return ret ? ret:(ssize_t)(dev->block_size * totalblks);
+}
+
+static ssize_t mmc_wrap_erase(struct bdev *bdev, off_t offset, size_t len)
+{
+    mmc_dev_t *dev = (mmc_dev_t *)bdev;
+    struct mmc_host *host = dev->host;
+    ssize_t ret = 0;
+
+    mutex_acquire(&host->lock);
+    if (mmc_switch_part(dev)) {
+        ret = ERR_IO;
+        goto done;
+    }
+
+    /* ATTENTION:
+     * We use TRIM here, which is block-based(512B) wipping,
+     * If using ERASE here, please ensure the offset & size are
+     * erase-group aligned,
+     * OTHERWISE, some valid data may be wiped. refer to JEDEC spec:
+     * The Device will ignore all LSB's below the Erase Group size,
+     * effectively ROUNDING the address DOWN to the Erase Group boundary. */
+    ASSERT(dev && len);
+    if ((offset % MMC_BLOCK_SIZE) || (len % MMC_BLOCK_SIZE)) {
+        dprintf(CRITICAL, "%s: offset(0x%llx)/len(%u) is not block-aligned!\n",
+                __func__, offset, len);
+        ret = ERR_IO;
+        goto done;
+    }
+
+    ASSERT(dev->card);
+    if (mmc_do_trim(dev->card, offset, len)){
+        ret = ERR_IO;
+        goto done;
+	}
+done:
+    mutex_release(&host->lock);
+    return ret ? ret:(ssize_t)len;
+}
+
+static int mmc_init_mem_card(struct mmc_host *host, struct mmc_card *card, u32 ocr)
+{
+    int err;
+
+    /*
+     * Sanity check the voltages that the card claims to
+     * support.
+     */
+    if (ocr & 0x7F)
+        ocr &= ~0x7F;
+
+    ocr = host->ocr = mmc_select_voltage(host, ocr);
+
+    /*
+     * Can we support the voltage(s) of the card(s)?
+     */
+    if (!host->ocr) {
+        err = MMC_ERR_FAILED;
+        goto out;
+    }
+
+    err = mmc_go_idle(host);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in GO_IDLE_STATE cmd\n");
+        goto out;
+    }
+
+    /* host support HCS[30] */
+    ocr |= (1 << 30);
+
+    /* send operation condition */
+    /* The extra bit indicates that we support high capacity */
+    err = mmc_send_op_cond(host, ocr, &card->ocr);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in SEND_OP_COND cmd\n");
+        goto out;
+    }
+
+    /* set hcs bit if a high-capacity card */
+    card->state |= ((card->ocr >> 30) & 0x1) ? MMC_STATE_HIGHCAPS : 0;
+    /* send cid */
+    err = mmc_all_send_cid(host);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in SEND_CID cmd\n");
+        goto out;
+    }
+
+    /* assign a rca */
+    card->rca = 0x1;
+
+    /* set/send rca */
+    err = mmc_send_relative_addr(host, card, &card->rca);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in SEND_RCA cmd\n");
+        goto out;
+    }
+
+    /* send csd */
+    err = mmc_read_csds(host, card);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in SEND_CSD cmd\n");
+        goto out;
+    }
+
+    /* select this card */
+    err = mmc_select_card(host, card);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in select card cmd\n");
+        goto out;
+    }
+
+    /* send ext csd */
+    err = mmc_read_ext_csd(host, card);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in SEND_EXT_CSD cmd\n");
+        goto out;
+    }
+
+    /* activate high speed (if supported) */
+    if ((card->ext_csd.hs_max_dtr != 0) && (host->caps & MMC_CAP_MMC_HIGHSPEED)) {
+        err = mmc_switch(host, card, EXT_CSD_CMD_SET_NORMAL, EXT_CSD_HS_TIMING, 1);
+        if (err == MMC_ERR_NONE) {
+            dprintf(INFO, "[eMMC] Switched to High-Speed mode!\n");
+            mmc_card_set_highspeed(card);
+        }
+    }
+
+    /* set bus width */
+    mmc_set_bus_width(host, card, HOST_BUS_WIDTH_8);
+
+    /* compute bus speed. */
+    card->maxhz = (unsigned int)-1;
+
+    if (mmc_card_highspeed(card)) {
+        if (card->maxhz > card->ext_csd.hs_max_dtr)
+            card->maxhz = card->ext_csd.hs_max_dtr;
+    } else if (card->maxhz > card->csd.max_dtr) {
+        card->maxhz = card->csd.max_dtr;
+    }
+
+    if (mmc_card_blockaddr(card)) {
+        /* The EXT_CSD sector count is in number or 512 byte sectors. */
+        card->blklen = MMC_BLOCK_SIZE;
+        card->nblks  = card->ext_csd.sectors;
+    } else {
+        /* The CSD capacity field is in units of read_blkbits.
+         * set_capacity takes units of 512 bytes.
+         */
+        card->blklen = MMC_BLOCK_SIZE;
+        card->nblks  = card->csd.capacity << (card->csd.read_blkbits - 9);
+    }
+
+    dprintf(CRITICAL,"[eMMC] Size: %d MB, Max.Speed: %d kHz, blklen(%d), nblks(%d), ro(%d)\n",
+            ((card->nblks / 1024) * card->blklen) / 1024 , card->maxhz / 1000,
+            card->blklen, card->nblks, mmc_card_readonly(card));
+
+    card->ready = 1;
+
+    dprintf(INFO, "[eMMC] Initialized\n");
+out:
+    return err;
+}
+
+static int mmc_init_card(struct mmc_host *host, struct mmc_card *card)
+{
+    int err;
+    u32 ocr;
+
+    dprintf(INFO, "[%s]: start\n", __func__);
+    memset(card, 0, sizeof(struct mmc_card));
+
+    mmc_card_set_present(card);
+    mmc_card_set_host(card, host);
+    mmc_card_set_unknown(card);
+
+    err = mmc_go_idle(host);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in GO_IDLE_STATE cmd\n");
+        goto out;
+    }
+
+    /* query operation condition */
+    err = mmc_send_op_cond(host, 0, &ocr);
+    if (err != MMC_ERR_NONE) {
+        dprintf(CRITICAL, "[eMMC] Fail in MMC_CMD_SEND_OP_COND/SD_ACMD_SEND_OP_COND cmd\n");
+        goto out;
+    }
+
+    err = mmc_init_mem_card(host, card, ocr);
+    if (err)
+        goto out;
+
+    /* change clock */
+    mmc_set_clock(host, card->state, card->maxhz);
+
+out:
+    if (err) {
+        dprintf(CRITICAL, "[%s]: failed, err=%d\n", __func__, err);
+        return err;
+    }
+    host->card = card;
+    dprintf(INFO, "[%s]: finish successfully\n",__func__);
+    return 0;
+}
+
+static inline int mmc_init_host(struct mmc_host *host)
+{
+	mutex_init(&host->lock);
+    return msdc_init(host);
+}
+
+static void mmc_bio_ops(const void *name, const int part_id, const int nblks,
+                        struct mmc_host *host, struct mmc_card *card)
+{
+    mmc_dev_t *dev;
+
+    dev = malloc(sizeof(mmc_dev_t));
+    /* malloc fail */
+    ASSERT(dev);
+    /* construct the block device */
+    memset(dev, 0, sizeof(mmc_dev_t));
+
+    /* setup partition id*/
+    dev->part_id = part_id;
+    /* setup host */
+    dev->host = host;
+    /* setup card */
+    dev->card = card;
+    /* bio block device register */
+    bio_initialize_bdev(&dev->bdev, name,
+                        card->blklen, nblks,
+                        0, NULL, BIO_FLAGS_NONE);
+    /* override our block device hooks */
+    dev->bdev.read_block = mmc_block_read;
+    dev->bdev.write_block = mmc_block_write;
+    dev->bdev.erase = mmc_wrap_erase;
+    dev->bdev.erase_byte = 0x0;
+    bio_register_device(&dev->bdev);
+    partition_publish(dev->bdev.name, 0x0);
+}
+
+int emmc_init(void)
+{
+    int err = MMC_ERR_NONE;
+    struct mmc_host *host;
+    struct mmc_card *card;
+    int boot_part_nblks = 0;
+
+    host = malloc(sizeof(struct mmc_host));
+    /* malloc fail */
+    ASSERT(host);
+    /* construct the block device */
+    memset(host, 0, sizeof(struct mmc_host));
+
+    card = malloc(sizeof(struct mmc_card));
+    /* malloc fail */
+    ASSERT(card);
+    /* construct the block device */
+    memset(card, 0, sizeof(struct mmc_card));
+
+    err = mmc_init_host(host);
+
+    if (err == MMC_ERR_NONE)
+        err = mmc_init_card(host, card);
+    /* mmc init fail */
+    ASSERT(err == MMC_ERR_NONE);
+
+	err = emmc_boot_prepare(card);
+    ASSERT(err == MMC_ERR_NONE);
+
+    mmc_bio_ops("mmc0", EXT_CSD_PART_CFG_DEFT_PART, card->nblks, host, card);
+    boot_part_nblks = card->ext_csd.boot_part_sz/card->blklen;
+    mmc_bio_ops("mmc0boot0", EXT_CSD_PART_CFG_BOOT_PART_1, boot_part_nblks,
+                host, card);
+    mmc_bio_ops("mmc0boot1", EXT_CSD_PART_CFG_BOOT_PART_2, boot_part_nblks,
+                host, card);
+
+    return err;
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/mmc/msdc.c b/src/bsp/lk/platform/mt2701/drivers/mmc/msdc.c
new file mode 100644
index 0000000..256bed7
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/mmc/msdc.c
@@ -0,0 +1,1957 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#define MSDC_DEBUG_KICKOFF
+
+#include <platform/msdc.h>
+#include <platform/mmc_core.h>
+#include <kernel/event.h>
+#include <kernel/vm.h>
+#include <platform/interrupts.h>
+#include <platform/mt_irq.h>
+#include <string.h>
+#include <assert.h>
+
+#define CMD_RETRIES        (5)
+#define CMD_TIMEOUT        (100) /* 100ms */
+
+#define PERI_MSDC_SRCSEL   (0xc100000c)
+
+/* Tuning Parameter */
+#define DEFAULT_DEBOUNCE   (8)  /* 8 cycles */
+#define DEFAULT_DTOC       (40) /* data timeout counter. 65536x40 sclk. */
+#define DEFAULT_WDOD       (0)  /* write data output delay. no delay. */
+#define DEFAULT_BSYDLY     (8)  /* card busy delay. 8 extend sclk */
+
+#define MSDC_USE_PATCH_BIT2_TURNING_WITH_ASYNC	1
+/* Declarations */
+static int msdc_send_cmd(struct mmc_host *host, struct mmc_command *cmd);
+static int msdc_wait_cmd_done(struct mmc_host *host, struct mmc_command *cmd);
+static int msdc_tune_cmdrsp(struct mmc_host *host, struct mmc_command *cmd);
+
+typedef struct {
+    int    autocmd;
+    int    rdsmpl;
+    int    wdsmpl;
+    int    rsmpl;
+    int    start_bit;
+} msdc_priv_t;
+
+static int msdc_rsp[] = {
+    0,  /* RESP_NONE */
+    1,  /* RESP_R1 */
+    2,  /* RESP_R2 */
+    3,  /* RESP_R3 */
+    4,  /* RESP_R4 */
+    1,  /* RESP_R5 */
+    1,  /* RESP_R6 */
+    1,  /* RESP_R7 */
+    7,  /* RESP_R1b */
+};
+
+struct msdc_cust {
+    unsigned char  clk_src;           /* host clock source             */
+    unsigned char  hclk_src;           /* host clock source             */
+    unsigned char  cmd_edge;          /* command latch edge            */
+    unsigned char  data_edge;         /* data latch edge               */
+#define MSDC_SMPL_RISING        (0)
+#define MSDC_SMPL_FALLING       (1)
+#define MSDC_SMPL_SEPERATE      (2)
+    unsigned char  clk_drv;           /* clock pad driving             */
+    unsigned char  cmd_drv;           /* command pad driving           */
+    unsigned char  dat_drv;           /* data pad driving              */
+    unsigned char  rst_drv;           /* reset pin pad driving         */
+    unsigned char  ds_drv;            /* ds pad driving                */
+    unsigned char  data_pins;         /* data pins                     */
+    unsigned int   data_offset;       /* data address offset           */
+    unsigned int   flags;             /* hardware capability flags     */
+#define MSDC_CD_PIN_EN      (1 << 0)  /* card detection pin is wired   */
+#define MSDC_WP_PIN_EN      (1 << 1)  /* write protection pin is wired */
+#define MSDC_RST_PIN_EN     (1 << 2)  /* emmc reset pin is wired       */
+#define MSDC_SDIO_IRQ       (1 << 3)  /* use internal sdio irq (bus)   */
+#define MSDC_EXT_SDIO_IRQ   (1 << 4)  /* use external sdio irq         */
+#define MSDC_REMOVABLE      (1 << 5)  /* removable slot                */
+#define MSDC_SYS_SUSPEND    (1 << 6)  /* suspended by system           */
+#define MSDC_HIGHSPEED      (1 << 7)  /* high-speed mode support       */
+#define MSDC_UHS1           (1 << 8)  /* uhs-1 mode support            */
+#define MSDC_DDR            (1 << 9)  /* ddr mode support              */
+#define MSDC_HS200          (1 << 10) /* hs200 mode support(eMMC4.5)   */
+#define MSDC_HS400          (1 << 11) /* hs200 mode support(eMMC5.0)   */
+} msdc_cap = {
+    MSDC30_CLKSRC_DEFAULT, /* host clock source          */
+    MSDC50_CLKSRC4HCLK_182MHZ, /* host clock source          */
+    MSDC_SMPL_RISING,   /* command latch edge            */
+    MSDC_SMPL_RISING,   /* data latch edge               */
+    MSDC_DRVN_GEAR1,    /* clock pad driving             */
+    MSDC_DRVN_GEAR1,    /* command pad driving           */
+    MSDC_DRVN_GEAR1,    /* data pad driving              */
+    MSDC_DRVN_GEAR1,    /* rst pad driving               */
+    MSDC_DRVN_GEAR1,    /* ds pad driving                */
+    8,                  /* data pins                     */
+    0,                  /* data address offset           */
+    MSDC_HIGHSPEED
+};
+
+static event_t msdc_int_event;
+static u32 g_int_status = 0;
+static msdc_priv_t msdc_priv;
+static u32 hclks_msdc30[] = { 26000000, 208000000, 200000000, 156000000,
+                              182000000, 156000000, 178280000
+                            };
+
+/* add function for MSDC_PAD_CTL handle */
+#ifndef FPGA_PLATFORM
+static void msdc_set_smt(struct mmc_host *host, int smt)
+{
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 1), MSDC_CLK_SMT(MSDC0),      smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 1), MSDC_CMD_SMT(MSDC0),      smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), MSDC_DAT_SMT(MSDC0,DAT0), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), MSDC_DAT_SMT(MSDC0,DAT1), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), MSDC_DAT_SMT(MSDC0,DAT2), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), MSDC_DAT_SMT(MSDC0,DAT3), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), MSDC_DAT_SMT(MSDC0,DAT4), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), MSDC_DAT_SMT(MSDC0,DAT5), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), MSDC_DAT_SMT(MSDC0,DAT6), smt);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), MSDC_DAT_SMT(MSDC0,DAT7), smt);
+}
+
+/* pull up means that host driver the line to HIGH
+ * pull down means that host driver the line to LOW */
+static void msdc_pin_set(struct mmc_host *host, MSDC_PIN_STATE mode)
+{
+    /* driver CLK/DAT pin */
+    ASSERT(host);
+    ASSERT(mode < MSDC_PST_MAX);
+
+    //MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 1), BIT_MSDC_CLK_R0R1PUPD,  mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 1), BIT_MSDC_CMD_R0R1PUPD,  mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), BIT_MSDC_DAT0_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), BIT_MSDC_DAT1_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), BIT_MSDC_DAT2_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 3), BIT_MSDC_DAT3_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), BIT_MSDC_DAT4_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), BIT_MSDC_DAT5_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), BIT_MSDC_DAT6_R0R1PUPD, mode);
+    MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 4), BIT_MSDC_DAT7_R0R1PUPD, mode);
+}
+
+/* host can modify from 0-7 */
+static void msdc_set_driving(struct mmc_host *host, struct msdc_cust *msdc_cap)
+{
+    ASSERT(host);
+    ASSERT(msdc_cap);
+
+    if (host && msdc_cap) {
+        MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 0), BIT_MSDC_CLK_E8E4E2, msdc_cap->clk_drv);
+        MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 1), BIT_MSDC_CMD_E8E4E2, msdc_cap->cmd_drv);
+        MSDC_SET_FIELD(MSDC_CTRL(MSDC0, 2), BIT_MSDC_DAT_E8E4E2, msdc_cap->dat_drv);
+    }
+}
+#endif
+
+#ifndef FPGA_PLATFORM /* don't power on/off device and use power-on default volt */
+static int pmic_config_interface(int a, int b, int c, int d)
+{
+    return 0;
+}
+
+static void msdc_set_card_pwr(struct mmc_host *host, int on)
+{
+    unsigned int ret;
+
+    ret = pmic_config_interface(0xAB,0x7,0x7,4); /* VMCH=3.3V */
+
+    if (ret == 0) {
+        if (on) {
+            ret = pmic_config_interface(0xAB,0x1,0x1,0); /* VMCH_EN=1 */
+        } else {
+            ret = pmic_config_interface(0xAB,0x0,0x1,0); /* VMCH_EN=0 */
+        }
+    }
+
+    if (ret != 0) {
+        dprintf(CRITICAL, "PMIC: Set MSDC Host Power Fail\n");
+    } else {
+        spin(3000);
+    }
+}
+
+static void msdc_set_host_pwr(struct mmc_host *host, int on)
+{
+    unsigned int ret;
+
+    ret = pmic_config_interface(0xA7,0x7,0x7,4); /* VMC=3.3V */
+
+    if (ret == 0) {
+        if (on) {
+            ret = pmic_config_interface(0xA7,0x1,0x1,0); /* VMC_EN=1 */
+        } else {
+            ret = pmic_config_interface(0xA7,0x0,0x1,0); /* VMC_EN=0 */
+        }
+    }
+
+    if (ret != 0) {
+        dprintf(CRITICAL, "PMIC: Set MSDC Card Power Fail\n");
+    }
+}
+#else
+#define PWR_GPIO            (0x10001E84)
+#define PWR_GPIO_EO         (0x10001E88)
+
+#define PWR_MASK_EN         (0x1 << 8)
+#define PWR_MASK_VOL_18     (0x1 << 9)
+#define PWR_MASK_VOL_33     (0x1 << 10)
+#define PWR_MASK_L4         (0x1 << 11)
+#define PWR_MSDC_DIR        (PWR_MASK_EN | PWR_MASK_VOL_18 | PWR_MASK_VOL_33 | PWR_MASK_L4)
+
+#define MSDC0_PWR_MASK_EN         (0x1 << 12)
+#define MSDC0_PWR_MASK_VOL_18     (0x1 << 13)
+#define MSDC0_PWR_MASK_VOL_33     (0x1 << 14)
+#define MSDC0_PWR_MASK_L4         (0x1 << 15)
+#define MSDC0_PWR_MSDC_DIR        (MSDC0_PWR_MASK_EN | MSDC0_PWR_MASK_VOL_18 | MSDC0_PWR_MASK_VOL_33 | MSDC0_PWR_MASK_L4)
+
+static void msdc_clr_gpio(u32 bits)
+{
+    u32 l_val = 0;
+
+    switch (bits) {
+        case MSDC0_PWR_MASK_EN:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (MSDC0_PWR_MASK_EN),0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (MSDC0_PWR_MASK_EN),0);
+            break;
+        case MSDC0_PWR_MASK_VOL_18:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 0);
+            break;
+        case MSDC0_PWR_MASK_VOL_33:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 0);
+            break;
+        case MSDC0_PWR_MASK_L4:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    MSDC0_PWR_MASK_L4, 0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, MSDC0_PWR_MASK_L4, 0);
+            break;
+        case PWR_MASK_EN:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    PWR_MASK_EN,0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, PWR_MASK_EN,0);
+            break;
+        case PWR_MASK_VOL_18:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 0);
+            break;
+        case PWR_MASK_VOL_33:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 0);
+            break;
+        case PWR_MASK_L4:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    PWR_MASK_L4, 0);
+            MSDC_SET_FIELD(PWR_GPIO_EO, PWR_MASK_L4, 0);
+            break;
+        default:
+            dprintf(CRITICAL, "[%s:%d]invalid value: 0x%x\n", __FILE__, __func__, bits);
+            break;
+    }
+}
+
+static void msdc_set_gpio(u32 bits)
+{
+    u32 l_val = 0;
+
+    switch (bits) {
+        case MSDC0_PWR_MASK_EN:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    MSDC0_PWR_MASK_EN,1);
+            MSDC_SET_FIELD(PWR_GPIO_EO, MSDC0_PWR_MASK_EN,1);
+            break;
+        case MSDC0_PWR_MASK_VOL_18:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 1);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 1);
+            break;
+        case MSDC0_PWR_MASK_VOL_33:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 2);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (MSDC0_PWR_MASK_VOL_18|MSDC0_PWR_MASK_VOL_33), 2);
+            break;
+        case MSDC0_PWR_MASK_L4:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    MSDC0_PWR_MASK_L4, 1);
+            MSDC_SET_FIELD(PWR_GPIO_EO, MSDC0_PWR_MASK_L4, 1);
+            break;
+        case PWR_MASK_EN:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    PWR_MASK_EN,1);
+            MSDC_SET_FIELD(PWR_GPIO_EO, PWR_MASK_EN,1);
+            break;
+        case PWR_MASK_VOL_18:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 1);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 1);
+            break;
+        case PWR_MASK_VOL_33:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 2);
+            MSDC_SET_FIELD(PWR_GPIO_EO, (PWR_MASK_VOL_18|PWR_MASK_VOL_33), 2);
+            break;
+        case PWR_MASK_L4:
+            /* check for set before */
+            MSDC_SET_FIELD(PWR_GPIO,    PWR_MASK_L4, 1);
+            MSDC_SET_FIELD(PWR_GPIO_EO, PWR_MASK_L4, 1);
+            break;
+        default:
+            dprintf(CRITICAL, "[%s:%s]invalid value: 0x%x\n", __FILE__, __func__, bits);
+            break;
+    }
+}
+
+static void msdc_set_card_pwr(struct mmc_host *host, int on)
+{
+    if (on) {
+        msdc_set_gpio(MSDC0_PWR_MASK_VOL_18);
+        msdc_set_gpio(MSDC0_PWR_MASK_L4);
+        msdc_set_gpio(MSDC0_PWR_MASK_EN);
+    } else {
+        msdc_clr_gpio(MSDC0_PWR_MASK_EN);
+        msdc_clr_gpio(MSDC0_PWR_MASK_VOL_18);
+        msdc_clr_gpio(MSDC0_PWR_MASK_L4);
+    }
+    spin(10000);
+}
+
+static void msdc_set_host_level_pwr(struct mmc_host *host, u32 level)
+{
+    msdc_clr_gpio(PWR_MASK_VOL_18);
+    msdc_clr_gpio(PWR_MASK_VOL_33);
+
+    if (level) {
+        msdc_set_gpio(PWR_MASK_VOL_18);
+    } else {
+        msdc_set_gpio(PWR_MASK_VOL_33);
+    }
+    msdc_set_gpio(PWR_MASK_L4);
+}
+
+static void msdc_set_host_pwr(struct mmc_host *host, int on)
+{
+    msdc_set_host_level_pwr(host, 0);
+}
+#endif
+
+static void msdc_set_startbit(struct mmc_host *host, u8 start_bit)
+{
+    u32 base = host->base;
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+
+    /* set start bit */
+    MSDC_SET_FIELD(MSDC_CFG, MSDC_CFG_START_BIT, start_bit);
+    priv->start_bit = start_bit;
+    dprintf(INFO, "start bit = %d, MSDC_CFG[0x%x]\n", start_bit, MSDC_READ32(MSDC_CFG));
+}
+
+#define TYPE_CMD_RESP_EDGE      (0)
+#define TYPE_WRITE_CRC_EDGE     (1)
+#define TYPE_READ_DATA_EDGE     (2)
+#define TYPE_WRITE_DATA_EDGE    (3)
+
+static void msdc_set_smpl(struct mmc_host *host, u8 HS400, u8 mode, u8 type)
+{
+    u32 base = host->base;
+    int i=0;
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+    static u8 read_data_edge[8] = {MSDC_SMPL_RISING, MSDC_SMPL_RISING, MSDC_SMPL_RISING, MSDC_SMPL_RISING,
+                                   MSDC_SMPL_RISING, MSDC_SMPL_RISING, MSDC_SMPL_RISING, MSDC_SMPL_RISING
+                                  };
+    static u8 write_data_edge[4] = {MSDC_SMPL_RISING, MSDC_SMPL_RISING, MSDC_SMPL_RISING, MSDC_SMPL_RISING};
+
+    switch (type) {
+        case TYPE_CMD_RESP_EDGE:
+            if (HS400) {
+                // eMMC5.0 only output resp at CLK pin, so no need to select DS pin
+                MSDC_SET_FIELD(EMMC50_CFG0, MSDC_EMMC50_CFG_PADCMD_LATCHCK, 0); //latch cmd resp at CLK pin
+                MSDC_SET_FIELD(EMMC50_CFG0, MSDC_EMMC50_CFG_CMD_RESP_SEL, 0);//latch cmd resp at CLK pin
+            }
+
+            if (mode == MSDC_SMPL_RISING || mode == MSDC_SMPL_FALLING) {
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_RSPL, mode);
+                priv->rsmpl = mode;
+            } else {
+                dprintf(CRITICAL, "[%s]: invalid resp parameter: HS400=%d, type=%d, mode=%d\n", __func__, HS400, type, mode);
+            }
+            break;
+
+        case TYPE_WRITE_CRC_EDGE:
+            if (HS400) {
+                MSDC_SET_FIELD(EMMC50_CFG0, MSDC_EMMC50_CFG_CRC_STS_SEL, 1);//latch write crc status at DS pin
+            } else {
+                MSDC_SET_FIELD(EMMC50_CFG0, MSDC_EMMC50_CFG_CRC_STS_SEL, 0);//latch write crc status at CLK pin
+            }
+
+            if (mode == MSDC_SMPL_RISING || mode == MSDC_SMPL_FALLING) {
+                if (HS400) {
+                    MSDC_SET_FIELD(EMMC50_CFG0, MSDC_EMMC50_CFG_CRC_STS_EDGE, mode);
+                } else {
+                    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL_SEL, 0);
+                    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL, mode);
+                }
+                priv->wdsmpl = mode;
+            } else if (mode == MSDC_SMPL_SEPERATE && !HS400) {
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D0SPL, write_data_edge[0]); //only dat0 is for write crc status.
+                priv->wdsmpl = mode;
+            } else {
+                dprintf(CRITICAL, "[%s]: invalid crc parameter: HS400=%d, type=%d, mode=%d\n", __func__, HS400, type, mode);
+            }
+            break;
+
+        case TYPE_READ_DATA_EDGE:
+            if (HS400) {
+                msdc_set_startbit(host, START_AT_RISING_AND_FALLING); //for HS400, start bit is output both on rising and falling edge
+                priv->start_bit = START_AT_RISING_AND_FALLING;
+            } else {
+                msdc_set_startbit(host, START_AT_RISING); //for the other mode, start bit is only output on rising edge. but DDR50 can try falling edge if error casued by pad delay
+                priv->start_bit = START_AT_RISING;
+            }
+            if (mode == MSDC_SMPL_RISING || mode == MSDC_SMPL_FALLING) {
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_R_D_SMPL_SEL, 0);
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_R_D_SMPL, mode);
+                priv->rdsmpl = mode;
+            } else if (mode == MSDC_SMPL_SEPERATE) {
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_R_D_SMPL_SEL, 1);
+                for (i=0; i<8; i++)
+                {
+                    MSDC_SET_FIELD(MSDC_IOCON, (MSDC_IOCON_R_D0SPL << i), read_data_edge[i]);
+                }
+                priv->rdsmpl = mode;
+            } else {
+                dprintf(CRITICAL, "[%s]: invalid read parameter: HS400=%d, type=%d, mode=%d\n", __func__, HS400, type, mode);
+            }
+            break;
+
+        case TYPE_WRITE_DATA_EDGE:
+            MSDC_SET_FIELD(EMMC50_CFG0, MSDC_EMMC50_CFG_CRC_STS_SEL, 0);//latch write crc status at CLK pin
+
+            if (mode == MSDC_SMPL_RISING|| mode == MSDC_SMPL_FALLING) {
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL_SEL, 0);
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL, mode);
+                priv->wdsmpl = mode;
+            } else if (mode == MSDC_SMPL_SEPERATE) {
+                MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL_SEL, 1);
+                for (i=0; i<4; i++)
+                {
+                    MSDC_SET_FIELD(MSDC_IOCON, (MSDC_IOCON_W_D0SPL << i), write_data_edge[i]);//dat0~4 is for SDIO card.
+                }
+                priv->wdsmpl = mode;
+            } else {
+                dprintf(CRITICAL, "[%s]: invalid write parameter: HS400=%d, type=%d, mode=%d\n", __func__, HS400, type, mode);
+            }
+            break;
+
+        default:
+            dprintf(CRITICAL, "[%s]: invalid parameter: HS400=%d, type=%d, mode=%d\n", __func__, HS400, type, mode);
+            break;
+    }
+}
+
+void msdc_set_timeout(struct mmc_host *host, u32 ns, u32 clks)
+{
+    u32 base = host->base;
+    u32 timeout, clk_ns;
+
+    if (host->sclk == 0) {
+        timeout = 0;
+    } else {
+        clk_ns  = 1000000000UL / host->sclk;
+        timeout = ns / clk_ns + clks;
+        timeout = timeout >> 20; /* in 1048576 sclk cycle unit (MT6589 MSDC IP)*/
+        timeout = timeout > 1 ? timeout - 1 : 0;
+        timeout = timeout > 255 ? 255 : timeout;
+
+        MSDC_SET_FIELD(SDC_CFG, SDC_CFG_DTOC, timeout);
+    }
+    dprintf(INFO, "[MSDC] Set read data timeout: %dns %dclks -> %d (65536 sclk cycles)\n",
+            ns, clks, timeout + 1);
+}
+
+static void msdc_abort(struct mmc_host *host)
+{
+    u32 base = host->base;
+
+    dprintf(CRITICAL, "[MSDC] Abort: MSDC_FIFOCS=%xh MSDC_PS=%xh SDC_STS=%xh\n",
+            MSDC_READ32(MSDC_FIFOCS), MSDC_READ32(MSDC_PS), MSDC_READ32(SDC_STS));
+    /* reset controller */
+    MSDC_RESET();
+
+    /* clear fifo */
+    MSDC_CLR_FIFO();
+
+    /* make sure txfifo and rxfifo are empty */
+    if (MSDC_TXFIFOCNT() != 0 || MSDC_RXFIFOCNT() != 0) {
+        dprintf(INFO, "[MSDC] Abort: TXFIFO(%d), RXFIFO(%d) != 0\n",
+                MSDC_TXFIFOCNT(), MSDC_RXFIFOCNT());
+    }
+
+    /* clear all interrupts */
+    MSDC_CLR_INT();
+}
+
+static int msdc_get_card_status(struct mmc_host *host, u32 *status)
+{
+    int err;
+    struct mmc_command cmd;
+
+    cmd.opcode  = MMC_CMD_SEND_STATUS;
+    cmd.arg     = host->card->rca << 16;
+    cmd.rsptyp  = RESP_R1;
+    cmd.retries = CMD_RETRIES;
+    cmd.timeout = CMD_TIMEOUT;
+
+    err = msdc_send_cmd(host, &cmd);
+    if (!err)
+        err = msdc_wait_cmd_done(host, &cmd);
+
+    if (err == MMC_ERR_NONE)
+        *status = cmd.resp[0];
+
+    return err;
+}
+
+int msdc_abort_handler(struct mmc_host *host, int abort_card)
+{
+    struct mmc_command stop;
+    u32 status = 0;
+    u32 state = 0;
+
+    while (state != 4) { // until status to "tran"
+        msdc_abort(host);
+        if (msdc_get_card_status(host, &status)) {
+            dprintf(CRITICAL, "Get card status failed\n");
+            return 1;
+        }
+        state = R1_CURRENT_STATE(status);
+        dprintf(INFO, "check card state<%d>\n", state);
+        if (state == 5 || state == 6) {
+            dprintf(INFO, "state<%d> need cmd12 to stop\n", state);
+            if (abort_card) {
+                stop.opcode  = MMC_CMD_STOP_TRANSMISSION;
+                stop.rsptyp  = RESP_R1B;
+                stop.arg     = 0;
+                stop.retries = CMD_RETRIES;
+                stop.timeout = CMD_TIMEOUT;
+                msdc_send_cmd(host, &stop);
+                msdc_wait_cmd_done(host, &stop); // don't tuning
+            } else if (state == 7) {  // busy in programing
+                dprintf(INFO, "state<%d> card is busy\n", state);
+                spin(100000);
+            } else if (state != 4) {
+                dprintf(INFO, "state<%d> ??? \n", state);
+                return 1;
+            }
+        }
+    }
+    msdc_abort(host);
+    return 0;
+}
+
+static u32 msdc_intr_wait(struct mmc_host *host, u32 intrs)
+{
+    u32 sts = 0;
+    u32 tmo = UINT_MAX;
+    int ret = 0;
+
+    /* warning that interrupts are not enabled */
+    ret = event_wait_timeout(&msdc_int_event, tmo);
+    if (ret != 0) {
+        u32 base = host->base;
+        dprintf(CRITICAL, "[%s]: failed to get event INT=0x%x\n",
+                __func__, MSDC_READ32(MSDC_INT));
+        g_int_status = 0;
+        return 0;
+    }
+
+    sts = g_int_status;
+    g_int_status = 0;
+
+    if (~intrs & sts)
+        dprintf(CRITICAL, "[MSDC]<CHECKME> Unexpected INT(0x%x)\n", ~intrs & sts);
+
+    return sts;
+}
+
+static enum handler_return msdc_interrupt_handler(void *arg)
+{
+    struct mmc_host *host = arg;
+    u32 base = host->base;
+
+    /* Save & Clear the interrupt */
+    g_int_status = MSDC_READ32(MSDC_INT);
+    MSDC_WRITE32(MSDC_INT, g_int_status & host->intr_mask);
+    MSDC_WRITE32(MSDC_INTEN, 0);
+    host->intr_mask = 0;
+
+    /* MUST BE *false*! otherwise, schedule in interrupt */
+    event_signal(&msdc_int_event, false);
+
+    return INT_RESCHEDULE;
+}
+
+static int msdc_send_cmd(struct mmc_host *host, struct mmc_command *cmd)
+{
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+    u32 base   = host->base;
+    u32 opcode = cmd->opcode;
+    u32 rsptyp = cmd->rsptyp;
+    u32 rawcmd;
+    u32 error = MMC_ERR_NONE;
+
+    /* rawcmd :
+     * vol_swt << 30 | auto_cmd << 28 | blklen << 16 | go_irq << 15 |
+     * stop << 14 | rw << 13 | dtype << 11 | rsptyp << 7 | brk << 6 | opcode
+     */
+    rawcmd = (opcode & ~(SD_CMD_BIT | SD_CMD_APP_BIT)) |
+             msdc_rsp[rsptyp] << 7 | host->blklen << 16;
+
+    if (opcode == MMC_CMD_WRITE_MULTIPLE_BLOCK) {
+        rawcmd |= ((2 << 11) | (1 << 13));
+        if (priv->autocmd & MSDC_AUTOCMD12) {
+            rawcmd |= (1 << 28);
+        } else if (priv->autocmd & MSDC_AUTOCMD23) {
+            rawcmd |= (2 << 28);
+        }
+    } else if (opcode == MMC_CMD_WRITE_BLOCK || opcode == MMC_CMD50) {
+        rawcmd |= ((1 << 11) | (1 << 13));
+    } else if (opcode == MMC_CMD_READ_MULTIPLE_BLOCK) {
+        rawcmd |= (2 << 11);
+        if (priv->autocmd & MSDC_AUTOCMD12) {
+            rawcmd |= (1 << 28);
+        } else if (priv->autocmd & MSDC_AUTOCMD23) {
+            rawcmd |= (2 << 28);
+        }
+    } else if (opcode == MMC_CMD_READ_SINGLE_BLOCK ||
+               opcode == SD_ACMD_SEND_SCR ||
+               opcode == SD_CMD_SWITCH ||
+               opcode == MMC_CMD_SEND_EXT_CSD ||
+               opcode == MMC_CMD_SEND_WRITE_PROT ||
+               opcode == MMC_CMD_SEND_WRITE_PROT_TYPE ||
+               opcode == MMC_CMD21) {
+        rawcmd |= (1 << 11);
+    } else if (opcode == MMC_CMD_STOP_TRANSMISSION) {
+        rawcmd |= (1 << 14);
+        rawcmd &= ~(0x0FFF << 16);
+    } else if (opcode == SD_IO_RW_EXTENDED) {
+        if (cmd->arg & 0x80000000)  /* R/W flag */
+            rawcmd |= (1 << 13);
+        if ((cmd->arg & 0x08000000) && ((cmd->arg & 0x1FF) > 1))
+            rawcmd |= (2 << 11); /* multiple block mode */
+        else
+            rawcmd |= (1 << 11);
+    } else if (opcode == SD_IO_RW_DIRECT) {
+        if ((cmd->arg & 0x80000000) && ((cmd->arg >> 9) & 0x1FFFF))/* I/O abt */
+            rawcmd |= (1 << 14);
+    } else if (opcode == SD_CMD_VOL_SWITCH) {
+        rawcmd |= (1 << 30);
+    } else if (opcode == SD_CMD_SEND_TUNING_BLOCK) {
+        rawcmd |= (1 << 11); /* CHECKME */
+        if (priv->autocmd & MSDC_AUTOCMD19)
+            rawcmd |= (3 << 28);
+    } else if (opcode == MMC_CMD_GO_IRQ_STATE) {
+        rawcmd |= (1 << 15);
+    } else if (opcode == MMC_CMD_WRITE_DAT_UNTIL_STOP) {
+        rawcmd |= ((1<< 13) | (3 << 11));
+    } else if (opcode == MMC_CMD_READ_DAT_UNTIL_STOP) {
+        rawcmd |= (3 << 11);
+    }
+
+    dprintf(INFO, "+[MSDC] CMD(%d): ARG(0x%x), RAW(0x%x), BLK_NUM(0x%x) RSP(%d)\n",
+            (opcode & ~(SD_CMD_BIT | SD_CMD_APP_BIT)), cmd->arg, rawcmd,
+            MSDC_READ32(SDC_BLK_NUM), rsptyp);
+
+    while (SDC_IS_CMD_BUSY());
+    if ((rsptyp == RESP_R1B) || (opcode == MMC_CMD_WRITE_MULTIPLE_BLOCK) ||
+            opcode == MMC_CMD_WRITE_BLOCK || opcode == MMC_CMD_READ_MULTIPLE_BLOCK ||
+            opcode == MMC_CMD_READ_SINGLE_BLOCK)
+        while (SDC_IS_BUSY());
+
+    SDC_SEND_CMD(rawcmd, cmd->arg);
+
+end:
+    cmd->error = error;
+
+    return error;
+}
+
+static int msdc_wait_cmd_done(struct mmc_host *host, struct mmc_command *cmd)
+{
+    u32 base   = host->base;
+    u32 rsptyp = cmd->rsptyp;
+    u32 status;
+    u32 opcode = (cmd->opcode & ~(SD_CMD_BIT | SD_CMD_APP_BIT));
+    u32 error = MMC_ERR_NONE;
+    u32 wints = MSDC_INT_CMDTMO | MSDC_INT_CMDRDY | MSDC_INT_RSPCRCERR |
+                MSDC_INT_ACMDRDY | MSDC_INT_ACMDCRCERR | MSDC_INT_ACMDTMO;
+    u32 *resp = &cmd->resp[0];
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+
+    while (1) {
+        /* Wait for interrupt coming */
+        while (((status = MSDC_READ32(MSDC_INT)) & wints) == 0);
+        MSDC_WRITE32(MSDC_INT, (status & wints));
+        if (~wints & status)
+            dprintf(CRITICAL, "[MSDC]<CHECKME> Unexpected INT(0x%x)\n",
+                    ~wints & status);
+
+        if (status & MSDC_INT_CMDRDY)
+            break;
+        else if (status & MSDC_INT_RSPCRCERR) {
+            dprintf(CRITICAL, "[MSDC] cmd%d CRCERR! (0x%x)\n", opcode, status);
+            error = MMC_ERR_BADCRC;
+            goto err;
+        } else if (status & MSDC_INT_CMDTMO) {
+            dprintf(CRITICAL, "[MSDC] cmd%d TMO! (0x%x)\n", opcode, status);
+            error = MMC_ERR_TIMEOUT;
+            goto err;
+        } else if (priv->autocmd & MSDC_AUTOCMD23) {
+            if (status & MSDC_INT_ACMDRDY)
+                /* Autocmd rdy is set prior to cmd rdy */
+                continue;
+            else if (status & MSDC_INT_ACMDCRCERR) {
+                dprintf(CRITICAL, "[MSDC] autocmd23 CRCERR! (0x%x)\n", status);
+                error = MMC_ERR_ACMD_RSPCRC;
+                goto err;
+            } else if (status & MSDC_INT_ACMDTMO) {
+                dprintf(CRITICAL, "[MSDC] autocmd23 TMO! (0x%x)\n", status);
+                error = MMC_ERR_ACMD_TIMEOUT;
+                goto err;
+            }
+        } else {
+            dprintf(CRITICAL, "[MSDC] cmd%d UNEXPECT status! (0x%x)\n",
+                    opcode, status);
+            error = MMC_ERR_UNEXPECT;
+            goto err;
+        }
+    }
+
+    switch (rsptyp) {
+        case RESP_NONE:
+            dprintf(INFO, "-[MSDC] CMD(%d): RSP(%d)\n",
+                    opcode, rsptyp);
+            break;
+        case RESP_R2:
+            *resp++ = MSDC_READ32(SDC_RESP3);
+            *resp++ = MSDC_READ32(SDC_RESP2);
+            *resp++ = MSDC_READ32(SDC_RESP1);
+            *resp++ = MSDC_READ32(SDC_RESP0);
+            dprintf(INFO, "-[MSDC] CMD(%d): RSP(%d) = 0x%x 0x%x 0x%x 0x%x\n",
+                    opcode, cmd->rsptyp, cmd->resp[0], cmd->resp[1],
+                    cmd->resp[2], cmd->resp[3]);
+            break;
+        default: /* Response types 1, 3, 4, 5, 6, 7(1b) */
+            cmd->resp[0] = MSDC_READ32(SDC_RESP0);
+            dprintf(INFO, "-[MSDC] CMD(%d): RSP(%d) = 0x%x\n",
+                    opcode, cmd->rsptyp, cmd->resp[0]);
+            break;
+    }
+
+err:
+    if (rsptyp == RESP_R1B)
+        while ((MSDC_READ32(MSDC_PS) & MSDC_PS_DAT0) != MSDC_PS_DAT0);
+
+    cmd->error = error;
+
+    return error;
+}
+
+int msdc_cmd(struct mmc_host *host, struct mmc_command *cmd)
+{
+    int err;
+
+    err = msdc_send_cmd(host, cmd);
+    if (err != MMC_ERR_NONE)
+        return err;
+
+    err = msdc_wait_cmd_done(host, cmd);
+
+    if (err) {
+        u32 base = host->base;
+        u32 tmp = MSDC_READ32(SDC_CMD);
+
+        /* check if data is used by the command or not */
+        if (tmp & SDC_CMD_DTYP) {
+            if (msdc_abort_handler(host, 1)) {
+                dprintf(CRITICAL, "[MSDC] abort failed\n");
+            }
+        }
+
+        err = msdc_tune_cmdrsp(host, cmd);
+    }
+
+    return err;
+}
+
+#ifdef MSDC_USE_DMA_MODE
+static void msdc_flush_membuf(void *buf, u32 len)
+{
+    arch_clean_invalidate_cache_range((addr_t)buf,len);
+}
+
+static int msdc_dma_wait_done(struct mmc_host *host, struct mmc_command *cmd)
+{
+    u32 base = host->base;
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+    u32 status;
+    u32 error = MMC_ERR_NONE;
+    u32 wints = MSDC_INT_XFER_COMPL | MSDC_INT_DATTMO | MSDC_INT_DATCRCERR |
+                MSDC_INT_DXFER_DONE | MSDC_INT_DMAQ_EMPTY |
+                MSDC_INT_ACMDRDY | MSDC_INT_ACMDTMO | MSDC_INT_ACMDCRCERR |
+                MSDC_INT_CMDRDY | MSDC_INT_CMDTMO | MSDC_INT_RSPCRCERR;
+
+    /* Deliver it to irq handler */
+    host->intr_mask = wints;
+
+    do {
+        status = msdc_intr_wait(host, wints);
+
+        if (status & MSDC_INT_XFER_COMPL) {
+            if (mmc_op_multi(cmd->opcode) && (priv->autocmd & MSDC_AUTOCMD12)) {
+                /* acmd rdy should be checked after xfer_done been held */
+                if (status & MSDC_INT_ACMDRDY) {
+                    break;
+                } else if (status & MSDC_INT_ACMDTMO) {
+                    dprintf(CRITICAL, "[MSDC] ACMD12 timeout(%xh)\n", status);
+                    error = MMC_ERR_ACMD_TIMEOUT;
+                    goto end;
+                } else if (status & MSDC_INT_ACMDCRCERR) {
+                    dprintf(CRITICAL, "[MSDC] ACMD12 CRC error(%xh)\n", status);
+                    error = MMC_ERR_ACMD_RSPCRC;
+                    goto end;
+                }
+            } else
+                break;
+        }
+
+        if (status == 0 || status & MSDC_INT_DATTMO) {
+            dprintf(CRITICAL, "[MSDC] DMA DAT timeout(%xh)\n", status);
+            error = MMC_ERR_TIMEOUT;
+            goto end;
+        } else if (status & MSDC_INT_DATCRCERR) {
+            dprintf(CRITICAL, "[MSDC] DMA DAT CRC error(%xh)\n", status);
+            error = MMC_ERR_BADCRC;
+            goto end;
+        } else {
+            dprintf(CRITICAL, "[MSDC] Unexpect status(0x%x)\n", status);
+            error = MMC_ERR_UNEXPECT;
+            goto end;
+        }
+    } while (1);
+
+end:
+    if (error)
+        MSDC_RESET();
+
+    return error;
+}
+
+int msdc_dma_transfer(struct mmc_host *host, struct mmc_data *data)
+{
+    u32 base = host->base;
+    int err;
+    paddr_t pa;
+
+    /* Set dma timeout */
+    msdc_set_timeout(host, data->timeout * 1000000, 0);
+    /* DRAM address */
+    pa = kvaddr_to_paddr(data->buf);
+    if (sizeof(pa) > 4)
+        dprintf(CRITICAL, "[MSDC] WARN: 64bit physical address!\n");
+    MSDC_WRITE32(MSDC_DMA_SA, (u32)pa);
+    MSDC_SET_FIELD(MSDC_DMA_CTRL, MSDC_DMA_CTRL_BURSTSZ, MSDC_DMA_BURST_64B);
+    /* BASIC_DMA mode */
+    MSDC_SET_FIELD(MSDC_DMA_CTRL, MSDC_DMA_CTRL_MODE, 0);
+    /* This is the last buffer */
+    MSDC_SET_FIELD(MSDC_DMA_CTRL, MSDC_DMA_CTRL_LASTBUF, 1);
+    /* Total transfer size */
+    MSDC_WRITE32(MSDC_DMA_LEN, data->blks * host->blklen);
+    /* Set interrupts bit */
+    MSDC_SET_BIT32(MSDC_INTEN,
+                   MSDC_INT_XFER_COMPL | MSDC_INT_DATTMO | MSDC_INT_DATCRCERR);
+    /* Clean & Invalidate cache */
+    msdc_flush_membuf(data->buf, data->blks * host->blklen);
+    /* Trigger DMA start */
+    MSDC_SET_FIELD(MSDC_DMA_CTRL, MSDC_DMA_CTRL_START, 1);
+    /* wait DMA transferring done */
+    err = msdc_dma_wait_done(host, data->cmd);
+    if (err) {
+        dprintf(CRITICAL, "[MSDC] DMA failed! err(%d)\n", err);
+        if (msdc_abort_handler(host, 1)) {
+            dprintf(CRITICAL, "[MSDC] eMMC cannot back to TRANS mode!\n");
+            return MMC_ERR_FAILED;
+        }
+    }
+
+    /* Check DMA status and stop DMA transfer */
+    MSDC_SET_FIELD(MSDC_DMA_CTRL, MSDC_DMA_CTRL_STOP, 1);
+    while (MSDC_READ32(MSDC_DMA_CFG) & MSDC_DMA_CFG_STS);
+
+    return err;
+}
+
+static int msdc_dma_rw(struct mmc_host *host, u8 *buf, u32 blkaddr, u32 nblks, bool rd)
+{
+    int multi, err;
+    struct mmc_command cmd;
+    struct mmc_data data;
+    u32 base = host->base;
+
+    ASSERT(nblks <= host->max_phys_segs);
+
+    dprintf(INFO, "[MSDC] %s data %d blks %s 0x%x\n",
+            rd ? "Read" : "Write", nblks, rd ? "from" : "to", blkaddr);
+
+    multi = nblks > 1 ? 1 : 0;
+    /* DMA and block number _MUST_BE_ set prior to issuing command */
+    MSDC_DMA_ON;
+    MSDC_WRITE32(SDC_BLK_NUM, nblks);
+
+    /* send read command */
+    if (rd)
+        cmd.opcode =
+            multi ? MMC_CMD_READ_MULTIPLE_BLOCK : MMC_CMD_READ_SINGLE_BLOCK;
+    else
+        cmd.opcode = multi ? MMC_CMD_WRITE_MULTIPLE_BLOCK : MMC_CMD_WRITE_BLOCK;
+    cmd.arg = blkaddr;
+    cmd.rsptyp  = RESP_R1;
+    cmd.retries = 0;
+    cmd.timeout = CMD_TIMEOUT;
+
+    err = msdc_cmd(host, &cmd);
+    if (err != MMC_ERR_NONE)
+        return err;
+
+    data.cmd = &cmd;
+    data.blks = nblks;
+    data.buf = buf;
+    if (rd)
+        data.timeout = 100;
+    else
+        data.timeout = 250;
+
+    err = msdc_dma_transfer(host, &data);
+    MSDC_DMA_OFF;
+
+    return err;
+}
+
+static int msdc_dma_bread(struct mmc_host *host, u8 *dst, u32 src, u32 nblks)
+{
+    return msdc_dma_rw(host, dst, src, nblks, true);
+}
+
+static int msdc_dma_bwrite(struct mmc_host *host, u32 dst, u8 *src, u32 nblks)
+{
+    return msdc_dma_rw(host, src, dst, nblks, false);
+}
+#else
+static int msdc_pio_read_word(struct mmc_host *host, u32 *ptr, u32 size)
+{
+    int err = MMC_ERR_NONE;
+    u32 base = host->base;
+    u32 ints = MSDC_INT_DATCRCERR | MSDC_INT_DATTMO | MSDC_INT_XFER_COMPL;
+    //u32 timeout = 100000;
+    u32 status;
+    u32 totalsz = size;
+    u8  done = 0;
+    u8 *u8ptr;
+    u32 dcrc=0;
+
+    while (1) {
+        status = MSDC_READ32(MSDC_INT);
+        MSDC_WRITE32(MSDC_INT, status);
+        if (status & ~ints)
+            dprintf(CRITICAL, "[MSDC]<CHECKME> Unexpected INT(0x%x)\n", status);
+        if (status & MSDC_INT_DATCRCERR) {
+            MSDC_GET_FIELD(SDC_DCRC_STS, SDC_DCRC_STS_POS|SDC_DCRC_STS_NEG, dcrc);
+            dprintf(CRITICAL, "[MSDC] DAT CRC error (0x%x), Left:%d/%d bytes, RXFIFO:%d,dcrc:0x%x\n",
+                    status, size, totalsz, MSDC_RXFIFOCNT(),dcrc);
+            err = MMC_ERR_BADCRC;
+            break;
+        } else if (status & MSDC_INT_DATTMO) {
+            dprintf(CRITICAL, "[MSDC] DAT TMO error (0x%x), Left: %d/%d bytes, RXFIFO:%d\n",
+                    status, size, totalsz, MSDC_RXFIFOCNT());
+            err = MMC_ERR_TIMEOUT;
+            break;
+        } else if (status & MSDC_INT_ACMDCRCERR) {
+            MSDC_GET_FIELD(SDC_DCRC_STS, SDC_DCRC_STS_POS|SDC_DCRC_STS_NEG, dcrc);
+            dprintf(CRITICAL, "[MSDC] AUTOCMD CRC error (0x%x), Left:%d/%d bytes, RXFIFO:%d,dcrc:0x%x\n",
+                    status, size, totalsz, MSDC_RXFIFOCNT(),dcrc);
+            err = MMC_ERR_ACMD_RSPCRC;
+            break;
+        } else if (status & MSDC_INT_XFER_COMPL) {
+            done = 1;
+        }
+
+        if (size == 0 && done)
+            break;
+
+        /* Note. RXFIFO count would be aligned to 4-bytes alignment size */
+        if ((size >=  MSDC_FIFO_THD) && (MSDC_RXFIFOCNT() >= MSDC_FIFO_THD)) {
+            int left = MSDC_FIFO_THD >> 2;
+            do {
+                *ptr++ = MSDC_FIFO_READ32();
+            } while (--left);
+            size -= MSDC_FIFO_THD;
+            dprintf(INFO, "[MSDC] Read %d bytes, RXFIFOCNT: %d,  Left: %d/%d\n",
+                    MSDC_FIFO_THD, MSDC_RXFIFOCNT(), size, totalsz);
+        } else if ((size < MSDC_FIFO_THD) && MSDC_RXFIFOCNT() >= size) {
+            while (size) {
+                if (size > 3) {
+                    *ptr++ = MSDC_FIFO_READ32();
+                } else {
+                    u8ptr = (u8 *)ptr;
+                    while (size --)
+                        *u8ptr++ = MSDC_FIFO_READ8();
+                }
+            }
+            dprintf(INFO, "[MSDC] Read left bytes, RXFIFOCNT: %d, Left: %d/%d\n",
+                    MSDC_RXFIFOCNT(), size, totalsz);
+        }
+    }
+
+    return err;
+}
+
+static int msdc_pio_read(struct mmc_host *host, u32 *ptr, u32 size)
+{
+    int err = msdc_pio_read_word(host, (u32 *)ptr, size);
+
+    if (err != MMC_ERR_NONE) {
+        msdc_abort(host); /* reset internal fifo and state machine */
+        dprintf(CRITICAL, "[MSDC] %d-bit PIO Read Error (%d)\n", 32, err);
+    }
+
+    return err;
+}
+
+static int msdc_pio_write_word(struct mmc_host *host, u32 *ptr, u32 size)
+{
+    int err = MMC_ERR_NONE;
+    u32 base = host->base;
+    u32 ints = MSDC_INT_DATCRCERR | MSDC_INT_DATTMO | MSDC_INT_XFER_COMPL;
+    //u32 timeout = 250000;
+    u32 status;
+    u8 *u8ptr;
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+
+    while (1) {
+        status = MSDC_READ32(MSDC_INT);
+        MSDC_WRITE32(MSDC_INT, status);
+        if (status & ~ints) {
+            dprintf(CRITICAL, "[MSDC]<CHECKME> Unexpected INT(0x%x)\n", status);
+        }
+        if (status & MSDC_INT_DATCRCERR) {
+            dprintf(CRITICAL, "[MSDC] DAT CRC error (0x%x), Left DAT: %d bytes\n",
+                    status, size);
+            err = MMC_ERR_BADCRC;
+            break;
+        } else if (status & MSDC_INT_DATTMO) {
+            dprintf(CRITICAL, "[MSDC] DAT TMO error (0x%x), Left DAT: %d bytes, MSDC_FIFOCS=%xh\n",
+                    status, size, MSDC_READ32(MSDC_FIFOCS));
+            err = MMC_ERR_TIMEOUT;
+            break;
+        } else if (status & MSDC_INT_ACMDCRCERR) {
+            dprintf(CRITICAL, "[MSDC] AUTO CMD CRC error (0x%x), Left DAT: %d bytes\n",
+                    status, size);
+            err = MMC_ERR_ACMD_RSPCRC;
+            break;
+        } else if (status & MSDC_INT_XFER_COMPL) {
+            if (size == 0) {
+                dprintf(INFO, "[MSDC] all data flushed to card\n");
+                break;
+            } else
+                dprintf(INFO, "[MSDC]<CHECKME> XFER_COMPL before all data written\n");
+        }
+
+        if (size == 0)
+            continue;
+
+        if (size >= MSDC_FIFO_SZ) {
+            if (MSDC_TXFIFOCNT() == 0) {
+                int left = MSDC_FIFO_SZ >> 2;
+                do {
+                    MSDC_FIFO_WRITE32(*ptr);
+                    ptr++;
+                } while (--left);
+                size -= MSDC_FIFO_SZ;
+            }
+        } else if (size < MSDC_FIFO_SZ && MSDC_TXFIFOCNT() == 0) {
+            while (size ) {
+                if (size > 3) {
+                    MSDC_FIFO_WRITE32(*ptr);
+                    ptr++;
+                    size -= 4;
+                } else {
+                    u8ptr = (u8 *)ptr;
+                    while (size --) {
+                        MSDC_FIFO_WRITE8(*u8ptr);
+                        u8ptr++;
+                    }
+                }
+            }
+        }
+    }
+
+    return err;
+}
+
+static int msdc_pio_write(struct mmc_host *host, u32 *ptr, u32 size)
+{
+    int err = msdc_pio_write_word(host, (u32 *)ptr, size);
+
+    if (err != MMC_ERR_NONE) {
+        msdc_abort(host); /* reset internal fifo and state machine */
+        dprintf(CRITICAL, "[MSDC] PIO Write Error (%d)\n", err);
+    }
+
+    return err;
+}
+
+static int msdc_pio_bread(struct mmc_host *host, u8 *dst, u32 src, u32 nblks)
+{
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+    u32 base = host->base;
+    u32 blksz = host->blklen;
+    int err = MMC_ERR_NONE, derr = MMC_ERR_NONE;
+    int multi;
+    struct mmc_command cmd;
+    struct mmc_command stop;
+    u32 *ptr = (u32 *)dst;
+
+    dprintf(INFO, "[MSDC] Read data %d bytes from 0x%x\n", nblks * blksz, src);
+
+    multi = nblks > 1 ? 1 : 0;
+
+    MSDC_CLR_FIFO();
+    MSDC_WRITE32(SDC_BLK_NUM, nblks);
+    msdc_set_timeout(host, 100000000, 0);
+
+    /* send read command */
+    cmd.opcode  = multi ? MMC_CMD_READ_MULTIPLE_BLOCK : MMC_CMD_READ_SINGLE_BLOCK;
+    cmd.rsptyp  = RESP_R1;
+    cmd.arg     = src;
+    cmd.retries = 0;
+    cmd.timeout = CMD_TIMEOUT;
+    err = msdc_cmd(host, &cmd);
+
+    if (err != MMC_ERR_NONE)
+        goto done;
+
+    err = derr = msdc_pio_read(host, (u32 *)ptr, nblks * blksz);
+
+done:
+    if (err != MMC_ERR_NONE) {
+        if (derr != MMC_ERR_NONE) {
+            dprintf(CRITICAL, "[MSDC] Read data error (%d)\n", derr);
+            if (msdc_abort_handler(host, 1))
+                dprintf(CRITICAL, "[MSDC] abort failed\n");
+        } else {
+            dprintf(CRITICAL, "[MSDC] Read error (%d)\n", err);
+        }
+    }
+    return (derr == MMC_ERR_NONE) ? err : derr;
+}
+
+static int msdc_pio_bwrite(struct mmc_host *host, u32 dst, u8 *src, u32 nblks)
+{
+    msdc_priv_t *priv = (msdc_priv_t *)host->priv;
+    u32 base = host->base;
+    int err = MMC_ERR_NONE, derr = MMC_ERR_NONE;
+    int multi;
+    u32 blksz = host->blklen;
+    struct mmc_command cmd;
+    struct mmc_command stop;
+    u32 *ptr = (u32 *)src;
+
+    dprintf(CRITICAL, "[MSDC] Write data %d bytes to 0x%x\n", nblks * blksz, dst);
+
+    multi = nblks > 1 ? 1 : 0;
+
+    MSDC_CLR_FIFO();
+    MSDC_WRITE32(SDC_BLK_NUM, nblks);
+
+    /* No need since MSDC always waits 8 cycles for write data timeout */
+
+    /* send write command */
+    cmd.opcode  = multi ? MMC_CMD_WRITE_MULTIPLE_BLOCK : MMC_CMD_WRITE_BLOCK;
+    cmd.rsptyp  = RESP_R1;
+    cmd.arg     = dst;
+    cmd.retries = 0;
+    cmd.timeout = CMD_TIMEOUT;
+    err = msdc_cmd(host, &cmd);
+
+    if (err != MMC_ERR_NONE)
+        goto done;
+
+    err = derr = msdc_pio_write(host, (u32 *)ptr, nblks * blksz);
+
+done:
+    if (err != MMC_ERR_NONE) {
+        if (derr != MMC_ERR_NONE) {
+            dprintf(CRITICAL, "[MSDC] Write data error (%d)\n", derr);
+            if (msdc_abort_handler(host, 1))
+                dprintf(CRITICAL, "[MSDC] abort failed\n");
+        } else {
+            dprintf(CRITICAL, "[MSDC] Write error (%d)\n", err);
+        }
+    }
+    return (derr == MMC_ERR_NONE) ? err : derr;
+}
+#endif
+
+static void msdc_config_clksrc(struct mmc_host *host, u32 clksrc, u32 hclksrc)
+{
+    // modify the clock
+    ASSERT(host);
+    /* Perloader and LK use 200 is ok, no need change source */
+    host->clksrc  = clksrc;
+    host->hclksrc = hclksrc;
+    host->clk     = hclks_msdc30[clksrc];
+
+#ifndef FPGA_PLATFORM
+    MSDC_SET_FIELD(TOPCKGEN_CTRL(CLKCFG2), MSDC_CKGEN_SEL(MSDC0,CLKCFG2), clksrc);
+#endif
+    dprintf(INFO, "[info][%s] pll_clk %u (%uMHz), pll_hclk %u\n",
+            __func__, host->clksrc, host->clk/1000000, host->hclksrc);
+}
+
+void msdc_config_clock(struct mmc_host *host, int state, u32 hz)
+{
+    msdc_priv_t *priv = host->priv;
+    u32 base = host->base;
+    u32 mode, hs400_src = 0;
+    u32 div;
+    u32 sclk;
+    u32 u4buswidth=0;
+
+    if (hz >= host->f_max) {
+        hz = host->f_max;
+    } else if (hz < host->f_min) {
+        hz = host->f_min;
+    }
+
+    msdc_config_clksrc(host, host->clksrc, host->hclksrc);
+
+    if (state&MMC_STATE_DDR) {
+        mode = 0x2; /* ddr mode and use divisor */
+        if (hz >= (host->clk >> 2)) {
+            div  = 0;              /* mean div = 1/2 */
+            sclk = host->clk >> 2; /* sclk = clk/div/2. 2: internal divisor */
+        } else {
+            div  = (host->clk + ((hz << 2) - 1)) / (hz << 2);
+            sclk = (host->clk >> 2) / div;
+            div  = (div >> 1);     /* since there is 1/2 internal divisor */
+        }
+    } else if (hz >= host->clk) {
+        mode = 0x1; /* no divisor and divisor is ignored */
+        div  = 0;
+        sclk = host->clk;
+    } else {
+        mode = 0x0; /* use divisor */
+        if (hz >= (host->clk >> 1)) {
+            div  = 0;              /* mean div = 1/2 */
+            sclk = host->clk >> 1; /* sclk = clk / 2 */
+        } else {
+            div  = (host->clk + ((hz << 2) - 1)) / (hz << 2);
+            sclk = (host->clk >> 2) / div;
+        }
+    }
+    host->sclk = sclk;
+
+    /* set clock mode and divisor */
+    MSDC_SET_FIELD(MSDC_CFG, (MSDC_CFG_CKMOD_HS400 | MSDC_CFG_CKMOD |MSDC_CFG_CKDIV),\
+                   (hs400_src << 14) | (mode << 12) | div);
+    /* wait clock stable */
+    while (!(MSDC_READ32(MSDC_CFG) & MSDC_CFG_CKSTB));
+
+    if (host->card && mmc_card_hs400(host->card)) {
+        msdc_set_smpl(host, 1, priv->rsmpl, TYPE_CMD_RESP_EDGE);
+        msdc_set_smpl(host, 1, priv->rdsmpl, TYPE_READ_DATA_EDGE);
+        msdc_set_smpl(host, 1, priv->wdsmpl, TYPE_WRITE_CRC_EDGE);
+        MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGCRCSTS,0);
+        MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CRCSTSENSEL,1);
+    } else {
+        msdc_set_smpl(host, 0, priv->rsmpl, TYPE_CMD_RESP_EDGE);
+        msdc_set_smpl(host, 0, priv->rdsmpl, TYPE_READ_DATA_EDGE);
+        msdc_set_smpl(host, 0, priv->wdsmpl, TYPE_WRITE_CRC_EDGE);
+#ifdef MSDC_USE_PATCH_BIT2_TURNING_WITH_ASYNC
+        MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGCRCSTS,1);
+#else
+        MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGCRCSTS,0);
+#endif
+        MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CRCSTSENSEL,0);
+    }
+    MSDC_GET_FIELD(SDC_CFG,SDC_CFG_BUSWIDTH,u4buswidth);
+
+    dprintf(INFO,
+            "[MSDC] SET_CLK(%dkHz): SCLK(%dkHz) MODE(%d) DIV(%d) DS(%d) RS(%d) buswidth(%s)\n",
+            hz/1000, sclk/1000, mode, div, msdc_cap.data_edge,
+            msdc_cap.cmd_edge,
+            (u4buswidth == 0) ?
+            "1-bit" : (u4buswidth == 1) ?
+            "4-bits" : (u4buswidth == 2) ?
+            "8-bits" : "undefined");
+}
+
+void msdc_config_bus(struct mmc_host *host, u32 width)
+{
+    u32 val,mode, div;
+    u32 base = host->base;
+
+    val = (width == HOST_BUS_WIDTH_8) ? 2 :
+          (width == HOST_BUS_WIDTH_4) ? 1 : 0;
+
+    MSDC_SET_FIELD(SDC_CFG, SDC_CFG_BUSWIDTH, val);
+    MSDC_GET_FIELD(MSDC_CFG,MSDC_CFG_CKMOD,mode);
+    MSDC_GET_FIELD(MSDC_CFG,MSDC_CFG_CKDIV,div);
+
+    dprintf(INFO, "CLK (%dMHz), SCLK(%dkHz) MODE(%d) DIV(%d) buswidth(%u-bits)\n",
+            host->clk/1000000, host->sclk/1000, mode, div, width);
+}
+
+static void msdc_config_pin(struct mmc_host *host, int mode)
+{
+    dprintf(INFO, "[MSDC] Pins mode(%d), none(0), down(1), up(2), keep(3)\n", mode);
+
+    switch (mode) {
+        case MSDC_PIN_PULL_UP:
+            msdc_pin_set(host,MSDC_PST_PU_10KOHM);
+            break;
+        case MSDC_PIN_PULL_DOWN:
+            msdc_pin_set(host,MSDC_PST_PD_50KOHM);
+            break;
+        case MSDC_PIN_PULL_NONE:
+        default:
+            msdc_pin_set(host,MSDC_PST_PD_50KOHM);
+            break;
+    }
+}
+
+void msdc_clock(struct mmc_host *host, int on)
+{
+#ifndef FPGA_PLATFORM
+    MSDC_SET_FIELD(TOPCKGEN_CTRL(CLKCFG2), MSDC_CKGEN_PDN(MSDC0,CLKCFG2), (on ? 0 : 1));
+#endif
+    dprintf(INFO, "[MSDC] Turn %s %s clock \n", on ? "on" : "off", "host");
+}
+
+static void msdc_host_power(struct mmc_host *host, int on)
+{
+    dprintf(INFO, "[MSDC] Turn %s %s power \n", on ? "on" : "off", "host");
+
+    if (on) {
+        msdc_config_pin(host, MSDC_PIN_PULL_UP);
+        msdc_set_host_pwr(host, 1);
+        msdc_clock(host, 1);
+    } else {
+        msdc_clock(host, 0);
+        msdc_set_host_pwr(host, 0);
+        msdc_config_pin(host, MSDC_PIN_PULL_DOWN);
+    }
+}
+
+static void msdc_card_power(struct mmc_host *host, int on)
+{
+    dprintf(INFO, "[MSDC] Turn %s %s power \n", on ? "on" : "off", "card");
+
+    if (on) {
+        msdc_set_card_pwr(host, 1);
+    } else {
+        msdc_set_card_pwr(host, 0);
+    }
+}
+
+void msdc_power(struct mmc_host *host, u8 mode)
+{
+    if (mode == MMC_POWER_ON || mode == MMC_POWER_UP) {
+        msdc_host_power(host, 1);
+        msdc_card_power(host, 1);
+    } else {
+        msdc_card_power(host, 0);
+        msdc_host_power(host, 0);
+    }
+}
+
+void msdc_reset_tune_counter(struct mmc_host *host)
+{
+    host->time_read = 0;
+}
+
+#ifdef FEATURE_MMC_CM_TUNING
+static int msdc_tune_cmdrsp(struct mmc_host *host, struct mmc_command *cmd)
+{
+    u32 base = host->base;
+    u32 sel = 0;
+    u32 rsmpl,cur_rsmpl, orig_rsmpl;
+    u32 rrdly,cur_rrdly, orig_rrdly;
+    u32 cntr,cur_cntr,orig_cmdrtc;
+    u32 dl_cksel, cur_dl_cksel, orig_dl_cksel;
+    u32 times = 0;
+    int result = MMC_ERR_CMDTUNEFAIL;
+    u32 tmp = 0;
+
+    if (host->sclk > 100000000) {
+        sel = 1;
+        //MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_CKGEN_RX_SDCLKO_SEL,0);
+    }
+
+    MSDC_GET_FIELD(MSDC_IOCON, MSDC_IOCON_RSPL, orig_rsmpl);
+    MSDC_GET_FIELD(MSDC_PAD_TUNE, MSDC_PAD_TUNE_CMDRDLY, orig_rrdly);
+    MSDC_GET_FIELD(MSDC_PATCH_BIT1, MSDC_PATCH_BIT1_CMD_RSP, orig_cmdrtc);
+    MSDC_GET_FIELD(MSDC_PATCH_BIT0, MSDC_INT_DAT_LATCH_CK_SEL, orig_dl_cksel);
+
+    dl_cksel = 0;
+    do {
+        cntr = 0;
+        do {
+            rrdly = 0;
+            do {
+                for (rsmpl = 0; rsmpl < 2; rsmpl++) {
+                    cur_rsmpl = (orig_rsmpl + rsmpl) % 2;
+                    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_RSPL, cur_rsmpl);
+                    if (host->sclk <= 400000) { //In sd/emmc init flow, fix rising edge for latching cmd response
+                        MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_RSPL, 0);
+                    }
+                    if (cmd->opcode != MMC_CMD_STOP_TRANSMISSION) {
+                        result = msdc_send_cmd(host, cmd);
+                        if (result == MMC_ERR_TIMEOUT)
+                            rsmpl--;
+                        if (result != MMC_ERR_NONE && cmd->opcode != MMC_CMD_STOP_TRANSMISSION) {
+                            if (cmd->opcode == MMC_CMD_READ_MULTIPLE_BLOCK || cmd->opcode == MMC_CMD_WRITE_MULTIPLE_BLOCK || cmd->opcode == MMC_CMD_READ_SINGLE_BLOCK ||cmd->opcode == MMC_CMD_WRITE_BLOCK)
+                                msdc_abort_handler(host,1);
+                            continue;
+                        }
+                        result = msdc_wait_cmd_done(host, cmd);
+                    } else if (cmd->opcode == MMC_CMD_STOP_TRANSMISSION) {
+                        result = MMC_ERR_NONE;
+                        goto done;
+                    } else
+                        result = MMC_ERR_BADCRC;
+
+                    times++;
+                    if (result == MMC_ERR_NONE)
+                        goto done;
+                    tmp = MSDC_READ32(SDC_CMD);
+                    /* check if data is used by the command or not */
+                    if (tmp & 0x1800) {
+                        if (msdc_abort_handler(host, 1))
+                            dprintf(CRITICAL, "[MSDC] abort failed\n");
+                    }
+                }
+                cur_rrdly = (orig_rrdly + rrdly + 1) % 32;
+                MSDC_SET_FIELD(MSDC_PAD_TUNE, MSDC_PAD_TUNE_CMDRDLY, cur_rrdly);
+            } while (++rrdly < 32);
+            if (!sel)
+                break;
+            cur_cntr = (orig_cmdrtc + cntr + 1) % 8;
+            MSDC_SET_FIELD(MSDC_PATCH_BIT1, MSDC_PATCH_BIT1_CMD_RSP, cur_cntr);
+        } while (++cntr < 8);
+        /* no need to update data ck sel */
+        if (!sel)
+            break;
+        cur_dl_cksel = (orig_dl_cksel +dl_cksel+1) % 8;
+        MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_INT_DAT_LATCH_CK_SEL, cur_dl_cksel);
+        dl_cksel++;
+    } while (dl_cksel < 8);
+    /* no need to update ck sel */
+    if (result != MMC_ERR_NONE)
+        result = MMC_ERR_CMDTUNEFAIL;
+done:
+
+    dprintf(ALWAYS, "[MSDC] <TUNE_CMD><%d><%s>\n", times, (result == MMC_ERR_NONE) ?"PASS" : "FAIL");
+    return result;
+}
+#endif
+
+#ifdef FEATURE_MMC_RD_TUNING
+int msdc_tune_bread(struct mmc_host *host, u8 *dst, u32 src, u32 nblks)
+{
+    u32 base = host->base;
+    u32 dcrc = 1, ddr = 0, sel = 0;
+    u32 cur_rxdly0, cur_rxdly1;
+    u32 dsmpl, cur_dsmpl, orig_dsmpl;
+    u32 dsel,cur_dsel,orig_dsel;
+    u32 dl_cksel,cur_dl_cksel,orig_dl_cksel;
+    u32 rxdly;
+    u32 cur_dat0, cur_dat1, cur_dat2, cur_dat3, cur_dat4, cur_dat5,
+        cur_dat6, cur_dat7;
+    u32 orig_dat0, orig_dat1, orig_dat2, orig_dat3, orig_dat4, orig_dat5,
+        orig_dat6, orig_dat7;
+    u32 orig_clkmode;
+    u32 times = 0;
+    int result = MMC_ERR_READTUNEFAIL;
+
+    if (host->sclk > 100000000)
+        sel = 1;
+    if (host->card)
+        ddr = mmc_card_ddr(host->card);
+    MSDC_GET_FIELD(MSDC_CFG,MSDC_CFG_CKMOD,orig_clkmode);
+    MSDC_GET_FIELD(MSDC_PATCH_BIT0, MSDC_CKGEN_MSDC_DLY_SEL, orig_dsel);
+    MSDC_GET_FIELD(MSDC_PATCH_BIT0, MSDC_INT_DAT_LATCH_CK_SEL, orig_dl_cksel);
+    MSDC_GET_FIELD(MSDC_IOCON, MSDC_IOCON_DSPL, orig_dsmpl);
+
+    /* Tune Method 2. delay each data line */
+    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_DDLSEL, 1);
+
+    dl_cksel = 0;
+    do {
+        dsel = 0;
+        do {
+            rxdly = 0;
+            do {
+                for (dsmpl = 0; dsmpl < 2; dsmpl++) {
+                    cur_dsmpl = (orig_dsmpl + dsmpl) % 2;
+                    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_DSPL, cur_dsmpl);
+                    result = host->blk_read(host, dst, src, nblks);
+                    if (result == MMC_ERR_CMDTUNEFAIL || result == MMC_ERR_CMD_RSPCRC || result == MMC_ERR_ACMD_RSPCRC)
+                        goto done;
+                    MSDC_GET_FIELD(SDC_DCRC_STS, SDC_DCRC_STS_POS|SDC_DCRC_STS_NEG, dcrc);
+
+                    if (!ddr)
+                        dcrc &= ~SDC_DCRC_STS_NEG;
+
+                    /* for debugging */
+                    times++;
+                    /* no cre error in this data line */
+                    if (result == MMC_ERR_NONE && dcrc == 0) {
+                        goto done;
+                    } else {
+                        result = MMC_ERR_BADCRC;
+                    }
+                }
+                cur_rxdly0 = MSDC_READ32(MSDC_DAT_RDDLY0);
+                cur_rxdly1 = MSDC_READ32(MSDC_DAT_RDDLY1);
+
+
+                orig_dat0 = (cur_rxdly0 >> 24) & 0x1F;
+                orig_dat1 = (cur_rxdly0 >> 16) & 0x1F;
+                orig_dat2 = (cur_rxdly0 >> 8) & 0x1F;
+                orig_dat3 = (cur_rxdly0 >> 0) & 0x1F;
+                orig_dat4 = (cur_rxdly1 >> 24) & 0x1F;
+                orig_dat5 = (cur_rxdly1 >> 16) & 0x1F;
+                orig_dat6 = (cur_rxdly1 >> 8) & 0x1F;
+                orig_dat7 = (cur_rxdly1 >> 0) & 0x1F;
+
+                if (ddr) {
+                    cur_dat0 = (dcrc & (1 << 0) || dcrc & (1 <<  8)) ? ((orig_dat0 + 1) % 32) : orig_dat0;
+                    cur_dat1 = (dcrc & (1 << 1) || dcrc & (1 <<  9)) ? ((orig_dat1 + 1) % 32) : orig_dat1;
+                    cur_dat2 = (dcrc & (1 << 2) || dcrc & (1 << 10)) ? ((orig_dat2 + 1) % 32) : orig_dat2;
+                    cur_dat3 = (dcrc & (1 << 3) || dcrc & (1 << 11)) ? ((orig_dat3 + 1) % 32) : orig_dat3;
+                    cur_dat4 = (dcrc & (1 << 4) || dcrc & (1 << 12)) ? ((orig_dat4 + 1) % 32) : orig_dat4;
+                    cur_dat5 = (dcrc & (1 << 5) || dcrc & (1 << 13)) ? ((orig_dat5 + 1) % 32) : orig_dat5;
+                    cur_dat6 = (dcrc & (1 << 6) || dcrc & (1 << 14)) ? ((orig_dat6 + 1) % 32) : orig_dat6;
+                    cur_dat7 = (dcrc & (1 << 7) || dcrc & (1 << 15)) ? ((orig_dat7 + 1) % 32) : orig_dat7;
+                } else {
+                    cur_dat0 = (dcrc & (1 << 0)) ? ((orig_dat0 + 1) % 32) : orig_dat0;
+                    cur_dat1 = (dcrc & (1 << 1)) ? ((orig_dat1 + 1) % 32) : orig_dat1;
+                    cur_dat2 = (dcrc & (1 << 2)) ? ((orig_dat2 + 1) % 32) : orig_dat2;
+                    cur_dat3 = (dcrc & (1 << 3)) ? ((orig_dat3 + 1) % 32) : orig_dat3;
+                    cur_dat4 = (dcrc & (1 << 4)) ? ((orig_dat4 + 1) % 32) : orig_dat4;
+                    cur_dat5 = (dcrc & (1 << 5)) ? ((orig_dat5 + 1) % 32) : orig_dat5;
+                    cur_dat6 = (dcrc & (1 << 6)) ? ((orig_dat6 + 1) % 32) : orig_dat6;
+                    cur_dat7 = (dcrc & (1 << 7)) ? ((orig_dat7 + 1) % 32) : orig_dat7;
+                }
+
+                cur_rxdly0 = ((cur_dat0 & 0x1F) << 24) | ((cur_dat1 & 0x1F) << 16) |
+                             ((cur_dat2 & 0x1F)<< 8) | ((cur_dat3 & 0x1F) << 0);
+                cur_rxdly1 = ((cur_dat4 & 0x1F) << 24) | ((cur_dat5 & 0x1F) << 16) |
+                             ((cur_dat6 & 0x1F) << 8) | ((cur_dat7 & 0x1F) << 0);
+
+                MSDC_WRITE32(MSDC_DAT_RDDLY0, cur_rxdly0);
+                MSDC_WRITE32(MSDC_DAT_RDDLY1, cur_rxdly1);
+            } while (++rxdly < 32);
+            if (!sel)
+                break;
+            cur_dsel = (orig_dsel + dsel + 1) % 32;
+            MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_CKGEN_MSDC_DLY_SEL, cur_dsel);
+        } while (++dsel < 32);
+        /* no need to update data ck sel */
+        if (orig_clkmode != 1)
+            break;
+
+        cur_dl_cksel = (orig_dl_cksel + dl_cksel + 1)% 8;
+        MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_INT_DAT_LATCH_CK_SEL, cur_dl_cksel);
+        dl_cksel++;
+    } while (dl_cksel < 8);
+done:
+    dprintf(ALWAYS, "[MSDC] <msdc_tune_bread<%d><%s><cmd%d>@msdc_tune_bread\n",
+            times, (result == MMC_ERR_NONE && dcrc == 0) ? "PASS" : "FAIL",
+            (nblks == 1 ? 17 : 18));
+
+    return result;
+}
+#define READ_TUNING_MAX_HS (2 * 32)
+#define READ_TUNING_MAX_UHS (2 * 32 * 32)
+#define READ_TUNING_MAX_UHS_CLKMOD1 (2 * 32 * 32 *8)
+
+int msdc_tune_read(struct mmc_host *host)
+{
+    return 0;
+#if 0
+    u32 base = host->base;
+    u32 dcrc, ddr = 0, sel = 0;
+    u32 cur_rxdly0 = 0 , cur_rxdly1 = 0;
+    u32 cur_dsmpl = 0, orig_dsmpl;
+    u32 cur_dsel = 0,orig_dsel;
+    u32 cur_dl_cksel = 0,orig_dl_cksel;
+    u32 cur_dat0 = 0, cur_dat1 = 0, cur_dat2 = 0, cur_dat3 = 0, cur_dat4 = 0, cur_dat5 = 0,
+        cur_dat6 = 0, cur_dat7 = 0;
+    u32 orig_dat0, orig_dat1, orig_dat2, orig_dat3, orig_dat4, orig_dat5,
+        orig_dat6, orig_dat7;
+    u32 orig_clkmode;
+    u32 times = 0;
+    int result = MMC_ERR_NONE;
+
+    if (host->sclk > 100000000)
+        sel = 1;
+    if (host->card)
+        ddr = mmc_card_ddr(host->card);
+    MSDC_GET_FIELD(MSDC_CFG,MSDC_CFG_CKMOD,orig_clkmode);
+    //if(orig_clkmode == 1)
+    //MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_CKGEN_RX_SDCLKO_SEL, 0);
+
+    MSDC_GET_FIELD(MSDC_PATCH_BIT0, MSDC_CKGEN_MSDC_DLY_SEL, orig_dsel);
+    MSDC_GET_FIELD(MSDC_PATCH_BIT0, MSDC_INT_DAT_LATCH_CK_SEL, orig_dl_cksel);
+    MSDC_GET_FIELD(MSDC_IOCON, MSDC_IOCON_DSPL, orig_dsmpl);
+
+    /* Tune Method 2. delay each data line */
+    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_DDLSEL, 1);
+
+
+    cur_dsmpl = (orig_dsmpl + 1) ;
+    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_DSPL, cur_dsmpl % 2);
+    if (cur_dsmpl >= 2) {
+        MSDC_GET_FIELD(SDC_DCRC_STS, SDC_DCRC_STS_POS|SDC_DCRC_STS_NEG, dcrc);
+        if (!ddr) dcrc &= ~SDC_DCRC_STS_NEG;
+
+        cur_rxdly0 = MSDC_READ32(MSDC_DAT_RDDLY0);
+        cur_rxdly1 = MSDC_READ32(MSDC_DAT_RDDLY1);
+
+        orig_dat0 = (cur_rxdly0 >> 24) & 0x1F;
+        orig_dat1 = (cur_rxdly0 >> 16) & 0x1F;
+        orig_dat2 = (cur_rxdly0 >> 8) & 0x1F;
+        orig_dat3 = (cur_rxdly0 >> 0) & 0x1F;
+        orig_dat4 = (cur_rxdly1 >> 24) & 0x1F;
+        orig_dat5 = (cur_rxdly1 >> 16) & 0x1F;
+        orig_dat6 = (cur_rxdly1 >> 8) & 0x1F;
+        orig_dat7 = (cur_rxdly1 >> 0) & 0x1F;
+
+        if (ddr) {
+            cur_dat0 = (dcrc & (1 << 0) || dcrc & (1 <<  8)) ? (orig_dat0 + 1) : orig_dat0;
+            cur_dat1 = (dcrc & (1 << 1) || dcrc & (1 <<  9)) ? (orig_dat1 + 1) : orig_dat1;
+            cur_dat2 = (dcrc & (1 << 2) || dcrc & (1 << 10)) ? (orig_dat2 + 1) : orig_dat2;
+            cur_dat3 = (dcrc & (1 << 3) || dcrc & (1 << 11)) ? (orig_dat3 + 1) : orig_dat3;
+            cur_dat4 = (dcrc & (1 << 4) || dcrc & (1 << 12)) ? (orig_dat4 + 1) : orig_dat4;
+            cur_dat5 = (dcrc & (1 << 5) || dcrc & (1 << 13)) ? (orig_dat5 + 1) : orig_dat5;
+            cur_dat6 = (dcrc & (1 << 6) || dcrc & (1 << 14)) ? (orig_dat6 + 1) : orig_dat6;
+            cur_dat7 = (dcrc & (1 << 7) || dcrc & (1 << 15)) ? (orig_dat7 + 1) : orig_dat7;
+        } else {
+            cur_dat0 = (dcrc & (1 << 0)) ? (orig_dat0 + 1) : orig_dat0;
+            cur_dat1 = (dcrc & (1 << 1)) ? (orig_dat1 + 1) : orig_dat1;
+            cur_dat2 = (dcrc & (1 << 2)) ? (orig_dat2 + 1) : orig_dat2;
+            cur_dat3 = (dcrc & (1 << 3)) ? (orig_dat3 + 1) : orig_dat3;
+            cur_dat4 = (dcrc & (1 << 4)) ? (orig_dat4 + 1) : orig_dat4;
+            cur_dat5 = (dcrc & (1 << 5)) ? (orig_dat5 + 1) : orig_dat5;
+            cur_dat6 = (dcrc & (1 << 6)) ? (orig_dat6 + 1) : orig_dat6;
+            cur_dat7 = (dcrc & (1 << 7)) ? (orig_dat7 + 1) : orig_dat7;
+        }
+
+        cur_rxdly0 = ((cur_dat0 & 0x1F) << 24) | ((cur_dat1 & 0x1F) << 16) |
+                     ((cur_dat2 & 0x1F) << 8) | ((cur_dat3 & 0x1F) << 0);
+        cur_rxdly1 = ((cur_dat4 & 0x1F) << 24) | ((cur_dat5 & 0x1F)<< 16) |
+                     ((cur_dat6 & 0x1F) << 8) | ((cur_dat7 & 0x1F) << 0);
+
+        MSDC_WRITE32(MSDC_DAT_RDDLY0, cur_rxdly0);
+        MSDC_WRITE32(MSDC_DAT_RDDLY1, cur_rxdly1);
+    }
+    if (cur_dat0 >= 32 || cur_dat1 >= 32 || cur_dat2 >= 32 || cur_dat3 >= 32 ||
+            cur_dat4 >= 32 || cur_dat5 >= 32 || cur_dat6 >= 32 || cur_dat7 >= 32) {
+        if (sel) {
+
+            cur_dsel = (orig_dsel + 1);
+            MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_CKGEN_MSDC_DLY_SEL, cur_dsel % 32);
+
+        }
+    }
+    if (cur_dsel >= 32) {
+        if (orig_clkmode == 1 && sel) {
+
+            cur_dl_cksel = (orig_dl_cksel + 1);
+            MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_INT_DAT_LATCH_CK_SEL, cur_dl_cksel % 8);
+        }
+    }
+    ++(host->time_read);
+    if ((sel == 1 && orig_clkmode == 1 && host->time_read == READ_TUNING_MAX_UHS_CLKMOD1)||
+            (sel == 1 && orig_clkmode != 1 && host->time_read == READ_TUNING_MAX_UHS)||
+            (sel == 0 && orig_clkmode != 1 && host->time_read == READ_TUNING_MAX_HS)) {
+
+        result = MMC_ERR_READTUNEFAIL;
+    }
+
+    return result;
+#endif
+}
+#endif
+
+#ifdef FEATURE_MMC_WR_TUNING
+int msdc_tune_bwrite(struct mmc_host *host, u32 dst, u8 *src, u32 nblks)
+{
+    u32 base = host->base;
+    u32 sel = 0;
+    u32 wrrdly, cur_wrrdly, orig_wrrdly;
+    u32 dsmpl, cur_dsmpl, orig_dsmpl;
+    u32 d_cntr,orig_d_cntr,cur_d_cntr;
+    u32 rxdly, cur_rxdly0;
+    u32 orig_dat0, orig_dat1, orig_dat2, orig_dat3;
+    u32 cur_dat0, cur_dat1, cur_dat2, cur_dat3;
+    u32 times = 0;
+    int result = MMC_ERR_WRITETUNEFAIL;
+
+    if (host->sclk > 100000000)
+        sel = 1;
+
+    MSDC_GET_FIELD(MSDC_PAD_TUNE, MSDC_PAD_TUNE_DATWRDLY, orig_wrrdly);
+    MSDC_GET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL, orig_dsmpl);
+    MSDC_GET_FIELD(MSDC_PATCH_BIT1, MSDC_PATCH_BIT1_WRDAT_CRCS, orig_d_cntr);
+
+    /* Tune Method 2. delay data0 line */
+    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_DDLSEL, 1);
+
+    cur_rxdly0 = MSDC_READ32(MSDC_DAT_RDDLY0);
+
+    orig_dat0 = (cur_rxdly0 >> 24) & 0x1F;
+    orig_dat1 = (cur_rxdly0 >> 16) & 0x1F;
+    orig_dat2 = (cur_rxdly0 >> 8) & 0x1F;
+    orig_dat3 = (cur_rxdly0 >> 0) & 0x1F;
+
+    d_cntr = 0;
+    do {
+        rxdly = 0;
+        do {
+            wrrdly = 0;
+            do {
+                for (dsmpl = 0; dsmpl < 2; dsmpl++) {
+                    cur_dsmpl = (orig_dsmpl + dsmpl) % 2;
+                    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_W_D_SMPL, cur_dsmpl);
+                    result = host->blk_write(host, dst, src, nblks);
+                    if (result == MMC_ERR_CMDTUNEFAIL || result == MMC_ERR_CMD_RSPCRC || result == MMC_ERR_ACMD_RSPCRC)
+                        goto done;
+
+                    times++;
+                    if (result == MMC_ERR_NONE) {
+                        goto done;
+                    }
+                }
+                cur_wrrdly = ++orig_wrrdly % 32;
+                MSDC_SET_FIELD(MSDC_PAD_TUNE, MSDC_PAD_TUNE_DATWRDLY, cur_wrrdly);
+            } while (++wrrdly < 32);
+
+            cur_dat0 = ++orig_dat0 % 32; /* only adjust bit-1 for crc */
+            cur_dat1 = orig_dat1;
+            cur_dat2 = orig_dat2;
+            cur_dat3 = orig_dat3;
+
+            cur_rxdly0 = (cur_dat0 << 24) | (cur_dat1 << 16) |
+                         (cur_dat2 << 8) | (cur_dat3 << 0);
+
+            MSDC_WRITE32(MSDC_DAT_RDDLY0, cur_rxdly0);
+        } while (++rxdly < 32);
+
+        /* no need to update data ck sel */
+        if (!sel)
+            break;
+
+        cur_d_cntr= (orig_d_cntr + d_cntr +1 )% 8;
+        MSDC_SET_FIELD(MSDC_PATCH_BIT1, MSDC_PATCH_BIT1_WRDAT_CRCS, cur_d_cntr);
+        d_cntr++;
+    } while (d_cntr < 8);
+done:
+    dprintf(ALWAYS, "[MSDC] <TUNE_BWRITE_%d><%s>\n", times, result == MMC_ERR_NONE ? "PASS" : "FAIL");
+
+    return result;
+}
+#endif
+
+int msdc_init(struct mmc_host *host)
+{
+    u32 base = MSDC0_BASE;
+    msdc_priv_t *priv;
+
+    dprintf(INFO, "[%s]: Host controller intialization start \n", __func__);
+
+    priv = &msdc_priv;
+    memset(priv, 0, sizeof(msdc_priv_t));
+
+    host->base   = base;
+    host->clksrc = msdc_cap.clk_src;
+    host->hclksrc= msdc_cap.hclk_src;
+#ifndef FPGA_PLATFORM
+    host->f_max  = hclks_msdc30[host->clksrc];
+#else
+    host->f_max  = MSDC_MAX_SCLK;
+#endif
+
+    host->f_min  = MSDC_MIN_SCLK;
+    host->blklen = 0;
+    host->priv   = (void *)priv;
+    host->caps   = MMC_CAP_MULTIWRITE;
+
+    if (msdc_cap.flags & MSDC_HIGHSPEED)
+        host->caps |= (MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED);
+    if (msdc_cap.flags & MSDC_DDR)
+        host->caps |= MMC_CAP_DDR;
+    if (msdc_cap.data_pins == 4)
+        host->caps |= MMC_CAP_4_BIT_DATA;
+    if (msdc_cap.data_pins == 8)
+        host->caps |= MMC_CAP_8_BIT_DATA | MMC_CAP_4_BIT_DATA;
+    if (msdc_cap.flags & MSDC_HS200)
+        host->caps |= MMC_CAP_EMMC_HS200;
+    if (msdc_cap.flags & MSDC_HS400)
+        host->caps |= MMC_CAP_EMMC_HS400;
+
+    host->ocr_avail = MMC_VDD_32_33;  /* TODO: To be customized */
+
+    /* Configure BASIC_DMA + AUTOCMD12 for better R/W performance
+     * NOTE: ACMD23 only support transferring size of up to 32M */
+    priv->autocmd = MSDC_AUTOCMD12;
+    if (priv->autocmd == MSDC_AUTOCMD23)
+        /* The maximal transferring size is size of *[15:0] number of blocks* */
+        host->max_phys_segs = 0xffff;
+    else
+        /* The maximal transferring size is size of DMA_LENGTH */
+        host->max_phys_segs = (UINT_MAX & ~511) >> MMC_BLOCK_BITS_SHFT;
+
+    priv->rdsmpl       = msdc_cap.data_edge;
+    priv->wdsmpl       = msdc_cap.data_edge;
+    priv->rsmpl       = msdc_cap.cmd_edge;
+
+#ifdef MSDC_USE_DMA_MODE
+    host->blk_read  = msdc_dma_bread;
+    host->blk_write = msdc_dma_bwrite;
+    dprintf(INFO, "Transfer method: DMA\n");
+#else
+    host->blk_read  = msdc_pio_bread;
+    host->blk_write = msdc_pio_bwrite;
+    dprintf(INFO, "Transfer method: PIO\n");
+#endif
+
+    priv->rdsmpl       = msdc_cap.data_edge;
+    priv->rsmpl       = msdc_cap.cmd_edge;
+
+    msdc_power(host, MMC_POWER_OFF);
+    msdc_power(host, MMC_POWER_ON);
+
+    /* set to SD/MMC mode */
+    MSDC_SET_FIELD(MSDC_CFG, MSDC_CFG_MODE, MSDC_SDMMC);
+    MSDC_SET_BIT32(MSDC_CFG, MSDC_CFG_PIO);
+
+    MSDC_RESET();
+    MSDC_CLR_FIFO();
+    MSDC_CLR_INT();
+
+    /* reset tuning parameter */
+    //MSDC_WRITE32(MSDC_PAD_CTL0, 0x0098000);
+    //MSDC_WRITE32(MSDC_PAD_CTL1, 0x00A0000);
+    //MSDC_WRITE32(MSDC_PAD_CTL2, 0x00A0000);
+    MSDC_WRITE32(MSDC_PAD_TUNE, 0x0000000);
+    MSDC_WRITE32(MSDC_DAT_RDDLY0, 0x00000000);
+    MSDC_WRITE32(MSDC_DAT_RDDLY1, 0x00000000);
+    MSDC_WRITE32(MSDC_IOCON, 0x00000000);
+
+    MSDC_WRITE32(MSDC_PATCH_BIT1, 0xFFFE00C9);//High 16 bit = 0 mean Power KPI is on, enable ECO for write timeout issue
+    //MSDC_PATCH_BIT1YD:WRDAT_CRCS_TA_CNTR need fix to 3'001 by default,(<50MHz) (>=50MHz set 3'001 as initial value is OK for tunning)
+    //YD:CMD_RSP_TA_CNTR need fix to 3'001 by default(<50MHz)(>=50MHz set 3'001as initial value is OK for tunning)
+    /* 2012-01-07 using internal clock instead of feedback clock */
+    //MSDC_SET_BIT32(MSDC_PATCH_BIT0, MSDC_CKGEN_MSDC_CK_SEL);
+
+#ifdef MSDC_USE_PATCH_BIT2_TURNING_WITH_ASYNC
+    MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGCRCSTS,1);
+    MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGRESP,0);
+#else
+    MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGCRCSTS,0);
+    MSDC_SET_FIELD(MSDC_PATCH_BIT2, MSDC_PB2_CFGRESP,1);
+#endif
+
+    /* enable wake up events */
+    //MSDC_SET_BIT32(SDC_CFG, SDC_CFG_INSWKUP);
+
+    /* eneable SMT for glitch filter */
+#ifdef FPGA_PLATFORM
+#if 0
+    MSDC_SET_BIT32(MSDC_PAD_CTL0, MSDC_PAD_CTL0_CLKSMT);
+    MSDC_SET_BIT32(MSDC_PAD_CTL1, MSDC_PAD_CTL1_CMDSMT);
+    MSDC_SET_BIT32(MSDC_PAD_CTL2, MSDC_PAD_CTL2_DATSMT);
+#endif
+#else
+    msdc_set_smt(host,1);
+#endif
+    /* set clk, cmd, dat pad driving */
+    msdc_set_driving(host, &msdc_cap);
+
+    /* set sampling edge */
+    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_RSPL, msdc_cap.cmd_edge);
+    MSDC_SET_FIELD(MSDC_IOCON, MSDC_IOCON_DSPL, msdc_cap.data_edge);
+
+    /* write crc timeout detection */
+    MSDC_SET_FIELD(MSDC_PATCH_BIT0, MSDC_PB0_DETWR_CRCTMO, 1);
+
+    msdc_set_startbit(host, START_AT_RISING);
+
+    msdc_config_bus(host, HOST_BUS_WIDTH_1);
+    msdc_config_clock(host, 0, MSDC_MIN_SCLK);
+    msdc_set_timeout(host, 100000000, 0);
+
+    /* disable SDIO func */
+    MSDC_SET_FIELD(SDC_CFG, SDC_CFG_SDIO, 0);
+    MSDC_SET_FIELD(SDC_CFG, SDC_CFG_SDIOIDE, 0);
+    MSDC_SET_FIELD(SDC_CFG, SDC_CFG_INSWKUP, 0);
+
+    /* Clear all interrupts first */
+    MSDC_CLR_INT();
+    MSDC_WRITE32(MSDC_INTEN, 0);
+
+#ifdef MSDC_USE_DMA_MODE
+    /* Register msdc irq */
+    mt_irq_set_sens(MT_MSDC0_IRQ_ID, LEVEL_SENSITIVE);
+    mt_irq_set_polarity(MT_MSDC0_IRQ_ID, MT65xx_POLARITY_LOW);
+    event_init(&msdc_int_event, false, EVENT_FLAG_AUTOUNSIGNAL);
+    register_int_handler(MT_MSDC0_IRQ_ID, msdc_interrupt_handler, host);
+    unmask_interrupt(MT_MSDC0_IRQ_ID);
+#endif
+
+    dprintf(INFO, "[%s]: Host controller intialization done\n", __func__);
+    return 0;
+}
+
diff --git a/src/bsp/lk/platform/mt2701/drivers/mmc/rules.mk b/src/bsp/lk/platform/mt2701/drivers/mmc/rules.mk
new file mode 100644
index 0000000..bb46477
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/mmc/rules.mk
@@ -0,0 +1,12 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+    $(LOCAL_DIR)/msdc.c \
+    $(LOCAL_DIR)/mmc_core.c \
+
+MODULE_DEPS += \
+    lib/bio \
+    lib/partition \
+
+include make/module.mk
diff --git a/src/bsp/lk/platform/mt2701/drivers/nand/mtk_nand.c b/src/bsp/lk/platform/mt2701/drivers/nand/mtk_nand.c
new file mode 100755
index 0000000..98288de
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/nand/mtk_nand.c
@@ -0,0 +1,2997 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#include <stdlib.h>
+#include <err.h>
+#include <assert.h>
+#include <pow2.h>
+#include <string.h>
+#include <errno.h>
+#include <malloc.h>
+#include <platform/typedefs.h>
+#include <platform/nand.h>
+#include <platform/nand_core.h>
+#include <platform/mt_reg_base.h>
+#include <platform/cust_nand.h>
+#include <lib/bio.h>
+#include <lib/partition.h>
+#include <kernel/mutex.h>
+#include <kernel/semaphore.h>
+
+#define       NAND_BBLK_TBL_SIZE          (0x2000)
+// from boot_device.h
+#define BOOTDEV_NAND            (0)
+
+/*Support bad block skip*/
+#define NFI_BAD_BLOCK_DBG_INFO      (0)
+
+/*Testing: mark some bad block to do testing*/
+#define NAND_MARK_BAD_BLOCK_TEST (0)
+
+/*To define if need skip bad block in erase process*/
+#define NAND_ERASE_SKIP_BAD  (1)
+
+/*The real MBR is saved in 512th page*/
+#define MTD_MBR_PARTITION_BLOCK_ID  (512/64)
+
+/*=======================================================================*/
+/* NAND Control                                                          */
+/*=======================================================================*/
+#define NAND_PAGE_SIZE                  (2048)  // (Bytes)
+#define NAND_BLOCK_BLKS                 (64)    // 64 nand pages = 128KB
+#define NAND_PAGE_SHIFT                 (9)
+#define NAND_LARGE_PAGE                 (11)    // large page
+#define NAND_SMALL_PAGE                 (9)     // small page
+#define NAND_BUS_WIDTH_8                (8)
+#define NAND_BUS_WIDTH_16               (16)
+#define NAND_FDM_SIZE                   (8)
+#define NAND_ECC_SW                     (0)
+#define NAND_ECC_HW                     (1)
+
+#define NFI_MAX_FDM_SIZE                (8)
+#define NFI_MAX_FDM_SEC_NUM             (8)
+#define NFI_MAX_LOCK_CHANNEL            (16)
+
+#define ECC_MAX_CORRECTABLE_BITS        (12)
+#define ECC_MAX_PARITY_SIZE             (20)    /* in bytes */
+
+#define ECC_ERR_LOCATION_MASK           (0x1FFF)
+#define ECC_ERR_LOCATION_SHIFT          (16)
+
+#define NAND_FFBUF_SIZE                 (2048+64)
+
+
+#define print(x...) do { if (1) { _dprintf(x); } } while (0)
+
+#define PMT_POOL_SIZE (2)
+
+#define USE_AHB_MODE    (0)
+
+#if (USE_AHB_MODE)
+#undef MANUAL_CORRECT
+#else
+#define MANUAL_CORRECT
+#endif
+
+
+
+u64  u8g_partition_start_ofst = 0; //the current partition start offset
+u32  u4g_partition_bad_num = 0;//the total bad block numbers in one read operation
+u8  *pAlignBuf = NULL;
+
+
+/******************************************************************************
+*
+* Macro definition
+*
+*******************************************************************************/
+
+#define NFI_SET_REG32(reg, value)   (DRV_WriteReg32(reg, DRV_Reg32(reg) | (value)))
+#define NFI_SET_REG16(reg, value)   (DRV_WriteReg16(reg, DRV_Reg16(reg) | (value)))
+#define NFI_CLN_REG32(reg, value)   (DRV_WriteReg32(reg, DRV_Reg32(reg) & (~(value))))
+#define NFI_CLN_REG16(reg, value)   (DRV_WriteReg16(reg, DRV_Reg16(reg) & (~(value))))
+
+#define FIFO_PIO_READY(x)  (0x1 & x)
+#define WAIT_NFI_PIO_READY(timeout) \
+    do { \
+        while((!FIFO_PIO_READY(DRV_Reg(NFI_PIO_DIRDY_REG16))) && (--timeout)); \
+        if(timeout == 0) { \
+           print("Error: FIFO_PIO_READY timeout at line=%d, file =%s\n", __LINE__, __FILE__); \
+        } \
+    } while(0);
+
+
+#define PARTITION_LIMITATION        (g_nand_chip.chipsize)
+//the offset should not over chip size
+#define NAND_CHECK_OFST_LIMITATION(offset) \
+    do { \
+        if (offset > PARTITION_LIMITATION) { \
+            print("Error: Nand Adjust offset overflow line=%d, file =%s\n", __LINE__, __FILE__); \
+            ASSERT(0); \
+        } \
+    } while(0);
+
+#define NAND_SKIP_LIMITATION         (5)  //5 continuous bad block should be abnormal
+#define NAND_CHECK_WRITE_SKIP(skip_num) \
+    do { \
+        if (skip_num > NAND_SKIP_LIMITATION) { \
+            print("Error: Nand write skip bad block[%d] abnormal line=%d, file =%s\n", skip_num,__LINE__, __FILE__); \
+            ASSERT(0); \
+        } \
+    } while(0);
+
+#define NAND_PRINT_PROCESS() \
+    do { \
+        int print_cnt = 0; \
+        print_cnt++; \
+        if (0 == ((u32)print_cnt % 64)) { \
+            print(".\n"); \
+        } \
+    } while(0);
+
+#define NAND_CHECK_ALLOC_MEMORY(pBuf) \
+    do { \
+        if (NULL == pBuf) { \
+            pBuf = memalign(CACHE_LINE, PAGE_SIZE); \
+            ASSERT(pBuf != NULL); \
+        } \
+    }while(0)
+
+#define NAND_FREE_MEMORY(pBuf) \
+    do { \
+        if (NULL != pBuf) { \
+            free(pBuf); \
+            pBuf = NULL; \
+        } \
+    }while(0)
+
+
+#define TIMEOUT_1   0x1fff
+#define TIMEOUT_2   0x8ff
+#define TIMEOUT_3   0xffff
+#define TIMEOUT_4   5000        //PIO
+
+#define STATUS_READY            (0x40)
+#define STATUS_FAIL             (0x01)
+#define STATUS_WR_ALLOW         (0x80)
+
+
+static enum flashdev_vendor gVendor;
+static u8 nand_nfi_buf[NAND_NFI_BUFFER_SIZE];
+
+//#define NAND_TEST
+
+#ifdef NAND_TEST
+#define MAX_PAGE_SIZE   2048
+#define MAX_SPARE_SIZE  128
+u8 test_buf[MAX_PAGE_SIZE + MAX_SPARE_SIZE];
+u8 *test_oob;
+#endif
+
+#define ERR_RTN_SUCCESS   0
+#define ERR_RTN_FAIL      -1
+#define ERR_RTN_BCH_FAIL  -2
+
+static struct nand_chip g_nand_chip;
+
+#define BLOCK_SIZE (g_nand_chip.erasesize)
+#define PAGE_SIZE  (g_nand_chip.page_size)
+
+
+/**************************************************************************
+*  MACRO LIKE FUNCTION
+**************************************************************************/
+
+static inline u32 PAGE_NUM(u64 logical_size)
+{
+    return ((unsigned long)(logical_size) / g_nand_chip.page_size);
+}
+
+inline u64 LOGICAL_ADDR(u32 page_addr)
+{
+    return ((unsigned long long)(page_addr) * g_nand_chip.page_size);
+}
+
+inline u64 BLOCK_ALIGN(u64 logical_addr)
+{
+    return (((u64) (logical_addr / BLOCK_SIZE)) * BLOCK_SIZE);
+}
+
+//---------------------------------------------------------------------------
+
+//-------------------------------------------------------------------------
+#ifndef REDUCE_NAND_PL_SIZE
+
+//-------------------------------------------------------------------------
+
+#endif
+
+
+__attribute__((aligned(4))) unsigned char g_nand_spare[128];
+
+unsigned int nand_maf_id;
+unsigned int nand_dev_id;
+uint8 ext_id1, ext_id2, ext_id3;
+
+static BOOL g_bInitDone;
+BOOL g_bHwEcc = TRUE;
+u8 *Bad_Block_Table;
+
+struct nand_ecclayout *nand_oob = NULL;
+
+struct nand_ecclayout nand_oob_64 = {
+    .eccbytes = 32,
+    .eccpos = {
+        32, 33, 34, 35, 36, 37, 38, 39,
+        40, 41, 42, 43, 44, 45, 46, 47,
+        48, 49, 50, 51, 52, 53, 54, 55,
+        56, 57, 58, 59, 60, 61, 62, 63
+    },
+    .oobfree = {{1, 7}, {9, 7}, {17, 7}, {25, 6}, {0, 0}}
+};
+
+struct nand_ecclayout nand_oob_128 = {
+    .eccbytes = 64,
+    .eccpos = {
+        64, 65, 66, 67, 68, 69, 70, 71,
+        72, 73, 74, 75, 76, 77, 78, 79,
+        80, 81, 82, 83, 84, 85, 86, 86,
+        88, 89, 90, 91, 92, 93, 94, 95,
+        96, 97, 98, 99, 100, 101, 102, 103,
+        104, 105, 106, 107, 108, 109, 110, 111,
+        112, 113, 114, 115, 116, 117, 118, 119,
+        120, 121, 122, 123, 124, 125, 126, 127
+    },
+    .oobfree = {{1, 7}, {9, 7}, {17, 7}, {25, 7}, {33, 7}, {41, 7}, {49, 7}, {57, 6}}
+};
+#ifndef REDUCE_NAND_PL_SIZE //not used.marked by xiaolei
+struct NAND_CMD {
+    u32 u4ColAddr;
+    u32 u4RowAddr;
+    u32 u4OOBRowAddr;
+    u8 au1OOB[64];
+    u8 *pDataBuf;
+};
+
+static struct NAND_CMD g_kCMD;
+#endif
+static flashdev_info devinfo;
+static char *nfi_buf;
+static bool mtk_nand_read_status(void);
+
+static bool get_device_info(u8 *id, flashdev_info *devinfo);
+
+#define OOB_INDEX_OFFSET        (29)
+#define PAGE_SIZE_BMT           (1 << g_nand_chip.page_shift)
+#define FAKE_INDEX              (0xAAAA)
+static int mtk_nand_read_page_hw(u32 page, u8 *dat, u8 *oob);
+static bool mark_block_bad_hw(u64 offset);
+static void set_bad_index_to_oob(u8 *oob, u16 index);
+static int mtk_nand_write_page_hw(u32 page, u8 *dat, u8 *oob);
+static bool nand_block_bad_hw(u64 logical_addr);
+
+static u32 nand_get_device_id(u8 *id, u32 len);
+
+//extern unsigned char g_bHwEcc;
+/* LEGACY - TO BE REMOVED { */
+static int nand_bread(bdev_t *bdev, void *buf, u32 blknr, u32 blks);
+static int nand_bwrite(bdev_t *bdev, void *buf, u32 blknr, u32 blks);
+static int nand_read_page_data(u8 *buf, u64 offset);
+static int nand_write_page_data(u8 *buf, u64 offset);
+static u64 nand_find_safe_block(u64 offset);
+/* LEGACY - TO BE REMOVED } */
+//extern bool nand_erase_data(u64 offset, u64 offset_limit, u64 size);
+static int nand_erase_data(u64 offset, u64 offset_limit, u64 size);
+static u32 mtk_nand_offset_to_block(u64 u8Offset);
+
+/*Support lock / unlock in multi-thread env*/
+mutex_t nand_mutex_lock;
+
+static int nand_get_device()
+{
+    //mutex
+    return mutex_acquire(&nand_mutex_lock);
+}
+
+
+static int nand_release_device()
+{
+    return mutex_release(&nand_mutex_lock);
+}
+
+
+static void nand_set_timing()
+{
+    print("default acccon 0x%x\n",DRV_Reg32(NFI_ACCCON_REG32));
+    print("default acccon1 0x%x\n",DRV_Reg32(NFI_ACCCON1_REG3));
+    print("default delay 0x%x\n",DRV_Reg32(NFI_DLYCTRL_REG32));
+
+    DRV_WriteReg32(NFI_ACCCON_REG32, 0x10804222);
+    DRV_WriteReg32(NFI_ACCCON1_REG3, 0x01010101);
+    DRV_WriteReg32(NFI_DLYCTRL_REG32, 0x148011);
+
+#if NFI_BAD_BLOCK_DBG_INFO
+    print("adjust acccon 0x%x \n",DRV_Reg32(NFI_ACCCON_REG32));
+    print("adjust acccon1 0x%x \n",DRV_Reg32(NFI_ACCCON1_REG3));
+    print("adjust delay 0x%x \n",DRV_Reg32(NFI_DLYCTRL_REG32));
+#endif
+
+
+}
+
+
+
+/*skip the bad block from partition beginning*/
+static u64 nand_get_adjust_offset(u64 u8StartOfst, u64* u8EndOfst)
+{
+    u32 u4StartBlockNo = mtk_nand_offset_to_block(u8StartOfst);
+    u32 u4EndBlockNo = mtk_nand_offset_to_block(u8EndOfst);
+    u32 i = 0;
+    u32 u4Bad_Num = 0;
+
+    for (i = u4StartBlockNo; i <= u4EndBlockNo; i++) {
+        if (nand_block_bad_hw((u64)(i * BLOCK_SIZE))) {
+            //bad block, count it
+            u4Bad_Num ++;
+        } else {
+            //good block, skip it
+            ;
+        }
+
+    }
+
+    return ((u64)(u4Bad_Num * BLOCK_SIZE) + u8EndOfst);
+
+}
+
+static inline unsigned int uffs(unsigned int x)
+{
+    unsigned int r = 1;
+
+    if (!x)
+        return 0;
+    if (!(x & 0xffff)) {
+        x >>= 16;
+        r += 16;
+    }
+    if (!(x & 0xff)) {
+        x >>= 8;
+        r += 8;
+    }
+    if (!(x & 0xf)) {
+        x >>= 4;
+        r += 4;
+    }
+    if (!(x & 3)) {
+        x >>= 2;
+        r += 2;
+    }
+    if (!(x & 1)) {
+        x >>= 1;
+        r += 1;
+    }
+    return r;
+}
+
+static void set_bad_index_to_oob(u8 *oob, u16 index)
+{
+    if (2048 == PAGE_SIZE_BMT) {
+        memcpy(oob + 13, &index, sizeof(index));
+    } else {
+        memcpy(oob + OOB_INDEX_OFFSET, &index, sizeof(index));
+    }
+}
+
+
+#define NAND_SECTOR_SIZE 512
+
+
+static u32 mtk_nand_page_transform(u64 logical_address, u32 *blk, u32 *map_blk)
+{
+    u64 start_address;
+    u32 block;
+    u32 page_in_block;
+    u32 mapped_block;
+    if (VEND_NONE != gVendor) {
+        start_address = (logical_address & ~(g_nand_chip.erasesize- 1));
+        if (start_address == 0xFFFFFFFF)
+            while (1);
+
+        block = (u32)(start_address/BLOCK_SIZE) ;
+        page_in_block = PAGE_NUM(logical_address-start_address);
+
+        mapped_block = block;
+    } else {
+        block = (u32)(logical_address/BLOCK_SIZE);
+        mapped_block = block;
+        page_in_block = (u32)(PAGE_NUM(logical_address) % (BLOCK_SIZE >> g_nand_chip.page_shift));
+    }
+    *blk = block;
+    *map_blk = mapped_block;
+
+    return mapped_block*(BLOCK_SIZE/g_nand_chip.page_size)+page_in_block;
+}
+
+
+static u32 mtk_nand_offset_to_block(u64 u8Offset)
+{
+    u32 u4BlockNo = u8Offset / (u64)BLOCK_SIZE;
+    return u4BlockNo;
+
+}
+
+
+static void mtk_nand_search_bad_block(u64 u8Offset)
+{
+    u32 u4StartBlockNo = 0;
+    u32 u4EndBlockNo = 0;
+    u32 i = 0;
+    u32 u4Bad_Num = 0;
+
+    if (0 == u8Offset) {
+        u4g_partition_bad_num = 0;
+        return;
+    }
+
+    u4StartBlockNo = mtk_nand_offset_to_block(u8g_partition_start_ofst);
+    u4EndBlockNo = mtk_nand_offset_to_block(u8Offset);
+
+    for (i = u4StartBlockNo; i <= u4EndBlockNo; i++) {
+        if (nand_block_bad_hw((u64)(i * BLOCK_SIZE))) {
+            //bad block, count it
+            u4Bad_Num ++;
+        } else {
+            //good block, skip it
+            ;
+        }
+    }
+#if NFI_BAD_BLOCK_DBG_INFO
+    print("update total bad block num from %d to %d\n",u4g_partition_bad_num,u4Bad_Num);
+#endif
+
+    u4g_partition_bad_num = u4Bad_Num;
+
+}
+
+/**************************************************************************
+*  reset descriptor
+**************************************************************************/
+static void mtk_nand_reset_descriptor(void)
+{
+    g_nand_chip.page_shift = 0;
+    g_nand_chip.page_size = 0;
+    g_nand_chip.ChipID = 0;     /* Type of DiskOnChip */
+    g_nand_chip.chips_name = 0;
+    g_nand_chip.chipsize = 0;
+    g_nand_chip.erasesize = 0;
+    g_nand_chip.mfr = 0;        /* Flash IDs - only one type of flash per device */
+    g_nand_chip.id = 0;
+    g_nand_chip.name = 0;
+    g_nand_chip.numchips = 0;
+    g_nand_chip.oobblock = 0;   /* Size of OOB blocks (e.g. 512) */
+    g_nand_chip.oobsize = 0;    /* Amount of OOB data per block (e.g. 16) */
+    g_nand_chip.eccsize = 0;
+    g_nand_chip.bus16 = 0;
+    g_nand_chip.nand_ecc_mode = 0;
+
+}
+
+static bool get_device_info(u8 *id, flashdev_info *devinfo)
+{
+    u32 i,m,n,mismatch;
+    int target=-1,target_id_len=-1;
+
+    for (i = 0; i<CHIP_CNT; i++) {
+        mismatch=0;
+        for (m=0; m<gen_FlashTable[i].id_length; m++) {
+            if (id[m]!=gen_FlashTable[i].id[m]) {
+                mismatch=1;
+                break;
+            }
+        }
+        if (mismatch == 0 && gen_FlashTable[i].id_length > target_id_len) {
+            target=i;
+            target_id_len=gen_FlashTable[i].id_length;
+        }
+    }
+
+    if (target != -1) {
+        dprintf(ALWAYS, "Recognize NAND: ID [");
+        for (n=0; n<gen_FlashTable[target].id_length; n++) {
+            dprintf(ALWAYS, "%x ", (unsigned char) id[n]);
+        }
+        dprintf(ALWAYS, "], Device Name [%s], Page Size [%d]B Spare Size [%d]B Total Size [%d]MB\n", \
+                gen_FlashTable[target].devciename,gen_FlashTable[target].pagesize,gen_FlashTable[target].sparesize,gen_FlashTable[target].totalsize);
+        devinfo->id_length=gen_FlashTable[i].id_length;
+        devinfo->blocksize = gen_FlashTable[target].blocksize;
+        devinfo->addr_cycle = gen_FlashTable[target].addr_cycle;
+        devinfo->iowidth = gen_FlashTable[target].iowidth;
+        devinfo->timmingsetting = gen_FlashTable[target].timmingsetting;
+        devinfo->advancedmode = gen_FlashTable[target].advancedmode;
+        devinfo->pagesize = gen_FlashTable[target].pagesize;
+        devinfo->sparesize = gen_FlashTable[target].sparesize;
+        devinfo->totalsize = gen_FlashTable[target].totalsize;
+        devinfo->sectorsize = gen_FlashTable[target].sectorsize;
+        devinfo->s_acccon= gen_FlashTable[target].s_acccon;
+        devinfo->s_acccon1= gen_FlashTable[target].s_acccon1;
+        devinfo->freq= gen_FlashTable[target].freq;
+        devinfo->vendor = gen_FlashTable[target].vendor;
+        gVendor = gen_FlashTable[target].vendor;
+        memcpy((u8 *)&devinfo->feature_set, (u8 *)&gen_FlashTable[target].feature_set, sizeof(struct MLC_feature_set));
+        memcpy(devinfo->devciename, gen_FlashTable[target].devciename, sizeof(devinfo->devciename));
+        return true;
+    } else {
+        dprintf(ALWAYS, "Not Found NAND: ID [");
+        for (n=0; n<NAND_MAX_ID; n++) {
+            dprintf(ALWAYS, "%x ",id[n]);
+        }
+        dprintf(ALWAYS, "]\n");
+        return false;
+    }
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_check_RW_count(u16 u2WriteSize)
+{
+    u32 timeout = 0xFFFF;
+    u16 u2SecNum = u2WriteSize >> g_nand_chip.sector_shift;
+    while (ADDRCNTR_CNTR(DRV_Reg32(NFI_ADDRCNTR_REG16)) < u2SecNum) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+    return TRUE;
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_status_ready(u32 u4Status)
+{
+    u32 timeout = 0xFFFF;
+    while ((DRV_Reg32(NFI_STA_REG32) & u4Status) != 0) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+    return TRUE;
+}
+
+//---------------------------------------------------------------------------
+static void mtk_nand_set_mode(u16 u2OpMode)
+{
+    u16 u2Mode = DRV_Reg16(NFI_CNFG_REG16);
+    u2Mode &= ~CNFG_OP_MODE_MASK;
+    u2Mode |= u2OpMode;
+    DRV_WriteReg16(NFI_CNFG_REG16, u2Mode);
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_set_command(u16 command)
+{
+    /* Write command to device */
+    DRV_WriteReg16(NFI_CMD_REG16, command);
+    return mtk_nand_status_ready(STA_CMD_STATE);
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_set_address(u32 u4ColAddr, u32 u4RowAddr, u16 u2ColNOB, u16 u2RowNOB)
+{
+    /* fill cycle addr */
+    DRV_WriteReg32(NFI_COLADDR_REG32, u4ColAddr);
+    DRV_WriteReg32(NFI_ROWADDR_REG32, u4RowAddr);
+    DRV_WriteReg16(NFI_ADDRNOB_REG16, u2ColNOB | (u2RowNOB << ADDR_ROW_NOB_SHIFT));
+    return mtk_nand_status_ready(STA_ADDR_STATE);
+}
+
+//---------------------------------------------------------------------------
+static void ECC_Decode_Start(void)
+{
+    /* wait for device returning idle */
+    while (!(DRV_Reg16(ECC_DECIDLE_REG16) & DEC_IDLE)) ;
+    DRV_WriteReg16(ECC_DECCON_REG16, DEC_EN);
+}
+
+//---------------------------------------------------------------------------
+static void ECC_Decode_End(void)
+{
+    /* wait for device returning idle */
+    while (!(DRV_Reg16(ECC_DECIDLE_REG16) & DEC_IDLE)) ;
+    DRV_WriteReg16(ECC_DECCON_REG16, DEC_DE);
+}
+
+//---------------------------------------------------------------------------
+static void ECC_Encode_Start(void)
+{
+    /* wait for device returning idle */
+    while (!(DRV_Reg32(ECC_ENCIDLE_REG32) & ENC_IDLE)) ;
+    DRV_WriteReg16(ECC_ENCCON_REG16, ENC_EN);
+}
+
+//---------------------------------------------------------------------------
+static void ECC_Encode_End(void)
+{
+    /* wait for device returning idle */
+    while (!(DRV_Reg32(ECC_ENCIDLE_REG32) & ENC_IDLE)) ;
+    DRV_WriteReg16(ECC_ENCCON_REG16, ENC_DE);
+}
+
+//---------------------------------------------------------------------------
+static void ECC_Config(u32 ecc_bit)
+{
+    u32 u4ENCODESize;
+    u32 u4DECODESize;
+
+    u32 ecc_bit_cfg = ECC_CNFG_ECC4;
+
+    switch (ecc_bit) {
+#ifdef MTK_COMBO_NAND_SUPPORT
+        case 4:
+            ecc_bit_cfg = ECC_CNFG_ECC4;
+            break;
+        case 8:
+            ecc_bit_cfg = ECC_CNFG_ECC8;
+            break;
+        case 10:
+            ecc_bit_cfg = ECC_CNFG_ECC10;
+            break;
+        case 12:
+            ecc_bit_cfg = ECC_CNFG_ECC12;
+            break;
+        case 14:
+            ecc_bit_cfg = ECC_CNFG_ECC14;
+            break;
+        case 16:
+            ecc_bit_cfg = ECC_CNFG_ECC16;
+            break;
+        case 18:
+            ecc_bit_cfg = ECC_CNFG_ECC18;
+            break;
+        case 20:
+            ecc_bit_cfg = ECC_CNFG_ECC20;
+            break;
+        case 22:
+            ecc_bit_cfg = ECC_CNFG_ECC22;
+            break;
+        case 24:
+            ecc_bit_cfg = ECC_CNFG_ECC24;
+            break;
+#endif
+        case 28:
+            ecc_bit_cfg = ECC_CNFG_ECC28;
+            break;
+        case 32:
+            ecc_bit_cfg = ECC_CNFG_ECC32;
+            break;
+        case 36:
+            ecc_bit_cfg = ECC_CNFG_ECC36;
+            break;
+        case 40:
+            ecc_bit_cfg = ECC_CNFG_ECC40;
+            break;
+        case 44:
+            ecc_bit_cfg = ECC_CNFG_ECC44;
+            break;
+        case 48:
+            ecc_bit_cfg = ECC_CNFG_ECC48;
+            break;
+        case 52:
+            ecc_bit_cfg = ECC_CNFG_ECC52;
+            break;
+        case 56:
+            ecc_bit_cfg = ECC_CNFG_ECC56;
+            break;
+        case 60:
+            ecc_bit_cfg = ECC_CNFG_ECC60;
+            break;
+        default:
+            break;
+
+    }
+
+    DRV_WriteReg16(ECC_DECCON_REG16, DEC_DE);
+    do {
+        ;
+    } while (!DRV_Reg16(ECC_DECIDLE_REG16));
+
+    DRV_WriteReg16(ECC_ENCCON_REG16, ENC_DE);
+    do {
+        ;
+    } while (!DRV_Reg32(ECC_ENCIDLE_REG32));
+
+    /* setup FDM register base */
+//    DRV_WriteReg32(ECC_FDMADDR_REG32, NFI_FDM0L_REG32);
+
+    u4ENCODESize = (g_nand_chip.sector_size + 8) << 3;
+    u4DECODESize = ((g_nand_chip.sector_size + 8) << 3) + ecc_bit * ECC_PARITY_BIT;
+
+    /* configure ECC decoder && encoder */
+    DRV_WriteReg32(ECC_DECCNFG_REG32, ecc_bit_cfg | DEC_CNFG_NFI | DEC_CNFG_EMPTY_EN | (u4DECODESize << DEC_CNFG_CODE_SHIFT));
+
+    DRV_WriteReg32(ECC_ENCCNFG_REG32, ecc_bit_cfg | ENC_CNFG_NFI | (u4ENCODESize << ENC_CNFG_MSG_SHIFT));
+
+#ifndef MANUAL_CORRECT
+    NFI_SET_REG32(ECC_DECCNFG_REG32, DEC_CNFG_CORRECT);
+#else
+    NFI_SET_REG32(ECC_DECCNFG_REG32, DEC_CNFG_EL);
+#endif
+    print("ECC_Config = %x\n", DRV_Reg32(ECC_DECCNFG_REG32));
+
+}
+
+/******************************************************************************
+* mtk_nand_check_bch_error
+*
+* DESCRIPTION:
+*   Check BCH error or not !
+*
+* PARAMETERS:
+*   struct mtd_info *mtd
+*    u8* pDataBuf
+*    u32 u4SecIndex
+*    u32 u4PageAddr
+*
+* RETURNS:
+*   None
+*
+* NOTES:
+*   None
+*
+******************************************************************************/
+static bool mtk_nand_check_bch_error(u8 *pDataBuf, u32 u4SecIndex, u32 u4PageAddr)
+{
+    bool bRet = TRUE;
+    u16 u2SectorDoneMask = 1 << u4SecIndex;
+    u32 u4ErrorNumDebug0, u4ErrorNumDebug1, i, u4ErrNum;
+    u32 timeout = 0xFFFF;
+
+#ifdef MANUAL_CORRECT
+    u32 au4ErrBitLoc[6];
+    u32 u4ErrByteLoc, u4BitOffset;
+    u32 u4ErrBitLoc1th, u4ErrBitLoc2nd;
+#endif
+
+    while (0 == (u2SectorDoneMask & DRV_Reg16(ECC_DECDONE_REG16))) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+#ifndef MANUAL_CORRECT
+    if (0 == (DRV_Reg32(NFI_STA_REG32) & STA_READ_EMPTY)) {
+        u4ErrorNumDebug0 = DRV_Reg32(ECC_DECENUM0_REG32);
+        u4ErrorNumDebug1 = DRV_Reg32(ECC_DECENUM1_REG32);
+
+        if (0 != (u4ErrorNumDebug0 & 0xFFFFFFFF) || 0 != (u4ErrorNumDebug1 & 0xFFFFFFFF)) {
+            for (i = 0; i <= u4SecIndex; ++i) {
+                u4ErrNum = (DRV_Reg32((ECC_DECENUM0_REG32+(i/4)))>>((i%4)*8))& ERR_NUM0;
+
+                if (ERR_NUM0 == u4ErrNum) {
+                    MSG(INIT, " UnCorrectable error Page=%d, Sector=%d\n", u4PageAddr, i);
+                    print( "PL UnCorrectable at Page=%d, Sector=%d\n", u4PageAddr, i);
+
+                    bRet = false;
+                }
+            }
+            if (false == bRet) {
+                if (0 != (DRV_Reg32(NFI_STA_REG32) & STA_READ_EMPTY)) {
+                    print( "Empty pg 0x%x\n", u4PageAddr);
+                    bRet = true;
+                }
+            }
+        }
+    }
+#else
+    /* We will manually correct the error bits in the last sector, not all the sectors of the page!*/
+    //memset(au4ErrBitLoc, 0x0, sizeof(au4ErrBitLoc));
+    u4ErrorNumDebug0 = DRV_Reg32(ECC_DECENUM0_REG32);
+    u4ErrNum = (DRV_Reg32((ECC_DECENUM0_REG32+(u4SecIndex/4)))>>((u4SecIndex%4)*8))& ERR_NUM0;
+
+    if (u4ErrNum) {
+        if (ERR_NUM0 == u4ErrNum) {
+            //mtd->ecc_stats.failed++;
+            bRet = FALSE;
+        } else {
+            for (i = 0; i < ((u4ErrNum + 1) >> 1); ++i) {
+                au4ErrBitLoc[i] = DRV_Reg32(ECC_DECEL0_REG32 + i);
+                u4ErrBitLoc1th = au4ErrBitLoc[i] & 0x3FFF;
+                if (u4ErrBitLoc1th < 0x2000) {
+                    u4ErrByteLoc = u4ErrBitLoc1th / 8;
+                    u4BitOffset = u4ErrBitLoc1th % 8;
+                    pDataBuf[u4ErrByteLoc] = pDataBuf[u4ErrByteLoc] ^ (1 << u4BitOffset);
+
+                    //mtd->ecc_stats.corrected++;
+                } else {
+                    //mtd->ecc_stats.failed++;
+                    MSG(INIT, "UnCorrectable ErrLoc=%d\n", au4ErrBitLoc[i]);
+                }
+                u4ErrBitLoc2nd = (au4ErrBitLoc[i] >> 16) & 0x3FFF;
+                if (0 != u4ErrBitLoc2nd) {
+                    if (u4ErrBitLoc2nd < 0x2000) {
+                        u4ErrByteLoc = u4ErrBitLoc2nd / 8;
+                        u4BitOffset = u4ErrBitLoc2nd % 8;
+                        pDataBuf[u4ErrByteLoc] = pDataBuf[u4ErrByteLoc] ^ (1 << u4BitOffset);
+                        //mtd->ecc_stats.corrected++;
+                    } else {
+                        //mtd->ecc_stats.failed++;
+                        MSG(INIT, "UnCorrectable High ErrLoc=%d\n", au4ErrBitLoc[i]);
+                    }
+                }
+            }
+        }
+        if (0 == (DRV_Reg16(ECC_DECFER_REG16) & (1 << u4SecIndex))) {
+            bRet = FALSE;
+        }
+    }
+#endif
+    return bRet;
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_RFIFOValidSize(u16 u2Size)
+{
+    u32 timeout = 0xFFFF;
+    while (FIFO_RD_REMAIN(DRV_Reg16(NFI_FIFOSTA_REG16)) < u2Size) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+    if (u2Size == 0) {
+        while (FIFO_RD_REMAIN(DRV_Reg16(NFI_FIFOSTA_REG16))) {
+            timeout--;
+            if (0 == timeout) {
+                return FALSE;
+            }
+        }
+    }
+    return TRUE;
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_WFIFOValidSize(u16 u2Size)
+{
+    u32 timeout = 0xFFFF;
+    while (FIFO_WR_REMAIN(DRV_Reg16(NFI_FIFOSTA_REG16)) > u2Size) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+    if (u2Size == 0) {
+        while (FIFO_WR_REMAIN(DRV_Reg16(NFI_FIFOSTA_REG16))) {
+            timeout--;
+            if (0 == timeout) {
+                return FALSE;
+            }
+        }
+    }
+    return TRUE;
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_reset(void)
+{
+    int timeout = 0xFFFF;
+    if (DRV_Reg16(NFI_MASTERSTA_REG16) & 0xFFF) { // master is busy
+        DRV_WriteReg32(NFI_CON_REG16, CON_FIFO_FLUSH | CON_NFI_RST);
+        while (DRV_Reg16(NFI_MASTERSTA_REG16) & 0xFFF) {
+            timeout--;
+            if (!timeout) {
+                print( "MASTERSTA timeout\n");
+            }
+        }
+    }
+    /* issue reset operation */
+    DRV_WriteReg32(NFI_CON_REG16, CON_FIFO_FLUSH | CON_NFI_RST);
+
+    return mtk_nand_status_ready(STA_NFI_FSM_MASK | STA_NAND_BUSY) && mtk_nand_RFIFOValidSize(0) && mtk_nand_WFIFOValidSize(0);
+}
+
+static bool mtk_nand_device_reset(void)
+{
+    u32 timeout = 0xFFFF;
+
+    mtk_nand_reset();
+
+    DRV_WriteReg(NFI_CNFG_REG16, CNFG_OP_RESET);
+
+    mtk_nand_set_command(NAND_CMD_RESET);
+
+    while (!(DRV_Reg32(NFI_STA_REG32) & STA_NAND_BUSY_RETURN) && (timeout--));
+
+    if (!timeout)
+        return FALSE;
+    else
+        return TRUE;
+}
+//---------------------------------------------------------------------------
+#if 0
+static bool mtk_nand_SetFeature(u16 cmd, u32 addr, u8 *value,  u8 bytes)
+{
+    kal_uint16           reg_val         = 0;
+    kal_uint8            write_count     = 0;
+    kal_uint32           timeout=TIMEOUT_3;//0xffff;
+
+    mtk_nand_reset();
+
+    reg_val |= (CNFG_OP_CUST | CNFG_BYTE_RW);
+    DRV_WriteReg(NFI_CNFG_REG16, reg_val);
+
+    mtk_nand_set_command(cmd);
+    mtk_nand_set_address(addr, 0, 1, 0);
+    //NFI_ISSUE_COMMAND(cmd, addr, 0, 1, 0)
+
+    //SAL_NFI_Config_Sector_Number(1);
+    DRV_WriteReg32(NFI_CON_REG16, 1 << CON_NFI_SEC_SHIFT);
+    NFI_SET_REG32(NFI_CON_REG16, CON_NFI_BWR);
+    DRV_WriteReg(NFI_STRDATA_REG16, 0x1);
+    //SAL_NFI_Start_Data_Transfer(KAL_FALSE, KAL_TRUE);
+    while ( (write_count < bytes) && timeout ) {
+        WAIT_NFI_PIO_READY(timeout)
+        if (timeout == 0) {
+            break;
+        }
+        DRV_WriteReg32(NFI_DATAW_REG32, *value++);
+        write_count++;
+        timeout = TIMEOUT_3;
+    }
+    while ( (DRV_Reg32(NFI_STA_REG32) & STA_NAND_BUSY) && (timeout) ) {timeout--;}
+    mtk_nand_read_status();
+
+    if (timeout != 0)
+        return TRUE;
+    else
+        return FALSE;
+}
+
+bool mtk_nand_GetFeature(u16 cmd, u32 addr, u8 *value,  u8 bytes)
+{
+    kal_uint16           reg_val         = 0;
+    kal_uint8            read_count     = 0;
+    kal_uint32           timeout=TIMEOUT_3;//0xffff;
+
+    mtk_nand_reset();
+
+    reg_val |= (CNFG_OP_CUST | CNFG_BYTE_RW | CNFG_READ_EN);
+    DRV_WriteReg(NFI_CNFG_REG16, reg_val);
+
+    mtk_nand_set_command(cmd);
+    mtk_nand_set_address(addr, 0, 1, 0);
+
+    //SAL_NFI_Config_Sector_Number(0);
+    DRV_WriteReg32(NFI_CON_REG16, 0 << CON_NFI_SEC_SHIFT);
+    reg_val = DRV_Reg32(NFI_CON_REG16);
+    reg_val &= ~CON_NOB_MASK;
+    reg_val |= ((4 << CON_NOB_SHIFT)|CON_NFI_SRD);
+    DRV_WriteReg32(NFI_CON_REG16, reg_val);
+    //NFI_SET_REG16(NFI_CON_REG16, CON_NFI_BWR);
+    DRV_WriteReg(NFI_STRDATA_REG16, 0x1);
+//  SAL_NFI_Start_Data_Transfer(KAL_TRUE, KAL_TRUE);
+    while ( (read_count < bytes) && timeout ) {
+        WAIT_NFI_PIO_READY(timeout)
+        if (timeout == 0) {
+            break;
+        }
+        *value++ = DRV_Reg32(NFI_DATAR_REG32);
+        read_count++;
+        timeout = TIMEOUT_3;
+    }
+    mtk_nand_read_status();
+    if (timeout != 0)
+        return TRUE;
+    else
+        return FALSE;
+
+}
+#endif
+
+static bool mtk_nand_read_status(void)
+{
+    int status = 0;
+    mtk_nand_reset();
+    unsigned int timeout;
+
+    mtk_nand_reset();
+
+    /* Disable HW ECC */
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+
+    /* Disable 16-bit I/O */
+    NFI_CLN_REG16(NFI_PAGEFMT_REG16, PAGEFMT_DBYTE_EN);
+    NFI_SET_REG16(NFI_CNFG_REG16, CNFG_OP_SRD | CNFG_READ_EN | CNFG_BYTE_RW);
+
+    DRV_WriteReg32(NFI_CON_REG16, CON_NFI_SRD | (1 << CON_NOB_SHIFT));
+
+    DRV_WriteReg32(NFI_CON_REG16, 0x3);
+    mtk_nand_set_mode(CNFG_OP_SRD);
+    DRV_WriteReg16(NFI_CNFG_REG16, 0x2042);
+    mtk_nand_set_command(NAND_CMD_STATUS);
+    DRV_WriteReg32(NFI_CON_REG16, 0x90);
+
+    timeout = TIMEOUT_4;
+    WAIT_NFI_PIO_READY(timeout);
+
+    if (timeout) {
+        status = (DRV_Reg16(NFI_DATAR_REG32));
+    }
+    //~  clear NOB
+    DRV_WriteReg32(NFI_CON_REG16, 0);
+
+    if (g_nand_chip.bus16 == NAND_BUS_WIDTH_16) {
+        NFI_SET_REG16(NFI_PAGEFMT_REG16, PAGEFMT_DBYTE_EN);
+        NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_BYTE_RW);
+    }
+    // check READY/BUSY status first
+    if (!(STATUS_READY & status)) {
+        print("status not ready\n");
+    }
+    // flash is ready now, check status code
+    if (STATUS_FAIL & status) {
+        if (!(STATUS_WR_ALLOW & status)) {
+            return FALSE;
+        } else {
+            return FALSE;
+        }
+    } else {
+    	//print("check status ok\n");
+        return TRUE;
+    }
+}
+
+//---------------------------------------------------------------------------
+
+static void mtk_nand_configure_fdm(u16 u2FDMSize)
+{
+    NFI_CLN_REG16(NFI_PAGEFMT_REG16, PAGEFMT_FDM_MASK | PAGEFMT_FDM_ECC_MASK);
+    NFI_SET_REG16(NFI_PAGEFMT_REG16, u2FDMSize << PAGEFMT_FDM_SHIFT);
+    NFI_SET_REG16(NFI_PAGEFMT_REG16, u2FDMSize << PAGEFMT_FDM_ECC_SHIFT);
+}
+
+//---------------------------------------------------------------------------
+static void mtk_nand_set_autoformat(bool bEnable)
+{
+    if (bEnable) {
+        NFI_SET_REG16(NFI_CNFG_REG16, CNFG_AUTO_FMT_EN);
+    } else {
+        NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_AUTO_FMT_EN);
+    }
+}
+
+//---------------------------------------------------------------------------
+static void mtk_nand_command_bp(unsigned command)
+{
+    switch (command) {
+        case NAND_CMD_READID:
+            /* Issue NAND chip reset command */
+            mtk_nand_device_reset();
+
+            mtk_nand_reset();
+
+            /* Disable HW ECC */
+            NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+
+            /* Disable 16-bit I/O */
+            NFI_CLN_REG16(NFI_PAGEFMT_REG16, PAGEFMT_DBYTE_EN);
+            NFI_SET_REG16(NFI_CNFG_REG16, CNFG_READ_EN | CNFG_BYTE_RW);
+            mtk_nand_reset();
+            mtk_nand_set_mode(CNFG_OP_SRD);
+            mtk_nand_set_command(NAND_CMD_READID);
+            mtk_nand_set_address(0, 0, 1, 0);
+            DRV_WriteReg32(NFI_CON_REG16, CON_NFI_SRD);
+            while (DRV_Reg32(NFI_STA_REG32) & STA_DATAR_STATE) ;
+            break;
+
+        default:
+            break;
+    }
+}
+
+//-----------------------------------------------------------------------------
+static u8 mtk_nand_read_byte(void)
+{
+    /* Check the PIO bit is ready or not */
+    u32 timeout = TIMEOUT_4;
+    WAIT_NFI_PIO_READY(timeout);
+    return DRV_Reg8(NFI_DATAR_REG32);
+}
+
+
+static bool getflashid(u8 *nand_id, int longest_id_number)
+{
+    u8 maf_id = 0;
+    u8 dev_id = 0;
+    int i = 0;
+    u8 *id = nand_id;
+    u32 regVal;
+    //PDN_Power_CONA_DOWN (PDN_PERI_NFI, FALSE);
+
+    DRV_WriteReg32(NFI_ACCCON_REG32, NFI_DEFAULT_ACCESS_TIMING);
+
+    DRV_WriteReg16(NFI_CNFG_REG16, 0);
+    regVal = DRV_Reg16(NFI_PAGEFMT_REG16) & 0x4;
+    DRV_WriteReg16(NFI_PAGEFMT_REG16, regVal);
+
+
+
+    mtk_nand_command_bp(NAND_CMD_READID);
+
+    maf_id = mtk_nand_read_byte();
+    dev_id = mtk_nand_read_byte();
+
+    if (maf_id == 0 || dev_id == 0) {
+        return FALSE;
+    }
+    //*id= (dev_id<<8)|maf_id;
+    //    *id= (maf_id<<8)|dev_id;
+    id[0] = maf_id;
+    id[1] = dev_id;
+
+    for (i = 2; i < longest_id_number; i++)
+        id[i] = mtk_nand_read_byte();
+
+    return TRUE;
+}
+
+
+
+/*******************************************************************************
+ * GPIO(PinMux) register definition
+ *******************************************************************************/
+#define EFUSE_GPIO_CFG  ((volatile u32 *)(0x102061c0))
+#define EFUSE_GPIO_1_8_ENABLE 0x00000008
+typedef enum GPIO_PIN {
+    GPIO_UNSUPPORTED = -1,
+    GPIO0 ,
+    GPIO1  , GPIO2  , GPIO3  , GPIO4  , GPIO5  , GPIO6  , GPIO7  ,
+    GPIO8  , GPIO9  , GPIO10 , GPIO11 , GPIO12 , GPIO13 , GPIO14 , GPIO15 ,
+    GPIO16 , GPIO17 , GPIO18 , GPIO19 , GPIO20 , GPIO21 , GPIO22 , GPIO23 ,
+    GPIO24 , GPIO25 , GPIO26 , GPIO27 , GPIO28 , GPIO29 , GPIO30 , GPIO31 ,
+    GPIO32 , GPIO33 , GPIO34 , GPIO35 , GPIO36 , GPIO37 , GPIO38 , GPIO39 ,
+    GPIO40 , GPIO41 , GPIO42 , GPIO43 , GPIO44 , GPIO45 , GPIO46 , GPIO47 ,
+    GPIO48 , GPIO49 , GPIO50 , GPIO51 , GPIO52 , GPIO53 , GPIO54 , GPIO55 ,
+    GPIO56 , GPIO57 , GPIO58 , GPIO59 , GPIO60 , GPIO61 , GPIO62 , GPIO63 ,
+    GPIO64 , GPIO65 , GPIO66 , GPIO67 , GPIO68 , GPIO69 , GPIO70 , GPIO71 ,
+    GPIO72 , GPIO73 , GPIO74 , GPIO75 , GPIO76 , GPIO77 , GPIO78 , GPIO79 ,
+    GPIO80 , GPIO81 , GPIO82 , GPIO83 , GPIO84 , GPIO85 , GPIO86 , GPIO87 ,
+    GPIO88 , GPIO89 , GPIO90 , GPIO91 , GPIO92 , GPIO93 , GPIO94 , GPIO95 ,
+    GPIO96 , GPIO97 , GPIO98 , GPIO99 , GPIO100, GPIO101, GPIO102, GPIO103,
+    GPIO104, GPIO105, GPIO106, GPIO107, GPIO108, GPIO109, GPIO110, GPIO111,
+    GPIO112, GPIO113, GPIO114, GPIO115, GPIO116, GPIO117, GPIO118, GPIO119,
+    GPIO120, GPIO121, GPIO122, GPIO123, GPIO124, GPIO125, GPIO126, GPIO127,
+    GPIO128, GPIO129, GPIO130, GPIO131, GPIO132, GPIO133, GPIO134, GPIO135,
+    GPIO136, GPIO137, GPIO138, GPIO139, GPIO140, GPIO141, GPIO142, GPIO143,
+    GPIO144, GPIO145, GPIO146, GPIO147, GPIO148, GPIO149, GPIO150, GPIO151,
+    GPIO152, GPIO153, GPIO154, GPIO155, GPIO156, GPIO157, GPIO158, GPIO159,
+    GPIO160, GPIO161, GPIO162, GPIO163, GPIO164, GPIO165, GPIO166, GPIO167,
+    GPIO168, GPIO169, GPIO170, GPIO171, GPIO172, GPIO173, GPIO174, GPIO175,
+    GPIO176, GPIO177, GPIO178, GPIO179, GPIO180, GPIO181, GPIO182, GPIO183,
+    GPIO184, GPIO185, GPIO186, GPIO187, GPIO188, GPIO189, GPIO190, GPIO191,
+    GPIO192, GPIO193, GPIO194, GPIO195, GPIO196, GPIO197, GPIO198, GPIO199,
+    GPIO200, GPIO201, GPIO202, GPIO203, GPIO204, GPIO205, GPIO206, GPIO207,
+    GPIO208, GPIO209, GPIO210, GPIO211, GPIO212, GPIO213, GPIO214, GPIO215,
+    GPIO216, GPIO217, GPIO218, GPIO219, GPIO220, GPIO221, GPIO222, GPIO223,
+    GPIO224, GPIO225, GPIO226, GPIO227, GPIO228, GPIO229, GPIO230, GPIO231,
+    GPIO232, GPIO233, GPIO234, GPIO235, GPIO236, GPIO237, GPIO238, GPIO239,
+    GPIO240, GPIO241, GPIO242, GPIO243, GPIO244, GPIO245, GPIO246, GPIO247,
+    GPIO248, GPIO249, GPIO250, GPIO251, GPIO252, GPIO253, GPIO254, GPIO255,
+    GPIO256, GPIO257, GPIO258, GPIO259, GPIO260, GPIO261, GPIO262, GPIO263,
+    GPIO264, GPIO265, GPIO266, GPIO267, GPIO268, GPIO269, GPIO270, GPIO271,
+    GPIO272, GPIO273, GPIO274, GPIO275, GPIO276, GPIO277, GPIO278, GPIO279,
+    MT_GPIO_BASE_MAX
+} GPIO_PIN;
+
+
+#if 0 /* use uffs should be OK */
+static unsigned short NFI_gpio_uffs(unsigned short x)
+{
+    unsigned int r = 1;
+
+    if (!x)
+        return 0;
+
+    if (!(x & 0xff)) {
+        x >>= 8;
+        r += 8;
+    }
+
+    if (!(x & 0xf)) {
+        x >>= 4;
+        r += 4;
+    }
+
+    if (!(x & 3)) {
+        x >>= 2;
+        r += 2;
+    }
+
+    if (!(x & 1)) {
+        x >>= 1;
+        r += 1;
+    }
+
+    return r;
+}
+#endif
+#define GPIO_WR32(addr, data)   __raw_writel(data, addr)
+#define MAX_GPIO_REG_BITS       (16)
+#define GPIO_MODE_BITS      (3)
+#define MAX_GPIO_MODE_PER_REG   (5)
+
+
+void mt_set_gpio_pull_enable(u32 pin, u32 enable)
+{
+    u32 pos;
+    u32 bit;
+    unsigned short *gpio_mode_p;
+
+    pos = pin / MAX_GPIO_REG_BITS;
+    bit = pin % MAX_GPIO_REG_BITS;
+
+    gpio_mode_p = (unsigned short *)( 0x10005150 + pos * 0x10);
+
+    if (enable == 0)
+        *gpio_mode_p &= ~(1L << bit);
+    else
+        *gpio_mode_p |= (1L << bit);
+}
+
+static void mt_set_gpio_pull_select(u32 pin, u32 select)
+{
+    u32 pos;
+    u32 bit;
+    unsigned short *gpio_mode_p;
+
+    pos = pin / MAX_GPIO_REG_BITS;
+    bit = pin % MAX_GPIO_REG_BITS;
+
+    gpio_mode_p = (unsigned short *)( 0x10005280 + pos * 0x10);
+    if (select == 0)
+        *gpio_mode_p &= ~(1L << bit);
+    else
+        *gpio_mode_p |= (1L << bit);
+}
+
+static void mt_set_gpio_mode(u32 pin, u32 mode)
+{
+    u32 pos;
+    u32 bit;
+    u32 reg;
+    u32 mask = (1L << GPIO_MODE_BITS) - 1;
+    unsigned short *gpio_mode_p;
+
+    pos = pin / MAX_GPIO_MODE_PER_REG;
+    bit = pin % MAX_GPIO_MODE_PER_REG;
+
+    gpio_mode_p = (unsigned short *)( 0x10005760 + pos * 0x10);
+
+    reg = (u32) *gpio_mode_p;
+
+    reg &= ~(mask << (GPIO_MODE_BITS*bit));
+    reg |= (mode << (GPIO_MODE_BITS*bit));
+
+    *gpio_mode_p = (unsigned short) reg;
+
+}
+
+
+
+static void NFI_GPIO_SET_FIELD(U32 reg, U32 field, U32 val)
+{
+    unsigned short tv = (unsigned short)(*(volatile uint16 *)(reg));
+    tv &= ~(field);
+    //tv |= ((val) << (NFI_gpio_uffs((unsigned short)(field)) - 1));
+    tv |= ((val) << (uffs((unsigned short)(field)) - 1));
+    (*(volatile uint16 *)(reg) = (uint16)(tv));
+}
+#define   EFUSE_Is_IO_33V()    (((*EFUSE_GPIO_CFG)&EFUSE_GPIO_1_8_ENABLE)?FALSE:TRUE) // 0 : 3.3v (MT8130 default), 
+
+static void mtk_nand_gpio_init(void)
+{
+    u32 value;
+    U16 ori_gpio, offset;
+    P_U16 gpio_mode_p;
+    u8 is33v;
+    u32 val;
+
+    NFI_GPIO_SET_FIELD(0x10005cc0,   0x700, 0x2);    //pullup with 50Kohm   ----PAD_MSDC0_CLK for 1.8v/3.3v
+    NFI_GPIO_SET_FIELD(0x10005cd0,   0x700, 0x3);   //pulldown with 50Kohm ----PAD_MSDC0_CMD for 1.8v/3.3v
+
+    mt_set_gpio_mode(GPIO43, 1);    //Switch NCLE to non-default mode
+    mt_set_gpio_mode(GPIO44, 1);    //Switch NCEB1 to non-default mode
+    mt_set_gpio_mode(GPIO45, 1);    //Switch NCEB0 to non-default mode
+    mt_set_gpio_mode(GPIO47, 1);    //Switch NREB to non-default mode
+    mt_set_gpio_mode(GPIO48, 1);   //Switch NRNB to non-default mode
+
+    mt_set_gpio_mode(GPIO111, 4);     //Switch NLD7 to non-default mode
+    mt_set_gpio_mode(GPIO112, 4);     //Switch NLD6 to non-default mode
+    mt_set_gpio_mode(GPIO113, 4);     //Switch NLD5 to non-default mode
+    mt_set_gpio_mode(GPIO114, 4);     //Switch NLD4 to non-default mode
+    mt_set_gpio_mode(GPIO115, 4);     //Switch NLD8 to non-default mode
+    mt_set_gpio_mode(GPIO116, 4);     //Switch NALE to non-default mode
+    mt_set_gpio_mode(GPIO117, 4);     //Switch NWEB to non-default mode
+    mt_set_gpio_mode(GPIO118, 4);     //Switch NLD3 to non-default mode
+    mt_set_gpio_mode(GPIO119, 4);     //Switch NLD2 to non-default mode
+    mt_set_gpio_mode(GPIO120, 4);     //Switch NLD1 to non-default mode
+    mt_set_gpio_mode(GPIO121, 4);     //Switch NLD0 to non-default mode
+
+    mt_set_gpio_pull_enable(GPIO48, 1);
+    mt_set_gpio_pull_select(GPIO48, 1);
+
+    is33v = EFUSE_Is_IO_33V();
+    val = is33v ? 0x0c : 0x00;
+
+    // is 3.3v is true for 8521p
+
+    NFI_GPIO_SET_FIELD(0x10005e20, 0xf,   0x0a);    /* TDSEL change value to 0x0a*/
+    NFI_GPIO_SET_FIELD(0x10005e20, 0x3f0, val);     /* RDSEL change value to 0x0c*/
+
+    NFI_GPIO_SET_FIELD(0x10005e30, 0xf,   0x0a);    /* TDSEL change value to 0x0a*/
+    NFI_GPIO_SET_FIELD(0x10005e30, 0x3f0, val); /* RDSEL change value to 0x0c*/
+
+    NFI_GPIO_SET_FIELD(0x10005d20, 0xf,   0x0a);    /* TDSEL change value to 0x0a*/
+    NFI_GPIO_SET_FIELD(0x10005d20, 0x3f0,val);      /* RDSEL change value to val*/
+
+    if (is33v)
+        NFI_GPIO_SET_FIELD(0x10005eb0, 0x000f,  0x5);
+
+    NFI_GPIO_SET_FIELD(0x10005cc0, 0x7, 0x3);
+    NFI_GPIO_SET_FIELD(0x10005cd0, 0x7, 0x3);
+    NFI_GPIO_SET_FIELD(0x10005ce0, 0x7, 0x3);
+    NFI_GPIO_SET_FIELD(0x10005f70, 0x7000, 0x3);   //set driving more than 4mA
+    NFI_GPIO_SET_FIELD(0x10005f80, 0x7, 0x3);  //set driving more than 4mA
+
+#if 0
+    dprintf(ALWAYS, "%x %x\n", 0x10005180, (*(volatile unsigned short *)( 0x10005180)));
+    dprintf(ALWAYS, "%x %x\n", 0x100052b0, (*(volatile unsigned short *)( 0x100052b0)));
+    dprintf(ALWAYS, "%x %x\n", 0x100057e0, (*(volatile unsigned short *)( 0x100057e0)));
+    dprintf(ALWAYS, "%x %x\n", 0x100057f0, (*(volatile unsigned short *)( 0x100057f0)));
+    dprintf(ALWAYS, "%x %x\n", 0x100058c0, (*(volatile unsigned short *)( 0x100058c0)));
+    dprintf(ALWAYS, "%x %x\n", 0x100058d0, (*(volatile unsigned short *)( 0x100058d0)));
+    dprintf(ALWAYS, "%x %x\n", 0x100058e0, (*(volatile unsigned short *)( 0x100058e0)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005e20, (*(volatile unsigned short *)( 0x10005e20)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005e30, (*(volatile unsigned short *)( 0x10005e30)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005d20, (*(volatile unsigned short *)( 0x10005d20)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005eb0, (*(volatile unsigned short *)( 0x10005eb0)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005cc0, (*(volatile unsigned short *)( 0x10005cc0)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005cd0, (*(volatile unsigned short *)( 0x10005cd0)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005ce0, (*(volatile unsigned short *)( 0x10005ce0)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005f70, (*(volatile unsigned short *)( 0x10005f70)));
+    dprintf(ALWAYS, "%x %x\n", 0x10005f80, (*(volatile unsigned short *)( 0x10005f80)));
+#endif
+}
+
+
+#define mtk_nand_cs_check(id, cs)  (1)
+#define mtk_nand_cs_on(cs, page)   (page)
+
+static int mtk_nand_init(void)
+{
+    int i, j, busw;
+    u8 id[NAND_MAX_ID];
+    u16 spare_bit = 0;
+
+    u16 spare_per_sector = 16;
+    u32 ecc_bit = 4;
+    // Config pin mux for NAND device
+
+    // remove GPIO init for now
+    mtk_nand_gpio_init();
+
+    nfi_buf = (unsigned char *)NAND_NFI_BUFFER;
+
+    //memset(&devinfo, 0, sizeof(devinfo)); // redundant
+
+    /* Dynamic Control */
+    g_bInitDone = FALSE;
+#ifndef REDUCE_NAND_PL_SIZE
+    g_kCMD.u4OOBRowAddr = (u32) - 1;
+#endif
+#if CFG_FPGA_PLATFORM       // FPGA NAND is placed at CS1
+    DRV_WriteReg16(NFI_CSEL_REG16, 0);
+#else
+    DRV_WriteReg16(NFI_CSEL_REG16, NFI_DEFAULT_CS);
+#endif
+
+
+    /* Set default NFI access timing control */
+    DRV_WriteReg32(NFI_ACCCON_REG32, NFI_DEFAULT_ACCESS_TIMING);
+
+    DRV_WriteReg16(NFI_CNFG_REG16, 0);
+    DRV_WriteReg16(NFI_PAGEFMT_REG16, 4);
+
+    /* Reset NFI HW internal state machine and flush NFI in/out FIFO */
+    mtk_nand_reset();
+
+    /* Read the first 4 byte to identify the NAND device */
+
+    g_nand_chip.page_shift = NAND_LARGE_PAGE;
+    g_nand_chip.page_size = 1 << g_nand_chip.page_shift;
+    g_nand_chip.oobblock = NAND_PAGE_SIZE;
+    g_nand_chip.oobsize = NAND_BLOCK_BLKS;
+
+    g_nand_chip.nand_ecc_mode = NAND_ECC_HW;
+
+    mtk_nand_command_bp(NAND_CMD_READID);
+
+    for (i=0; i<NAND_MAX_ID; i++) {
+        id[i]=mtk_nand_read_byte ();
+    }
+    nand_maf_id = id[0];
+    nand_dev_id = id[1];
+    memset(&devinfo, 0, sizeof(devinfo));
+
+    if (!get_device_info(id, &devinfo)) {
+        dprintf(ALWAYS, "NAND unsupport\n");
+        ASSERT(0);
+    }
+    dprintf(ALWAYS, "NAND support!!!\n");
+    g_nand_chip.name = devinfo.devciename;
+    g_nand_chip.chipsize = ((u64)devinfo.totalsize) << 20;
+    g_nand_chip.page_size = devinfo.pagesize;
+    g_nand_chip.page_shift = uffs(g_nand_chip.page_size) - 1;
+    g_nand_chip.oobblock = g_nand_chip.page_size;
+    {
+        g_nand_chip.erasesize = devinfo.blocksize << 10;
+    }
+    dprintf(ALWAYS, "%dMB\n", (u32)(g_nand_chip.chipsize/1024/1024));
+    dprintf(ALWAYS, " erasesize: %x,blocksize :%x\n",g_nand_chip.erasesize,devinfo.blocksize );
+    //BLOCK_SIZE = (u32) devinfo.blocksize << 10;
+    //PAGE_SIZE = (u32) devinfo.pagesize;
+    g_nand_chip.phys_erase_shift = uffs(g_nand_chip.erasesize) - 1;
+    g_nand_chip.sector_size = NAND_SECTOR_SIZE;
+    g_nand_chip.sector_shift= 9;
+    if (devinfo.sectorsize == 1024) {
+        g_nand_chip.sector_size = 1024;
+        g_nand_chip.sector_shift= 10;
+        NFI_CLN_REG32(NFI_PAGEFMT_REG16, PAGEFMT_SECTOR_SEL);
+    }
+
+    g_nand_chip.bus16 = devinfo.iowidth;
+#if 0
+    if (devinfo.vendor == VEND_MICRON) {
+        if (devinfo.feature_set.FeatureSet.Async_timing.feature != 0xFF) {
+            struct gFeatureSet *feature_set = &(devinfo.feature_set.FeatureSet);
+            mtk_nand_SetFeature((u16) feature_set->sfeatureCmd, \
+                                feature_set->Async_timing.address, (u8 *)&feature_set->Async_timing.feature,\
+                                sizeof(feature_set->Async_timing.feature));
+        }
+    }
+#endif
+    DRV_WriteReg32(NFI_ACCCON_REG32, devinfo.timmingsetting);
+    //MSG(INIT, "[NAND]Timing: 0x%X\n", DRV_Reg32(NFI_ACCCON_REG32));
+    if (!devinfo.sparesize)
+        g_nand_chip.oobsize = (8 << ((ext_id2 >> 2) & 0x01)) * (g_nand_chip.oobblock / g_nand_chip.sector_size);
+    else
+        g_nand_chip.oobsize = devinfo.sparesize;
+    spare_per_sector = g_nand_chip.oobsize / (g_nand_chip.page_size / g_nand_chip.sector_size);
+
+    MSG(INIT, "[NAND] g_nand_chip.oobsize: %d  ,g_nand_chip.sector_size %d \n", g_nand_chip.oobsize,g_nand_chip.sector_size);
+    MSG(INIT, "[NAND] spare_per_sector: %d  , devinfo.sparesize %d ,g_nand_chip.page_size %d \n", spare_per_sector,devinfo.sparesize,g_nand_chip.page_size);
+
+
+    switch (spare_per_sector) {
+#ifdef MTK_COMBO_NAND_SUPPORT
+        case 16:
+            spare_bit = PAGEFMT_SPARE_16;
+            ecc_bit = 4;
+            spare_per_sector = 16;
+            break;
+        case 26:
+        case 27:
+        case 28:
+            spare_bit = PAGEFMT_SPARE_26;
+            ecc_bit = 10;
+            spare_per_sector = 26;
+            break;
+        case 32:
+            ecc_bit = 12;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_32_1KS;
+            else
+                spare_bit = PAGEFMT_SPARE_32;
+            spare_per_sector = 32;
+            break;
+        case 40:
+            ecc_bit = 18;
+            spare_bit = PAGEFMT_SPARE_40;
+            spare_per_sector = 40;
+            break;
+        case 44:
+            ecc_bit = 20;
+            spare_bit = PAGEFMT_SPARE_44;
+            spare_per_sector = 44;
+            break;
+        case 48:
+        case 49:
+            ecc_bit = 22;
+            spare_bit = PAGEFMT_SPARE_48;
+            spare_per_sector = 48;
+            break;
+        case 50:
+        case 51:
+            ecc_bit = 24;
+            spare_bit = PAGEFMT_SPARE_50;
+            spare_per_sector = 50;
+            break;
+        case 52:
+        case 54:
+        case 56:
+            ecc_bit = 24;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_52_1KS;
+            else
+                spare_bit = PAGEFMT_SPARE_52;
+            spare_per_sector = 52;
+            break;
+#endif
+        case 62:
+        case 63:
+            ecc_bit = 28;
+            spare_bit = PAGEFMT_SPARE_62;
+            spare_per_sector = 62;
+            break;
+        case 64:
+            ecc_bit = 32;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_64_1KS;
+            else
+                spare_bit = PAGEFMT_SPARE_64;
+            spare_per_sector = 64;
+            break;
+        case 72:
+            ecc_bit = 36;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_72_1KS;
+            spare_per_sector = 72;
+            break;
+        case 80:
+            ecc_bit = 40;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_80_1KS;
+            spare_per_sector = 80;
+            break;
+        case 88:
+            ecc_bit = 44;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_88_1KS;
+            spare_per_sector = 88;
+            break;
+        case 96:
+        case 98:
+            ecc_bit = 48;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_96_1KS;
+            spare_per_sector = 96;
+            break;
+        case 100:
+        case 102:
+        case 104:
+            ecc_bit = 52;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_100_1KS;
+            spare_per_sector = 100;
+            break;
+        case 124:
+        case 126:
+        case 128:
+            ecc_bit = 60;
+            if (devinfo.sectorsize == 1024)
+                spare_bit = PAGEFMT_SPARE_124_1KS;
+            spare_per_sector = 124;
+            break;
+        default:
+            MSG(INIT, "[NAND]: NFI not support oobsize: %x\n", spare_per_sector);
+            ASSERT(0);
+    }
+
+    dprintf(ALWAYS, "ecc bits = %d %d %d\n", ecc_bit, spare_bit, spare_per_sector);
+
+    g_nand_chip.oobsize = spare_per_sector * (g_nand_chip.page_size / g_nand_chip.sector_size);
+    print( "[NAND]: sector_size:%x ,erasesize: %x\n", g_nand_chip.sector_size,g_nand_chip.erasesize);
+    print( "[NAND]: page_size:%x ,oobsize: %x\n", g_nand_chip.page_size,g_nand_chip.oobsize);
+
+    if (g_nand_chip.bus16 == NAND_BUS_WIDTH_16) {
+#ifdef  DBG_PRELOADER
+        MSG(INIT, "USE 16 IO\n");
+#endif
+        NFI_SET_REG16(NFI_PAGEFMT_REG16, PAGEFMT_DBYTE_EN);
+    }
+
+    if (16384 == g_nand_chip.oobblock) {
+        NFI_SET_REG16(NFI_PAGEFMT_REG16, (spare_bit << PAGEFMT_SPARE_SHIFT) | PAGEFMT_16K_1KS);
+        nand_oob = &nand_oob_128;
+    } else if (8192 == g_nand_chip.oobblock) {
+        NFI_SET_REG16(NFI_PAGEFMT_REG16, (spare_bit << PAGEFMT_SPARE_SHIFT) | PAGEFMT_8K_1KS);
+        nand_oob = &nand_oob_128;
+    }
+#ifndef REDUCE_NAND_PL_SIZE
+    else if (4096 == g_nand_chip.oobblock) {
+        if (devinfo.sectorsize == 512)
+            NFI_SET_REG16(NFI_PAGEFMT_REG16, (spare_bit << PAGEFMT_SPARE_SHIFT) | PAGEFMT_4K);
+        else
+            NFI_SET_REG16(NFI_PAGEFMT_REG16, (spare_bit << PAGEFMT_SPARE_SHIFT) | PAGEFMT_4K_1KS);
+        nand_oob = &nand_oob_128;
+    } else if (2048 == g_nand_chip.oobblock) {
+        if (devinfo.sectorsize == 512)
+            NFI_SET_REG16(NFI_PAGEFMT_REG16, (spare_bit << PAGEFMT_SPARE_SHIFT) | PAGEFMT_2K);
+        else
+            NFI_SET_REG16(NFI_PAGEFMT_REG16, (spare_bit << PAGEFMT_SPARE_SHIFT) | PAGEFMT_2K_1KS);
+        nand_oob = &nand_oob_64;
+    }
+#endif
+
+    if (g_nand_chip.nand_ecc_mode == NAND_ECC_HW) {
+        // MSG (INIT, "Use HW ECC\n");
+        NFI_SET_REG32(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+        ECC_Config(ecc_bit);
+        mtk_nand_configure_fdm(8);
+    }
+
+    /* Initilize interrupt. Clear interrupt, read clear. */
+    DRV_Reg16(NFI_INTR_REG16);
+
+    /* Interrupt arise when read data or program data to/from AHB is done. */
+    DRV_WriteReg16(NFI_INTR_EN_REG16, 0);
+
+    //g_nand_chip.chipsize -= (g_nand_chip.sector_size == 512?g_nand_chip.erasesize : g_nand_chip.erasesize*2) * (PMT_POOL_SIZE);
+    //g_nand_chip.chipsize -= (g_nand_chip.erasesize) * (PMT_POOL_SIZE);
+
+    mutex_init(&nand_mutex_lock);
+    //set default timing
+    nand_set_timing();
+
+    return 0;
+}
+
+//-----------------------------------------------------------------------------
+static void mtk_nand_stop_read(void)
+{
+
+    NFI_CLN_REG32(NFI_CON_REG16, CON_NFI_BRD);
+    if (g_bHwEcc) {
+        ECC_Decode_End();
+    }
+}
+
+//-----------------------------------------------------------------------------
+static void mtk_nand_stop_write(void)
+{
+    NFI_CLN_REG32(NFI_CON_REG16, CON_NFI_BWR);
+    if (g_bHwEcc) {
+        ECC_Encode_End();
+    }
+}
+
+//-----------------------------------------------------------------------------
+static bool mtk_nand_check_dececc_done(u32 u4SecNum)
+{
+    u32 timeout, dec_mask;
+    timeout = 0xffffff;
+    //dec_mask = (1 << u4SecNum) - 1;
+    dec_mask = (1 << (u4SecNum - 1));
+    //while ((dec_mask != DRV_Reg(ECC_DECDONE_REG16)) && timeout > 0)
+
+    while (dec_mask != (DRV_Reg(ECC_DECDONE_REG16) & dec_mask)) {
+        if (timeout == 0) {
+            MSG(INIT, "ECC_DEC timeout 0x%x %d\n",DRV_Reg(ECC_DECDONE_REG16),u4SecNum);
+            return false;
+        }
+        timeout--;
+    }
+    timeout = 0xffffff;
+    while ((DRV_Reg32(ECC_DECFSM_REG32)&(0x7f0f0f0f)) != ECC_DECFSM_IDLE) {
+        if (timeout == 0) {
+            MSG(INIT, "ECC_DEC timeout 0x%x %d\n",DRV_Reg(ECC_DECDONE_REG16),u4SecNum);
+            return false;
+        }
+        timeout--;
+    }
+    return true;
+}
+
+#ifdef MANUAL_CORRECT
+void ECC_correct_sec0(u8 *buf, u32 page)
+{
+    if (DRV_Reg32(ECC_DECENUM0_REG32) & 0x3f)
+        mtk_nand_check_bch_error(buf, 0, page);
+}
+
+void ECC_correct_sec1(u8 *buf, u32 page)
+{
+    if (DRV_Reg32(ECC_DECENUM0_REG32) & 0x3f00)
+        mtk_nand_check_bch_error(buf, 1, page);
+}
+#endif
+//-----------------------------------------------------------------------------
+#ifdef MANUAL_CORRECT
+static bool mtk_nand_read_page_data(u32 *buf, u32 page)
+#else
+static bool mtk_nand_read_page_data(u32 *buf)
+#endif
+{
+    u32 timeout = 0xFFFF;
+    u32 u4Size = g_nand_chip.oobblock;
+    u32 i;
+    u32 *pBuf32;
+
+#if (USE_AHB_MODE)
+    pBuf32 = (u32 *) buf;
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_BYTE_RW);
+
+    DRV_Reg16(NFI_INTR_REG16);
+    DRV_WriteReg16(NFI_INTR_EN_REG16, INTR_AHB_DONE_EN);
+    NFI_SET_REG32(NFI_CON_REG16, CON_NFI_BRD);
+
+    while (!(DRV_Reg16(NFI_INTR_REG16) & INTR_AHB_DONE)) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+
+    timeout = 0xFFFF;
+    while ((u4Size >> g_nand_chip.sector_shift) > ((DRV_Reg32(NFI_BYTELEN_REG16) & 0x1f000) >> 12)) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+
+#else
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_BYTE_RW);
+    NFI_SET_REG32(NFI_CON_REG16, CON_NFI_BRD);
+    pBuf32 = (u32 *) buf;
+
+    for (i = 0; (i < (u4Size >> 2)) && (timeout > 0);) {
+        if (DRV_Reg16(NFI_PIO_DIRDY_REG16) & 1) {
+                *pBuf32++ = DRV_Reg32(NFI_DATAR_REG32);
+                i++;
+                if (i == 0x120) {
+                    ECC_correct_sec0(buf, page);
+                }
+            } else {
+                timeout--;
+            }
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+#endif
+    return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+static bool mtk_nand_write_page_data(u32 *buf)
+{
+    u32 timeout = 0xFFFF;
+    u32 u4Size = g_nand_chip.oobblock;
+
+#if (USE_AHB_MODE)
+    u32 *pBuf32;
+    pBuf32 = (u32 *) buf;
+
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_BYTE_RW);
+
+    DRV_Reg16(NFI_INTR_REG16);
+    DRV_WriteReg16(NFI_INTR_EN_REG16, INTR_AHB_DONE_EN);
+    NFI_SET_REG32(NFI_CON_REG16, CON_NFI_BWR);
+    while (!(DRV_Reg16(NFI_INTR_REG16) & INTR_AHB_DONE)) {
+        timeout--;
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+
+#else
+    u32 i;
+    u32 *pBuf32;
+    pBuf32 = (u32 *) buf;
+
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_BYTE_RW);
+    NFI_SET_REG32(NFI_CON_REG16, CON_NFI_BWR);
+
+    for (i = 0; (i < (u4Size >> 2)) && (timeout > 0);) {
+        if (DRV_Reg16(NFI_PIO_DIRDY_REG16) & 1) {
+            DRV_WriteReg32(NFI_DATAW_REG32, *pBuf32++);
+            i++;
+        } else {
+            timeout--;
+        }
+
+        if (0 == timeout) {
+            return FALSE;
+        }
+    }
+#endif
+    return TRUE;
+}
+
+//-----------------------------------------------------------------------------
+static void mtk_nand_read_fdm_data(u32 u4SecNum, u8 *spare_buf)
+{
+    u32 i;
+    u32 *pBuf32 = (u32 *) spare_buf;
+
+    for (i = 0; i < u4SecNum; ++i) {
+        *pBuf32++ = DRV_Reg32(NFI_FDM0L_REG32 + (i << 3));
+        *pBuf32++ = DRV_Reg32(NFI_FDM0M_REG32 + (i << 3));
+    }
+}
+
+//-----------------------------------------------------------------------------
+static void mtk_nand_write_fdm_data(u32 u4SecNum, u8 *oob)
+{
+    u32 i;
+    u32 *pBuf32 = (u32 *) oob;
+
+    for (i = 0; i < u4SecNum; ++i) {
+        DRV_WriteReg32(NFI_FDM0L_REG32 + (i << 3), *pBuf32++);
+        DRV_WriteReg32(NFI_FDM0M_REG32 + (i << 3), *pBuf32++);
+    }
+}
+
+//---------------------------------------------------------------------------
+static bool mtk_nand_ready_for_read(u32 page_addr, u32 sec_num, u8 *buf)
+{
+    u32 u4RowAddr = page_addr;
+    u32 colnob = 2;
+    u32 rownob = devinfo.addr_cycle - colnob;
+    bool bRet = FALSE;
+
+    if (!mtk_nand_reset()) {
+        goto cleanup;
+    }
+    //enable ecc decoder
+    *ECC_DECCNFG_REG32 |= (0x20 | 0x3000);
+    *ECC_DECCON_REG16 = 0;
+    *ECC_DECCON_REG16 = 0x1;
+
+
+    /* Enable HW ECC */
+    NFI_SET_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+
+    mtk_nand_set_mode(CNFG_OP_READ);
+    NFI_SET_REG16(NFI_CNFG_REG16, CNFG_READ_EN);
+    DRV_WriteReg32(NFI_CON_REG16, sec_num << CON_NFI_SEC_SHIFT);
+
+#if USE_AHB_MODE
+    NFI_SET_REG16(NFI_CNFG_REG16, CNFG_AHB|CNFG_BURST_EN);
+#else
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_AHB);
+#endif
+    DRV_WriteReg32(NFI_STRADDR_REG32, buf);
+    if (g_bHwEcc) {
+        NFI_SET_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+    } else {
+        NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+    }
+
+    mtk_nand_set_autoformat(TRUE);
+    if (g_bHwEcc) {
+        ECC_Decode_Start();
+    }
+    if (!mtk_nand_set_command(NAND_CMD_READ0)) {
+        goto cleanup;
+    }
+    if (!mtk_nand_set_address(0, u4RowAddr, colnob, rownob)) {
+        goto cleanup;
+    }
+
+    if (!mtk_nand_set_command(NAND_CMD_READSTART)) {
+        goto cleanup;
+    }
+
+    if (!mtk_nand_status_ready(STA_NAND_BUSY)) {
+        goto cleanup;
+    }
+
+    bRet = TRUE;
+
+cleanup:
+    return bRet;
+}
+
+//-----------------------------------------------------------------------------
+static bool mtk_nand_ready_for_write(u32 page_addr, u32 sec_num, u8 *buf)
+{
+    bool bRet = FALSE;
+    u32 u4RowAddr = page_addr;
+    u32 colnob = 2;
+    u32 rownob = devinfo.addr_cycle - colnob;
+
+    if (!mtk_nand_reset()) {
+        return FALSE;
+    }
+
+    mtk_nand_set_mode(CNFG_OP_PRGM);
+
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_READ_EN);
+
+    DRV_WriteReg32(NFI_CON_REG16, sec_num << CON_NFI_SEC_SHIFT);
+
+#if USE_AHB_MODE
+    NFI_SET_REG16(NFI_CNFG_REG16, CNFG_AHB|CNFG_BURST_EN);
+    DRV_WriteReg32(NFI_STRADDR_REG32, buf);
+#else
+    NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_AHB);
+#endif
+
+    if (g_bHwEcc) {
+        NFI_SET_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+    } else {
+        NFI_CLN_REG16(NFI_CNFG_REG16, CNFG_HW_ECC_EN);
+    }
+    mtk_nand_set_autoformat(TRUE);
+    if (g_bHwEcc) {
+        ECC_Encode_Start();
+    }
+
+    if (!mtk_nand_set_command(NAND_CMD_SEQIN)) {
+        goto cleanup;
+    }
+
+    if (!mtk_nand_set_address(0, u4RowAddr, colnob, rownob)) {
+        goto cleanup;
+    }
+
+    if (!mtk_nand_status_ready(STA_NAND_BUSY)) {
+        goto cleanup;
+    }
+
+    bRet = TRUE;
+cleanup:
+
+    return bRet;
+}
+
+/*now using partition erase, it will affect the erase behavior*/
+static int mtk_nand_partition_erase(void)
+{
+    return 1;
+}
+
+//#############################################################################
+//# NAND Driver : Page Read
+//#
+//# NAND Page Format (Large Page 2KB)
+//#  |------ Page:2048 Bytes ----->>||---- Spare:64 Bytes -->>|
+//#
+//# Parameter Description:
+//#     page_addr               : specify the starting page in NAND flash
+//#
+//#############################################################################
+static int mtk_nand_read_page_hwecc(u64 logical_addr, char *buf)
+{
+    int i, start, len, offset = 0;
+    u32 page_no;
+    u32 block, mapped_block;
+    int rtn = ERR_RTN_SUCCESS;
+    u8 *oob = (u8 *)buf + g_nand_chip.page_size;
+
+    page_no = mtk_nand_page_transform(logical_addr,&block,&mapped_block);
+    rtn = mtk_nand_read_page_hw(page_no, buf, g_nand_spare);
+    if (rtn != ERR_RTN_SUCCESS) { // g_nand_spare
+        return ERR_RTN_FAIL;
+    }
+#if 0
+    for (i = 0; i < MTD_MAX_OOBFREE_ENTRIES && nand_oob->oobfree[i].length; i++) {
+        /* Set the reserved bytes to 0xff */
+        start = nand_oob->oobfree[i].offset;
+        len = nand_oob->oobfree[i].length;
+        memcpy(oob + offset, g_nand_spare + start, len);
+        offset += len;
+    }
+#endif
+    return ERR_RTN_SUCCESS;
+}
+
+static int mtk_nand_read_page_hw(u32 page, u8 *dat, u8 *oob)
+{
+    int bRet = ERR_RTN_SUCCESS;
+    u8 *pPageBuf;
+    u32 u4SecNum = g_nand_chip.oobblock >> g_nand_chip.sector_shift;
+    pPageBuf = (u8 *) dat;
+    bool readRetry = FALSE;
+    int retryCount = 0;
+    int i=0;
+
+    do {
+        if (mtk_nand_ready_for_read(page, u4SecNum, pPageBuf)) {
+#ifdef MANUAL_CORRECT
+            if (!mtk_nand_read_page_data((u32 *) pPageBuf, page)) {
+                bRet = ERR_RTN_FAIL;
+            }
+#else
+            if (!mtk_nand_read_page_data((u32 *) pPageBuf)) {
+                bRet = ERR_RTN_FAIL;
+            }
+
+#endif
+            if (!mtk_nand_status_ready(STA_NAND_BUSY)) {
+                bRet = ERR_RTN_FAIL;
+            }
+            if (g_bHwEcc) {
+                if (!mtk_nand_check_dececc_done(u4SecNum)) {
+                    bRet = ERR_RTN_FAIL;
+                }
+            }
+            mtk_nand_read_fdm_data(u4SecNum, oob);
+	    ECC_correct_sec1(pPageBuf + (1 << g_nand_chip.sector_shift), page);
+#ifndef MANUAL_CORRECT
+            if (g_bHwEcc) {
+                if (!mtk_nand_check_bch_error(pPageBuf, u4SecNum - 1, page)) {
+                    print("bch error page=%x\n", page);
+                    if (devinfo.vendor != VEND_NONE) {
+                        readRetry = TRUE;
+                    }
+                    bRet = ERR_RTN_BCH_FAIL;
+                }
+            }
+#endif
+            if (0 != (DRV_Reg32(NFI_STA_REG32) & STA_READ_EMPTY)) {
+                if (retryCount != 0) {
+                    MSG(INIT, "RR empty page return uncorrectable!\n");
+                    bRet = ERR_RTN_BCH_FAIL;
+                } else {
+                    memset(pPageBuf, 0xFF,g_nand_chip.page_size);
+                    memset(oob, 0xFF,8*u4SecNum);
+                    readRetry = FALSE;
+                    bRet = ERR_RTN_SUCCESS;
+                }
+            }
+            mtk_nand_stop_read();
+
+        }
+
+        if (bRet == ERR_RTN_BCH_FAIL) {
+            if (retryCount < devinfo.feature_set.FeatureSet.readRetryCnt) {
+                retryCount++;
+            } else {
+                readRetry = FALSE;
+            }
+        } else {
+            if (retryCount != 0) {
+                u32 feature = devinfo.feature_set.FeatureSet.readRetryDefault;
+            }
+            readRetry = FALSE;
+        }
+        if (TRUE == readRetry)
+            bRet = ERR_RTN_SUCCESS;
+    } while (readRetry);
+    return bRet;
+}
+
+//#############################################################################
+//# NAND Driver : Page Write
+//#
+//# NAND Page Format (Large Page 2KB)
+//#  |------ Page:2048 Bytes ----->>||---- Spare:64 Bytes -->>|
+//#
+//# Parameter Description:
+//#     page_addr               : specify the starting page in NAND flash
+//#
+//#############################################################################
+
+static int mtk_nand_write_page_hwecc(u64 logical_addr, char *buf)
+{
+    u32 block,mapped_block;
+    u32 page_no;
+    u8 *oob = (u8 *) buf + g_nand_chip.oobblock;
+    int i;
+    int start, len, offset;
+    static int print_cnt = 0;
+
+    page_no = mtk_nand_page_transform(logical_addr ,&block, &mapped_block);
+    NAND_PRINT_PROCESS();
+
+
+    for (i = 0; i < sizeof(g_nand_spare); i++)
+        *(g_nand_spare + i) = 0xFF;
+#if 0
+    offset = 0;
+    for (i = 0; i < MTD_MAX_OOBFREE_ENTRIES && nand_oob->oobfree[i].length; i++) {
+        /* Set the reserved bytes to 0xff */
+        start = nand_oob->oobfree[i].offset;
+        len = nand_oob->oobfree[i].length;
+        memcpy((g_nand_spare + start), (oob + offset), len);
+        offset += len;
+    }
+#endif
+
+    // write bad index into oob
+    if (mapped_block != block) {
+        set_bad_index_to_oob(g_nand_spare, block);
+    } else {
+        set_bad_index_to_oob(g_nand_spare, FAKE_INDEX);
+    }
+
+    if (!mtk_nand_write_page_hw(page_no, buf, g_nand_spare)) {
+        print("write fail @ block 0x%x, page 0x%x\n", mapped_block, page_no);
+        return ERR_RTN_FAIL;
+    }
+
+    return ERR_RTN_SUCCESS;
+}
+
+static int mtk_nand_write_page_hw(u32 page, u8 *dat, u8 *oob)
+{
+    bool bRet = TRUE;
+    u32 pagesz = g_nand_chip.oobblock;
+    u32 timeout, u4SecNum = pagesz >> g_nand_chip.sector_shift;
+
+    int i, j, start, len;
+    bool empty = TRUE;
+    u8 oob_checksum = 0;
+
+    for (i = 0; i < MTD_MAX_OOBFREE_ENTRIES && nand_oob->oobfree[i].length; i++) {
+        /* Set the reserved bytes to 0xff */
+        start = nand_oob->oobfree[i].offset;
+        len = nand_oob->oobfree[i].length;
+        for (j = 0; j < len; j++) {
+            oob_checksum ^= oob[start + j];
+            if (oob[start + j] != 0xFF)
+                empty = FALSE;
+        }
+    }
+
+    if (!empty) {
+        oob[nand_oob->oobfree[i - 1].offset + nand_oob->oobfree[i - 1].length] = oob_checksum;
+    }
+
+    while (DRV_Reg32(NFI_STA_REG32) & STA_NAND_BUSY) ;
+    MSG(INIT, "Do write  page 0x%x ==",page);
+    for (i=0; i<8; i++)
+        MSG(INIT, "0x%x ",dat[i*1024+1]);
+    if (mtk_nand_ready_for_write(page, u4SecNum, dat)) {
+        mtk_nand_write_fdm_data(u4SecNum, oob);
+        if (!mtk_nand_write_page_data((u32 *) dat)) {
+            bRet = FALSE;
+        }
+        if (!mtk_nand_check_RW_count(g_nand_chip.oobblock)) {
+            bRet = FALSE;
+        }
+        mtk_nand_stop_write();
+        mtk_nand_set_command(NAND_CMD_PAGEPROG);
+        mtk_nand_status_ready(STA_NAND_BUSY);
+        return mtk_nand_read_status();
+    } else {
+
+        return FALSE;
+    }
+
+    return bRet;
+}
+
+static u64 nand_block_bad(u64 logical_addr)
+{
+    int block = (int)(logical_addr/BLOCK_SIZE);
+    int mapped_block;
+    mtk_nand_page_transform(logical_addr,&block,&mapped_block);
+    if (nand_block_bad_hw((u64)mapped_block * BLOCK_SIZE)) {
+        print("NAND skip bad block %d @ offset 0x%llx\n",mapped_block, logical_addr);
+        return (logical_addr + (u64)BLOCK_SIZE);
+    }
+
+    return logical_addr;
+}
+
+static bool nand_block_bad_hw(u64 logical_addr)
+{
+    bool bRet = FALSE;
+    u32 page = (u32)(logical_addr / g_nand_chip.oobblock);
+
+    int page_num = (BLOCK_SIZE / g_nand_chip.oobblock);
+    //unsigned char *pspare;
+    char *tmp = (char *)nfi_buf;
+    memset(tmp, 0x0, g_nand_chip.oobblock + g_nand_chip.oobsize);
+    memset(g_nand_spare, 0x0, 128);
+
+    u32 u4SecNum = g_nand_chip.oobblock >> g_nand_chip.sector_shift;
+    page &= ~(page_num - 1);
+    //page_no = mtk_nand_page_transform(logical_addr,&block,&mapped_block);
+    if (mtk_nand_ready_for_read(page, u4SecNum, tmp)) {
+#ifdef MANUAL_CORRECT
+        if (!mtk_nand_read_page_data((u32 *) tmp, page)) {
+            bRet = FALSE;
+        }
+#else
+        if (!mtk_nand_read_page_data((u32 *) tmp)) {
+            bRet = FALSE;
+        }
+#endif
+
+        if (!mtk_nand_status_ready(STA_NAND_BUSY)) {
+            bRet = FALSE;
+        }
+
+        if (!mtk_nand_check_dececc_done(u4SecNum)) {
+            bRet = FALSE;
+        }
+
+        mtk_nand_read_fdm_data(u4SecNum, g_nand_spare);
+	ECC_correct_sec1(tmp + (1 << g_nand_chip.sector_shift), page);
+
+#ifndef MANUAL_CORRECT
+        if (!mtk_nand_check_bch_error((u8 *)tmp, u4SecNum - 1, page)) {
+            MSG(ERASE, "check bch error !\n");
+            bRet = FALSE;
+        }
+#endif
+        if (0 != (DRV_Reg32(NFI_STA_REG32) & STA_READ_EMPTY)) {
+            memset(nfi_buf, 0xFF,g_nand_chip.page_size);
+            memset(g_nand_spare, 0xFF,8*u4SecNum);
+            bRet = FALSE;
+        }
+
+        mtk_nand_stop_read();
+    }
+    if (g_nand_spare[0] != 0xFF || g_nand_spare[8] != 0xFF ) {
+        MSG(ERASE, "check bch error in SLC !\n");
+        bRet = TRUE;
+        // break;
+    }
+    return bRet;
+}
+
+static bool mark_block_bad(u64 logical_addr)
+{
+    u32 block;
+    u32 mapped_block;
+    mtk_nand_page_transform(logical_addr,&block,&mapped_block);
+    return mark_block_bad_hw((u64)mapped_block * BLOCK_SIZE);
+}
+
+static bool mark_block_bad_hw(u64 offset)
+{
+    bool bRet = FALSE;
+    u32 index;
+    u32 page_addr = (u32)(offset / g_nand_chip.oobblock);
+    u32 u4SecNum = g_nand_chip.oobblock >> g_nand_chip.sector_shift;
+    unsigned char *pspare;
+    int i, page_num = (BLOCK_SIZE/ g_nand_chip.oobblock);
+    unsigned char *buf = nand_nfi_buf;
+    //memset(buf, 0x00, STORAGE_BUFFER_SIZE);
+    memset(buf, 0x00, NAND_NFI_BUFFER_SIZE);
+
+    for (index = 0; index < 64; index++)
+        *(g_nand_spare + index) = 0xFF;
+
+    pspare = g_nand_spare;
+
+    for (index = 8, i = 0; i < 4; i++)
+        pspare[i * index] = 0x0;
+
+    page_addr &= ~(page_num - 1);
+
+    MSG(BAD, "Mark bad block 0x%x\n", page_addr);
+    while (DRV_Reg32(NFI_STA_REG32) & STA_NAND_BUSY) ;
+
+    if (mtk_nand_ready_for_write(page_addr, u4SecNum, buf)) {
+        mtk_nand_write_fdm_data(u4SecNum, pspare);
+        if (!mtk_nand_write_page_data((u32 *) & buf)) {
+            bRet = FALSE;
+        }
+        if (!mtk_nand_check_RW_count(g_nand_chip.oobblock)) {
+            bRet = FALSE;
+        }
+        mtk_nand_stop_write();
+        mtk_nand_set_command(NAND_CMD_PAGEPROG);
+        mtk_nand_status_ready(STA_NAND_BUSY);
+    } else {
+        return FALSE;
+    }
+    for (index = 0; index < 64; index++)
+        *(pspare + index) = 0xFF;
+    return bRet;
+}
+
+//#############################################################################
+//# NAND Driver : Page Write
+//#
+//# NAND Page Format (Large Page 2KB)
+//#  |------ Page:2048 Bytes ----->>||---- Spare:64 Bytes -->>|
+//#
+//# Parameter Description:
+//#     page_addr               : specify the starting page in NAND flash
+//#
+//#############################################################################
+static bool mtk_nand_erase_hw(u64 offset)
+{
+    bool bRet = TRUE;
+    u32 rownob = devinfo.addr_cycle - 2;
+    u32 page_addr = (u32)(offset / g_nand_chip.oobblock);
+
+    MSG(INIT, "mtk_nand_erase_hw : page_addr: %x ,g_nand_chip.oobblock :%x \n",page_addr,g_nand_chip.oobblock);
+
+    if (nand_block_bad_hw(offset)) {
+    /* TODO: bad block handle */
+        //return FALSE;
+    }
+
+    mtk_nand_reset();
+    mtk_nand_set_mode(CNFG_OP_ERASE);
+    mtk_nand_set_command(NAND_CMD_ERASE1);
+    mtk_nand_set_address(0, page_addr, 0, rownob);
+
+    mtk_nand_set_command(NAND_CMD_ERASE2);
+    if (!mtk_nand_status_ready(STA_NAND_BUSY)) {
+        return FALSE;
+    }
+
+    if (!mtk_nand_read_status()) {
+        return FALSE;
+    }
+    return bRet;
+}
+
+static int mtk_nand_erase(u64 logical_addr)
+{
+    u32 block;
+    u32 mapped_block;
+
+#if NAND_MARK_BAD_BLOCK_TEST
+    //test bad block
+    u32 block_no = mtk_nand_offset_to_block(logical_addr);
+    if (//(block_no == 7)  || //MBR
+        (block_no == 12) || //LK1
+        (block_no == 20) || //TEE1
+        (block_no == 100)  //BOOTIMG
+        //|| (block_no == 37) //UBOOT1
+        ) {
+        return FALSE;
+    }
+#endif
+    mtk_nand_page_transform((u64)logical_addr,&block,&mapped_block);
+
+    if (!mtk_nand_erase_hw((u64)mapped_block * BLOCK_SIZE)) {
+        print( "erase block 0x%x failed\n", mapped_block);
+        return FALSE;
+    }
+
+    return TRUE;
+}
+
+static bool mtk_nand_wait_for_finish(void)
+{
+    while (DRV_Reg32(NFI_STA_REG32) & STA_NAND_BUSY) ;
+    return TRUE;
+}
+
+/**************************************************************************
+*  MACRO LIKE FUNCTION
+**************************************************************************/
+static int nand_read(struct bdev *bdev, void *_buf, off_t offset, size_t len)
+{
+    u8 *buf = (u8 *)_buf;
+    ssize_t bytes_read = 0;
+    bnum_t block;
+    int err = 0;
+    size_t block_offset = 0;
+    size_t tocopy = 0;
+    uint32_t num_blocks = 0;
+
+    // If the device requires alignment AND our buffer is not alread aligned.
+    bool requires_alignment =
+    (bdev->flags & BIO_FLAG_CACHE_ALIGNED_READS) &&
+    (IS_ALIGNED((size_t)buf, CACHE_LINE) == false);
+
+    //in LK, the access unit is page size, check it
+    ASSERT(bdev->block_size == PAGE_SIZE);
+
+
+    /*specially after write MBR,uplayer would read some data but not start from partition beginning,So we  add patch here*/
+    if (mtk_nand_offset_to_block(offset) <= MTD_MBR_PARTITION_BLOCK_ID) {
+#if NFI_BAD_BLOCK_DBG_INFO
+        print("input offset block %d, force take it as MBR patition(%d)\n",
+        mtk_nand_offset_to_block(offset),MTD_MBR_PARTITION_BLOCK_ID);
+#endif
+
+        u8g_partition_start_ofst = 0;
+        mtk_nand_search_bad_block(offset);
+    } else {
+        u8g_partition_start_ofst = offset;
+        u4g_partition_bad_num = 0;
+    }
+
+
+    /* find the starting page */
+    block = offset / PAGE_SIZE;
+
+    print( "nand_read:buf %p, offset 0x%llx, page No. %d, Block No. [%d ~ %d],len %d\n",
+    	  buf, offset, block, mtk_nand_offset_to_block(offset),mtk_nand_offset_to_block(offset+len),len);
+    /* handle partial first block */
+    if ((offset % PAGE_SIZE) != 0) {
+        NAND_CHECK_ALLOC_MEMORY(pAlignBuf);
+        /* read in the block */
+        err = nand_bread(bdev, pAlignBuf, block, 1);
+        if (err < 0) {
+        goto ERR_HANDLE;
+    } else if ((size_t)err != PAGE_SIZE) {
+        err = ERR_IO;
+        goto ERR_HANDLE;
+    }
+        /* copy what we need */
+        block_offset = offset % PAGE_SIZE;
+        tocopy = MIN(PAGE_SIZE - block_offset, len);
+        memcpy(buf, pAlignBuf + block_offset, tocopy);
+
+        /* increment our buffers */
+        buf += tocopy;
+        len -= tocopy;
+        bytes_read += tocopy;
+        block++;
+    }
+
+
+    /* handle middle blocks */
+    if (requires_alignment) {
+        while (len >= PAGE_SIZE) {
+            NAND_CHECK_ALLOC_MEMORY(pAlignBuf);
+            /* do the middle reads */
+            err = nand_bread(bdev, pAlignBuf, block, 1);
+            if (err < 0) {
+                goto ERR_HANDLE;
+            } else if ((size_t)err != PAGE_SIZE) {
+                err = ERR_IO;
+                goto ERR_HANDLE;
+            }
+            memcpy(buf, pAlignBuf, PAGE_SIZE);
+
+            buf += bdev->block_size;
+            len -= bdev->block_size;
+            bytes_read += bdev->block_size;
+            block++;
+        }
+    } else {
+        //some case len will less than 1 page
+        //if (len >= PAGE_SIZE)
+        num_blocks = divpow2(len, bdev->block_shift);
+#if NFI_BAD_BLOCK_DBG_INFO
+        print("do bread, page count %d\n",num_blocks);
+#endif
+        err = nand_bread(bdev, buf, block, num_blocks);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != PAGE_SIZE * num_blocks) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+
+        buf += err;
+        len -= err;
+        bytes_read += err;
+        block += num_blocks;
+    }
+
+    /* handle partial last block */
+    if (len > 0) {
+        /* read the block */
+#if NFI_BAD_BLOCK_DBG_INFO
+        print("partial read, len 0x%x\n",(int)len);
+#endif
+        NAND_CHECK_ALLOC_MEMORY(pAlignBuf);
+        err = nand_bread(bdev, pAlignBuf, block, 1);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != PAGE_SIZE) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+
+        /* copy the partial block from our temp buffer */
+        memcpy(buf, pAlignBuf, len);
+
+        bytes_read += len;
+    }
+
+    ERR_HANDLE:
+    NAND_FREE_MEMORY(pAlignBuf);
+    print("nand read finish ret 0x%x bytes_read 0x%x ,bad num %d\n",err,bytes_read,u4g_partition_bad_num);
+    /* return error or bytes read */
+    return (err >= 0) ? bytes_read : err;
+}
+
+
+static int nand_write(struct bdev *bdev, const void *_buf, off_t offset, size_t len)
+{
+    const u8 *buf = (const u8 *)_buf;
+    ssize_t bytes_written = 0;
+    bnum_t block;
+    int err = 0;
+    // If the device requires alignment AND our buffer is not alread aligned.
+    bool requires_alignment =
+        (bdev->flags & BIO_FLAG_CACHE_ALIGNED_WRITES) &&
+        (IS_ALIGNED((size_t)buf, CACHE_LINE) == false);
+
+    //in LK, the access unit is page size, check it
+    ASSERT(bdev->block_size == PAGE_SIZE);
+
+    u4g_partition_bad_num = 0;
+
+    /* find the starting block */
+    block = offset / bdev->block_size;
+
+    print("nand_write:buf %p offset %lld, block [%d ~ %d], len %zd\n", buf, offset, mtk_nand_offset_to_block(offset),mtk_nand_offset_to_block(offset + len), len);
+
+    /* handle partial first block */
+    if ((offset % bdev->block_size) != 0) {
+        NAND_CHECK_ALLOC_MEMORY(pAlignBuf);
+        /* read in the block */
+        err = nand_bread(bdev, pAlignBuf, block, 1);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != bdev->block_size) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+
+        /* copy what we need */
+        size_t block_offset = offset % bdev->block_size;
+        size_t tocopy = MIN(bdev->block_size - block_offset, len);
+        memcpy(pAlignBuf + block_offset, buf, tocopy);
+
+        /* write it back out */
+        //err = bio_write_block(bdev, pAlignBuf, block, 1);
+        err = nand_bwrite(bdev, pAlignBuf, block, 1);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != bdev->block_size) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+
+        /* increment our buffers */
+        buf += tocopy;
+        len -= tocopy;
+        bytes_written += tocopy;
+        block++;
+    }
+
+
+    #if NFI_BAD_BLOCK_DBG_INFO
+    print("requires_alignment  %d\n",requires_alignment);
+    #endif
+
+    /* handle middle blocks */
+    if (requires_alignment) {
+        while (len >= bdev->block_size) {
+            NAND_CHECK_ALLOC_MEMORY(pAlignBuf);
+            /* do the middle reads */
+            memcpy(pAlignBuf, buf, bdev->block_size);
+            err = nand_bwrite(bdev, pAlignBuf, block, 1);
+            if (err < 0) {
+                goto ERR_HANDLE;
+            } else if ((size_t)err != bdev->block_size) {
+                err = ERR_IO;
+                goto ERR_HANDLE;
+            }
+
+            buf += bdev->block_size;
+            len -= bdev->block_size;
+            bytes_written += bdev->block_size;
+            block++;
+        }
+    } else {
+        uint32_t block_count = divpow2(len, bdev->block_shift);
+
+#if NFI_BAD_BLOCK_DBG_INFO
+        print("write block cnt %d @line %d\n",block_count,__LINE__);
+#endif
+
+        err = nand_bwrite(bdev, buf, block, block_count);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != bdev->block_size * block_count) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+
+        ASSERT((size_t)err == (block_count * bdev->block_size));
+
+        buf += err;
+        len -= err;
+        bytes_written += err;
+        block += block_count;
+    }
+
+    /* handle partial last block */
+    if (len > 0) {
+#if NFI_BAD_BLOCK_DBG_INFO
+        print("partial write %p, page No. %d, len %d\n", buf, block, len);
+#endif
+        NAND_CHECK_ALLOC_MEMORY(pAlignBuf);
+        /* read the block */
+        err = nand_bread(bdev, pAlignBuf, block, 1);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != bdev->block_size) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+
+        /* copy the partial block from our temp buffer */
+        memcpy(pAlignBuf, buf, len);
+
+        /* write it back out */
+        err = nand_bwrite(bdev, pAlignBuf, block, 1);
+        if (err < 0) {
+            goto ERR_HANDLE;
+        } else if ((size_t)err != bdev->block_size) {
+            err = ERR_IO;
+            goto ERR_HANDLE;
+        }
+        bytes_written += len;
+    }
+
+    ERR_HANDLE:
+    NAND_FREE_MEMORY(pAlignBuf);
+        
+    print("\nnand write finish ret 0x%x ,bad num %d\n",err,u4g_partition_bad_num);
+    /* return error or bytes written */
+    return (err >= 0) ? bytes_written : err;
+}
+
+
+/*blknr: start block number
+*  blks:  page number
+   bdev->block_size :  it's page size actually
+*/
+static int nand_bread(bdev_t *bdev, void *buf, u32 blknr, u32 blks)
+{
+    u32 i;
+    u64 offset = (u64)blknr * bdev->block_size;
+    u32 u4TotalSz = 0;
+
+    nand_get_device();
+
+    for (i = 0; i < blks; i++) {
+        if (nand_read_page_data(buf, offset)) {
+            return ERR_RTN_FAIL;
+        } else {
+            offset += bdev->block_size;
+            buf += bdev->block_size;
+            u4TotalSz += bdev->block_size;
+        }
+    }
+
+    nand_release_device();
+
+    /*
+    for (i=0; i<100 ; i++)
+        MSG(INIT, "0x%x ",*(ptr+i));
+    MSG(INIT, "printf buf over in nand_bread  \n");
+    */
+
+    return u4TotalSz;
+}
+
+static int nand_bwrite(bdev_t *bdev, void *buf, u32 blknr, u32 blks)
+{
+    u32 i, j;
+    u64 offset = (u64)blknr * bdev->block_size;
+    u32 u4TotalSz = 0;
+
+    nand_get_device();
+
+    for (i = 0; i < blks; i++) {
+        if (ERR_RTN_FAIL == nand_write_page_data(buf, offset)) {
+            return ERR_RTN_FAIL;
+        } else {
+            offset += bdev->block_size;
+            buf += bdev->block_size;
+            u4TotalSz += bdev->block_size;
+        }
+    }
+
+    nand_release_device();
+
+    return u4TotalSz;
+}
+
+static int nand_erase(bdev_t *dev, off_t offset, size_t len)
+{
+    u64 off = (u64) offset;
+    u64 offset_limit = (u64)dev->total_size;
+    u64 size = (u64) len;
+
+    nand_get_device();
+
+    nand_erase_data(off, offset_limit, size);
+
+    nand_release_device();
+
+    return len;
+}
+
+
+
+// ==========================================================
+// NAND Common Interface - Init
+// ==========================================================
+
+u32 nand_init_device(void)
+{
+    struct bdev *nand_bdev;
+
+    mtk_nand_reset_descriptor();
+    mtk_nand_init();
+
+    dprintf(ALWAYS,"\n");
+    nand_bdev = (struct bdev *)malloc(sizeof(struct bdev));
+    ASSERT(nand_bdev);
+    memset(nand_bdev, 0, sizeof(struct bdev));
+
+    bio_initialize_bdev(nand_bdev, "nand0", g_nand_chip.page_size, (g_nand_chip.chipsize / g_nand_chip.page_size), 0, NULL, BIO_FLAGS_NONE);
+    dprintf(ALWAYS,"nand_bdev->total_size = %x\n", (unsigned int) nand_bdev->total_size);
+    dprintf(ALWAYS,"nand_bdev->block_size = %x\n", nand_bdev->block_size);
+    dprintf(ALWAYS,"nand_bdev->block_shift = %x\n", nand_bdev->block_shift);
+    dprintf(ALWAYS,"nand_bdev->block_count = %x\n", nand_bdev->block_count);
+
+    nand_bdev->read = nand_read;
+    nand_bdev->read_block = NULL;//nand_bread;
+    nand_bdev->write = nand_write;
+    nand_bdev->write_block = NULL;//nand_bwrite;
+    nand_bdev->erase = nand_erase;
+    nand_bdev->erase_byte = 0xff;
+    bio_register_device(nand_bdev);
+    dprintf(ALWAYS,"NAND register bdev done\n ");
+    partition_publish("nand0", 0x0);
+    //
+#ifdef NAND_TEST
+    {
+        bdev_t *n0;
+        ssize_t rt;
+        u8 *buf = (u8 *)0x95000000;
+        int i, check_fail=0;
+        n0 = bio_open("nand0");
+        if (n0 == NULL) printf("nand open fail\n");
+        else printf("nand open ok\n");
+
+        rt = bio_erase(n0, 0x2000000, 0x100000);
+        printf("nand erase rt:0x%x\n", (u32)rt);
+        for (i = 0; i < 0x100000; i++)
+            buf[i] = (u8) (i & 0xff);
+        rt = bio_write(n0, buf, 0x2000000, 0x100000);
+        printf("nand write rt:0x%x\n", (u32)rt);
+        memset(buf, 0x0, 0x100000);
+        rt = bio_read(n0, buf, 0x2000000, 0x100000);
+        for (i = 0; i < 0x100000; i++) {
+            if (buf[i] != (u8) (i & 0xff))
+                check_fail=1;
+        }
+        printf("nand read rt:0x%x %x %x %x %x, check_fail=%d\n", (u32)rt, buf[0], buf[1], buf[2], buf[3], check_fail);
+
+
+        bio_close(n0);
+    }
+#endif
+
+    return 0;
+}
+
+
+static u32 nand_get_device_id(u8 *id, u32 len)
+{
+    u8 buf[16];
+
+    if (TRUE != getflashid(buf, len))
+        return -1;
+
+    len = len > 16 ? 16 : len;
+
+    memcpy(id, buf, len);
+
+    return 0;
+}
+
+/* LEGACY - TO BE REMOVED { */
+// ==========================================================
+// NAND Common Interface - Correct R/W Address
+// ==========================================================
+static u64 nand_find_safe_block(u64 offset)
+{
+    u64 u8new_offset = 0;
+    //the offset should be block aligned,otherwise ,should be use aligned buffer ?
+    if ((u32)offset & (PAGE_SIZE - 1)) {
+        print("Warning! the offset is not block align..%s @ line %d\n",__FUNCTION__,__LINE__);
+        ASSERT(0);
+    }
+
+    /* new_offset is block alignment, in current case, not need care u64
+    *  search until we found a good block
+    */
+    while ((u8new_offset = nand_block_bad(offset)) != offset)
+    //if ((u8new_offset = nand_block_bad(offset)) != offset)
+    {
+        offset = u8new_offset;
+        u4g_partition_bad_num ++;
+    }
+
+    return u8new_offset;
+}
+
+/* LEGACY - TO BE REMOVED } */
+
+// ==========================================================
+// NAND Common Interface - Read Function
+// ==========================================================
+static int nand_read_page_data(u8 *buf, u64 offset)
+{
+#if NFI_BAD_BLOCK_DBG_INFO
+    u64 u8old_ofst =  offset;
+#endif
+    u64 u8new_ofst =  offset;
+
+
+	//update the start offset
+	u8new_ofst += (u4g_partition_bad_num * BLOCK_SIZE);
+
+	// make sure the block is safe to flash
+	u8new_ofst = nand_find_safe_block(u8new_ofst);
+
+	//check the adjust offset not overflow current partition
+	NAND_CHECK_OFST_LIMITATION(u8new_ofst);
+
+
+#if NFI_BAD_BLOCK_DBG_INFO
+    if (u8old_ofst != u8new_ofst)
+        print("nand read ofst update from block %d to %d ,bad num %d\n",
+            mtk_nand_offset_to_block(u8old_ofst), mtk_nand_offset_to_block(u8new_ofst),u4g_partition_bad_num);
+
+#endif
+
+    if (mtk_nand_read_page_hwecc(u8new_ofst, (char *)buf)) {
+        print("nand_read_page_data fail at ofst 0x%x block %d ,line %d\n", (u32)u8new_ofst,mtk_nand_offset_to_block(u8new_ofst),__LINE__);
+        return ERR_RTN_FAIL;
+    }
+
+
+    return ERR_RTN_SUCCESS;
+}
+
+// ==========================================================
+// NAND Common Interface - Write Function
+// ==========================================================
+static int nand_write_page_data(u8 *buf, u64 offset)
+{
+#if NFI_BAD_BLOCK_DBG_INFO
+    u64 u8old_ofst =  offset;
+#endif
+    u32 u4skip_num = 0;
+
+nand_write_skip:
+
+    //check : it's problem if there  are many skip block behavior
+    NAND_CHECK_WRITE_SKIP(u4skip_num);
+
+    //update the start offset, write offset also start from partition beginning?
+    offset += (u4g_partition_bad_num * BLOCK_SIZE);
+
+    // make sure the block is safe to flash
+    offset = nand_find_safe_block(offset);
+
+    //check the adjust offset not overflow current partition
+    NAND_CHECK_OFST_LIMITATION(offset);
+
+#if NFI_BAD_BLOCK_DBG_INFO
+    if (u8old_ofst != offset)
+        print("nand write ofst update from block %d to %d ,bad num %d\n",
+            mtk_nand_offset_to_block(u8old_ofst), mtk_nand_offset_to_block(offset),u4g_partition_bad_num);
+
+#endif
+
+    if (mtk_nand_write_page_hwecc(offset, (char *)buf)) {
+        mark_block_bad(offset);
+        //if we need jump to next block to continue write
+        //offset += BLOCK_SIZE;
+        u4g_partition_bad_num++;
+        u4skip_num++;
+
+        print("nand_write_page_data fail,mark bad block@0x%x ,write skip %d\n",(u32)offset,u4skip_num);
+        goto nand_write_skip;
+
+    }
+
+    return ERR_RTN_SUCCESS;
+}
+
+
+// ==========================================================
+// NAND Common Interface - Erase Function
+// ==========================================================
+static int nand_erase_data(u64 offset, u64 offset_limit, u64 size)
+{
+
+    u64 img_size = size;
+    //u32 tpgsz;
+    u32 tblksz;
+    u64 cur_offset;
+    u64 old_offset;
+    u32 u4Start_Block = 0;
+    u32 u4End_Block = 0;
+
+    u4g_partition_bad_num = 0;
+
+    // do block alignment check
+    if (offset % BLOCK_SIZE != 0) {
+        print("offset must be block alignment (0x%x)\n", BLOCK_SIZE);
+        ASSERT(0);
+    }
+    // calculate block number of this image
+    if ((img_size % BLOCK_SIZE) == 0) {
+        tblksz = img_size / BLOCK_SIZE;
+    } else {
+        tblksz = (img_size / BLOCK_SIZE) + 1;
+    }
+
+    u4Start_Block = offset / BLOCK_SIZE;
+    u4End_Block = tblksz;
+
+    print("[ERASE] image size = 0x%llx\n", img_size);
+    print("[ERASE] the number of nand block of this image [%d ~ %d]\n", u4Start_Block,u4End_Block);
+
+    // erase nand block
+    cur_offset = offset;
+    while (tblksz != 0) {
+        //update the start offset
+        //cur_offset += (u4g_partition_bad_num * BLOCK_SIZE);
+        old_offset = cur_offset;
+
+#if NAND_ERASE_SKIP_BAD
+        // erase skip bad block
+        cur_offset = nand_find_safe_block(cur_offset);
+        if (cur_offset != old_offset) {
+            tblksz -= ((cur_offset - old_offset)/ BLOCK_SIZE);
+            //print("update offset from 0x%llx to 0x%llx ,block num change to %d \n",old_offset,cur_offset,tblksz);
+        }
+#endif
+
+        //check the adjust offset not overflow current partition
+        NAND_CHECK_OFST_LIMITATION(cur_offset);
+
+        if (mtk_nand_erase(cur_offset) == FALSE) {
+            print("[ERASE] erase fail,mark bad block  %d @ofst 0x%x\n",mtk_nand_offset_to_block(cur_offset),(u32)cur_offset);
+            u4g_partition_bad_num++;
+            mark_block_bad(cur_offset);
+
+            /*
+            * if partition erase, not need care write oveflow, just erase the blocks number as the required
+            */
+            if (mtk_nand_partition_erase())
+                tblksz--;
+        } else {
+            tblksz--;
+        }
+        cur_offset += BLOCK_SIZE;
+
+
+        if (tblksz != 0 && cur_offset >= offset_limit) {
+            print("[ERASE] cur offset (0x%llx) exceeds erase limit address (0x%llx)\n", cur_offset, offset_limit);
+            return TRUE;
+        }
+    }
+
+    print("#######erase finished OK, total bad numbers %d #########\n",u4g_partition_bad_num);
+    return TRUE;
+}
+
diff --git a/src/bsp/lk/platform/mt2701/drivers/nand/rules.mk b/src/bsp/lk/platform/mt2701/drivers/nand/rules.mk
new file mode 100644
index 0000000..d912c0a
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/nand/rules.mk
@@ -0,0 +1,11 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+    $(LOCAL_DIR)/mtk_nand.c \
+
+MODULE_DEPS += \
+    lib/bio \
+    lib/partition \
+
+include make/module.mk
diff --git a/src/bsp/lk/platform/mt2701/drivers/nor/mtk_nor.c b/src/bsp/lk/platform/mt2701/drivers/nor/mtk_nor.c
new file mode 100644
index 0000000..edadf08
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/nor/mtk_nor.c
@@ -0,0 +1,796 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <arch/ops.h>
+#include <errno.h>
+#include <kernel/vm.h>
+#include <lib/bio.h>
+#include <lib/partition.h>
+#include <kernel/event.h>
+#include <platform/mt_reg_base.h>
+#include <platform/mtk_nor.h>
+#include <platform/interrupts.h>
+#include <platform/mt_irq.h>
+#include <string.h>
+#include <sys/types.h>
+
+/* nor register offset */
+#define SFLASH_MEM_BASE         ((u32)0x30000000)//mmu on
+#define SFLASH_CMD_REG          ((u32)0x00)
+#define SFLASH_CNT_REG          ((u32)0x04)
+#define SFLASH_RDSR_REG         ((u32)0x08)
+#define SFLASH_RDATA_REG        ((u32)0x0C)
+#define SFLASH_RADR0_REG        ((u32)0x10)
+#define SFLASH_RADR1_REG        ((u32)0x14)
+#define SFLASH_RADR2_REG        ((u32)0x18)
+#define SFLASH_WDATA_REG        ((u32)0x1C)
+#define SFLASH_PRGDATA0_REG     ((u32)0x20)
+#define SFLASH_PRGDATA1_REG     ((u32)0x24)
+#define SFLASH_PRGDATA2_REG     ((u32)0x28)
+#define SFLASH_PRGDATA3_REG     ((u32)0x2C)
+#define SFLASH_PRGDATA4_REG     ((u32)0x30)
+#define SFLASH_PRGDATA5_REG     ((u32)0x34)
+#define SFLASH_SHREG0_REG       ((u32)0x38)
+#define SFLASH_SHREG1_REG       ((u32)0x3C)
+#define SFLASH_SHREG2_REG       ((u32)0x40)
+#define SFLASH_SHREG3_REG       ((u32)0x44)
+#define SFLASH_SHREG4_REG       ((u32)0x48)
+#define SFLASH_SHREG5_REG       ((u32)0x4C)
+#define SFLASH_SHREG6_REG       ((u32)0x50)
+#define SFLASH_SHREG7_REG       ((u32)0x54)
+#define SFLASH_SHREG8_REG       ((u32)0x58)
+#define SFLASH_SHREG9_REG       ((u32)0x5C)
+#define SFLASH_FLHCFG_REG       ((u32)0x84)
+#define SFLASH_PP_DATA_REG      ((u32)0x98)
+#define SFLASH_PREBUF_STUS_REG  ((u32)0x9C)
+#define SFLASH_SF_INTRSTUS_REG  ((u32)0xA8)
+#define SFLASH_SF_INTREN_REG    ((u32)0xAC)
+#define SFLASH_SF_TIME_REG      ((u32)0x94)
+#define SFLASH_CHKSUM_CTL_REG   ((u32)0xB8)
+#define SFLASH_CHECKSUM_REG     ((u32)0xBC)
+#define SFLASH_CMD2_REG         ((u32)0xC0)
+#define SFLASH_WRPROT_REG       ((u32)0xC4)
+#define SFLASH_RADR3_REG        ((u32)0xC8)
+#define SFLASH_READ_DUAL_REG    ((u32)0xCC)
+#define SFLASH_DELSEL0_REG      ((u32)0xA0)
+#define SFLASH_DELSEL1_REG      ((u32)0xA4)
+#define SFLASH_DELSEL2_REG      ((u32)0xD0)
+#define SFLASH_DELSEL3_REG      ((u32)0xD4)
+#define SFLASH_DELSEL4_REG      ((u32)0xD8)
+
+#define SFLASH_CFG1_REG         ((u32)0x60)
+#define SFLASH_CFG2_REG         ((u32)0x64)
+#define SFLASH_CFG3_REG         ((u32)0x68)
+#define SFLASH_STATUS0_REG      ((u32)0x70)
+#define SFLASH_STATUS1_REG      ((u32)0x74)
+#define SFLASH_STATUS2_REG      ((u32)0x78)
+#define SFLASH_STATUS3_REG      ((u32)0x7C)
+
+#define SFLASH_WRBUF_SIZE           128
+#define SFLASH_POLLINGREG_COUNT     500000
+#define SFLASH_WRITEBUSY_TIMEOUT    500000
+#define SFLASH_ERASESECTOR_TIMEOUT  500000
+#define SFLASH_HW_ALIGNMENT         4
+#define SFLASH_DAM_READ_ALIGN       0x10U
+
+/* nor register read/write */
+#define IO_READ8(base, offset)            ((*(volatile u32 *)(base + offset)) & 0xFF)
+#define IO_WRITE8(offset, value)          (*(volatile u32 *)(offset)) = (value)
+#define IO_READ32(base, offset)           (*(volatile u32 *)(base + offset))
+#define IO_WRITE32(offset, value)         (*(volatile u32 *)(offset)) = (value)
+
+#define SFLASH_WREG8(offset, value)       IO_WRITE8(NOR_BASE + (offset), (value))
+#define SFLASH_RREG8(offset)              IO_READ8(NOR_BASE, (offset))
+#define SFLASH_WREG32(offset, value)      IO_WRITE32(NOR_BASE + (offset), (value))
+#define SFLASH_RREG32(offset)             IO_READ32(NOR_BASE, (offset))
+
+#define SFLASH_DMA_REG_BASE         ((uint32_t)(0x11000000 + 0x14718))
+#define SFLASH_DMA_WREG32(offset, value) \
+    (*(volatile unsigned int *)(SFLASH_DMA_REG_BASE + offset)) = (value)
+#define SFLASH_DMA_RREG32(offset) \
+    (*(volatile unsigned int *)(SFLASH_DMA_REG_BASE + offset))
+#define SFLASH_DMA_ALIGN    0x10U
+
+#define LoWord(d)     ((uint16_t)(d & 0x0000ffffL))
+#define HiWord(d)     ((uint16_t)((d >> 16) & 0x0000ffffL))
+#define LoByte(w)     ((uint8_t)(w & 0x00ff))
+#define HiByte(w)     ((uint8_t)((w >> 8) & 0x00ff))
+
+#define NOR_TEST      0
+#define CMD_4KB       0x20
+#define CMD_64KB      0xD8
+#define BLK_SIZE      512
+
+typedef struct {
+    u8   u1MenuID;
+    u8   u1DevID1;
+    u8   u1DevID2;
+    u32  u4ChipSize;
+    u32  u4SecSize;
+    u8   u1WRENCmd;
+} SFLASHHW_VENDOR_T;
+
+static SFLASHHW_VENDOR_T *flash_desc;
+static SFLASHHW_VENDOR_T vendor_desc[] = {
+    {0xC2, 0x20, 0x13, 0x00080000, 0x10000, 0x06}, /* MXIC(25L400) */
+    {0xC2, 0x20, 0x14, 0x00100000, 0x10000, 0x06}, /* MXIC(25L80) */
+    {0xC2, 0x20, 0x15, 0x00200000, 0x10000, 0x06}, /* MXIC(25L160) */
+    {0xC2, 0x24, 0x15, 0x00200000, 0x10000, 0x06}, /* MXIC(25L1635D) */
+    {0xC2, 0x20, 0x16, 0x00400000, 0x10000, 0x06}, /* MXIC(25L320) */
+    {0xC2, 0x20, 0x17, 0x00800000, 0x10000, 0x06}, /* MXIC(25L640) */
+    {0xC2, 0x20, 0x18, 0x01000000, 0x10000, 0x06}, /* MXIC(25L1280) */
+    {0xC2, 0x5E, 0x16, 0x00400000, 0x10000, 0x06}, /* MXIC(25L3235D) */
+    {0xC2, 0x20, 0x19, 0x02000000, 0x10000, 0x06}, /* MXIC(25L256) */
+    {0xC2, 0x20, 0x1A, 0x02000000, 0x10000, 0x06}, /* MXIC(25L512) */
+    {0xC2, 0x25, 0x39, 0x02000000, 0x10000, 0x06}, /* MXIC(25U256) */
+    {0xEF, 0x30, 0x13, 0x00080000, 0x10000, 0x06}, /* WINBOND(W25X40) */
+    {0xEF, 0x30, 0x14, 0x00100000, 0x10000, 0x06}, /* WINBOND(W25X80) */
+    {0xEF, 0x30, 0x15, 0x00200000, 0x10000, 0x06}, /* WINBOND(W25X16) */
+    {0xEF, 0x30, 0x16, 0x00400000, 0x10000, 0x06}, /* WINBOND(W25X32) */
+    {0xEF, 0x60, 0x16, 0x00400000, 0x10000, 0x06}, /* WINBOND(W25X32) */
+    {0xEF, 0x30, 0x17, 0x00800000, 0x10000, 0x06}, /* WINBOND(W25X64) */
+    {0xEF, 0x40, 0x19, 0x02000000, 0x10000, 0x06}, /* WINBOND(W25Q256FV) */
+    {0xEF, 0x40, 0x15, 0x00200000, 0x10000, 0x06}, /* WINBOND(W25Q16CV) */
+    {0xEF, 0x40, 0x16, 0x00400000, 0x10000, 0x06}, /* WINBOND(W25Q32BV) */
+    {0xEF, 0x40, 0x17, 0x00800000, 0x10000, 0x06}, /* WINBOND(W25Q64BV) */
+    {0xEF, 0x40, 0x18, 0x01000000, 0x10000, 0x06}, /* WINBOND(W25Q128BV) */
+    {0x00, 0x00, 0x00, 0x00000000, 0x00000, 0x00}, /* NULL Device */
+};
+static event_t nor_int_event;
+
+static void NOR_GPIO_SET_FIELD(unsigned int  reg, unsigned int  field,
+                               unsigned int  val)
+{
+    unsigned short tv = (unsigned short)(*(volatile unsigned short *)(reg));
+    tv &= ~(field);
+    tv |= ((val) << (__builtin_ffs((unsigned short)(field)) - 1));
+    (*(volatile unsigned short *)(reg) = (unsigned short)(tv));
+}
+
+static void NOR_set_gpio_mode(int gpio, int value)
+{
+    unsigned short ori_gpio, offset, rev_gpio;
+    unsigned short *gpio_mode_p;
+
+    gpio_mode_p = (unsigned short *)( 0x10005760+ (gpio/5) * 0x10);
+
+    ori_gpio = *gpio_mode_p;
+    offset = gpio % 5;
+    rev_gpio = ori_gpio & (0xffff ^ (0x7<<(offset*3)));
+    rev_gpio = rev_gpio |(value << (offset*3));
+    *gpio_mode_p = rev_gpio;
+}
+
+#define EFUSE_Is_IO_33V() \
+    (((*((volatile unsigned int *)(0x102061c0))) & 0x8) ? false : true)
+
+static void SF_NOR_GPIO_INIT(void)
+{
+    unsigned char is33v;
+    unsigned int  val;
+
+    dprintf(INFO,"NOR GPIO Init here  \n");
+    NOR_set_gpio_mode(236, 1);
+    NOR_set_gpio_mode(237, 1);
+    NOR_set_gpio_mode(238, 1);
+    NOR_set_gpio_mode(239, 1);
+    NOR_set_gpio_mode(240, 1);
+    NOR_set_gpio_mode(241, 1);
+
+    NOR_GPIO_SET_FIELD(0x10005360, 0xF000, 0xF);
+    NOR_GPIO_SET_FIELD(0x10005370, 0x3, 0x3);
+    is33v = EFUSE_Is_IO_33V();
+    val = is33v ? 0x0c : 0x00;
+
+    NOR_GPIO_SET_FIELD(0x10005c00, 0xf,   0x0a);
+    NOR_GPIO_SET_FIELD(0x10005c00, 0x3f0, val);
+}
+
+static int _PollingReg(u8 u1RegOffset, u8 u8Compare)
+{
+    u32 u4Polling;
+    u8 u1Reg;
+
+    u4Polling = 0;
+    while (1) {
+        u1Reg = SFLASH_RREG8(u1RegOffset);
+        if (0x00 == (u1Reg & u8Compare))
+            break;
+
+        u4Polling++;
+        if (u4Polling > SFLASH_POLLINGREG_COUNT)
+            return 1;
+    }
+    return 0;
+}
+
+static int SFLASH_UnLock(void)
+{
+    SFLASH_WREG32(SFLASH_WRPROT_REG, 0x30);
+    return SFLASH_RREG32(SFLASH_WRPROT_REG);
+}
+
+static inline void _SendFlashCommand(u8 u1Val)
+{
+    SFLASH_WREG8(SFLASH_CMD_REG, u1Val);
+}
+
+static int _SetFlashWriteEnable(void)
+{
+    SFLASH_WREG8(SFLASH_PRGDATA5_REG, flash_desc->u1WRENCmd);
+    SFLASH_WREG8(SFLASH_CNT_REG,8);
+
+    _SendFlashCommand(0x4);
+
+    return _PollingReg(SFLASH_CMD_REG, 0x04);
+}
+
+static int SFLASHHW_ReadFlashStatus(u8 *pu1Val)
+{
+    if (pu1Val == NULL)
+        return 1;
+
+    _SendFlashCommand(0x2);
+
+    if (_PollingReg(SFLASH_CMD_REG, 0x2) != 0)
+        return 1;
+
+    *pu1Val = SFLASH_RREG8(SFLASH_RDSR_REG);
+    return 0;
+}
+
+static int _WaitForWriteBusy(u32 u4Timeout)
+{
+    u32 u4Count;
+    u8 u1Reg;
+
+    u4Count = 0;
+    while (1) {
+        if (SFLASHHW_ReadFlashStatus(&u1Reg) != 0)
+            return 1;
+
+        if (0 == (u1Reg & 0x1))
+            break;
+
+        u4Count++;
+        if (u4Count > u4Timeout)
+            return 1;
+    }
+    return 0;
+}
+
+static int _WBEnable(void)
+{
+    u32 u4Polling;
+    u8 u1Reg;
+    u8 u1Tmp = 0x01;
+
+    SFLASH_WREG8(SFLASH_CFG2_REG, u1Tmp);
+
+    u4Polling = 0;
+    while (1) {
+        u1Reg = SFLASH_RREG8(SFLASH_CFG2_REG);
+        if (0x01 == (u1Reg & 0x1))
+            break;
+
+        u4Polling++;
+        if (u4Polling > SFLASH_POLLINGREG_COUNT)
+            return 1;
+    }
+    return 0;
+}
+
+static int _WBDisable(void)
+{
+    u32 u4Polling;
+    u8 u1Reg;
+    u8 u1Tmp = 0x00;
+
+    SFLASH_WREG8(SFLASH_CFG2_REG, u1Tmp);
+    u4Polling = 0;
+    while (1) {
+        u1Reg = SFLASH_RREG8(SFLASH_CFG2_REG);
+        if (u1Tmp == (u1Reg & 0xF))
+            break;
+
+        u4Polling++;
+        if (u4Polling > SFLASH_POLLINGREG_COUNT)
+            return 1;
+    }
+    return 0;
+}
+
+static int _ExecuteWriteCmd(void)
+{
+    u8 u1Reg;
+
+    _SendFlashCommand(0x10);
+    while (1) {
+        u1Reg = SFLASH_RREG8(SFLASH_CMD_REG);
+        if (0x0 == (u1Reg & 0x10))
+            break;
+    }
+    return 0;
+}
+
+static int SFLASHHW_WriteProtect(bool fgEnable)
+{
+    if (_WaitForWriteBusy(SFLASH_WRITEBUSY_TIMEOUT) != 0)
+        return -1;
+
+    if (_SetFlashWriteEnable() != 0)
+        return -1;
+    if (fgEnable)
+        SFLASH_WREG8(SFLASH_PRGDATA5_REG, 0x0);
+    else
+        SFLASH_WREG32(SFLASH_PRGDATA5_REG, 0x0);
+
+    SFLASH_WREG8(SFLASH_CNT_REG,8);
+    _SendFlashCommand(0x20);
+
+    if (_PollingReg(SFLASH_CMD_REG, 0x20) != 0)
+        return -1;
+    return _WaitForWriteBusy(SFLASH_WRITEBUSY_TIMEOUT);
+}
+
+static int SFLASHHW_GetID(u8 *pu1MenuID, u8 *pu1DevID1, u8 *pu1DevID2)
+{
+    SFLASH_WREG8(SFLASH_PRGDATA5_REG, 0x9F);
+    SFLASH_WREG8(SFLASH_PRGDATA4_REG, 0x00);
+    SFLASH_WREG8(SFLASH_PRGDATA3_REG, 0x00);
+    SFLASH_WREG8(SFLASH_PRGDATA2_REG, 0x00);
+    SFLASH_WREG8(SFLASH_CNT_REG, 32);
+
+    _SendFlashCommand(0x4);
+
+    if ( _PollingReg(SFLASH_CMD_REG, 0x04) !=0)
+        return 1;
+
+    *pu1DevID2 = SFLASH_RREG8(SFLASH_SHREG0_REG);
+    *pu1DevID1 = SFLASH_RREG8(SFLASH_SHREG1_REG);
+    *pu1MenuID = SFLASH_RREG8(SFLASH_SHREG2_REG);
+
+    _SendFlashCommand(0x0);
+    if ((pu1MenuID != NULL) && (pu1DevID1 != NULL)) {
+        if ((*pu1MenuID == 0xFF) || (*pu1DevID1 == 0x00))
+            return 1;
+    }
+    return 0;
+}
+
+static int _DoIdentify(u32 *pu4Vendor)
+{
+    u32 i;
+    u8 menuID, devID1, devID2;
+
+    if (pu4Vendor == NULL)
+        return 1;
+    *pu4Vendor = 0xFFFFFFFF;
+    if (SFLASHHW_GetID(&menuID, &devID1, &devID2) != 0)
+        return 1;
+
+    dprintf(CRITICAL,"MenuID: 0x%X, DeviceID1: 0x%X, DeviceID2: 0x%X\n",
+            menuID, devID1, devID2);
+    i = 0;
+
+    while (vendor_desc[i].u1MenuID != (u8)0x0) {
+        if ( (vendor_desc[i].u1MenuID == menuID) &&
+                (vendor_desc[i].u1DevID1 == devID1) &&
+                (vendor_desc[i].u1DevID2 == devID2)) {
+            *pu4Vendor = i;
+            flash_desc = &vendor_desc[i];
+            return 0;
+        }
+        i++;
+    }
+    return 1;
+}
+
+static int SFLASHHW_Init(void)
+{
+    u32 u4VendorIdx;
+    u32 _u4ChipCount = 0;
+
+    SFLASH_WREG32(SFLASH_WRPROT_REG, 0x30);
+    SF_NOR_GPIO_INIT();
+
+    if (_DoIdentify(&u4VendorIdx) != 0)
+        return 1;
+
+    dprintf(CRITICAL, "Detect flash #%d\n", u4VendorIdx);
+    _u4ChipCount++;
+
+    if (_u4ChipCount == 0) {
+        dprintf(CRITICAL, "There is no flash!\n");
+        return 1;
+    }
+
+    SFLASH_WREG8(SFLASH_READ_DUAL_REG, 0);
+    /* Clear interrupts */
+    SFLASH_WREG8(SFLASH_SF_INTREN_REG, 0x0);
+    SFLASH_WREG8(SFLASH_SF_INTRSTUS_REG, 0xff);
+
+    return 0;
+}
+
+static enum handler_return nor_interrupt_handler(void *arg)
+{
+    /* Clear event bits */
+    SFLASH_WREG8(SFLASH_SF_INTREN_REG, 0x0);
+    SFLASH_WREG8(SFLASH_SF_INTRSTUS_REG, SFLASH_RREG8(SFLASH_SF_INTRSTUS_REG));
+    /* MUST BE *false*! otherwise, schedule in intr routine */
+    event_signal(&nor_int_event, false);
+
+    return INT_RESCHEDULE;
+}
+
+static int NOR_Init(void)
+{
+    SFLASH_UnLock();
+    if (SFLASHHW_Init() != 0)
+        return -1;
+
+    /* Register msdc irq */
+    mt_irq_set_sens(MT_NOR_IRQ_ID, LEVEL_SENSITIVE);
+    mt_irq_set_polarity(MT_NOR_IRQ_ID, MT65xx_POLARITY_LOW);
+    event_init(&nor_int_event, false, EVENT_FLAG_AUTOUNSIGNAL);
+    register_int_handler(MT_NOR_IRQ_ID, nor_interrupt_handler, NULL);
+    unmask_interrupt(MT_NOR_IRQ_ID);
+
+    return 0;
+}
+
+static int _WriteBuffer(u32 u4Addr, u32 u4Len, const u8 *pu1Buf)
+{
+    u32 i, j, u4BufIdx, u4Data;
+
+    if (pu1Buf == NULL)
+        return 1;
+
+    ASSERT(u4Len <= SFLASH_WRBUF_SIZE);
+    ASSERT((u4Addr%SFLASH_HW_ALIGNMENT) == 0);
+    ASSERT((u4Len%SFLASH_HW_ALIGNMENT) == 0);
+
+    SFLASH_WREG8(SFLASH_RADR2_REG, LoByte(HiWord(u4Addr)));
+    SFLASH_WREG8(SFLASH_RADR1_REG, HiByte(LoWord(u4Addr)));
+    SFLASH_WREG8(SFLASH_RADR0_REG, LoByte(LoWord(u4Addr)));
+
+    u4BufIdx = 0;
+    for (i=0; i<u4Len; i+=4) {
+        for (j=0; j<4; j++) {
+            (*((u8 *)&u4Data + j)) = pu1Buf[u4BufIdx];
+            u4BufIdx++;
+        }
+        SFLASH_WREG32(SFLASH_PP_DATA_REG, u4Data);
+    }
+
+    if (_ExecuteWriteCmd() != 0)
+        return 1;
+
+    if (_WaitForWriteBusy(SFLASH_WRITEBUSY_TIMEOUT) != 0)
+        return 1;
+    return 0;
+}
+
+static int SFLASHHW_WriteSector(u32 u4Addr, u32 u4Len, const u8 *pu1Buf)
+{
+    u32 u4Count;
+
+    if (_SetFlashWriteEnable() != 0)
+        return -EIO;
+
+    if (_WBEnable() != 0)
+        return -EIO;
+
+    while ((int)u4Len > 0) {
+        if (u4Len >= SFLASH_WRBUF_SIZE)
+            u4Count = SFLASH_WRBUF_SIZE;
+        else
+            break;
+
+        if (_WriteBuffer(u4Addr, u4Count, pu1Buf) != 0) {
+            dprintf(CRITICAL, "Write err! addr: 0x%x\n", u4Addr);
+            if (_WBDisable() != 0)
+                return -EIO;
+            return -EIO;
+        }
+        u4Len -= u4Count;
+        u4Addr += u4Count;
+        pu1Buf += u4Count;
+    }
+    if (_WBDisable() != 0)
+        return -EIO;
+
+    return 0;
+}
+
+static ssize_t dma_read(uint32_t u4SrcAddr, uint32_t u4DestAddr, uint32_t u4Size)
+{
+    int ret;
+
+    arch_clean_invalidate_cache_range(u4DestAddr, u4Size);
+
+    SFLASH_WREG8(SFLASH_SF_INTREN_REG, 0x80);
+    SFLASH_WREG8(SFLASH_SF_INTRSTUS_REG, 0xff);
+
+    /* Do reset */
+    SFLASH_DMA_WREG32(0x0, 0x0);
+    SFLASH_DMA_WREG32(0x0, 0x2);
+    SFLASH_DMA_WREG32(0x0, 0x4);
+
+    /* Flash source address and DRAM destination address */
+    SFLASH_DMA_WREG32(0x4, u4SrcAddr);
+    SFLASH_DMA_WREG32(0x8, u4DestAddr);
+    SFLASH_DMA_WREG32(0xC, u4DestAddr + u4Size);
+
+    /* Trigger DMA */
+    SFLASH_DMA_WREG32(0x0, 0x5);
+
+    /* Wait for interrupt */
+    ret = event_wait_timeout(&nor_int_event, 10000);
+    if (ret != 0) {
+        dprintf(CRITICAL, "%s -> wait interrupt timeout\n", __func__);
+        /* Clear IntrEn */
+        SFLASH_WREG8(SFLASH_SF_INTREN_REG, 0x0);
+        return ret;
+    }
+
+    return u4Size;
+}
+
+static int SFLASHHW_EraseSector(u32 u4Addr, u32 command)
+{
+    u32 u4Polling;
+    u8 u1Reg;
+
+    if (SFLASHHW_WriteProtect(false) != 0) {
+        dprintf(INFO,"Disable Flash write protect fail!\n");
+        return -1;
+    }
+    if (_WaitForWriteBusy(SFLASH_WRITEBUSY_TIMEOUT) != 0) {
+        return 1;
+        dprintf(INFO,"_WaitForWriteBusy fail!\n");
+    }
+
+    if (_SetFlashWriteEnable() != 0) {
+        dprintf(INFO,"_SetFlashWriteEnable fail!\n");
+        return 1;
+    }
+    /* Execute sector erase command */
+    SFLASH_WREG8(SFLASH_PRGDATA5_REG, command);
+    SFLASH_WREG8(SFLASH_PRGDATA4_REG, LoByte(HiWord(u4Addr)));
+    SFLASH_WREG8(SFLASH_PRGDATA3_REG, HiByte(LoWord(u4Addr)));
+    SFLASH_WREG8(SFLASH_PRGDATA2_REG, LoByte(LoWord(u4Addr)));
+    SFLASH_WREG8(SFLASH_CNT_REG, 32);
+
+    _SendFlashCommand(0x4);
+    u4Polling = 0;
+    while (1) {
+        if (SFLASHHW_ReadFlashStatus(&u1Reg) != 0) {
+            dprintf(INFO,"SFLASHHW_ReadFlashStatus fail!\n");
+            return 1;
+        }
+
+        if (0 == (u1Reg & 0x1))
+            break;
+
+        u4Polling++;
+        if (u4Polling > SFLASH_ERASESECTOR_TIMEOUT) {
+            dprintf(INFO,"SFLASH_ERASESECTOR_TIMEOUT timeout!\n");
+            return 1;
+        }
+    }
+    if (SFLASHHW_WriteProtect(true) != 0) {
+        dprintf(INFO,"Disable Flash write protect fail!\n");
+        return -1;
+    }
+    return 0;
+}
+
+static ssize_t nor_erase(struct bdev *bdev, off_t offset, size_t len)
+{
+    u32 i = 0;
+    u32 block_number = 0;
+    u32 addr = (u32)offset;
+    u32 block_size = 0;
+    u32 command = 0;
+
+    dprintf(INFO, "addr %d, blksz %u\n", addr, flash_desc->u4SecSize);
+    if (!addr || addr > flash_desc->u4SecSize) {
+        block_size = flash_desc->u4SecSize;
+        command = CMD_64KB;
+    } else {
+        block_size = 4*1024;
+        command = CMD_4KB;
+    }
+
+    dprintf(INFO, "nor erase block size %d\n", block_size);
+    if ((len % bdev->block_size) != 0) {
+        dprintf(ALWAYS,"nor erase fail: length must align block size!\n");
+        return -1;
+    }
+
+    block_number = len / block_size;
+    for (i = 0; i < block_number; i++) {
+        dprintf(INFO, "erase offset from %d to %d\n", addr + i * block_size,
+                addr + (i + 1) * block_size);
+        if (SFLASHHW_EraseSector(addr + i * block_size, command))
+            return -1;
+    }
+
+    /* erase redundant block */
+    dprintf(INFO, "erase redundant from %d to %d\n", addr + len - block_size,
+            addr + len);
+    if (SFLASHHW_EraseSector(addr + len - block_size, command))
+        return -1;
+
+    return len;
+}
+
+static ssize_t nor_read_block(struct bdev *bdev, void *buf, bnum_t blknr, uint blks)
+{
+    u32 offset, len;
+
+    if (((u32)buf & SFLASH_DMA_ALIGN)!=0) {
+        dprintf(ALWAYS, "Address or size is not 16-byte alignment!\n");
+        return -EINVAL;
+    }
+
+    offset = blknr * bdev->block_size;
+    len = (u32)blks * bdev->block_size;
+
+    return dma_read(offset, (u32)buf, len);
+}
+
+static ssize_t nor_write_block(struct bdev *bdev, const void *buf, bnum_t blknr, uint blks)
+{
+    int ret;
+    u32 addr, len;
+
+    ret = SFLASHHW_WriteProtect(false);
+    if (ret != 0) {
+        dprintf(CRITICAL, "Disable Flash write protect fail!\n");
+        return -EIO;
+    }
+
+    addr = blknr * bdev->block_size;
+    len = (u32)blks * bdev->block_size;
+
+    dprintf(INFO, "nor write from %d to %d\n", addr, addr + len);
+    ret = SFLASHHW_WriteSector(addr, len, buf);
+    if (ret) {
+        dprintf(CRITICAL, "Write flash error ! (%d)\n", ret);
+        SFLASHHW_WriteProtect(true);
+        return ret;
+    }
+
+    ret = SFLASHHW_WriteProtect(true);
+    if (ret != 0) {
+        dprintf(CRITICAL, "Enable flash write protect fail!\n");
+        return -EIO;
+    }
+
+    return len;
+}
+
+#if NOR_TEST
+#define NOR_WRITE_OFFSET 0x700000
+#define NOR_TEST_SIZE 4096
+static void nor_test(struct bdev *dev)
+{
+    int i;
+    u8 *w_buf = malloc(NOR_TEST_SIZE);
+
+    dprintf(INFO, "nor driver UT test start!\n");
+    if (!SFLASHHW_EraseSector(NOR_WRITE_OFFSET, CMD_64KB))
+        dprintf(INFO, "nor erase 1 success!\n");
+    else
+        dprintf(INFO, "nor erase 1 failed!\n");
+    spin(100000);
+
+    memset(w_buf, 0xff, 4096);
+    if (bio_read(dev, (void *)w_buf, (off_t)NOR_WRITE_OFFSET, 4096) != 4096)
+        dprintf(INFO, "===nor Read after write success!===\n");
+    else
+        dprintf(INFO, "===nor Read after write failed!===\n");
+
+    for (i=0; i<1006; i++)
+        dprintf(INFO, " 0x%x",w_buf[i]);
+    dprintf(INFO, "\n");
+
+    for (i=0; i<4096; i++)
+        w_buf[i] = i & 0xFF;
+    if (bio_read(dev, (void *)w_buf, (off_t)NOR_WRITE_OFFSET, 4095) != 4095)
+        dprintf(INFO, "===nor Write success!===\n");
+    else
+        dprintf(INFO, "===nor Write failed!===\n");
+
+    memset(w_buf, 0xff, 4096);
+    if (bio_read(dev, (void *)w_buf, (off_t)NOR_WRITE_OFFSET, 4096) != 4096)
+        dprintf(INFO, "===nor Read after write success!===\n");
+    else
+        dprintf(INFO, "===nor Read after write failed!===\n");
+
+    for (i=0; i<1006; i++)
+        dprintf(INFO, " 0x%x", w_buf[i]);
+    dprintf(INFO, "\n");
+
+    free(w_buf);
+}
+
+static void nor_write_test(struct bdev *dev)
+{
+    u8 *w_buf = malloc(NOR_TEST_SIZE);
+    ssize_t ret;
+
+    if (!SFLASHHW_EraseSector(NOR_WRITE_OFFSET, CMD_64KB))
+        dprintf(INFO, "nor erase 1 success!\n");
+    else
+        dprintf(INFO, "nor erase 1 failed!\n");
+    spin(100000);
+
+    memset(w_buf, 0x11, 4096);
+    ret = bio_write(dev, (void *)w_buf, (off_t)NOR_WRITE_OFFSET, 4096);
+
+    if (ret != 4096)
+        dprintf(ALWAYS, "===write failed!===\n");
+    else
+        dprintf(ALWAYS, "===write sucesss!===\n");
+
+    dprintf(ALWAYS, "ret = %ld\n", ret);
+
+    free(w_buf);
+}
+#endif
+
+void nor_init_device(void)
+{
+    struct bdev *dev;
+
+    if (NOR_Init())
+        ASSERT(0);
+
+    dev = malloc(sizeof(struct bdev));
+    ASSERT(dev);
+    memset(dev, 0, sizeof(struct bdev));
+
+    /* construct the block device */
+    bio_initialize_bdev(dev, "nor0",
+                        BLK_SIZE, flash_desc->u4ChipSize / BLK_SIZE, 0, NULL,
+                        (BIO_FLAG_CACHE_ALIGNED_READS));
+
+    /* override our block device hooks */
+    dev->read_block = nor_read_block;
+    dev->write_block = nor_write_block;
+    dev->erase = nor_erase;
+    dev->erase_byte = 0xff;
+
+    bio_register_device(dev);
+    partition_publish(dev->name, 0x0);
+
+#if NOR_TEST
+    nor_test(dev);
+    nor_write_test(dev);
+#endif
+}
+
diff --git a/src/bsp/lk/platform/mt2701/drivers/nor/rules.mk b/src/bsp/lk/platform/mt2701/drivers/nor/rules.mk
new file mode 100644
index 0000000..cda5b02
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/nor/rules.mk
@@ -0,0 +1,11 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+MODULE := $(LOCAL_DIR)
+
+MODULE_SRCS += \
+    $(LOCAL_DIR)/mtk_nor.c \
+
+MODULE_DEPS += \
+    lib/bio \
+    lib/partition \
+
+include make/module.mk
diff --git a/src/bsp/lk/platform/mt2701/drivers/pll/pll.c b/src/bsp/lk/platform/mt2701/drivers/pll/pll.c
new file mode 100644
index 0000000..c419187
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/pll/pll.c
@@ -0,0 +1,1394 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <debug.h>
+#include <platform/mt2701.h>
+#include <platform/pll.h>
+#include <platform/spm.h>
+
+static int mt_mempll_cali(void);
+
+#define r_bias_en_stb_time              (0x00000000 << 24)
+#define r_bias_lpf_en_stb_time          (0x00000000 << 16)
+#define r_mempll_en_stb_time            (0x00000000 << 8)
+#define r_dmall_ck_en_stb_time          (0x00000000 << 0)
+
+#define r_dds_en_stb_time               (0x00000000 << 24)
+#define r_div_en_stb_time               (0x00000000 << 16)
+#define r_dmpll2_ck_en_stb_time         (0x00000000 << 8)
+#define r_iso_en_stb_time               (0x00000000 << 0)
+
+#define r_bias_en_stb_dis               (0x00000001 << 28)
+#define r_bias_en_src_sel               (0x00000001 << 24)
+#define r_bias_lpf_en_stb_dis           (0x00000001 << 20)
+#define r_bias_lpf_en_src_sel           (0x00000001 << 16)
+#define r_mempll4_en_stb_dis            (0x00000001 << 15)
+#define r_mempll3_en_stb_dis            (0x00000001 << 14)
+#define r_mempll2_en_stb_dis            (0x00000001 << 13)
+#define r_mempll_en_stb_dis             (0x00000001 << 12)
+#define r_mempll4_en_src_sel            (0x00000001 << 11)
+#define r_mempll3_en_src_sel            (0x00000001 << 10)
+#define r_mempll2_en_src_sel            (0x00000001 << 9)
+#define r_mempll_en_src_sel             (0x00000001 << 8)
+#define r_dmall_ck_en_stb_dis           (0x00000001 << 4)
+#define r_dmall_ck_en_src_sel           (0x00000001 << 0)
+
+#define r_dds_en_stb_dis                (0x00000001 << 28)
+#define r_dds_en_src_sel                (0x00000001 << 24)
+#define r_div_en_stb_dis                (0x00000001 << 20)
+#define r_div_en_src_sel                (0x00000001 << 16)
+#define r_dmpll2_ck_en_stb_dis          (0x00000001 << 12)
+#define r_dmpll2_ck_en_src_sel          (0x00000001 << 8)
+#define r_iso_en_stb_dis                (0x00000001 << 4)
+#define r_iso_en_src_sel                (0x00000001 << 0)
+
+#define r_dmbyp_pll4                    (0x00000001 << 0)
+#define r_dmbyp_pll3                    (0x00000001 << 1)
+#define r_dm1pll_sync_mode              (0x00000001 << 2)
+#define r_dmall_ck_en                   (0x00000001 << 4)
+#define r_dmpll2_clk_en                 (0x00000001 << 5)
+
+#define pllc1_prediv_1_0_n1             (0x00000000 << 16)
+#define pllc1_vco_div_sel_n2            (0x00000000 << 13)
+#define pllc1_postdiv_1_0_n5            (0x00000000 << 14)
+#define pllc1_mempll_n_info_chg         (0x00000001 << 0)
+#define pllc1_mempll_div_en             (0x00000001 << 24)
+#define pllc1_mempll_div_6_0            (0x00000052 << 25)
+#define pllc1_mempll_reserve_2          (0x00000001 << 18)
+#define pllc1_mempll_top_reserve_2_0    (0x00000000 << 16)
+#define pllc1_mempll_bias_en            (0x00000001 << 14)
+#define pllc1_mempll_bias_lpf_en        (0x00000001 << 15)
+#define pllc1_mempll_en                 (0x00000001 << 2)
+#define pllc1_mempll_sdm_prd_1          (0x00000001 << 11)
+
+#define mempll2_prediv_1_0              (0x00000000 << 0)
+#define mempll2_br                      (0x00000001 << 26)
+#define mempll2_bp                      (0x00000001 << 27)
+#define mempll2_fbksel_1_0              (0x00000000 << 10)
+#define mempll2_posdiv_1_0              (0x00000000 << 30)
+#define mempll2_ref_dl_4_0              (0x00000000 << 27)
+#define mempll2_fb_dl_4_0               (0x00000000 << 22)
+#define mempll2_en                      (0x00000001 << 18)
+
+#define mempll3_prediv_1_0              (0x00000000 << 0)
+#define mempll3_br                      (0x00000001 << 26)
+#define mempll3_bp                      (0x00000001 << 27)
+#define mempll3_fbksel_1_0              (0x00000000 << 10)
+#define mempll3_posdiv_1_0              (0x00000000 << 30)
+#define mempll3_ref_dl_4_0              (0x00000000 << 27)
+#define mempll3_fb_dl_4_0               (0x00000000 << 22)
+#define mempll3_en                      (0x00000001 << 18)
+
+#define mempll4_prediv_1_0              (0x00000000 << 0)
+#define mempll4_br                      (0x00000001 << 26)
+#define mempll4_bp                      (0x00000001 << 27)
+#define mempll4_fbksel_1_0              (0x00000000 << 10)
+#define mempll4_posdiv_1_0              (0x00000000 << 30)
+#define mempll4_ref_dl_4_0              (0x00000000 << 27)
+#define mempll4_fb_dl_4_0               (0x00000000 << 22)
+#define mempll4_en                      (0x00000001 << 18)
+
+/*
+*    Mempll
+*/
+void mt_mempll_init(int freq, int mode)
+{
+    int pllc1_mempll_n_info_30_24_n4;
+    int pllc1_mempll_n_info_23_0_n4;
+
+    int mempll2_vco_div_sel;
+    int mempll2_m4pdiv_1_0;
+    int mempll2_fbdiv_6_0;
+    int mempll2_fb_mck_sel;
+
+    int mempll3_vco_div_sel;
+    int mempll3_m4pdiv_1_0;
+    int mempll3_fbdiv_6_0;
+    int mempll3_fb_mck_sel;
+
+    int mempll4_vco_div_sel;
+    int mempll4_m4pdiv_1_0;
+    int mempll4_fbdiv_6_0;
+    int mempll4_fb_mck_sel;
+
+    /*********************************
+    * (1) Setup DDRPHY operation mode
+    **********************************/
+
+    *((volatile unsigned int *)(DRAMC0_BASE + 0x007c)) |= 0x00000001; //DFREQ_DIV2=1
+    *((volatile unsigned int *)(DDRPHY_BASE + 0x007c)) |= 0x00000001;
+
+#ifdef DRAMC_ASYNC
+    if (mode ==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000022;
+    else if (mode==3)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000020; //3PLL mode, OK
+    else if (mode==1)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000003; //1PLL async mode, OK
+#else
+    if (mode ==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000022;
+    else if (mode==3)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000020; //3PLL sync mode, OK
+    else if (mode==1)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000007; //1PLL sync mode, OK
+#endif
+
+    /*****************************************************************************************
+    * (2) Setup MEMPLL operation case & frequency. May set according to dram type & frequency
+    ******************************************************************************************/
+    switch (freq) {
+        case 533:
+            pllc1_mempll_n_info_30_24_n4    = 0x00000046 << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x000aaaaa << 1;
+            mempll2_fbdiv_6_0               = 0x06 <<2;
+            break;
+        case 667:
+            pllc1_mempll_n_info_30_24_n4    = 0x0000004b << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x00210e10 << 1;
+            mempll2_fbdiv_6_0               = 0x07 <<2;
+            break;
+        case 800:
+            pllc1_mempll_n_info_30_24_n4    = 0x0000004e << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x00d89d89 << 1;
+            mempll2_fbdiv_6_0               = 0x08 <<2;
+            break;
+        case 1066:
+            pllc1_mempll_n_info_30_24_n4    = 0x0000004c << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x0068ba2e << 1;
+            mempll2_fbdiv_6_0               = 0x0b <<2;
+            break;
+        case 1333:
+            pllc1_mempll_n_info_30_24_n4    = 0x00000050 << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x00d8fe7c << 1;
+            mempll2_fbdiv_6_0               = 0x0d <<2;
+            break;
+        case 1466:
+            pllc1_mempll_n_info_30_24_n4    = 0x0000004d << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x000f18f1 << 1;
+            mempll2_fbdiv_6_0               = 0x0f <<2;
+            break;
+        case 1600:
+            pllc1_mempll_n_info_30_24_n4    = 0x0000004e << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x00d89d89 << 1;
+            mempll2_fbdiv_6_0               = 0x10 <<2;
+            break;
+        default:/* 533 */
+            pllc1_mempll_n_info_30_24_n4    = 0x00000046 << 25;
+            pllc1_mempll_n_info_23_0_n4     = 0x000aaaaa << 1;
+            mempll2_fbdiv_6_0               = 0x06 <<2;
+            break;
+    }
+
+
+    if (freq == 533)
+        mempll2_m4pdiv_1_0              = (0x00000001 << 10);
+    else
+        mempll2_m4pdiv_1_0              = (0x00000000 << 10);
+
+    if (freq < 1333)
+        mempll2_vco_div_sel = (0x00000001 << 29);
+    else
+        mempll2_vco_div_sel = (0x00000000 << 29);
+
+    if (mode == 1)
+        mempll2_fb_mck_sel = (0x00000000 << 9);
+    else
+        mempll2_fb_mck_sel = (0x00000001 << 9);
+
+    /*mempll3&4 copy from mempll2*/
+    /* mempll3 */
+    mempll3_vco_div_sel = mempll2_vco_div_sel;
+    mempll3_fb_mck_sel = mempll2_fb_mck_sel;
+
+    mempll3_m4pdiv_1_0 = mempll2_m4pdiv_1_0;
+    mempll3_fbdiv_6_0 = mempll2_fbdiv_6_0;
+
+    /* mempll4 */
+    mempll4_vco_div_sel = mempll2_vco_div_sel;
+    mempll4_fb_mck_sel = mempll2_fb_mck_sel;
+
+    mempll4_m4pdiv_1_0 = mempll2_m4pdiv_1_0;
+    mempll4_fbdiv_6_0 = mempll2_fbdiv_6_0;
+
+    /*****************************************************************************************
+        * (2) Setup MEMPLL operation case & frequency. May set according to dram type & frequency
+        ******************************************************************************************/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0170 <<2))) = r_bias_en_stb_time | r_bias_lpf_en_stb_time | r_mempll_en_stb_time | r_dmall_ck_en_stb_time;
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0171 <<2))) = r_dds_en_stb_time | r_div_en_stb_time | r_dmpll2_ck_en_stb_time | r_iso_en_stb_time;
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0172 <<2))) = r_bias_en_stb_dis| r_bias_en_src_sel | r_bias_lpf_en_stb_dis| r_bias_lpf_en_src_sel | r_mempll4_en_stb_dis| r_mempll3_en_stb_dis| r_mempll2_en_stb_dis| r_mempll_en_stb_dis| r_mempll4_en_src_sel | r_mempll3_en_src_sel | r_mempll2_en_src_sel | r_mempll_en_src_sel | r_dmall_ck_en_stb_dis | r_dmall_ck_en_src_sel;
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0173 <<2))) = r_dds_en_stb_dis| r_dds_en_src_sel | r_div_en_stb_dis| r_div_en_src_sel | r_dmpll2_ck_en_stb_dis| r_dmpll2_ck_en_src_sel | r_iso_en_stb_dis| r_iso_en_src_sel | r_dmbyp_pll4 | r_dmbyp_pll3 | r_dm1pll_sync_mode | r_dmall_ck_en | r_dmpll2_clk_en;
+
+    /*default val: [0x0180] = 0x044c8000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0180 <<2))) = pllc1_prediv_1_0_n1  | pllc1_vco_div_sel_n2 | pllc1_postdiv_1_0_n5  | 0x044c0000;
+
+    /*default val: [0x0181] = 0x00000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0181 <<2))) = pllc1_mempll_div_6_0 | pllc1_mempll_reserve_2;
+
+    /*default val: [0x0183] = 0x60000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0183 <<2))) = mempll2_vco_div_sel | mempll2_m4pdiv_1_0 | mempll2_fb_mck_sel | mempll2_posdiv_1_0 | mempll2_br | mempll2_bp;
+
+    /*default val: [0x0182] = 0x00000044*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0182 <<2))) = pllc1_mempll_top_reserve_2_0 | mempll2_prediv_1_0 | mempll2_fbdiv_6_0 | mempll2_fbksel_1_0;
+
+    /*default val: [0x0185] = 0x60000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0185 <<2))) = mempll3_vco_div_sel | mempll3_m4pdiv_1_0 | mempll3_fb_mck_sel | mempll3_posdiv_1_0 | mempll3_br | mempll3_bp;
+    /*default val: [0x0184] = 0x00000044*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0184 <<2))) = mempll2_ref_dl_4_0 | mempll2_fb_dl_4_0 | mempll3_prediv_1_0 | mempll3_fbdiv_6_0 | mempll3_fbksel_1_0;
+    /*default val: [0x0187] = 0x60000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0187 <<2))) = mempll4_vco_div_sel | mempll4_m4pdiv_1_0 | mempll4_fb_mck_sel | mempll4_posdiv_1_0 | mempll4_br | mempll4_bp;
+    /*default val: [0x0186] = 0x00000044*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0186 <<2))) = mempll3_fb_dl_4_0 | mempll3_ref_dl_4_0 | mempll4_prediv_1_0 | mempll4_fbdiv_6_0 | mempll4_fbksel_1_0;
+    /*default val: [0x0188] = 0x00008c00*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0188 <<2))) = mempll4_ref_dl_4_0 | mempll4_fb_dl_4_0 | 0x00008c00;
+    /*default val: [0x0189] = 0x00000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0189 <<2))) = pllc1_mempll_n_info_23_0_n4 | pllc1_mempll_n_info_30_24_n4  | pllc1_mempll_n_info_chg;
+    //tongle 624[0] for info change
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0189 <<2))) = (*((volatile unsigned int *)(DDRPHY_BASE + (0x0189 <<2))))^0x01;
+
+#ifdef DRAMC_ASYNC
+    if (mode==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000022  | r_dmpll2_clk_en;
+    else if (mode==3)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000020 | r_dmpll2_clk_en;    //3PLL mode
+    else if (mode==1)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000003 ; //1PLL async mode
+#else
+    if (mode==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000022  | r_dmpll2_clk_en;
+    else if (mode==3)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000020 | r_dmpll2_clk_en; //3PLL sync mode
+    else if (mode==1)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000007 ; //1PLL sync mode
+#endif
+
+    /***********************************
+    * (3) Setup MEMPLL power on sequence
+    ************************************/
+
+    spin(2);
+
+    /*default val: [0x0181] = 0x00000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0181 <<2))) = pllc1_mempll_div_6_0 | pllc1_mempll_reserve_2 | (pllc1_mempll_bias_en );
+    spin(2);
+
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0181 <<2))) = pllc1_mempll_div_6_0 | pllc1_mempll_reserve_2 | (pllc1_mempll_bias_en | pllc1_mempll_bias_lpf_en);
+    spin(1000);
+
+    /*default val: [0x0180] = 0x044c8000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0180 <<2))) = pllc1_prediv_1_0_n1  | pllc1_vco_div_sel_n2 | pllc1_postdiv_1_0_n5  | (pllc1_mempll_en) | 0x044c0000;
+    spin(20);
+
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x018a <<2))) = 0x10000000;
+    spin(2);
+
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x018a <<2))) = 0x18000000;
+
+    /*default val: [0x0188] = 0x00008c00*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0188 <<2))) = mempll4_ref_dl_4_0 | mempll4_fb_dl_4_0 | ( pllc1_mempll_sdm_prd_1) | 0x00008c00;
+    spin(2);
+
+    /*default val: [0x0180] = 0x044c8000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0180 <<2))) =  (pllc1_prediv_1_0_n1  | pllc1_vco_div_sel_n2  | pllc1_postdiv_1_0_n5  | (pllc1_mempll_en)) | 0x044c0000;
+
+    /*default val: [0x0181] = 0x00000000*/
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0181 <<2))) = pllc1_mempll_div_en | (pllc1_mempll_div_6_0 | pllc1_mempll_reserve_2 | (pllc1_mempll_bias_en | pllc1_mempll_bias_lpf_en));
+    spin(23);
+
+    if (mode==3 || mode==2) {
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0183 <<2))) = mempll2_en | mempll2_vco_div_sel | mempll2_m4pdiv_1_0 | mempll2_fb_mck_sel | mempll2_posdiv_1_0;
+        if (mode==2) {
+            *((volatile unsigned int *)(DDRPHY_BASE + (0x0185 <<2))) = (~mempll3_en) & (mempll3_vco_div_sel | mempll3_m4pdiv_1_0 | mempll3_fb_mck_sel | mempll3_posdiv_1_0);
+        } else {
+            *((volatile unsigned int *)(DDRPHY_BASE + (0x0185 <<2))) = mempll3_en | mempll3_vco_div_sel | mempll3_m4pdiv_1_0 | mempll3_fb_mck_sel | mempll3_posdiv_1_0;
+        }
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0187 <<2))) = mempll4_en | mempll4_vco_div_sel | mempll4_m4pdiv_1_0 | mempll4_fb_mck_sel | mempll4_posdiv_1_0;
+    } else {
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0183 <<2))) = mempll2_en | mempll2_vco_div_sel | mempll2_m4pdiv_1_0 | mempll2_fb_mck_sel | mempll2_posdiv_1_0;
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0185 <<2))) =              mempll3_vco_div_sel | mempll3_m4pdiv_1_0 | mempll3_fb_mck_sel | mempll3_posdiv_1_0;
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0187 <<2))) =              mempll4_vco_div_sel | mempll4_m4pdiv_1_0 | mempll4_fb_mck_sel | mempll4_posdiv_1_0;
+    }
+
+    spin(23);
+
+#ifdef DRAMC_ASYNC
+    if (mode==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000022  | r_dmpll2_clk_en | r_dmall_ck_en;
+    else if (mode==3)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000020 | r_dmpll2_clk_en | r_dmall_ck_en;    //3PLL mode
+    else if (mode==1)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000003 | r_dmpll2_clk_en | r_dmall_ck_en; // 1PLL async mode
+#else
+    if (mode==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000022  | r_dmpll2_clk_en | r_dmall_ck_en;
+    else if (mode==3)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000020 | r_dmpll2_clk_en | r_dmall_ck_en; //3PLL sync mode
+    else if (mode==1)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0190 <<2))) = 0x00000007 | r_dmpll2_clk_en | r_dmall_ck_en; //1PLL sync mode
+#endif
+
+    /**********************************
+    * (4) MEMPLL control switch to SPM
+    ***********************************/
+
+    // for ISO_EN zero delay
+    if (mode==3 || mode==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0173 <<2))) = 0x00101010; //[0]ISO_EN_SRC=0,[22]DIV_EN_SC_SRC=0 (pll2off),[16]DIV_EN_SRC=0,[8]PLL2_CK_EN_SRC=1(1pll),[8]PLL2_CK_EN_SRC=0(3pll)
+    else
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0173 <<2))) = 0x00101010; //[0]ISO_EN_SRC=0,[22]DIV_EN_SC_SRC=0 (pll2off),[16]DIV_EN_SRC=0,[8]PLL2_CK_EN_SRC=1(1pll),[8]PLL2_CK_EN_SRC=0(3pll)
+
+    // setting for MEMPLL_EN control by sc_mempll3_off
+    if (mode==3 || mode==2)
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0172 <<2))) = 0x0000F010; //[24]BIAS_EN_SRC=0,[16]BIAS_LPF_EN_SRC=0,[8]MEMPLL_EN,[9][10][11]MEMPLL2,3,4_EN_SRC,[0]ALL_CK_EN_SRC=0
+    else
+        *((volatile unsigned int *)(DDRPHY_BASE + (0x0172 <<2))) = 0x0000FC10; //1PLL mode, MEMPLL3,4_EN no change to spm controller. always keep to 1'b0 for power saving.
+
+    *((volatile unsigned int *)(DDRPHY_BASE + (0x0170 <<2))) = 0x003C1B96; //setting for delay time
+#ifdef DDRPHY_3PLL_MODE
+    mt_mempll_cali();
+#endif
+}
+
+#ifndef MEMPLL_CALI_V1_0
+/*old mempll cali flow*/
+static int mt_mempll_cali(void)
+{
+    int zero_count = 0;
+    int pll2_done = 0, pll3_done = 0, pll4_done = 0, ret = 0;
+
+    unsigned int temp = 0, pll2_dl = 0, pll3_dl = 0, pll4_dl = 0;
+
+    /***********************************************
+    * 1. Set jitter meter clock to internal FB path
+    ************************************************/
+    //temp = readl(0x1000F60C);
+    //write_r(0x1000F60C, temp & ~0x200); // 0x1000F60C[9] = 0 PLL2
+
+    //temp = readl(0x1000F614);
+    //write_r(0x1000F614, temp & ~0x200); // 0x1000F614[9] = 0 PLL3
+
+    //temp = readl(0x1000F61C);
+    //write_r(0x1000F61C, temp & ~0x200); // 0x1000F61C[9] = 0 PLL4
+
+    /***********************************************
+    * 2. Set jitter meter count number
+    ************************************************/
+
+    temp = readl(0x1000F1CC) & 0x0000FFFF;
+    write_r(0x1000F1CC, (temp | 0x04000000)); // 0x1000F1CC[31:16] PLL2 0x400 = 1024 count
+
+    temp = readl(0x1000F1D0) & 0x0000FFFF;
+    write_r(0x1000F1D0, (temp | 0x04000000)); // 0x1000F1D0[31:16] PLL3 0x400 = 1024 count
+
+    temp = readl(0x1000F1D4) & 0x0000FFFF;
+    write_r(0x1000F1D4, (temp | 0x04000000)); // 0x1000F1D4[31:16] PLL4 0x400 = 1024 count
+
+    while (1) {
+        /***********************************************
+        * 3. Adjust delay chain tap number
+        ************************************************/
+
+        if (!pll2_done) {
+            temp = readl(0x1000F610) & ~0xF8000000;
+            write_r(0x1000F610, (temp | (0x10 << 27 ))); // 0x1000F610[31:27] PLL2 REF 0x10 fix
+
+            temp = readl(0x1000F610) & ~0x07C00000;
+            write_r(0x1000F610, (temp | (pll2_dl << 22))); // 0x1000F610[26:22] PLL2 FB inc 1
+        }
+
+        if (!pll3_done) {
+            temp = readl(0x1000F618) & ~0xF8000000;
+            write_r(0x1000F618, (temp | (0x10 << 27 ))); // 0x1000F618[31:27] PLL3 REF 0x10 fix
+
+            temp = readl(0x1000F618) & ~0x07C00000;
+            write_r(0x1000F618, (temp | (pll3_dl << 22))); // 0x1000F618[26:22] PLL3 FB inc 1
+        }
+
+        if (!pll4_done) {
+            temp = readl(0x1000F620) & ~0xF8000000;
+            write_r(0x1000F620, (temp | (0x10 << 27 ))); // 0x1000F620[31:27] PLL4 REF 0x10 fix
+
+            temp = readl(0x1000F620) & ~0x07C00000;
+            write_r(0x1000F620, (temp | (pll4_dl << 22))); // 0x1000F620[26:22] PLL4 FB inc 1
+        }
+
+        /***********************************************
+        * 4. Enable jitter meter
+        ************************************************/
+
+        if (!pll2_done) {
+            temp = readl(0x1000F1CC);
+            write_r(0x1000F1CC, temp | 0x1); // 0x1000F1CC[0]=1 PLL2
+        }
+
+        if (!pll3_done) {
+            temp = readl(0x1000F1D0);
+            write_r(0x1000F1D0, temp | 0x1); // 0x1000F1D0[0]=1 PLL3
+        }
+
+        if (!pll4_done) {
+            temp = readl(0x1000F1D4);
+            write_r(0x1000F1D4, temp | 0x1); // 0x1000F1D4[0]=1 PLL4
+        }
+
+        spin(40); // wait for jitter meter complete
+
+        /***********************************************
+        * 5. Check jitter meter counter value
+        ************************************************/
+
+        if (!pll2_done) {
+            readl(0x1000F320); // 0x1000F320[31:16] PLL2 one count
+            zero_count = readl(0x1000F320) & 0x0000FFFF; // 0x1000F320[15:0] PLL2 zero count
+
+            if (zero_count > 512) {
+                //dprintf(CRITICAL, "[mt_pll_init] PLL2 FB_DL: 0x%x, 1/0 = %d/%d\n", ((readl(0x1000F610)& 0x07C00000) >> 22), one_count, zero_count);
+                pll2_done = 1;
+            }
+        }
+
+        if (!pll3_done) {
+            readl(0x1000F324); // 0x1000F324[31:16] PLL3 one count
+            zero_count = readl(0x1000F324) & 0x0000FFFF; // 0x1000F324[15:0] PLL3 zero count
+
+            if (zero_count > 512) {
+                //dprintf(CRITICAL, "[mt_pll_init] PLL3 FB_DL: 0x%x, 1/0 = %d/%d\n", ((readl(0x1000F618) & 0x07C00000) >> 22), one_count, zero_count);
+                pll3_done = 1;
+            }
+        }
+
+        if (!pll4_done) {
+            readl(0x1000F328); // 0x1000F328[31:16] PLL4 one count
+            zero_count = readl(0x1000F328) & 0x0000FFFF; // 0x1000F328[15:0] PLL4 zero count
+
+            if (zero_count > 512) {
+                //dprintf(CRITICAL, "[mt_pll_init] PLL4 FB_DL: 0x%x, 1/0 = %d/%d\n", ((readl(0x1000F620) & 0x07C00000) >> 22), one_count, zero_count);
+                pll4_done = 1;
+            }
+        }
+
+        /***********************************************
+        * 6. Reset jitter meter value
+        ************************************************/
+
+        if (!pll2_done) {
+            pll2_dl++;
+            temp = readl(0x1000F1CC);
+            write_r(0x1000F1CC, temp & ~0x1); // 0x1000F1CC[0]=0 PLL2
+        }
+
+        if (!pll3_done) {
+            pll3_dl++;
+            temp = readl(0x1000F1D0);
+            write_r(0x1000F1D0, temp & ~0x1); // 0x1000F1D0[0]=0 PLL3
+        }
+
+        if (!pll4_done) {
+            pll4_dl++;
+            temp = readl(0x1000F1D4);
+            write_r(0x1000F1D4, temp & ~0x1); // 0x1000F1D4[0]=0 PLL4
+        }
+
+        /*************************************************************
+        * Then return to step 1 to adjust next delay chain tap value.
+        * Until we have ~ 50% of one or zero count on jitter meter
+        **************************************************************/
+
+        if (pll2_done && pll3_done && pll4_done) {
+            ret = 0;
+            break;
+        }
+
+        if (pll2_dl >= 32 || pll3_dl >= 32 || pll4_dl >= 32) {
+            ret = -1;
+            break;
+        }
+    }
+
+#if 0
+    if (ret != 0)
+        dprintf(CRITICAL, "[mt_pll_init] MEMPLL 3PLL mode calibration fail\n");
+#endif
+
+    /***********************************************
+    * 7. Set jitter meter clock to external FB path
+    ************************************************/
+
+    temp = readl(0x1000F60C);
+    write_r(0x1000F60C, temp | 0x200); // 0x1000F60C[9] = 1 PLL2
+
+    temp = readl(0x1000F614);
+    write_r(0x1000F614, temp | 0x200); // 0x1000F614[9] = 1 PLL3
+
+    temp = readl(0x1000F61C);
+    write_r(0x1000F61C, temp | 0x200); // 0x1000F61C[9] = 1 PLL4
+
+    return ret;
+}
+
+#else
+/*new mempll cali flow*/
+#define MEMPLL_JMETER_CNT               1024
+#define MEMPLL_JMETER_CONFIDENCE_CNT    (MEMPLL_JMETER_CNT/10)
+#define MEMPLL_JMETER_WAIT_TIME_BASE    10 // time base
+#define MEMPLL_JMETER_WAIT_TIMEOUT      1000/MEMPLL_JMETER_WAIT_TIME_BASE // timeout ~ 1000us
+static int mt_mempll_cali(void)
+{
+    int one_count = 0, zero_count = 0;
+    int pll2_done = 0, pll3_done = 0, pll4_done = 0, ret = 0;
+
+    unsigned int temp = 0, pll2_dl = 0, pll3_dl = 0, pll4_dl = 0;
+    int pll2_phase=0, pll3_phase=0, pll4_phase=0;
+    unsigned int jmeter_wait_count;
+
+    /***********************************************
+    * 1. Set jitter meter clock to internal FB path
+    ************************************************/
+    //temp = readl(0x1000F60C);
+    //write_r(0x1000F60C, temp & ~0x200); // 0x1000F60C[9] = 0 PLL2
+
+    //temp = readl(0x1000F614);
+    //write_r(0x1000F614, temp & ~0x200); // 0x1000F614[9] = 0 PLL3
+
+    //temp = readl(0x1000F61C);
+    //write_r(0x1000F61C, temp & ~0x200); // 0x1000F61C[9] = 0 PLL4
+
+    /***********************************************
+    * 2. Set jitter meter count number
+    ************************************************/
+
+    temp = readl(0x1000F1CC) & 0x0000FFFF;
+    write_r(0x1000F1CC, (temp | (MEMPLL_JMETER_CNT<<16))); // 0x1000F1CC[31:16] PLL2 0x400 = 1024 count
+
+    temp = readl(0x1000F1D0) & 0x0000FFFF;
+    write_r(0x1000F1D0, (temp | (MEMPLL_JMETER_CNT<<16))); // 0x1000F1D0[31:16] PLL3 0x400 = 1024 count
+
+    temp = readl(0x1000F1D4) & 0x0000FFFF;
+    write_r(0x1000F1D4, (temp | (MEMPLL_JMETER_CNT<<16))); // 0x1000F1D4[31:16] PLL4 0x400 = 1024 count
+
+    while (1) {
+        /***********************************************
+        * 3. Adjust delay chain tap number
+        ************************************************/
+
+        if (!pll2_done) {
+            if (pll2_phase == 0) {  // initial phase set to 0 for REF and FBK
+                temp = readl(0x1000F610) & ~0xF8000000;
+                write_r(0x1000F610, (temp | (0x00 << 27 )));
+
+                temp = readl(0x1000F610) & ~0x07C00000;
+                write_r(0x1000F610, (temp | (0x00 << 22)));
+            } else if (pll2_phase == 1) { // REF lag FBK, delay FBK
+                temp = readl(0x1000F610) & ~0xF8000000;
+                write_r(0x1000F610, (temp | (0x00 << 27 )));
+
+                temp = readl(0x1000F610) & ~0x07C00000;
+                write_r(0x1000F610, (temp | (pll2_dl << 22)));
+            } else { // REF lead FBK, delay REF
+                temp = readl(0x1000F610) & ~0xF8000000;
+                write_r(0x1000F610, (temp | (pll2_dl << 27 )));
+
+                temp = readl(0x1000F610) & ~0x07C00000;
+                write_r(0x1000F610, (temp | (0x00 << 22)));
+            }
+        }
+
+        if (!pll3_done) {
+            if (pll3_phase == 0) {  // initial phase set to 0 for REF and FBK
+                temp = readl(0x1000F618) & ~0xF8000000;
+                write_r(0x1000F618, (temp | (0x00 << 27 )));
+
+                temp = readl(0x1000F618) & ~0x07C00000;
+                write_r(0x1000F618, (temp | (0x00 << 22)));
+            } else if (pll3_phase == 1) { // REF lag FBK, delay FBK
+                temp = readl(0x1000F618) & ~0xF8000000;
+                write_r(0x1000F618, (temp | (0x00 << 27 )));
+
+                temp = readl(0x1000F618) & ~0x07C00000;
+                write_r(0x1000F618, (temp | (pll3_dl << 22)));
+            } else { // REF lead FBK, delay REF
+                temp = readl(0x1000F618) & ~0xF8000000;
+                write_r(0x1000F618, (temp | (pll3_dl << 27 )));
+
+                temp = readl(0x1000F618) & ~0x07C00000;
+                write_r(0x1000F618, (temp | (0x00 << 22)));
+            }
+        }
+
+        if (!pll4_done) {
+            if (pll4_phase == 0) {  // initial phase set to 0 for REF and FBK
+                temp = readl(0x1000F620) & ~0xF8000000;
+                write_r(0x1000F620, (temp | (0x00 << 27 )));
+
+                temp = readl(0x1000F620) & ~0x07C00000;
+                write_r(0x1000F620, (temp | (0x00 << 22)));
+            } else if (pll4_phase == 1) { // REF lag FBK, delay FBK
+                temp = readl(0x1000F620) & ~0xF8000000;
+                write_r(0x1000F620, (temp | (0x00 << 27 )));
+
+                temp = readl(0x1000F620) & ~0x07C00000;
+                write_r(0x1000F620, (temp | (pll4_dl << 22)));
+            } else { // REF lead FBK, delay REF
+                temp = readl(0x1000F620) & ~0xF8000000;
+                write_r(0x1000F620, (temp | (pll4_dl << 27 )));
+
+                temp = readl(0x1000F620) & ~0x07C00000;
+                write_r(0x1000F620, (temp | (0x00 << 22)));
+            }
+        }
+        spin(20); // wait for external loop ready
+
+        /***********************************************
+        * 4. Enable jitter meter
+        ************************************************/
+
+        if (!pll2_done) {
+            temp = readl(0x1000F1CC);
+            write_r(0x1000F1CC, temp | 0x1); // 0x1000F1CC[0]=1 PLL2
+        }
+
+        if (!pll3_done) {
+            temp = readl(0x1000F1D0);
+            write_r(0x1000F1D0, temp | 0x1); // 0x1000F1D0[0]=1 PLL3
+        }
+
+        if (!pll4_done) {
+            temp = readl(0x1000F1D4);
+            write_r(0x1000F1D4, temp | 0x1); // 0x1000F1D4[0]=1 PLL4
+        }
+
+        // wait for jitter meter done
+        //spin(60); // wait for jitter meter complete (depend on Fref)
+
+        // 1000F32C[2:0] : PLL4 PLL3 PLL2
+
+        jmeter_wait_count = 0;
+        if (!pll2_done) {
+            while (!(readl(0x1000F32C)&0x00000001)) {
+                spin(MEMPLL_JMETER_WAIT_TIME_BASE);
+                jmeter_wait_count++;
+                if (jmeter_wait_count>MEMPLL_JMETER_WAIT_TIMEOUT) {
+                    dprintf(CRITICAL, "[ERROR] PLL2 Jeter Meter Count Timeout > %d us!!\n", MEMPLL_JMETER_WAIT_TIME_BASE*MEMPLL_JMETER_WAIT_TIMEOUT);
+                    break;
+                }
+            }
+        }
+
+        jmeter_wait_count = 0;
+        if (!pll3_done) {
+            while (!(readl(0x1000F32C)&0x00000002)) {
+                spin(MEMPLL_JMETER_WAIT_TIME_BASE);
+                jmeter_wait_count++;
+                if (jmeter_wait_count>MEMPLL_JMETER_WAIT_TIMEOUT) {
+                    dprintf(CRITICAL, "[ERROR] PLL3 Jeter Meter Count Timeout > %d us!!\n", MEMPLL_JMETER_WAIT_TIME_BASE*MEMPLL_JMETER_WAIT_TIMEOUT);
+                    break;
+                }
+            }
+        }
+
+        jmeter_wait_count = 0;
+        if (!pll4_done) {
+            while (!(readl(0x1000F32C)&0x00000004)) {
+                spin(MEMPLL_JMETER_WAIT_TIME_BASE);
+                jmeter_wait_count++;
+                if (jmeter_wait_count>MEMPLL_JMETER_WAIT_TIMEOUT) {
+                    dprintf(CRITICAL, "[ERROR] PLL4 Jeter Meter Count Timeout > %d us!!\n", MEMPLL_JMETER_WAIT_TIME_BASE*MEMPLL_JMETER_WAIT_TIMEOUT);
+                    break;
+                }
+            }
+        }
+
+        /***********************************************
+        * 5. Check jitter meter counter value
+        ************************************************/
+
+        if (!pll2_done) {
+            one_count = readl(0x1000F320) >> 16; // 0x1000F320[31:16] PLL2 one count
+            zero_count = readl(0x1000F320) & 0x0000FFFF; // 0x1000F320[15:0] PLL2 zero count
+
+            if (pll2_phase == 0) {
+                if (one_count > (zero_count+MEMPLL_JMETER_CONFIDENCE_CNT)) {
+                    // REF lag FBK
+                    pll2_phase = 1;
+                    pll2_dl++;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL2 initial phase: REF lag FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                } else if (zero_count > (one_count+MEMPLL_JMETER_CONFIDENCE_CNT)) {
+                    // REF lead FBK
+                    pll2_phase = 2;
+                    pll2_dl++;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL2 initial phase: REF lead FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                } else {
+                    // in phase at initial
+                    pll2_done = 1;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL2 initial phase: REF in-phase FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                }
+            } else if (pll2_phase == 1) {
+                if ((zero_count+MEMPLL_JMETER_CONFIDENCE_CNT) >= one_count) {
+                    pll2_done = 1;
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL2 REF_DL: 0x0, FBK_DL: 0x%x, one_cnt/zero_cnt = %d/%d\n", pll2_dl, one_count, zero_count);
+                } else {
+                    pll2_dl++;
+                }
+            } else {
+                if ((one_count+MEMPLL_JMETER_CONFIDENCE_CNT) >= zero_count) {
+                    pll2_done = 1;
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL2 REF_DL: 0x%x, FBK_DL: 0x0, one_cnt/zero_cnt = %d/%d\n", pll2_dl, one_count, zero_count);
+                } else {
+                    pll2_dl++;
+                }
+            }
+        }
+
+        if (!pll3_done) {
+            one_count = readl(0x1000F324) >> 16; // 0x1000F324[31:16] PLL3 one count
+            zero_count = readl(0x1000F324) & 0x0000FFFF; // 0x1000F324[15:0] PLL3 zero count
+
+            if (pll3_phase == 0) {
+                if (one_count > (zero_count+MEMPLL_JMETER_CONFIDENCE_CNT)) {
+                    // REF lag FBK
+                    pll3_phase = 1;
+                    pll3_dl++;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL3 initial phase: REF lag FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                } else if (zero_count > (one_count+MEMPLL_JMETER_CONFIDENCE_CNT)) {
+                    // REF lead FBK
+                    pll3_phase = 2;
+                    pll3_dl++;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL3 initial phase: REF lead FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                } else {
+                    // in phase at initial
+                    pll3_done = 1;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL3 initial phase: REF in-phase FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                }
+            } else if (pll3_phase == 1) {
+                if ((zero_count+MEMPLL_JMETER_CONFIDENCE_CNT) >= one_count) {
+                    pll3_done = 1;
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL3 REF_DL: 0x0, FBK_DL: 0x%x, one_cnt/zero_cnt = %d/%d\n", pll3_dl, one_count, zero_count);
+                } else {
+                    pll3_dl++;
+                }
+            } else {
+                if ((one_count+MEMPLL_JMETER_CONFIDENCE_CNT) >= zero_count) {
+                    pll3_done = 1;
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL3 REF_DL: 0x%x, FBK_DL: 0x0, one_cnt/zero_cnt = %d/%d\n", pll3_dl, one_count, zero_count);
+                } else {
+                    pll3_dl++;
+                }
+            }
+        }
+
+        if (!pll4_done) {
+            one_count = readl(0x1000F328) >> 16; // 0x1000F328[31:16] PLL4 one count
+            zero_count = readl(0x1000F328) & 0x0000FFFF; // 0x1000F328[15:0] PLL4 zero count
+
+            if (pll4_phase == 0) {
+                if (one_count > (zero_count+MEMPLL_JMETER_CONFIDENCE_CNT)) {
+                    // REF lag FBK
+                    pll4_phase = 1;
+                    pll4_dl++;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL4 initial phase: REF lag FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                } else if (zero_count > (one_count+MEMPLL_JMETER_CONFIDENCE_CNT)) {
+                    // REF lead FBK
+                    pll4_phase = 2;
+                    pll4_dl++;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL4 initial phase: REF lead FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                } else {
+                    // in phase at initial
+                    pll4_done = 1;
+
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL4 initial phase: REF in-phase FBK, one_cnt/zero_cnt = %d/%d\n", one_count, zero_count);
+                }
+            } else if (pll4_phase == 1) {
+                if ((zero_count+MEMPLL_JMETER_CONFIDENCE_CNT) >= one_count) {
+                    pll4_done = 1;
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL4 REF_DL: 0x0, FBK_DL: 0x%x, one_cnt/zero_cnt = %d/%d\n", pll4_dl, one_count, zero_count);
+                } else {
+                    pll4_dl++;
+                }
+            } else {
+                if ((one_count+MEMPLL_JMETER_CONFIDENCE_CNT) >= zero_count) {
+                    pll4_done = 1;
+                    dprintf(CRITICAL, "[PLL_Phase_Calib] PLL4 REF_DL: 0x%x, FBK_DL: 0x0, one_cnt/zero_cnt = %d/%d\n", pll4_dl, one_count, zero_count);
+                } else {
+                    pll4_dl++;
+                }
+            }
+        }
+
+        /***********************************************
+        * 6. Reset jitter meter value
+        ************************************************/
+
+        if (!pll2_done) {
+            pll2_dl++;
+            temp = readl(0x1000F1CC);
+            write_r(0x1000F1CC, temp & ~0x1); // 0x1000F1CC[0]=0 PLL2
+        }
+
+        if (!pll3_done) {
+            pll3_dl++;
+            temp = readl(0x1000F1D0);
+            write_r(0x1000F1D0, temp & ~0x1); // 0x1000F1D0[0]=0 PLL3
+        }
+
+        if (!pll4_done) {
+            pll4_dl++;
+            temp = readl(0x1000F1D4);
+            write_r(0x1000F1D4, temp & ~0x1); // 0x1000F1D4[0]=0 PLL4
+        }
+
+        /*************************************************************
+        * Then return to step 1 to adjust next delay chain tap value.
+        * Until we have ~ 50% of one or zero count on jitter meter
+        **************************************************************/
+
+        if (pll2_done && pll3_done && pll4_done) {
+            ret = 0;
+            break;
+        }
+
+        if (pll2_dl >= 32 || pll3_dl >= 32 || pll4_dl >= 32) {
+            ret = -1;
+            break;
+        }
+    }
+
+#if 0 // for FT, enable it...
+    if (ret != 0) {
+        dprintf(CRITICAL, "[mt_pll_init] MEMPLL 3PLL mode calibration fail\n");
+        while (1); // TBD
+    }
+#endif
+
+    /***********************************************
+    * 7. Set jitter meter clock to external FB path
+    ************************************************/
+
+    temp = readl(0x1000F60C);
+    write_r(0x1000F60C, temp | 0x200); // 0x1000F60C[9] = 1 PLL2
+
+    temp = readl(0x1000F614);
+    write_r(0x1000F614, temp | 0x200); // 0x1000F614[9] = 1 PLL3
+
+    temp = readl(0x1000F61C);
+    write_r(0x1000F61C, temp | 0x200); // 0x1000F61C[9] = 1 PLL4
+
+    return ret;
+}
+
+
+#endif
+
+unsigned int mt_get_mem_freq(void)
+{
+    int output = 0;
+    unsigned int temp, clk26cali_0, clk_cfg_8, clk_misc_cfg_1;
+
+    clk26cali_0 = readl(CLK26CALI_0);
+    write_r(CLK26CALI_0, clk26cali_0 | 0x80); // enable fmeter_en
+
+    clk_misc_cfg_1 = readl(CLK_MISC_CFG_1);
+    write_r(CLK_MISC_CFG_1, 0xFFFFFF00); // select divider
+
+    clk_cfg_8 = readl(CLK_CFG_8);
+    write_r(CLK_CFG_8, (14 << 8)); // select abist_cksw
+
+    temp = readl(CLK26CALI_0);
+    write_r(CLK26CALI_0, temp | 0x1); // start fmeter
+
+    /* wait frequency meter finish */
+    while (readl(CLK26CALI_0) & 0x1) {
+        dprintf(CRITICAL, "wait for frequency meter finish, CLK26CALI = 0x%x\n", readl(CLK26CALI_0));
+        //spin(10000);
+    }
+
+    temp = readl(CLK26CALI_1) & 0xFFFF;
+
+    output = (temp * 26000) / 1024; // Khz
+
+    write_r(CLK_CFG_8, clk_cfg_8);
+    write_r(CLK_MISC_CFG_1, clk_misc_cfg_1);
+    write_r(CLK26CALI_0, clk26cali_0);
+
+    //dprintf(CRITICAL, "CLK26CALI = 0x%x, mem frequency = %d Khz\n", temp, output);
+
+    return output;
+}
+
+#define ENABLE_ARMPLL       1
+#define ENABLE_MAINPLL      1
+#define ENABLE_VENCPLL      1
+#define ENABLE_MMPLL        1
+#define ENABLE_UNIVPLL      1
+#define ENABLE_MSDCPLL      1
+#define ENABLE_AUD1PLL      1
+#define ENABLE_TRGPLL       1
+#define ENABLE_ETHPLL       1
+#define ENABLE_VDECPLL      1
+#define ENABLE_HADDS2PLL    1
+#define ENABLE_AUD2PLL      1
+#define ENABLE_TVDPLL       1
+#define ENABLE_TVD2PLL      1
+
+static void mt_HADDS2PLL_pre_init(void)
+{
+    write_r(0x10006000, 0x0b160001);
+
+    write_r(0x10005170, 0x0000f81f);
+
+    write_r(0x1000623c, 0x0000ff16);
+    write_r(0x1000623c, 0x0000ff1e);
+    write_r(0x1000623c, 0x0000ff0e);
+    write_r(0x1000623c, 0x0000ff0c);
+    write_r(0x1000623c, 0x0000fe0c);
+    write_r(0x1000623c, 0x0000fc0c);
+    write_r(0x1000623c, 0x0000f80c);
+    write_r(0x1000623c, 0x0000f00c);
+    write_r(0x1000623c, 0x0000f00d);
+
+    write_r(0x1000629c, 0x0000ff16);
+    write_r(0x1000629c, 0x0000ff1e);
+    write_r(0x1000629c, 0x0000ff0e);
+    write_r(0x1000629c, 0x0000ff0c);
+    write_r(0x1000629c, 0x0000fe0c);
+    write_r(0x1000629c, 0x0000fc0c);
+    write_r(0x1000629c, 0x0000f80c);
+    write_r(0x1000629c, 0x0000f00c);
+    write_r(0x1000629c, 0x0000f00d);
+    write_r(0x1c000100, 0x00000000);
+    write_r(0x1c000110, 0x00000000);
+    write_r(0x1c000104, 0x00000000);
+    write_r(0x1c00001c, 0xfb5aab4f);
+    write_r(0x1c000020, 0x00000000);
+    write_r(0x1c000024, 0x00000001);
+};
+
+void mt_pll_init(void)
+{
+    volatile unsigned int temp;
+
+    write_r(ACLKEN_DIV, 0x12); // MCU Bus DIV2
+
+    write_r(CLKSQ_STB_CON0, 0x00050001); // reduce CLKSQ disable time
+
+    write_r(PLL_ISO_CON0, 0x00000888); // extend PWR/ISO control timing to 1us
+
+    write_r(AP_PLL_CON3, 0x00F80000); // set CHG bypass delay
+
+    //write_r(ACLKEN_DIV, 0x12); // MCU Bus DIV2
+
+    /*************
+    * xPLL PWR ON
+    **************/
+
+#if ENABLE_ARMPLL
+    temp = readl(ARMPLL_PWR_CON0);
+    write_r(ARMPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_MAINPLL
+    temp = readl(MAINPLL_PWR_CON0);
+    write_r(MAINPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_VENCPLL
+    temp = readl(VENCPLL_PWR_CON0);
+    write_r(VENCPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_MMPLL
+    temp = readl(MMPLL_PWR_CON0);
+    write_r(MMPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_UNIVPLL
+    temp = readl(UNIVPLL_PWR_CON0);
+    write_r(UNIVPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_MSDCPLL
+    temp = readl(MSDCPLL_PWR_CON0);
+    write_r(MSDCPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_AUD1PLL
+    temp = readl(AUD1PLL_PWR_CON0);
+    write_r(AUD1PLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_TRGPLL
+    temp = readl(TRGPLL_PWR_CON0);
+    write_r(TRGPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_ETHPLL
+    temp = readl(ETHPLL_PWR_CON0);
+    write_r(ETHPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_VDECPLL
+    temp = readl(VDECPLL_PWR_CON0);
+    write_r(VDECPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_HADDS2PLL
+    temp = readl(HADDS2PLL_PWR_CON0);
+    write_r(HADDS2PLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_AUD2PLL
+    temp = readl(AUD2PLL_PWR_CON0);
+    write_r(AUD2PLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_TVDPLL
+    temp = readl(TVDPLL_PWR_CON0);
+    write_r(TVDPLL_PWR_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_TVD2PLL
+    temp = readl(TVD2PLL_PWR_CON0);
+    write_r(TVD2PLL_PWR_CON0, temp | 0x1);
+#endif
+
+    spin(5); // wait for xPLL_PWR_ON ready (min delay is 1us)
+
+    /******************
+    * xPLL ISO Disable
+    *******************/
+
+#if ENABLE_ARMPLL
+    temp = readl(ARMPLL_PWR_CON0);
+    write_r(ARMPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_MAINPLL
+    temp = readl(MAINPLL_PWR_CON0);
+    write_r(MAINPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_VENCPLL
+    temp = readl(VENCPLL_PWR_CON0);
+    write_r(VENCPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_MMPLL
+    temp = readl(MMPLL_PWR_CON0);
+    write_r(MMPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_UNIVPLL
+    temp = readl(UNIVPLL_PWR_CON0);
+    write_r(UNIVPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_MSDCPLL
+    temp = readl(MSDCPLL_PWR_CON0);
+    write_r(MSDCPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_AUD1PLL
+    temp = readl(AUD1PLL_PWR_CON0);
+    write_r(AUD1PLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_TRGPLL
+    temp = readl(TRGPLL_PWR_CON0);
+    write_r(TRGPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_ETHPLL
+    temp = readl(ETHPLL_PWR_CON0);
+    write_r(ETHPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_VDECPLL
+    temp = readl(VDECPLL_PWR_CON0);
+    write_r(VDECPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_HADDS2PLL
+    temp = readl(HADDS2PLL_PWR_CON0);
+    write_r(HADDS2PLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_AUD2PLL
+    temp = readl(AUD2PLL_PWR_CON0);
+    write_r(AUD2PLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_TVDPLL
+    temp = readl(TVDPLL_PWR_CON0);
+    write_r(TVDPLL_PWR_CON0, temp & ~0x2);
+#endif
+
+#if ENABLE_TVD2PLL
+    temp = readl(TVD2PLL_PWR_CON0);
+    write_r(TVD2PLL_PWR_CON0, temp & ~0x2);
+#endif
+
+    /********************
+    * xPLL Frequency Set
+    *********************/
+
+#if ENABLE_ARMPLL
+    // write_r(ARMPLL_CON1, 0x8109A000); // 500.5Mhz
+    //write_r(ARMPLL_CON1, 0x800CE000); // 1339Mhz
+    //write_r(ARMPLL_CON1, 0x800A2000); // 1053Mhz
+    write_r(ARMPLL_CON1, 0x800A0000); // 1040Mhz
+    // write_r(ARMPLL_CON1, 0x800A8000); // 1092Mhz
+#endif // ENABLE_ARMPLL
+
+#if ENABLE_MAINPLL
+    write_r(MAINPLL_CON1, 0x80150000);
+#endif
+
+#if ENABLE_VENCPLL
+    temp = ((~(readl(VENCPLL_CON1) & 0x80000000)) & 0x80000000);
+    write_r(VENCPLL_CON1, temp | 0x000B6000);
+#endif
+
+#if ENABLE_MMPLL
+    //write_r(MMPLL_CON1, 0x80134000);
+    write_r(MMPLL_CON1, 0x80134000);
+#endif
+
+#if ENABLE_UNIVPLL
+    write_r(UNIVPLL_CON1, 0x800C0000);
+#endif
+
+#if ENABLE_MSDCPLL
+    //write_r(MSDCPLL_CON1, 0x800F6275);
+    write_r(MSDCPLL_CON1, 0x800F6276);
+#endif
+
+#if ENABLE_AUD1PLL
+    write_r(AUD1PLL_CON1, 0xAD5EFEE5);
+#endif
+
+#if ENABLE_TRGPLL
+    write_r(TRGPLL_CON1, 0xB7C4EC52);
+#endif
+
+#if ENABLE_ETHPLL
+    write_r(ETHPLL_CON1, 0xCCEC4EC6);
+#endif
+
+#if ENABLE_VDECPLL
+    write_r(VDECPLL_CON1, 0xB4000000);
+#endif
+
+#if ENABLE_HADDS2PLL
+    mt_HADDS2PLL_pre_init();
+    write_r(0x1c006118, 0x00000100);
+    write_r(0x1c006154, 0x2d5efee6);
+    write_r(0x1c006118, 0x00000300);
+#endif
+
+#if ENABLE_AUD2PLL
+    write_r(AUD2PLL_CON1, 0xA9AF46FD);
+#endif
+
+#if ENABLE_TVDPLL
+    write_r(TVDPLL_CON1, 0x800B6C4E);
+#endif
+
+#if ENABLE_TVD2PLL
+    write_r(TVD2PLL_CON1, 0x800B6C4E);
+#endif
+
+    /***********************
+    * xPLL Frequency Enable
+    ************************/
+
+#if ENABLE_ARMPLL
+    temp = readl(ARMPLL_CON0);
+    write_r(ARMPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_MAINPLL
+    temp = readl(MAINPLL_CON0);
+    write_r(MAINPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_VENCPLL
+    temp = readl(VENCPLL_CON0);
+    write_r(VENCPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_MMPLL
+    temp = readl(MMPLL_CON0);
+    write_r(MMPLL_CON0, (temp & ~0x70) | 0x20);
+
+    temp = readl(MMPLL_CON0);
+    write_r(MMPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_UNIVPLL
+    temp = readl(UNIVPLL_CON0);
+    write_r(UNIVPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_MSDCPLL
+    temp = readl(MSDCPLL_CON0);
+    write_r(MSDCPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_AUD1PLL
+    temp = readl(AUD1PLL_CON0);
+    write_r(AUD1PLL_CON0, (temp & ~0x70) | 0x20);  // AUD1PLL_POSDIV = 4
+
+    temp = readl(AUD1PLL_CON0);
+    write_r(AUD1PLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_TRGPLL
+    temp = readl(TRGPLL_CON0);
+    write_r(TRGPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_ETHPLL
+    temp = readl(ETHPLL_CON0);
+    write_r(ETHPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_VDECPLL
+    temp = readl(VDECPLL_CON0);
+    write_r(VDECPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_HADDS2PLL
+    temp = readl(HADDS2PLL_CON0);
+    write_r(HADDS2PLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_AUD2PLL
+    temp = readl(AUD2PLL_CON0);
+    write_r(AUD2PLL_CON0, (temp & ~0x70) | 0x20);
+
+    temp = readl(AUD2PLL_CON0);
+    write_r(AUD2PLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_TVDPLL
+    temp = readl(TVDPLL_CON0);
+    write_r(TVDPLL_CON0, temp | 0x1);
+#endif
+
+#if ENABLE_TVD2PLL
+    temp = readl(TVD2PLL_CON0);
+    write_r(TVD2PLL_CON0, temp | 0x1);
+#endif
+
+    spin(40); // wait for PLL stable (min delay is 20us)
+
+    /***************
+    * xPLL DIV RSTB
+    ****************/
+
+#if ENABLE_MAINPLL
+    temp = readl(MAINPLL_CON0);
+    write_r(MAINPLL_CON0, temp | 0x01000000);
+#endif
+
+#if ENABLE_UNIVPLL
+    temp = readl(UNIVPLL_CON0);
+    write_r(UNIVPLL_CON0, temp | 0x01000000);
+#endif
+
+    /*****************
+    * xPLL HW Control
+    ******************/
+
+    temp = readl(AP_PLL_CON1);
+    //write_r(AP_PLL_CON1, temp & 0xFF00FC0C);
+    write_r(AP_PLL_CON1, temp & 0xFFCCFDCC); // UNIVPLL, MMPLL SW Control
+    //write_r(AP_PLL_CON1, temp & 0xFFEEFFEC); // UNIVPLL, MMPLL, MAINPLL SW Control
+
+    temp = readl(AP_PLL_CON2);
+    //write_r(AP_PLL_CON2, temp & 0xFFFFFFF8);
+    write_r(AP_PLL_CON2, temp & 0xFFFFFFFC); // UNIVPLL, MMPLL SW Control
+    //write_r(AP_PLL_CON2, temp & 0xFFFFFFFE); // UNIVPLL, MMPLL, MAINPLL SW Control
+
+    /* PCIe PHY:
+     * bit2 = 1'1
+     * bit6 = 1'1
+     */
+    write_r(AP_PLL_CON0, 0x7e00e177);
+
+    /*************
+    * MEMPLL Init
+    **************/
+    mt_mempll_init(DDR533, DDR_PLL_MODE);
+
+    /**************
+    * INFRA CLKMUX
+    ***************/
+
+    temp = readl(TOP_DCMCTL);
+    write_r(TOP_DCMCTL, temp | 0x1); // enable infrasys DCM
+
+    write_r(TOP_CKDIV1, 0x0); // CPU clock divide by 1
+
+    temp = readl(TOP_CKMUXSEL) & ~0xC;
+    write_r(TOP_CKMUXSEL, temp | 0x4); // switch CPU clock to ARMPLL
+
+    /************
+    * TOP CLKMUX
+    *************/
+
+    write_r(CLK_CFG_0, 0x01010101);
+
+    write_r(CLK_CFG_1, 0x00010100); // cmtg = pwm = 26Mhz
+
+    write_r(CLK_CFG_2, 0x01010100); // uart = 26Mhz
+
+    write_r(CLK_CFG_3, 0x01000101); // audio = 26Mhz
+
+    write_r(CLK_CFG_4, 0x01050100); // scp = 66Mhz, pmic = 26Mhz
+
+    write_r(CLK_CFG_5, 0x00010106); // MT8127
+
+    write_r(CLK_CFG_6, 0x02000100); // MT8127
+
+    write_r(CLK_CFG_7, 0x02020102); // MT2701
+
+    write_r(CLK_CFG_12, 0x01000201); // MT2701
+
+    write_r(CLK_CFG_13, 0x01000002); // MT2701
+
+    write_r(CLK_CFG_14, 0x01020100); // MT2701
+
+    write_r(CLK_CFG_15, 0x00000101); // MT2701
+
+    write_r(CLK_SCP_CFG_0, 0x3FF); // enable scpsys clock off control
+
+    write_r(CLK_SCP_CFG_1, 0x11); // enable scpsys clock off control
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/rules.mk b/src/bsp/lk/platform/mt2701/drivers/rules.mk
new file mode 100644
index 0000000..10e52f9
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/rules.mk
@@ -0,0 +1,19 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+MODULE := $(LOCAL_DIR)
+
+
+MODULE_SRCS += \
+    $(LOCAL_DIR)/uart/uart.c \
+    $(LOCAL_DIR)/timer/timer.c \
+    $(LOCAL_DIR)/pll/pll.c \
+    $(LOCAL_DIR)/wdt/mtk_wdt.c \
+    $(LOCAL_DIR)/usb/mt_usb.c \
+    $(LOCAL_DIR)/key/mtk_key.c \
+
+MODULE_DEPS += \
+    lib/bio \
+    lib/partition \
+    lib/fdt \
+    $(LOCAL_DIR)/../../../../dramk_2701/dram \
+
+include make/module.mk
diff --git a/src/bsp/lk/platform/mt2701/drivers/timer/timer.c b/src/bsp/lk/platform/mt2701/drivers/timer/timer.c
new file mode 100644
index 0000000..03ce29a
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/timer/timer.c
@@ -0,0 +1,84 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#include <platform/mt2701.h>
+#include <platform/mt_reg_base.h>
+
+#define GPT6__CON                   ((volatile unsigned int *)(APMCU_GPTIMER_BASE+0x0060))
+#define GPT6__CLK                   ((volatile unsigned int *)(APMCU_GPTIMER_BASE+0x0064))
+
+#define GPT4_EN                     0x0001
+#define GPT4_FREERUN                0x0030
+#define GPT4_SYS_CLK                0x0000
+
+#define GPT_SET_BITS(BS,REG)       ((*(volatile u32*)(REG)) |= (u32)(BS))
+#define GPT_CLR_BITS(BS,REG)       ((*(volatile u32*)(REG)) &= ~((u32)(BS)))
+
+void set_cntfrq(unsigned long freq)
+{
+    __asm__ volatile("mcr p15, 0, %0, c14, c0, 0\n" :: "r"(freq));
+}
+
+/*
+ * GPT6 fixed 13MHz counter
+ */
+static void gpt_power_on(uchar bPowerOn)
+{
+#define AP_PERI_GLOBALCON_PDN0 (PERI_CON_BASE+0x10)
+    if (!bPowerOn) {
+        GPT_SET_BITS(1<<13, AP_PERI_GLOBALCON_PDN0);
+    } else {
+        GPT_CLR_BITS(1<<13, AP_PERI_GLOBALCON_PDN0);
+    }
+
+    set_cntfrq(13000000);
+}
+
+static void gpt6_start(void)
+{
+    *GPT6__CLK = (GPT4_SYS_CLK);
+    *GPT6__CON = (GPT4_FREERUN | GPT4_EN);
+}
+
+static void gpt6_stop(void)
+{
+    *GPT6__CON = 0x0;   /* disable */
+    *GPT6__CON = 0x2;   /* clear counter */
+}
+
+void gpt6_init(uchar bStart)
+{
+    /* power on GPT */
+    /* gpt_power_on(TRUE); */
+    /* clear GPT6 first */
+    gpt6_stop();
+    /* enable GPT6 without lock */
+    if (bStart) {
+        gpt6_start();
+    }
+}
+
+void mtk_timer_init (void)
+{
+    gpt6_init(true);
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/uart/uart.c b/src/bsp/lk/platform/mt2701/drivers/uart/uart.c
new file mode 100644
index 0000000..db0cef9
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/uart/uart.c
@@ -0,0 +1,270 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <debug.h>
+#include <reg.h>
+#include <dev/uart.h>
+#include <platform/mt_reg_base.h>
+#include <platform/mt_uart.h>
+#include <string.h>
+
+typedef enum {
+    UART1 = UART1_BASE,
+    UART2 = UART2_BASE,
+    UART3 = UART3_BASE,
+    UART4 = UART4_BASE
+} MTK_UART;
+
+/* FCR */
+#define UART_FCR_FIFOE              (1 << 0)
+#define UART_FCR_CLRR               (1 << 1)
+#define UART_FCR_CLRT               (1 << 2)
+#define UART_FCR_DMA1               (1 << 3)
+#define UART_FCR_RXFIFO_1B_TRI      (0 << 6)
+#define UART_FCR_RXFIFO_6B_TRI      (1 << 6)
+#define UART_FCR_RXFIFO_12B_TRI     (2 << 6)
+#define UART_FCR_RXFIFO_RX_TRI      (3 << 6)
+#define UART_FCR_TXFIFO_1B_TRI      (0 << 4)
+#define UART_FCR_TXFIFO_4B_TRI      (1 << 4)
+#define UART_FCR_TXFIFO_8B_TRI      (2 << 4)
+#define UART_FCR_TXFIFO_14B_TRI     (3 << 4)
+
+#define UART_FCR_FIFO_INIT          (UART_FCR_FIFOE|UART_FCR_CLRR|UART_FCR_CLRT)
+#define UART_FCR_NORMAL             (UART_FCR_FIFO_INIT | \
+                                     UART_FCR_TXFIFO_4B_TRI| \
+                                     UART_FCR_RXFIFO_12B_TRI)
+
+/* LCR */
+#define UART_LCR_BREAK              (1 << 6)
+#define UART_LCR_DLAB               (1 << 7)
+
+#define UART_WLS_5                  (0 << 0)
+#define UART_WLS_6                  (1 << 0)
+#define UART_WLS_7                  (2 << 0)
+#define UART_WLS_8                  (3 << 0)
+#define UART_WLS_MASK               (3 << 0)
+
+#define UART_1_STOP                 (0 << 2)
+#define UART_2_STOP                 (1 << 2)
+#define UART_1_5_STOP               (1 << 2)    /* Only when WLS=5 */
+#define UART_STOP_MASK              (1 << 2)
+
+#define UART_NONE_PARITY            (0 << 3)
+#define UART_ODD_PARITY             (0x1 << 3)
+#define UART_EVEN_PARITY            (0x3 << 3)
+#define UART_MARK_PARITY            (0x5 << 3)
+#define UART_SPACE_PARITY           (0x7 << 3)
+#define UART_PARITY_MASK            (0x7 << 3)
+
+/* MCR */
+#define UART_MCR_DTR                (1 << 0)
+#define UART_MCR_RTS                (1 << 1)
+#define UART_MCR_OUT1               (1 << 2)
+#define UART_MCR_OUT2               (1 << 3)
+#define UART_MCR_LOOP               (1 << 4)
+#define UART_MCR_XOFF               (1 << 7)    /* read only */
+#define UART_MCR_NORMAL             (UART_MCR_DTR|UART_MCR_RTS)
+
+/* LSR */
+#define UART_LSR_DR                 (1 << 0)
+#define UART_LSR_OE                 (1 << 1)
+#define UART_LSR_PE                 (1 << 2)
+#define UART_LSR_FE                 (1 << 3)
+#define UART_LSR_BI                 (1 << 4)
+#define UART_LSR_THRE               (1 << 5)
+#define UART_LSR_TEMT               (1 << 6)
+#define UART_LSR_FIFOERR            (1 << 7)
+
+/* MSR */
+#define UART_MSR_DCTS               (1 << 0)
+#define UART_MSR_DDSR               (1 << 1)
+#define UART_MSR_TERI               (1 << 2)
+#define UART_MSR_DDCD               (1 << 3)
+#define UART_MSR_CTS                (1 << 4)
+#define UART_MSR_DSR                (1 << 5)
+#define UART_MSR_RI                 (1 << 6)
+#define UART_MSR_DCD                (1 << 7)
+
+#define CONFIG_BAUDRATE         921600
+
+#define UART_BASE(uart)                   (uart)
+
+#define UART_RBR(uart)                    (UART_BASE(uart)+0x0)  /* Read only */
+#define UART_THR(uart)                    (UART_BASE(uart)+0x0)  /* Write only */
+#define UART_IER(uart)                    (UART_BASE(uart)+0x4)
+#define UART_IIR(uart)                    (UART_BASE(uart)+0x8)  /* Read only */
+#define UART_FCR(uart)                    (UART_BASE(uart)+0x8)  /* Write only */
+#define UART_LCR(uart)                    (UART_BASE(uart)+0xc)
+#define UART_MCR(uart)                    (UART_BASE(uart)+0x10)
+#define UART_LSR(uart)                    (UART_BASE(uart)+0x14)
+#define UART_MSR(uart)                    (UART_BASE(uart)+0x18)
+#define UART_SCR(uart)                    (UART_BASE(uart)+0x1c)
+#define UART_DLL(uart)                    (UART_BASE(uart)+0x0)  /* Only when LCR.DLAB = 1 */
+#define UART_DLH(uart)                    (UART_BASE(uart)+0x4)  /* Only when LCR.DLAB = 1 */
+#define UART_EFR(uart)                    (UART_BASE(uart)+0x8)  /* Only when LCR = 0xbf */
+#define UART_XON1(uart)                   (UART_BASE(uart)+0x10) /* Only when LCR = 0xbf */
+#define UART_XON2(uart)                   (UART_BASE(uart)+0x14) /* Only when LCR = 0xbf */
+#define UART_XOFF1(uart)                  (UART_BASE(uart)+0x18) /* Only when LCR = 0xbf */
+#define UART_XOFF2(uart)                  (UART_BASE(uart)+0x1c) /* Only when LCR = 0xbf */
+#define UART_AUTOBAUD_EN(uart)            (UART_BASE(uart)+0x20)
+#define UART_HIGHSPEED(uart)              (UART_BASE(uart)+0x24)
+#define UART_SAMPLE_COUNT(uart)           (UART_BASE(uart)+0x28)
+#define UART_SAMPLE_POINT(uart)           (UART_BASE(uart)+0x2c)
+#define UART_AUTOBAUD_REG(uart)           (UART_BASE(uart)+0x30)
+#define UART_RATE_FIX_AD(uart)            (UART_BASE(uart)+0x34)
+#define UART_AUTOBAUD_SAMPLE(uart)        (UART_BASE(uart)+0x38)
+#define UART_GUARD(uart)                  (UART_BASE(uart)+0x3c)
+#define UART_ESCAPE_DAT(uart)             (UART_BASE(uart)+0x40)
+#define UART_ESCAPE_EN(uart)              (UART_BASE(uart)+0x44)
+#define UART_SLEEP_EN(uart)               (UART_BASE(uart)+0x48)
+#define UART_VFIFO_EN(uart)               (UART_BASE(uart)+0x4c)
+#define UART_RXTRI_AD(uart)               (UART_BASE(uart)+0x50)
+
+// output uart port
+volatile unsigned int g_uart;
+
+//extern unsigned int mtk_get_bus_freq(void);
+#define UART_SRC_CLK 26000000
+
+static void uart_setbrg(void)
+{
+    unsigned int byte,speed;
+    unsigned int highspeed;
+    unsigned int quot, divisor, remainder;
+    unsigned int uartclk;
+    unsigned short data, high_speed_div, sample_count, sample_point;
+    unsigned int tmp_div;
+
+    speed = CONFIG_BAUDRATE;
+    uartclk = UART_SRC_CLK;
+    //uartclk = (unsigned int)(mtk_get_bus_freq()*1000/4);
+    if (speed <= 115200 ) {
+        highspeed = 0;
+        quot = 16;
+    } else {
+        highspeed = 3;
+        quot = 1;
+    }
+
+    if (highspeed < 3) { /*0~2*/
+        /* Set divisor DLL and DLH  */
+        divisor   =  uartclk / (quot * speed);
+        remainder =  uartclk % (quot * speed);
+
+        if (remainder >= (quot / 2) * speed)
+            divisor += 1;
+
+        writel(highspeed, UART_HIGHSPEED(g_uart));
+        byte = readl(UART_LCR(g_uart));   /* DLAB start */
+        writel((byte | UART_LCR_DLAB), UART_LCR(g_uart));
+        writel((divisor & 0x00ff), UART_DLL(g_uart));
+        writel(((divisor >> 8)&0x00ff), UART_DLH(g_uart));
+        writel(byte, UART_LCR(g_uart));   /* DLAB end */
+    } else {
+        data=(unsigned short)(uartclk/speed);
+        high_speed_div = (data>>8) + 1; // divided by 256
+
+        tmp_div=uartclk/(speed*high_speed_div);
+        divisor =  (unsigned short)tmp_div;
+
+        remainder = (uartclk)%(high_speed_div*speed);
+        /*get (sample_count+1)*/
+        if (remainder >= ((speed)*(high_speed_div))>>1)
+            divisor =  (unsigned short)(tmp_div+1);
+        else
+            divisor =  (unsigned short)tmp_div;
+
+        sample_count=divisor-1;
+
+        /*get the sample point*/
+        sample_point=(sample_count-1)>>1;
+
+        /*configure register*/
+        writel(highspeed, UART_HIGHSPEED(g_uart));
+
+        byte = readl(UART_LCR(g_uart));    /* DLAB start */
+        writel((byte | UART_LCR_DLAB), UART_LCR(g_uart));
+        writel((high_speed_div & 0x00ff), UART_DLL(g_uart));
+        writel(((high_speed_div >> 8)&0x00ff), UART_DLH(g_uart));
+        writel(sample_count, UART_SAMPLE_COUNT(g_uart));
+        writel(sample_point, UART_SAMPLE_POINT(g_uart));
+        writel(byte, UART_LCR(g_uart));   /* DLAB end */
+    }
+}
+
+static void mtk_set_current_uart(MTK_UART uart_base)
+{
+    g_uart = uart_base;
+}
+
+void uart_init_early(void)
+{
+    mtk_set_current_uart(UART1);
+    /* clear fifo */
+    writel(readl(UART_FCR(g_uart)) + UART_FCR_FIFO_INIT, UART_FCR(g_uart));
+    writel(UART_NONE_PARITY | UART_WLS_8 | UART_1_STOP, UART_LCR(g_uart));
+    uart_setbrg();
+}
+
+void uart_init(void)
+{
+}
+
+int uart_putc(int port, char c)
+{
+    while (!(readl(UART_LSR(g_uart)) & UART_LSR_THRE));
+
+    if (c == '\n')
+        writel((unsigned int)'\r', UART_THR(g_uart));
+
+    writel((unsigned int)c, UART_THR(g_uart));
+
+    return 0;
+}
+
+int uart_getc(int port, bool wait)  /* returns -1 if no data available */
+{
+    do{
+        if(!(readl(UART_LSR(g_uart)) & UART_LSR_DR))
+          return (int)readl(UART_RBR(g_uart));
+    }while(wait);
+    return -1;
+}
+
+int uart_pputc(int port, char c)
+{
+    return uart_putc(port, c);
+}
+
+int uart_pgetc(int port)
+{
+    return uart_getc(port, 0);
+}
+
+bool check_uart_enter(void)
+{
+    if ((int)readl(UART_RBR(g_uart)) == 13)
+        return true;
+    else
+        return false;
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/usb/mt_usb.c b/src/bsp/lk/platform/mt2701/drivers/usb/mt_usb.c
new file mode 100644
index 0000000..08f951c
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/usb/mt_usb.c
@@ -0,0 +1,1976 @@
+/*
+ * Copyright (c) 2012 MediaTek Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *  * Redistributions of source code must retain the above copyright
+ *  notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *  notice, this list of conditions and the following disclaimer in
+ *  the documentation and/or other materials provided with the
+ *  distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+ * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <debug.h>
+#include <dev/udc.h>
+#include <err.h>
+#include <kernel/thread.h>
+#include <platform/interrupts.h>
+#include <platform/mt_irq.h>
+#include <platform/mt_reg_base.h>
+#include <platform/mt_usb.h>
+#include <platform/udc-common.h>
+#include <reg.h>
+#include <string.h>
+#include <stdlib.h>
+#include <sys/types.h>
+
+#define writew(v, a) (*REG16(a) = (v))
+#define readw(a) (*REG16(a))
+
+#define USB_DOUBLE_BUF
+
+#define USB_GINTR
+
+#ifdef USB_DEBUG
+/* DEBUG INFO Sections */
+#define DBG_USB_DUMP_DESC 0
+#define DBG_USB_DUMP_DATA 0
+#define DBG_USB_DUMP_SETUP 1
+#define DBG_USB_FIFO 0
+#define DBG_USB_GENERAL 1
+#define DBG_PHY_CALIBRATION 0
+#endif
+
+#define DBG_C(x...) dprintf(CRITICAL, x)
+#define DBG_I(x...) dprintf(INFO, x)
+#define DBG_S(x...) dprintf(SPEW, x)
+
+#if DBG_USB_GENERAL
+#define DBG_IRQ(x...) dprintf(INFO, x)
+#else
+#define DBG_IRQ(x...) do{} while(0)
+#endif
+
+/* bits used in all the endpoint status registers */
+#define EPT_TX(n) (1 << ((n) + 16))
+#define EPT_RX(n) (1 << (n))
+
+/* udc.h wrapper for usbdcore */
+
+static unsigned char usb_config_value = 0;
+EP0_STATE ep0_state = EP0_IDLE;
+int set_address = 0;
+u32 fifo_addr = FIFO_ADDR_START;
+
+#define EP0 0
+
+/* USB transfer directions */
+#define USB_DIR_IN  DEVICE_WRITE    /* val: 0x80 */
+#define USB_DIR_OUT DEVICE_READ /* val: 0x00 */
+
+#define EP0_MAX_PACKET_SIZE 64
+
+/* Request types */
+#define USB_TYPE_STANDARD   (0x00 << 5)
+#define USB_TYPE_CLASS      (0x01 << 5)
+#define USB_TYPE_VENDOR     (0x02 << 5)
+#define USB_TYPE_RESERVED   (0x03 << 5)
+
+/* values used in GET_STATUS requests */
+#define USB_STAT_SELFPOWERED    0x01
+
+/* USB recipients */
+#define USB_RECIP_DEVICE    0x00
+#define USB_RECIP_INTERFACE 0x01
+#define USB_RECIP_ENDPOINT  0x02
+#define USB_RECIP_OTHER     0x03
+
+/* Endpoints */
+#define USB_EP_NUM_MASK 0x0f        /* in bEndpointAddress */
+#define USB_EP_DIR_MASK 0x80
+
+#define USB_TYPE_MASK   0x60
+#define USB_RECIP_MASK  0x1f
+
+#if CFG_FPGA_PLATFORM
+/* for usb phy */
+#include <platform/mt_i2c.h>
+#endif
+
+#define URB_BUF_SIZE 512
+
+struct urb {
+    struct udc_endpoint *endpoint;
+    struct udc_device *device;
+    struct setup_packet device_request;
+
+    u8 *buffer;
+    unsigned int actual_length;
+};
+
+static struct udc_endpoint *ep0in, *ep0out;
+static struct udc_request *ep0req;
+struct urb mt_ep0_urb;
+struct urb mt_tx_urb;
+struct urb mt_rx_urb;
+struct urb *ep0_urb = &mt_ep0_urb;
+struct urb *tx_urb = &mt_tx_urb;
+struct urb *rx_urb = &mt_rx_urb;
+
+/* endpoint data - mt_ep */
+struct udc_endpoint {
+    /* rx side */
+    struct urb *rcv_urb;    /* active urb */
+
+    /* tx side */
+    struct urb *tx_urb; /* active urb */
+
+    /* info from hsusb */
+    struct udc_request *req;
+    unsigned int bit;   /* EPT_TX/EPT_RX */
+    unsigned char num;
+    unsigned char in;
+    unsigned short maxpkt;
+    int status; /* status for error handling */
+
+    unsigned int sent;      /* data already sent */
+    unsigned int last;      /* data sent in last packet XXX do we need this */
+    unsigned char mode; /* double buffer */
+};
+
+/* from mt_usbtty.h */
+#define NUM_ENDPOINTS   3
+
+/* origin endpoint_array */
+struct udc_endpoint ep_list[NUM_ENDPOINTS + 1]; /* one extra for control endpoint */
+
+static int usb_online = 0;
+
+static u8 dev_address = 0;
+
+static struct udc_device *the_device;
+static struct udc_gadget *the_gadget;
+/* end from hsusb.c */
+
+/* declare ept_complete handle */
+static void handle_ept_complete(struct udc_endpoint *ept);
+
+/* use mt_typedefs.h */
+#define USBPHY_READ8(offset)        readb(USB20_PHY_BASE+offset)
+#define USBPHY_WRITE8(offset, value)    writeb(value, USB20_PHY_BASE+offset)
+#define USBPHY_SET8(offset, mask)   USBPHY_WRITE8(offset, (USBPHY_READ8(offset)) | (mask))
+#define USBPHY_CLR8(offset, mask)   USBPHY_WRITE8(offset, (USBPHY_READ8(offset)) & (~mask))
+
+#define USB11PHY_READ8(offset)      readb(USB11_PHY_BASE+offset)
+#define USB11PHY_WRITE8(offset, value)  writeb(value, USB11_PHY_BASE+offset)
+#define USB11PHY_SET8(offset, mask) USB11PHY_WRITE8(offset, (USB11PHY_READ8(offset)) | (mask))
+#define USB11PHY_CLR8(offset, mask) USB11PHY_WRITE8(offset, (USB11PHY_READ8(offset)) & (~mask))
+
+static void mt_usb_phy_poweron(void)
+{
+    /*
+     * swtich to USB function.
+     * (system register, force ip into usb mode).
+     */
+    USBPHY_CLR8(0x6b, 0x04);
+    USBPHY_CLR8(0x6e, 0x01);
+
+    /* RG_USB20_BC11_SW_EN = 1'b0 */
+    USBPHY_CLR8(0x1a, 0x80);
+
+    /* RG_USB20_DP_100K_EN = 1'b0 */
+    /* RG_USB20_DP_100K_EN = 1'b0 */
+    USBPHY_CLR8(0x22, 0x03);
+
+    /* release force suspendm */
+    USBPHY_CLR8(0x6a, 0x04);
+
+    spin(800);
+
+    /* force enter device mode */
+    USBPHY_CLR8(0x6c, 0x10);
+    USBPHY_SET8(0x6c, 0x2E);
+    USBPHY_SET8(0x6d, 0x3E);
+
+    return;
+}
+
+static void mt_usb_phy_savecurrent(void)
+{
+    /*
+     * swtich to USB function.
+     * (system register, force ip into usb mode).
+     */
+    USBPHY_CLR8(0x6b, 0x04);
+    USBPHY_CLR8(0x6e, 0x01);
+
+    /* release force suspendm */
+    USBPHY_CLR8(0x6a, 0x04);
+    /* RG_DPPULLDOWN./RG_DMPULLDOWN. */
+    USBPHY_SET8(0x68, 0xc0);
+    /* RG_XCVRSEL[1:0] = 2'b01 */
+    USBPHY_CLR8(0x68, 0x30);
+    USBPHY_SET8(0x68, 0x10);
+    /* RG_TERMSEL = 1'b1 */
+    USBPHY_SET8(0x68, 0x04);
+    /* RG_DATAIN[3:0] = 4'b0000 */
+    USBPHY_CLR8(0x69, 0x3c);
+
+    /*
+     * force_dp_pulldown, force_dm_pulldown,
+     * force_xcversel, force_termsel.
+     */
+    USBPHY_SET8(0x6a, 0xba);
+
+    /* RG_USB20_BC11_SW_EN = 1'b0 */
+    USBPHY_CLR8(0x1a, 0x80);
+    /* RG_USB20_OTG_VBUSSCMP_EN = 1'b0 */
+    USBPHY_CLR8(0x1a, 0x10);
+
+    spin(800);
+
+    /* rg_usb20_pll_stable = 1 */
+    USBPHY_SET8(0x63, 0x02);
+
+    spin(1);
+
+    /* force suspendm = 1 */
+    USBPHY_SET8(0x6a, 0x04);
+
+    spin(1);
+
+    return;
+}
+
+static void mt_usb_phy_recover(void)
+{
+
+    USBPHY_SET8(0x00, 0x20);
+    /* clean PUPD_BIST_EN */
+    /* PUPD_BIST_EN = 1'b0 */
+    /* PMIC will use it to detect charger type */
+    USBPHY_CLR8(0x1d, 0x10);
+
+    /* force_uart_en = 1'b0 */
+    USBPHY_CLR8(0x6b, 0x04);
+    /* RG_UART_EN = 1'b0 */
+    USBPHY_CLR8(0x6e, 0x01);
+    /* force_uart_en = 1'b0 */
+    USBPHY_CLR8(0x6a, 0x04);
+
+    USBPHY_CLR8(0x68, 0xf4);
+
+    /* RG_DATAIN[3:0] = 4'b0000 */
+    USBPHY_CLR8(0x69, 0x3c);
+
+    USBPHY_CLR8(0x6a, 0xba);
+
+    /* RG_USB20_BC11_SW_EN = 1'b0 */
+    USBPHY_CLR8(0x1a, 0x80);
+    /* RG_USB20_OTG_VBUSSCMP_EN = 1'b1 */
+    USBPHY_SET8(0x1a, 0x10);
+
+    spin(800);
+
+    /* force enter device mode */
+    USBPHY_CLR8(0x6c, 0x10);
+    USBPHY_SET8(0x6c, 0x2E);
+    USBPHY_SET8(0x6d, 0x3E);
+}
+
+static void Charger_Detect_Init(void)
+{
+    /* RG_USB20_BC11_SW_EN = 1'b1 */
+    USBPHY_SET8(0x1a, 0x80);
+}
+
+static void Charger_Detect_Release(void)
+{
+    /* RG_USB20_BC11_SW_EN = 1'b0 */
+    USBPHY_CLR8(0x1a, 0x80);
+}
+
+static void board_usb_init(void)
+{
+    mt_usb_phy_poweron();
+}
+
+struct udc_descriptor {
+    struct udc_descriptor *next;
+    unsigned short tag; /* ((TYPE << 8) | NUM) */
+    unsigned short len; /* total length */
+    unsigned char data[0];
+};
+
+#if DBG_USB_DUMP_SETUP
+static void dump_setup_packet(const char *str, struct setup_packet *sp)
+{
+    DBG_I("\n");
+    DBG_I(str);
+    DBG_I("	   bmRequestType = %x\n", sp->type);
+    DBG_I("	   bRequest = %x\n", sp->request);
+    DBG_I("	   wValue = %x\n", sp->value);
+    DBG_I("	   wIndex = %x\n", sp->index);
+    DBG_I("	   wLength = %x\n", sp->length);
+}
+#else
+static void dump_setup_packet(const char *str, struct setup_packet *sp) {}
+#endif
+
+static void copy_desc(struct urb *urb, void *data, int length)
+{
+
+#if DBG_USB_FIFO
+    DBG_I("%s: urb: %x, data %x, length: %d, actual_length: %d\n",
+          __func__, urb->buffer, data, length, urb->actual_length);
+#endif
+
+    //memcpy(urb->buffer + urb->actual_length, data, length);
+    memcpy(urb->buffer, data, length);
+    //urb->actual_length += length;
+    urb->actual_length = length;
+#if DBG_USB_FIFO
+    DBG_I("%s: urb: %x, data %x, length: %d, actual_length: %d\n",
+          __func__, urb, data, length, urb->actual_length);
+#endif
+}
+
+
+static struct udc_descriptor *udc_descriptor_alloc(unsigned type, unsigned num,
+        unsigned len) {
+    struct udc_descriptor *desc;
+    if ((len > 255) || (len < 2) || (num > 255) || (type > 255))
+        return 0;
+
+    if (!(desc = malloc(sizeof(struct udc_descriptor) + len)))
+        return 0;
+
+    desc->next = 0;
+    desc->tag = (type << 8) | num;
+    desc->len = len;
+    desc->data[0] = len;
+    desc->data[1] = type;
+
+    return desc;
+}
+
+static struct udc_descriptor *desc_list = 0;
+static unsigned next_string_id = 1;
+
+static void udc_descriptor_register(struct udc_descriptor *desc)
+{
+    desc->next = desc_list;
+    desc_list = desc;
+}
+
+static unsigned udc_string_desc_alloc(const char *str)
+{
+    unsigned len;
+    struct udc_descriptor *desc;
+    unsigned char *data;
+
+    if (next_string_id > 255)
+        return 0;
+
+    if (!str)
+        return 0;
+
+    len = strlen(str);
+    desc = udc_descriptor_alloc(TYPE_STRING, next_string_id, len * 2 + 2);
+    if (!desc)
+        return 0;
+    next_string_id++;
+
+    /* expand ascii string to utf16 */
+    data = desc->data + 2;
+    while (len-- > 0) {
+        *data++ = *str++;
+        *data++ = 0;
+    }
+
+    udc_descriptor_register(desc);
+    return desc->tag & 0xff;
+}
+
+static int mt_read_fifo(struct udc_endpoint *endpoint)
+{
+
+    struct urb *urb = endpoint->rcv_urb;
+    int len = 0, count = 0;
+    int ep_num = endpoint->num;
+    int index;
+    unsigned char *cp;
+    u32 *wp;
+#if !CFG_FPGA_PLATFORM
+    u16 dma_cntl = 0;
+#endif
+
+    if (ep_num == EP0)
+        urb = ep0_urb;
+
+    if (urb) {
+        index = readb(INDEX);
+        writeb(ep_num, INDEX);
+
+        cp = (u8 *) (urb->buffer + urb->actual_length);
+        wp = (u32 *) cp;
+#if DBG_USB_FIFO
+        DBG_I("%s: ep_num: %d, urb: %x, urb->buffer: %x, urb->actual_length = %d\n",
+              __func__, ep_num, urb, urb->buffer, urb->actual_length);
+#endif
+
+        count = len = readw(IECSR + RXCOUNT);
+        if (ep_num != 0) {
+#if DBG_USB_FIFO
+            DBG_I("%s: ep_num: %d count = %d\n",
+                  __func__, ep_num, count);
+#endif
+        }
+
+        /* FIX: DMA has problem write now */
+
+#if !CFG_FPGA_PLATFORM
+        arch_clean_invalidate_cache_range((addr_t) cp, count);
+
+        if (ep_num != 0) {
+            writel((u32)wp, USB_DMA_ADDR (ep_num));
+            writel(count, USB_DMA_COUNT (ep_num));
+            dma_cntl =
+                USB_DMA_BURST_MODE_3 | (ep_num << USB_DMA_ENDPNT_OFFSET) |
+                USB_DMA_EN;
+            writew(dma_cntl, USB_DMA_CNTL (ep_num));
+            while (readw(USB_DMA_CNTL (ep_num)) & USB_DMA_EN);
+        } else
+
+#endif
+        {
+            while (len > 0) {
+                if (len >= 4) {
+                    *wp++ = readl(FIFO(ep_num));
+                    cp = (unsigned char *) wp;
+                    //DBG_I("USB READ FIFO: wp = %lu, cp = %lu\n", wp, cp);
+                    len -= 4;
+                } else {
+                    *cp++ = readb(FIFO(ep_num));
+                    //DBG_I("USB READ FIFO: wp = %lu, cp = %lu\n", wp, cp);
+                    len--;
+                }
+            }
+        }
+
+#if DBG_USB_DUMP_DATA
+        if (ep_num != 0) {
+            DBG_I("%s: &urb->buffer: %x\n", __func__, urb->buffer);
+            DBG_I("[USB] dump data:\n");
+            hexdump8(urb->buffer, count);
+        }
+#endif
+
+        urb->actual_length += count;
+
+        writeb(index, INDEX);
+    }
+
+    return count;
+}
+
+static int mt_write_fifo(struct udc_endpoint *endpoint)
+{
+    struct urb *urb = endpoint->tx_urb;
+    int last = 0, count = 0;
+    int ep_num = endpoint->num;
+    int index;
+    unsigned char *cp = NULL;
+#ifdef USB_TX_DMA_MODE_0
+    u32 *wp;
+    u16 dma_cntl = 0;
+#endif
+
+    if (ep_num == EP0)
+        urb = ep0_urb;
+
+    if (urb) {
+        index = readb(INDEX);
+        writeb(ep_num, INDEX);
+
+#if DBG_USB_DUMP_DESC
+        DBG_I("%s: dump desc\n", __func__);
+        hexdump8(urb->buffer, urb->actual_length);
+#endif
+
+
+#if DBG_USB_FIFO
+        DBG_I("%s: ep_num: %d urb: %x, actual_length: %d\n",
+              __func__, ep_num, urb, urb->actual_length);
+        DBG_I("%s: sent: %d, tx_pkt_size: %d\n", __func__, endpoint->sent, endpoint->maxpkt);
+#endif
+
+        count = last = MIN (urb->actual_length - endpoint->sent,  endpoint->maxpkt);
+        //count = last = urb->actual_length;
+
+#if DBG_USB_FIFO
+        DBG_I("%s: count: %d\n", __func__, count);
+        DBG_I("%s: urb->actual_length = %d\n", __func__, urb->actual_length);
+        DBG_I("%s: endpoint->sent = %d\n", __func__, endpoint->sent);
+#endif
+
+        if (count < 0) {
+            DBG_C("%s: something is wrong, count < 0", __func__);
+        }
+
+        if (count) {
+            cp = urb->buffer + endpoint->sent;
+#ifdef USB_TX_DMA_MODE_0
+            wp = (u32 *)cp;
+
+            arch_clean_invalidate_cache_range((addr_t) cp, count);
+
+            if (ep_num != 0) {
+                writel(wp, USB_DMA_ADDR(ep_num));
+                writel(count, USB_DMA_COUNT(ep_num));
+                dma_cntl =
+                    USB_DMA_BURST_MODE_3 | (ep_num << USB_DMA_ENDPNT_OFFSET) |
+                    USB_DMA_EN | USB_DMA_DIR;
+                writew(dma_cntl, USB_DMA_CNTL(ep_num));
+                while (readw(USB_DMA_CNTL (ep_num)) & USB_DMA_EN);
+            } else
+#endif
+            {
+                //DBG("---------write USB fifo---------\n");
+                while (count > 0) {
+                    //hexdump8(cp, 1);
+                    writeb(*cp, FIFO (ep_num));
+                    cp++;
+                    count--;
+                }
+            }
+        }
+
+        endpoint->last = last;
+        endpoint->sent += last;
+
+        writeb(index, INDEX);
+    }
+
+    return last;
+}
+
+static struct udc_endpoint *mt_find_ep(int ep_num, u8 dir) {
+    int i;
+    u8 in = 0;
+
+    /* convert dir to in */
+    if (dir == USB_DIR_IN) /* dir == USB_DIR_IN */
+        in = 1;
+
+    /* for (i = 0; i < udc_device->max_endpoints; i++) */
+    /* for (i = 0; i < the_gadget->ifc_endpoints; i++) */
+    for (i = 0; i < MT_EP_NUM; i++) {
+        if ((ep_list[i].num == ep_num) && (ep_list[i].in == in)) {
+#if DBG_USB_GENERAL
+            DBG_I("%s: find ep!\n", __func__);
+#endif
+            return &ep_list[i];
+        }
+    }
+    return NULL;
+}
+
+static void mt_udc_flush_fifo(u8 ep_num, u8 dir)
+{
+    u16 tmpReg16;
+    u8 index;
+    struct udc_endpoint *endpoint;
+
+    index = readb(INDEX);
+    writeb(ep_num, INDEX);
+
+    if (ep_num == 0) {
+        tmpReg16 = readw(IECSR + CSR0);
+        tmpReg16 |= EP0_FLUSH_FIFO;
+        writew(tmpReg16, IECSR + CSR0);
+        writew(tmpReg16, IECSR + CSR0);
+    } else {
+        endpoint = mt_find_ep(ep_num, dir);
+        if (endpoint->in == 0) { /* USB_DIR_OUT */
+            tmpReg16 = readw(IECSR + RXCSR);
+            tmpReg16 |= EPX_RX_FLUSHFIFO;
+            writew(tmpReg16, IECSR + RXCSR);
+            writew(tmpReg16, IECSR + RXCSR);
+        } else {
+            tmpReg16 = readw(IECSR + TXCSR);
+            tmpReg16 |= EPX_TX_FLUSHFIFO;
+            writew(tmpReg16, IECSR + TXCSR);
+            writew(tmpReg16, IECSR + TXCSR);
+        }
+    }
+
+    /* recover index register */
+    writeb(index, INDEX);
+}
+
+/* the endpoint does not support the received command, stall it!! */
+static void udc_stall_ep(unsigned int ep_num, u8 dir)
+{
+    struct udc_endpoint *endpoint = mt_find_ep(ep_num, dir);
+    u8 index;
+    u16 csr;
+
+    DBG_C("[USB] %s\n", __func__);
+
+    index = readb(INDEX);
+    writeb(ep_num, INDEX);
+
+    if (ep_num == 0) {
+        csr = readw(IECSR + CSR0);
+        csr |= EP0_SENDSTALL;
+        writew(csr, IECSR + CSR0);
+        mt_udc_flush_fifo(ep_num, USB_DIR_OUT);
+    } else {
+        if (endpoint->in == 0) { /* USB_DIR_OUT */
+            csr = readb(IECSR + RXCSR);
+            csr |= EPX_RX_SENDSTALL;
+            writew(csr, IECSR + RXCSR);
+            mt_udc_flush_fifo(ep_num, USB_DIR_OUT);
+        } else {
+            csr = readb(IECSR + TXCSR);
+            csr |= EPX_TX_SENDSTALL;
+            writew(csr, IECSR + TXCSR);
+            mt_udc_flush_fifo(ep_num, USB_DIR_IN);
+        }
+    }
+    //mt_udc_flush_fifo (ep_num, USB_DIR_OUT);
+    //mt_udc_flush_fifo (ep_num, USB_DIR_IN);
+
+    ep0_state = EP0_IDLE;
+
+    writeb(index, INDEX);
+
+    return;
+}
+
+/*
+ * If abnormal DATA transfer happened, like USB unplugged,
+ * we cannot fix this after mt_udc_reset().
+ * Because sometimes there will come reset twice.
+ */
+static void mt_udc_suspend(void)
+{
+    /* handle abnormal DATA transfer if we had any */
+    struct udc_endpoint *endpoint;
+    int i;
+
+    /* deal with flags */
+    usb_online = 0;
+    usb_config_value = 0;
+    the_gadget->notify(the_gadget, UDC_EVENT_OFFLINE);
+
+    /* error out any pending reqs */
+    for (i = 1; i < MT_EP_NUM; i++) {
+        /* ensure that ept_complete considers
+         * this to be an error state
+         */
+#if DBG_USB_GENERAL
+        DBG_I("%s: ep: %i, in: %s, req: %x\n",
+              __func__, ep_list[i].num, ep_list[i].in ? "IN" : "OUT", ep_list[i].req);
+#endif
+        if ((ep_list[i].req && (ep_list[i].in == 0)) || /* USB_DIR_OUT */
+                (ep_list[i].req && (ep_list[i].in == 1))) { /* USB_DIR_IN */
+            ep_list[i].status = -1; /* HALT */
+            endpoint = &ep_list[i];
+            handle_ept_complete(endpoint);
+        }
+    }
+}
+
+static void mt_udc_rxtxmap_recover(void)
+{
+    int i;
+
+    for (i = 1; i < MT_EP_NUM; i++) {
+        if (ep_list[i].num != 0) { /* allocated */
+
+            writeb(ep_list[i].num, INDEX);
+
+            if (ep_list[i].in == 0) /* USB_DIR_OUT */
+                writel(ep_list[i].maxpkt, (IECSR + RXMAP));
+            else
+                writel(ep_list[i].maxpkt, (IECSR + TXMAP));
+        }
+    }
+}
+
+static void mt_udc_reset(void)
+{
+
+    /* MUSBHDRC automatically does the following when reset signal is detected */
+    /* 1. Sets FAddr to 0
+     * 2. Sets Index to 0
+     * 3. Flush all endpoint FIFOs
+     * 4. Clears all control/status registers
+     * 5. Enables all endpoint interrupts
+     * 6. Generates a Rest interrupt
+     */
+
+    DBG_I("[USB] %s\n", __func__);
+
+    /* disable all endpoint interrupts */
+    writeb(0, INTRTXE);
+    writeb(0, INTRRXE);
+    writeb(0, INTRUSBE);
+
+    writew(SWRST_SWRST | SWRST_DISUSBRESET, SWRST);
+
+    dev_address = 0;
+
+    /* flush FIFO */
+    mt_udc_flush_fifo(0, USB_DIR_OUT);
+    mt_udc_flush_fifo(1, USB_DIR_OUT);
+    mt_udc_flush_fifo(1, USB_DIR_IN);
+    //mt_udc_flush_fifo (2, USB_DIR_IN);
+
+    /* detect USB speed */
+    if (readb(POWER) & PWR_HS_MODE) {
+        DBG_I("[USB] USB High Speed\n");
+//      enable_highspeed();
+    } else {
+        DBG_I("[USB] USB Full Speed\n");
+    }
+
+    /* restore RXMAP and TXMAP if the endpoint has been configured */
+    mt_udc_rxtxmap_recover();
+
+    /* enable suspend */
+    writeb((INTRUSB_SUSPEND | INTRUSB_RESUME | INTRUSB_RESET |INTRUSB_DISCON), INTRUSBE);
+
+}
+
+static void mt_udc_ep0_write(void)
+{
+
+    struct udc_endpoint *endpoint = &ep_list[EP0];
+    int count = 0;
+    u16 csr0 = 0;
+    u8 index = 0;
+
+    index = readb(INDEX);
+    writeb(0, INDEX);
+
+    csr0 = readw(IECSR + CSR0);
+    if (csr0 & EP0_TXPKTRDY) {
+        DBG_I("mt_udc_ep0_write: ep0 is not ready to be written\n");
+        return;
+    }
+
+    count = mt_write_fifo(endpoint);
+
+#if DBG_USB_GENERAL
+    DBG_I("%s: count = %d\n", __func__, count);
+#endif
+
+    if (count < EP0_MAX_PACKET_SIZE) {
+        /* last packet */
+        csr0 |= (EP0_TXPKTRDY | EP0_DATAEND);
+        ep0_urb->actual_length = 0;
+        endpoint->sent = 0;
+        ep0_state = EP0_IDLE;
+    } else {
+        /* more packets are waiting to be transferred */
+        csr0 |= EP0_TXPKTRDY;
+    }
+
+    writew(csr0, IECSR + CSR0);
+    writeb(index, INDEX);
+
+    return;
+}
+
+static void mt_udc_ep0_read(void)
+{
+
+    struct udc_endpoint *endpoint = &ep_list[EP0];
+    int count = 0;
+    u16 csr0 = 0;
+    u8 index = 0;
+
+    index = readb(INDEX);
+    writeb(EP0, INDEX);
+
+    csr0 = readw(IECSR + CSR0);
+
+    /* erroneous ep0 interrupt */
+    if (!(csr0 & EP0_RXPKTRDY)) {
+        return;
+    }
+
+    count = mt_read_fifo(endpoint);
+
+    if (count <= EP0_MAX_PACKET_SIZE) {
+        /* last packet */
+        csr0 |= (EP0_SERVICED_RXPKTRDY | EP0_DATAEND);
+        ep0_state = EP0_IDLE;
+    } else {
+        /* more packets are waiting to be transferred */
+        csr0 |= EP0_SERVICED_RXPKTRDY;
+    }
+
+    writew(csr0, IECSR + CSR0);
+
+    writeb(index, INDEX);
+
+    return;
+}
+
+static int ep0_standard_setup(struct urb *urb)
+{
+    struct setup_packet *request;
+    struct udc_descriptor *desc;
+    //struct udc_device *device;
+    u8 *cp = urb->buffer;
+#if 0
+    if (!urb || !urb->device) {
+        DBG ("\n!urb || !urb->device\n");
+        return false;
+    }
+#endif
+
+    request = &urb->device_request;
+    //device = urb->device;
+
+    dump_setup_packet("[USB] Device Request\n", request);
+
+    if ((request->type & USB_TYPE_MASK) != 0) {
+        return false;           /* Class-specific requests are handled elsewhere */
+    }
+
+    /* handle all requests that return data (direction bit set on bm RequestType) */
+    if ((request->type & USB_EP_DIR_MASK)) {
+        /* send the descriptor */
+        ep0_state = EP0_TX;
+
+        switch (request->request) {
+                /* data stage: from device to host */
+            case GET_STATUS:
+#if DBG_USB_GENERAL
+                DBG_I("GET_STATUS\n");
+#endif
+                urb->actual_length = 2;
+                cp[0] = cp[1] = 0;
+                switch (request->type & USB_RECIP_MASK) {
+                    case USB_RECIP_DEVICE:
+                        cp[0] = USB_STAT_SELFPOWERED;
+                        break;
+                    case USB_RECIP_OTHER:
+                        urb->actual_length = 0;
+                        break;
+                    default:
+                        break;
+                }
+
+                return 0;
+
+            case GET_DESCRIPTOR:
+#if DBG_USB_GENERAL
+                DBG_I("GET_DESCRIPTOR\n");
+#endif
+                /* usb_highspeed? */
+
+                for (desc = desc_list; desc; desc = desc->next) {
+#if DBG_USB_DUMP_DESC
+                    DBG_I("desc->tag: %x: request->value: %x\n", desc->tag, request->value);
+#endif
+                    if (desc->tag == request->value) {
+
+#if DBG_USB_DUMP_DESC
+                        DBG_I("Find packet!\n");
+#endif
+                        unsigned len = desc->len;
+                        if (len > request->length)
+                            len = request->length;
+
+#if DBG_USB_GENERAL
+                        DBG_I("%s: urb: %x, cp: %p\n", __func__, urb, cp);
+#endif
+                        copy_desc(urb, desc->data, len);
+                        return 0;
+                    }
+                }
+                /* descriptor lookup failed */
+                return false;
+
+            case GET_CONFIGURATION:
+#if DBG_USB_GENERAL
+                DBG_I("GET_CONFIGURATION\n");
+                DBG_I("USB_EP_DIR_MASK\n");
+#endif
+#if 0
+                urb->actual_length = 1;
+                ((char *) urb->buffer)[0] = device->configuration;
+#endif
+//          return 0;
+                break;
+
+            case GET_INTERFACE:
+#if DBG_USB_GENERAL
+                DBG_I("GET_INTERFACE\n");
+#endif
+
+#if 0
+                urb->actual_length = 1;
+                ((char *) urb->buffer)[0] = device->alternate;
+                return 0;
+#endif
+            default:
+                DBG_C("Unsupported command with TX data stage\n");
+                break;
+        }
+    } else {
+
+        switch (request->request) {
+
+            case SET_ADDRESS:
+#if DBG_USB_GENERAL
+                DBG_I("SET_ADDRESS\n");
+#endif
+
+                dev_address = (request->value);
+                set_address = 1;
+                return 0;
+
+            case SET_CONFIGURATION:
+#if DBG_USB_GENERAL
+                DBG_I("SET_CONFIGURATION\n");
+#endif
+#if 0
+                device->configuration = (request->value) & 0x7f;
+                device->interface = device->alternate = 0;
+#endif
+                if (request->value == 1) {
+                    usb_config_value = 1;
+                    the_gadget->notify(the_gadget, UDC_EVENT_ONLINE);
+                } else {
+                    usb_config_value = 0;
+                    the_gadget->notify(the_gadget, UDC_EVENT_OFFLINE);
+                }
+
+                usb_online = request->value ? 1 : 0;
+                //usb_status(request->value ? 1 : 0, usb_highspeed);
+
+                return 0;
+
+            default:
+                DBG_C("Unsupported command with RX data stage\n");
+                break;
+
+        }
+    }
+    return false;
+}
+
+static void mt_udc_ep0_setup(void)
+{
+    struct udc_endpoint *endpoint = &ep_list[0];
+    u8 index;
+    u8 stall = 0;
+    u16 csr0;
+    struct setup_packet *request;
+
+#ifdef USB_DEBUG
+    u16 count;
+#endif
+
+    index = readb(INDEX);
+    writeb(0, INDEX);
+    /* Read control status register for endpiont 0 */
+    csr0 = readw(IECSR + CSR0);
+
+    /* check whether RxPktRdy is set? */
+    if (!(csr0 & EP0_RXPKTRDY))
+        return;
+
+    /* unload fifo */
+    ep0_urb->actual_length = 0;
+
+#ifndef USB_DEBUG
+    mt_read_fifo(endpoint);
+#else
+    count = mt_read_fifo(endpoint);
+
+#if DBG_USB_FIFO
+    DBG_I("%s: mt_read_fifo count = %d\n", __func__, count);
+#endif
+#endif
+    /* decode command */
+    request = &ep0_urb->device_request;
+    memcpy(request, ep0_urb->buffer, sizeof(struct setup_packet));
+
+    if (((request->type) & USB_TYPE_MASK) == USB_TYPE_STANDARD) {
+#if DBG_USB_GENERAL
+        DBG_I("[USB] Standard Request\n");
+#endif
+        stall = ep0_standard_setup(ep0_urb);
+        if (stall) {
+            dump_setup_packet("[USB] STANDARD REQUEST NOT SUPPORTED\n", request);
+        }
+    } else if (((request->type) & USB_TYPE_MASK) == USB_TYPE_CLASS) {
+#if DBG_USB_GENERAL
+        DBG_I("[USB] Class-Specific Request\n");
+#endif
+//      stall = ep0_class_setup(ep0_urb);
+        if (stall) {
+            dump_setup_packet("[USB] CLASS REQUEST NOT SUPPORTED\n", request);
+        }
+    } else if (((request->type) & USB_TYPE_MASK) == USB_TYPE_VENDOR) {
+#if DBG_USB_GENERAL
+        DBG_I("[USB] Vendor-Specific Request\n");
+        /* do nothing now */
+        DBG_I("[USB] ALL VENDOR-SPECIFIC REQUESTS ARE NOT SUPPORTED!!\n");
+#endif
+    }
+
+    if (stall) {
+        /* the received command is not supported */
+        udc_stall_ep(0, USB_DIR_OUT);
+        return;
+    }
+
+    switch (ep0_state) {
+        case EP0_TX:
+            /* data stage: from device to host */
+#if DBG_USB_GENERAL
+            DBG_I("%s: EP0_TX\n", __func__);
+#endif
+            csr0 = readw(IECSR + CSR0);
+            csr0 |= (EP0_SERVICED_RXPKTRDY);
+            writew(csr0, IECSR + CSR0);
+
+            mt_udc_ep0_write();
+
+            break;
+        case EP0_RX:
+            /* data stage: from host to device */
+#if DBG_USB_GENERAL
+            DBG_I("%s: EP0_RX\n", __func__);
+#endif
+            csr0 = readw(IECSR + CSR0);
+            csr0 |= (EP0_SERVICED_RXPKTRDY);
+            writew(csr0, IECSR + CSR0);
+
+            break;
+        case EP0_IDLE:
+            /* no data stage */
+#if DBG_USB_GENERAL
+            DBG_I("%s: EP0_IDLE\n", __func__);
+#endif
+            csr0 = readw(IECSR + CSR0);
+            csr0 |= (EP0_SERVICED_RXPKTRDY | EP0_DATAEND);
+
+            writew(csr0, IECSR + CSR0);
+            writew(csr0, IECSR + CSR0);
+
+            break;
+        default:
+            break;
+    }
+
+    writeb(index, INDEX);
+    return;
+
+}
+
+static void mt_udc_ep0_handler(void)
+{
+
+    u16 csr0;
+    u8 index = 0;
+
+    index = readb(INDEX);
+    writeb(0, INDEX);
+
+    csr0 = readw(IECSR + CSR0);
+
+    if (csr0 & EP0_SENTSTALL) {
+#if DBG_USB_GENERAL
+        DBG_I("USB: [EP0] SENTSTALL\n");
+#endif
+        /* needs implementation for exception handling here */
+        ep0_state = EP0_IDLE;
+    }
+
+    if (csr0 & EP0_SETUPEND) {
+#if DBG_USB_GENERAL
+        DBG_I("USB: [EP0] SETUPEND\n");
+#endif
+        csr0 |= EP0_SERVICE_SETUP_END;
+        writew(csr0, IECSR + CSR0);
+
+        ep0_state = EP0_IDLE;
+    }
+
+    switch (ep0_state) {
+        case EP0_IDLE:
+#if DBG_USB_GENERAL
+            DBG_I("%s: EP0_IDLE\n", __func__);
+#endif
+            if (set_address) {
+                writeb(dev_address, FADDR);
+                set_address = 0;
+            }
+            mt_udc_ep0_setup();
+            break;
+        case EP0_TX:
+#if DBG_USB_GENERAL
+            DBG_I("%s: EP0_TX\n", __func__);
+#endif
+            mt_udc_ep0_write();
+            break;
+        case EP0_RX:
+#if DBG_USB_GENERAL
+            DBG_I("%s: EP0_RX\n", __func__);
+#endif
+            mt_udc_ep0_read();
+            break;
+        default:
+            break;
+    }
+
+    writeb(index, INDEX);
+
+    return;
+}
+
+
+/*
+ * udc_setup_ep - setup endpoint
+ *
+ * Associate a physical endpoint with endpoint_instance and initialize FIFO
+ */
+static void mt_setup_ep(unsigned int ep, struct udc_endpoint *endpoint)
+{
+    u8 index;
+    u16 csr;
+    u16 csr0;
+    u16 max_packet_size;
+    u8 fifosz = 0;
+
+    /* EP table records in bits hence bit 1 is ep0 */
+    index = readb(INDEX);
+    writeb(ep, INDEX);
+
+    if (ep == EP0) {
+        /* Read control status register for endpiont 0 */
+        csr0 = readw(IECSR + CSR0);
+
+        /* check whether RxPktRdy is set? */
+        if (!(csr0 & EP0_RXPKTRDY))
+            return;
+    }
+
+    /* Configure endpoint fifo */
+    /* Set fifo address, fifo size, and fifo max packet size */
+#if DBG_USB_GENERAL
+    DBG_I("%s: endpoint->in: %d, maxpkt: %d\n",
+          __func__, endpoint->in, endpoint->maxpkt);
+#endif
+    if (endpoint->in == 0) { /* USB_DIR_OUT */
+        /* Clear data toggle to 0 */
+        csr = readw(IECSR + RXCSR);
+        /* pangyen 20090911 */
+        csr |= EPX_RX_CLRDATATOG | EPX_RX_FLUSHFIFO;
+        writew(csr, IECSR + RXCSR);
+        /* Set fifo address */
+        writew(fifo_addr >> 3, RXFIFOADD);
+        /* Set fifo max packet size */
+        max_packet_size = endpoint->maxpkt;
+        writew(max_packet_size, IECSR + RXMAP);
+        /* Set fifo size (double buffering is currently not enabled) */
+        switch (max_packet_size) {
+            case 8:
+            case 16:
+            case 32:
+            case 64:
+            case 128:
+            case 256:
+            case 512:
+            case 1024:
+            case 2048:
+                if (endpoint->mode == DOUBLE_BUF)
+                    fifosz |= FIFOSZ_DPB;
+                fifosz |= __builtin_ffs(max_packet_size >> 4);
+                writeb(fifosz, TXFIFOSZ);
+                break;
+            case 4096:
+                fifosz |= __builtin_ffs(max_packet_size >> 4);
+                writeb(fifosz, RXFIFOSZ);
+                break;
+            case 3072:
+                fifosz = __builtin_ffs(4096 >> 4);
+                writeb(fifosz, RXFIFOSZ);
+                break;
+
+            default:
+                DBG_C("The max_packet_size for ep %d is not supported\n", ep);
+        }
+    } else {
+        /* Clear data toggle to 0 */
+        csr = readw(IECSR + TXCSR);
+        /* pangyen 20090911 */
+        csr |= EPX_TX_CLRDATATOG | EPX_TX_FLUSHFIFO;
+        writew(csr, IECSR + TXCSR);
+        /* Set fifo address */
+        writew(fifo_addr >> 3, TXFIFOADD);
+        /* Set fifo max packet size */
+        max_packet_size = endpoint->maxpkt;
+        writew(max_packet_size, IECSR + TXMAP);
+        /* Set fifo size(double buffering is currently not enabled) */
+        switch (max_packet_size) {
+            case 8:
+            case 16:
+            case 32:
+            case 64:
+            case 128:
+            case 256:
+            case 512:
+            case 1024:
+            case 2048:
+                if (endpoint->mode == DOUBLE_BUF)
+                    fifosz |= FIFOSZ_DPB;
+                fifosz |= __builtin_ffs(max_packet_size >> 4);
+                writeb(fifosz, TXFIFOSZ);
+                break;
+            case 4096:
+                fifosz |= __builtin_ffs(max_packet_size >> 4);
+                writeb(fifosz, TXFIFOSZ);
+                break;
+            case 3072:
+                fifosz = __builtin_ffs(4096 >> 4);
+                writeb(fifosz, TXFIFOSZ);
+                break;
+
+            default:
+                DBG_C("The max_packet_size for ep %d is not supported\n", ep);
+        }
+    }
+
+    if (endpoint->mode == DOUBLE_BUF)
+        fifo_addr += (max_packet_size << 1);
+    else
+        fifo_addr += max_packet_size;
+
+    /* recover INDEX register */
+    writeb(index, INDEX);
+}
+
+struct udc_endpoint *_udc_endpoint_alloc(unsigned char num, unsigned char in,
+        unsigned short max_pkt) {
+    int i;
+
+    /*
+     * find an unused slot in ep_list from EP1 to MAX_EP
+     * for example, EP1 will use 2 slot one for IN and the other for OUT
+     */
+    if (num != EP0) {
+        for (i = 1; i < MT_EP_NUM; i++) {
+            if (ep_list[i].num == 0) /* usable */
+                break;
+        }
+
+        if (i == MT_EP_NUM) /* ep has been exhausted. */
+            return NULL;
+
+        if (in) { /* usb EP1 tx */
+            ep_list[i].tx_urb = tx_urb;
+#ifdef USB_DOUBLE_BUF
+            ep_list[i].mode = DOUBLE_BUF;
+#endif
+        } else {    /* usb EP1 rx */
+            ep_list[i].rcv_urb = rx_urb;
+#ifdef USB_DOUBLE_BUF
+            ep_list[i].mode = DOUBLE_BUF;
+#endif
+        }
+    } else {
+        i = EP0;    /* EP0 */
+    }
+
+    ep_list[i].maxpkt = max_pkt;
+    ep_list[i].num = num;
+    ep_list[i].in = in;
+    ep_list[i].req = NULL;
+
+    /* store EPT_TX/RX info */
+    if (ep_list[i].in) {
+        ep_list[i].bit = EPT_TX(num);
+    } else {
+        ep_list[i].bit = EPT_RX(num);
+    }
+
+    /* write parameters to this ep (write to hardware) */
+    mt_setup_ep(num, &ep_list[i]);
+
+    DBG_I("[USB] ept%d %s @%p/%p max=%d bit=%x\n",
+          num, in ? "in" : "out", &ep_list[i], &ep_list, max_pkt, ep_list[i].bit);
+
+    return &ep_list[i];
+}
+
+#define SETUP(type,request) (((type) << 8) | (request))
+
+static unsigned long ept_alloc_table = EPT_TX(0) | EPT_RX(0);
+
+struct udc_endpoint *udc_endpoint_alloc(unsigned type, unsigned maxpkt) {
+    struct udc_endpoint *ept;
+    unsigned n;
+    unsigned in;
+
+    if (type == UDC_BULK_IN) {
+        in = 1;
+    } else if (type == UDC_BULK_OUT) {
+        in = 0;
+    } else {
+        return 0;
+    }
+
+    /* udc_endpoint_alloc is used for EPx except EP0 */
+    for (n = 1; n < 16; n++) {
+        unsigned long bit = in ? EPT_TX(n) : EPT_RX(n);
+        if (ept_alloc_table & bit)
+            continue;
+        ept = _udc_endpoint_alloc(n, in, maxpkt);
+        if (ept)
+            ept_alloc_table |= bit;
+        return ept;
+    }
+
+    return 0;
+}
+
+static void handle_ept_complete(struct udc_endpoint *ept)
+{
+    unsigned int actual;
+    int status;
+    struct udc_request *req;
+
+    req = ept->req;
+    if (req) {
+#if DBG_USB_GENERAL
+        DBG_I("%s: req: %x: req->length: %d: status: %d\n", __func__, req, req->length, ept->status);
+#endif
+        /* release this request for processing next */
+        ept->req = NULL;
+
+        if (ept->status == -1) {
+            actual = 0;
+            status = -1;
+            DBG_C("%s: EP%d/%s FAIL status: %x\n",
+                  __func__, ept->num, ept->in ? "in" : "out", status);
+        } else {
+            actual = req->length;
+            status = 0;
+        }
+        if (req->complete)
+            req->complete(req, actual, status);
+    }
+
+}
+
+static void mt_udc_epx_handler(u8 ep_num, u8 dir)
+{
+    u8 index;
+    u16 csr;
+    u32 count;
+    struct udc_endpoint *endpoint;
+    struct urb *urb;
+    struct udc_request *req;    /* for event signaling */
+    u8 intrrxe;
+
+    endpoint = mt_find_ep(ep_num, dir);
+
+    index = readb(INDEX);
+    writeb(ep_num, INDEX);
+
+#if DBG_USB_GENERAL
+    DBG_I("EP%d Interrupt\n", ep_num);
+    DBG_I("dir: %x\n", dir);
+#endif
+
+    switch (dir) {
+        case USB_DIR_OUT:
+            /* transfer direction is from host to device */
+            /* from the view of usb device, it's RX */
+            csr = readw(IECSR + RXCSR);
+
+            if (csr & EPX_RX_SENTSTALL) {
+                DBG_C("EP %d(RX): STALL\n", ep_num);
+                /* exception handling: implement this!! */
+                return;
+            }
+
+            if (!(csr & EPX_RX_RXPKTRDY)) {
+#if DBG_USB_GENERAL
+                DBG_I("EP %d: ERRONEOUS INTERRUPT\n", ep_num); // normal
+#endif
+                return;
+            }
+
+            //DBG_C("mt_read_fifo, start\n");
+            count = mt_read_fifo(endpoint);
+            //DBG_C("mt_read_fifo, end\n");
+
+#if DBG_USB_GENERAL
+            DBG_I("EP%d(RX), count = %d\n", ep_num, count);
+#endif
+
+            csr &= ~EPX_RX_RXPKTRDY;
+            writew(csr, IECSR + RXCSR);
+            if (readw(IECSR + RXCSR) & EPX_RX_RXPKTRDY) {
+#if DBG_USB_GENERAL
+                DBG_I("%s: rxpktrdy clear failed\n", __func__);
+#endif
+            }
+
+            /* do signaling */
+            req = endpoint->req;
+            /* workaround: if req->lenth == 64 bytes (not huge data transmission)
+             * do normal return */
+#if DBG_USB_GENERAL
+            DBG_I("%s: req->length: %x, endpoint->rcv_urb->actual_length: %x\n",
+                  __func__, req->length, endpoint->rcv_urb->actual_length);
+#endif
+
+            /* Deal with FASTBOOT command */
+            if ((req->length >= endpoint->rcv_urb->actual_length) && req->length == 64) {
+                req->length = count;
+
+                /* mask EPx INTRRXE */
+                /* The buffer is passed from the AP caller.
+                 * It happens that AP is dealing with the buffer filled data by driver,
+                 * but the driver is still receiving the next data packets onto the buffer.
+                 * Data corrupted happens if the every request use the same buffer.
+                 * Mask the EPx to ensure that AP and driver are not accessing the buffer parallely.
+                 */
+                intrrxe = readb(INTRRXE);
+                writeb((intrrxe &= ~(1 << ep_num)), INTRRXE);
+            }
+
+            /* Deal with DATA transfer */
+            if ((req->length == endpoint->rcv_urb->actual_length) ||
+                    ((req->length >= endpoint->rcv_urb->actual_length) && req->length == 64)) {
+                handle_ept_complete(endpoint);
+
+                /* mask EPx INTRRXE */
+                /* The buffer is passed from the AP caller.
+                 * It happens that AP is dealing with the buffer filled data by driver,
+                 * but the driver is still receiving the next data packets onto the buffer.
+                 * Data corrupted happens if the every request use the same buffer.
+                 * Mask the EPx to ensure that AP and driver are not accessing the buffer parallely.
+                 */
+                intrrxe = readb(INTRRXE);
+                writeb((intrrxe &= ~(1 << ep_num)), INTRRXE);
+            }
+            break;
+        case USB_DIR_IN:
+            /* transfer direction is from device to host */
+            /* from the view of usb device, it's tx */
+            csr = readw(IECSR + TXCSR);
+
+            if (csr & EPX_TX_SENTSTALL) {
+                DBG_C("EP %d(TX): STALL\n", ep_num);
+                endpoint->status = -1;
+                handle_ept_complete(endpoint);
+                /* exception handling: implement this!! */
+                return;
+            }
+
+            if (csr & EPX_TX_TXPKTRDY) {
+                DBG_C
+                ("mt_udc_epx_handler: ep%d is not ready to be written\n",
+                 ep_num);
+                return;
+            }
+
+            urb = endpoint->tx_urb;
+            if (endpoint->sent == urb->actual_length) {
+                /* do signaling */
+                handle_ept_complete(endpoint);
+                break;
+            }
+
+            /* send next packet of the same urb */
+            count = mt_write_fifo(endpoint);
+#if DBG_USB_GENERAL
+            DBG_I("EP%d(TX), count = %d\n", ep_num, endpoint->sent);
+#endif
+
+            if (count != 0) {
+                /* not the interrupt generated by the last tx packet of the transfer */
+                csr |= EPX_TX_TXPKTRDY;
+                writew(csr, IECSR + TXCSR);
+            }
+
+            break;
+        default:
+            break;
+    }
+
+    writeb(index, INDEX);
+
+    return;
+}
+
+static void mt_udc_irq(u8 intrtx, u8 intrrx, u8 intrusb)
+{
+
+    int i;
+
+    DBG_IRQ("[USB] INTERRUPT\n");
+
+    if (intrusb) {
+        if (intrusb & INTRUSB_RESUME) {
+            DBG_IRQ("[USB] INTRUSB: RESUME\n");
+        }
+
+        if (intrusb & INTRUSB_SESS_REQ) {
+            DBG_IRQ("[USB] INTRUSB: SESSION REQUEST\n");
+        }
+
+        if (intrusb & INTRUSB_VBUS_ERROR) {
+            DBG_IRQ("[USB] INTRUSB: VBUS ERROR\n");
+        }
+
+        if (intrusb & INTRUSB_SUSPEND) {
+            DBG_IRQ("[USB] INTRUSB: SUSPEND\n");
+            mt_udc_suspend();
+        }
+
+        if (intrusb & INTRUSB_CONN) {
+            DBG_IRQ("[USB] INTRUSB: CONNECT\n");
+        }
+
+        if (intrusb & INTRUSB_DISCON) {
+            DBG_IRQ("[USB] INTRUSB: DISCONNECT\n");
+        }
+
+        if (intrusb & INTRUSB_RESET) {
+            DBG_IRQ("[USB] INTRUSB: RESET\n");
+            mt_udc_reset();
+        }
+
+        if (intrusb & INTRUSB_SOF) {
+            DBG_IRQ("[USB] INTRUSB: SOF\n");
+        }
+    }
+
+    /* endpoint 0 interrupt? */
+    if (intrtx & EPMASK (0)) {
+        mt_udc_ep0_handler();
+        intrtx &= ~0x1;
+    }
+
+    if (intrtx) {
+        for (i = 1; i < MT_EP_NUM; i++) {
+            if (intrtx & EPMASK (i)) {
+                mt_udc_epx_handler(i, USB_DIR_IN);
+            }
+        }
+    }
+
+    if (intrrx) {
+        for (i = 1; i < MT_EP_NUM; i++) {
+            if (intrrx & EPMASK (i)) {
+                mt_udc_epx_handler(i, USB_DIR_OUT);
+            }
+        }
+    }
+
+}
+
+static enum handler_return service_interrupts(void *arg)
+{
+
+    volatile u8 intrtx, intrrx, intrusb;
+    /* polling interrupt status for incoming interrupts and service it */
+    intrtx = readb(INTRTX) & readb(INTRTXE);
+    intrrx = readb(INTRRX) & readb(INTRRXE);
+    intrusb = readb(INTRUSB) & readb(INTRUSBE);
+
+    writeb(intrtx, INTRTX);
+    writeb(intrrx, INTRRX);
+    writeb(intrusb, INTRUSB);
+
+    intrusb &= ~INTRUSB_SOF;
+
+    if (intrtx | intrrx | intrusb) {
+        mt_udc_irq(intrtx, intrrx, intrusb);
+    }
+
+    return INT_RESCHEDULE;
+}
+
+static int mt_usb_irq_init(void)
+{
+    /* disable all endpoint interrupts */
+    writeb(0, INTRTXE);
+    writeb(0, INTRRXE);
+    writeb(0, INTRUSBE);
+
+    /* 2. Ack all gpt irq if needed */
+    //writel(0x3F, GPT_IRQ_ACK);
+
+    /* 3. Register usb irq */
+    mt_irq_set_sens(MT_USB0_IRQ_ID, LEVEL_SENSITIVE);
+    mt_irq_set_polarity(MT_USB0_IRQ_ID, MT65xx_POLARITY_LOW);
+
+    return 0;
+}
+
+/* Turn on the USB connection by enabling the pullup resistor */
+static void mt_usb_connect_internal(void)
+{
+    u8 tmpReg8;
+
+    /* connect */
+    tmpReg8 = readb(POWER);
+    tmpReg8 |= PWR_SOFT_CONN;
+    tmpReg8 |= PWR_ENABLE_SUSPENDM;
+
+#ifdef USB_FORCE_FULL_SPEED
+    tmpReg8 &= ~PWR_HS_ENAB;
+#else
+    tmpReg8 |= PWR_HS_ENAB;
+#endif
+    writeb(tmpReg8, POWER);
+}
+
+/* Turn off the USB connection by disabling the pullup resistor */
+static void mt_usb_disconnect_internal(void)
+{
+    u8 tmpReg8;
+
+    /* connect */
+    tmpReg8 = readb(POWER);
+    tmpReg8 &= ~PWR_SOFT_CONN;
+    writeb(tmpReg8, POWER);
+}
+
+int udc_init(struct udc_device *dev)
+{
+    struct udc_descriptor *desc = NULL;
+#ifdef USB_GINTR
+#ifdef USB_HSDMA_ISR
+    u32 usb_dmaintr;
+#endif
+    u32 usb_l1intm;
+#endif
+
+    DBG_I("[USB] %s:\n", __func__);
+
+    DBG_I("[USB] ep0_urb: %p\n", ep0_urb);
+    /* RESET */
+    mt_usb_disconnect_internal();
+    thread_sleep(20);
+    mt_usb_connect_internal();
+    thread_sleep(20);
+
+    /* usb phy init */
+    board_usb_init();
+    mt_usb_phy_recover();
+    thread_sleep(20);
+    /* allocate ep0 */
+    ep0out = _udc_endpoint_alloc(EP0, 0, EP0_MAX_PACKET_SIZE);
+    ep0in = _udc_endpoint_alloc(EP0, 1, EP0_MAX_PACKET_SIZE);
+    ep0req = udc_request_alloc();
+    ep0req->buffer = malloc(4096);
+    ep0_urb->buffer = (u8 *)malloc(4096);
+    if (!ep0req->buffer || !ep0req->buffer) {
+        dprintf(CRITICAL, "[USB] allocate buffer failed\n");
+        return ERR_NO_MEMORY;
+    }
+
+    {
+        /* create and register a language table descriptor */
+        /* language 0x0409 is US English */
+        desc = udc_descriptor_alloc(TYPE_STRING, EP0, 4);
+        desc->data[2] = 0x09;
+        desc->data[3] = 0x04;
+        udc_descriptor_register(desc);
+    }
+#ifdef USB_HSDMA_ISR
+    /* setting HSDMA interrupt register */
+    usb_dmaintr = (0xff | 0xff << USB_DMA_INTR_UNMASK_SET_OFFSET);
+    writel(usb_dmaintr, USB_DMA_INTR);
+#endif
+
+#ifdef USB_GINTR
+    usb_l1intm = (TX_INT_STATUS | RX_INT_STATUS | USBCOM_INT_STATUS | DMA_INT_STATUS);
+    writel(usb_l1intm, USB_L1INTM);
+#endif
+    the_device = dev;
+    return 0;
+}
+
+void udc_endpoint_free(struct udc_endpoint *ept)
+{
+    /* todo */
+}
+
+struct udc_request *udc_request_alloc(void) {
+    struct udc_request *req;
+    req = malloc(sizeof(*req));
+    req->buffer = NULL;
+    req->length = 0;
+    return req;
+}
+
+
+void udc_request_free(struct udc_request *req)
+{
+    free(req);
+}
+
+/* Called to start packet transmission. */
+/* It must be applied in udc_request_queue when polling mode is used.
+ * (When USB_GINTR is undefined).
+ * If interrupt mode is used, you can use
+ * mt_udc_epx_handler(ept->num, USB_DIR_IN); to replace mt_ep_write make ISR
+ * do it for you.
+ */
+static int mt_ep_write(struct udc_endpoint *endpoint)
+{
+    int ep_num = endpoint->num;
+    int count;
+    u8 index;
+    u16 csr;
+
+    index = readb(INDEX);
+    writeb(ep_num, INDEX);
+
+    /* udc_endpoint_write: cannot write ep0 */
+    if (ep_num == 0)
+        return false;
+
+    /* udc_endpoint_write: cannot write USB_DIR_OUT */
+    if (endpoint->in == 0)
+        return false;
+
+    csr = readw(IECSR + TXCSR);
+    if (csr & EPX_TX_TXPKTRDY) {
+#if DBG_USB_GENERAL
+        DBG_I("[USB]: udc_endpoint_write: ep%d is not ready to be written\n",
+              ep_num);
+
+#endif
+        return false;
+    }
+    count = mt_write_fifo(endpoint);
+
+    csr |= EPX_TX_TXPKTRDY;
+    writew(csr, IECSR + TXCSR);
+
+    writeb(index, INDEX);
+
+    return count;
+}
+
+int udc_request_queue(struct udc_endpoint *ept, struct udc_request *req)
+{
+    u8 intrrxe;
+
+#if DBG_USB_GENERAL
+    DBG_I("[USB] %s: ept%d %s queue req=%p, req->length=%x\n",
+          __func__, ept->num, ept->in ? "in" : "out", req, req->length);
+    DBG_I("[USB] %s: ept%d: %x, ept->in: %s, ept->rcv_urb->buffer: %x, req->buf: %x\n",
+          __func__, ept->num, ept, ept->in ? "IN" : "OUT" , ept->rcv_urb->buffer, req->buf);
+#endif
+
+    THREAD_LOCK(state); /* enter_critical_section */
+    ept->req = req;
+    ept->status = 0;    /* ACTIVE */
+
+    ept->sent = 0;
+    ept->last = 0;
+
+    /* read */
+    if (!ept->in) {
+        ept->rcv_urb->buffer = req->buffer;
+        ept->rcv_urb->actual_length = 0;
+
+        /* unmask EPx INTRRXE */
+        /*
+         * To avoid the parallely access the buffer,
+         * it is umasked here and umask at complete.
+         */
+        intrrxe = readb(INTRRXE);
+        intrrxe |= (1 << ept->num);
+        writeb(intrrxe, INTRRXE);
+    }
+
+    /* write */
+    if (ept->in) {
+        ept->tx_urb->buffer = req->buffer;
+        ept->tx_urb->actual_length = req->length;
+
+        mt_ep_write(ept);
+    }
+    THREAD_UNLOCK(state); /* exit_critical_section */
+    return 0;
+}
+
+int udc_register_gadget(struct udc_gadget *gadget)
+{
+    if (the_gadget) {
+        DBG_C("only one gadget supported\n");
+        return false;
+    }
+    the_gadget = gadget;
+    return 0;
+}
+
+static void udc_ept_desc_fill(struct udc_endpoint *ept, unsigned char *data)
+{
+    data[0] = 7;
+    data[1] = TYPE_ENDPOINT;
+    data[2] = ept->num | (ept->in ? USB_DIR_IN : USB_DIR_OUT);
+    data[3] = 0x02;     /* bulk -- the only kind we support */
+    data[4] = ept->maxpkt;
+    data[5] = ept->maxpkt >> 8;
+    data[6] = ept->in ? 0x00 : 0x01;
+}
+
+static unsigned udc_ifc_desc_size(struct udc_gadget *g)
+{
+    return 9 + g->ifc_endpoints * 7;
+}
+
+static void udc_ifc_desc_fill(struct udc_gadget *g, unsigned char *data)
+{
+    unsigned n;
+
+    data[0] = 0x09;
+    data[1] = TYPE_INTERFACE;
+    data[2] = 0x00;     /* ifc number */
+    data[3] = 0x00;     /* alt number */
+    data[4] = g->ifc_endpoints;
+    data[5] = g->ifc_class;
+    data[6] = g->ifc_subclass;
+    data[7] = g->ifc_protocol;
+    data[8] = udc_string_desc_alloc(g->ifc_string);
+
+    data += 9;
+    for (n = 0; n < g->ifc_endpoints; n++) {
+        udc_ept_desc_fill(g->ept[n], data);
+        data += 7;
+    }
+}
+
+static int udc_start_cond(void)
+{
+    struct udc_descriptor *desc;
+    unsigned char *data;
+    unsigned size;
+
+    DBG_C("[USB] %s\n", __func__);
+
+    if (!the_device) {
+        DBG_C("udc cannot start before init\n");
+        return false;
+    }
+    if (!the_gadget) {
+        DBG_C("udc has no gadget registered\n");
+        return false;
+    }
+
+    /* create our device descriptor */
+    desc = udc_descriptor_alloc(TYPE_DEVICE, EP0, 18);
+    data = desc->data;
+    data[2] = 0x00;     /* usb spec minor rev */
+    data[3] = 0x02;     /* usb spec major rev */
+    data[4] = 0x00;     /* class */
+    data[5] = 0x00;     /* subclass */
+    data[6] = 0x00;     /* protocol */
+    data[7] = 0x40;     /* max packet size on ept 0 */
+    memcpy(data + 8, &the_device->vendor_id, sizeof(short));
+    memcpy(data + 10, &the_device->product_id, sizeof(short));
+    memcpy(data + 12, &the_device->version_id, sizeof(short));
+    data[14] = udc_string_desc_alloc(the_device->manufacturer);
+    data[15] = udc_string_desc_alloc(the_device->product);
+    data[16] = udc_string_desc_alloc(the_device->serialno);
+    data[17] = 1;       /* number of configurations */
+    udc_descriptor_register(desc);
+
+    /* create our configuration descriptor */
+    size = 9 + udc_ifc_desc_size(the_gadget);
+    desc = udc_descriptor_alloc(TYPE_CONFIGURATION, EP0, size);
+    data = desc->data;
+    data[0] = 0x09;
+    data[2] = size;
+    data[3] = size >> 8;
+    data[4] = 0x01;     /* number of interfaces */
+    data[5] = 0x01;     /* configuration value */
+    data[6] = 0x00;     /* configuration string */
+    data[7] = 0x80;     /* attributes */
+    data[8] = 0x80;     /* max power (250ma) -- todo fix this */
+
+    udc_ifc_desc_fill(the_gadget, data + 9);
+    udc_descriptor_register(desc);
+
+#if DBG_USB_DUMP_DESC
+    DBG_I("%s: dump desc_list\n", __func__);
+    for (desc = desc_list; desc; desc = desc->next) {
+        DBG_I("tag: %04x\n", desc->tag);
+        DBG_I("len: %d\n", desc->len);
+        DBG_I("data:");
+        hexdump8(desc->data, desc->len);
+    }
+#endif
+
+    /* register interrupt handler */
+    mt_usb_irq_init();
+    register_int_handler(MT_USB0_IRQ_ID, service_interrupts, NULL);
+
+    /* go to RUN mode */
+    mt_usb_phy_recover();
+
+    /* clear INTRTX, INTRRX and INTRUSB */
+    writew(0xffff, INTRTX); /* writew */
+    writew(0xffff, INTRRX); /* writew */
+    writeb(0xff, INTRUSB); /* writeb */
+
+    /* unmask usb irq */
+#ifdef USB_GINTR
+    unmask_interrupt(MT_USB0_IRQ_ID);
+#endif
+    writeb((INTRUSB_SUSPEND | INTRUSB_RESUME | INTRUSB_RESET |INTRUSB_DISCON), INTRUSBE);
+
+    return 0;
+}
+
+int udc_start(void)
+{
+    return udc_start_cond();
+}
+
+int udc_stop(void)
+{
+    thread_sleep(10);
+
+    mt_usb_disconnect_internal();
+    mt_usb_phy_savecurrent();
+    free(ep0_urb->buffer);
+    return 0;
+}
diff --git a/src/bsp/lk/platform/mt2701/drivers/wdt/mtk_wdt.c b/src/bsp/lk/platform/mt2701/drivers/wdt/mtk_wdt.c
new file mode 100644
index 0000000..0c88feb
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/drivers/wdt/mtk_wdt.c
@@ -0,0 +1,351 @@
+#include <debug.h>
+#include <platform/mtk_wdt.h>
+#include <platform/mtk_timer.h>
+
+#if ENABLE_WDT_MODULE
+
+static bool mtk_wd_CheckNonResetReg2(unsigned int offset)
+{
+    u32 tmp;
+    tmp = readl(MTK_WDT_NONRST_REG2);
+    if (tmp & (1U << offset))
+        return true;
+    else
+        return false;
+}
+
+static void mtk_wd_SetNonResetReg2(unsigned int offset, bool value)
+{
+    u32 reg;
+
+    reg = readl(MTK_WDT_NONRST_REG2);
+    if (value)
+        reg |= (1U << offset);
+    else
+        reg &= ~(1U << offset);
+
+    writel(reg, MTK_WDT_NONRST_REG2);
+}
+
+void set_clr_fastboot_mode(bool flag)
+{
+    if (flag==true)
+        mtk_wd_SetNonResetReg2(0x2, 1);
+    else if (flag==false)
+        mtk_wd_SetNonResetReg2(0x2, 0);
+
+    dprintf(INFO, "set_clr_fastboot_mode\n");
+}
+
+void set_clr_recovery_mode(bool flag)
+{
+    if (flag==true)
+        mtk_wd_SetNonResetReg2(0x1, 1);
+    else if (flag==false)
+        mtk_wd_SetNonResetReg2(0x1, 0);
+
+    dprintf(INFO, "set_clr_recovery_mode\n");
+}
+
+bool check_fastboot_mode(void)
+{
+    return mtk_wd_CheckNonResetReg2(0x2);
+}
+
+bool check_recovery_mode(void)
+{
+    return mtk_wd_CheckNonResetReg2(0x1);
+}
+
+void mtk_wdt_disable(void)
+{
+    u32 tmp;
+
+    tmp = readl(MTK_WDT_MODE);
+    tmp &= ~MTK_WDT_MODE_ENABLE;       /* disable watchdog */
+    tmp |= (MTK_WDT_MODE_KEY);         /* need key then write is allowed */
+    writel(tmp, MTK_WDT_MODE);
+}
+
+static void mtk_wdt_reset(char mode)
+{
+    /* Watchdog Rest */
+    unsigned int wdt_mode_val;
+    writel(MTK_WDT_RESTART_KEY, MTK_WDT_RESTART);
+
+    wdt_mode_val = readl(MTK_WDT_MODE);
+    /* clear autorestart bit: autoretart: 1, bypass power key, 0: not bypass power key */
+    wdt_mode_val &=(~MTK_WDT_MODE_AUTO_RESTART);
+    /* make sure WDT mode is hw reboot mode, can not config isr mode  */
+    wdt_mode_val &=(~(MTK_WDT_MODE_IRQ|MTK_WDT_MODE_ENABLE | MTK_WDT_MODE_DUAL_MODE));
+
+    if (mode) { /* mode != 0 means by pass power key reboot, We using auto_restart bit as by pass power key flag */
+        wdt_mode_val = wdt_mode_val | (MTK_WDT_MODE_KEY|MTK_WDT_MODE_EXTEN|MTK_WDT_MODE_AUTO_RESTART);
+        writel(wdt_mode_val, MTK_WDT_MODE);
+
+    } else {
+        wdt_mode_val = wdt_mode_val | (MTK_WDT_MODE_KEY|MTK_WDT_MODE_EXTEN);
+        writel(wdt_mode_val,MTK_WDT_MODE);
+
+    }
+
+    spin(100);
+    writel(MTK_WDT_SWRST_KEY, MTK_WDT_SWRST);
+}
+
+static unsigned int mtk_wdt_check_status(void)
+{
+    static unsigned int status = 0;
+
+    /*
+     * Because WDT_STA register will be cleared after writing WDT_MODE,
+     * we use a static variable to store WDT_STA.
+     * After reset, static varialbe will always be clear to 0,
+     * so only read WDT_STA when static variable is 0 is OK
+     */
+    if (0 == status)
+        status = readl(MTK_WDT_STATUS);
+
+    return status;
+}
+
+static void mtk_wdt_mode_config(bool dual_mode_en,
+                                bool irq,
+                                bool ext_en,
+                                bool ext_pol,
+                                bool wdt_en)
+{
+    unsigned int tmp;
+
+    tmp = readl(MTK_WDT_MODE);
+    tmp |= MTK_WDT_MODE_KEY;
+
+    // Bit 0 : Whether enable watchdog or not
+    if (wdt_en == true)
+        tmp |= MTK_WDT_MODE_ENABLE;
+    else
+        tmp &= ~MTK_WDT_MODE_ENABLE;
+
+    // Bit 1 : Configure extern reset signal polarity.
+    if (ext_pol == true)
+        tmp |= MTK_WDT_MODE_EXT_POL;
+    else
+        tmp &= ~MTK_WDT_MODE_EXT_POL;
+
+    // Bit 2 : Whether enable external reset signal
+    if (ext_en == true)
+        tmp |= MTK_WDT_MODE_EXTEN;
+    else
+        tmp &= ~MTK_WDT_MODE_EXTEN;
+
+    // Bit 3 : Whether generating interrupt instead of reset signal
+    if (irq == true)
+        tmp |= MTK_WDT_MODE_IRQ;
+    else
+        tmp &= ~MTK_WDT_MODE_IRQ;
+
+    // Bit 6 : Whether enable debug module reset
+    if (dual_mode_en == true)
+        tmp |= MTK_WDT_MODE_DUAL_MODE;
+    else
+        tmp &= ~MTK_WDT_MODE_DUAL_MODE;
+
+    // Bit 4: WDT_Auto_restart, this is a reserved bit, we use it as bypass powerkey flag.
+    //      Because HW reboot always need reboot to kernel, we set it always.
+    tmp |= MTK_WDT_MODE_AUTO_RESTART;
+
+    writel(tmp, MTK_WDT_MODE);
+    //dual_mode(1); //always dual mode
+    //mdelay(100);
+    dprintf(INFO,"mtk_wdt_mode_config LK mode value=%x", readl(MTK_WDT_MODE));
+}
+
+static void mtk_wdt_set_time_out_value(uint32_t value)
+{
+    static unsigned int timeout;
+
+    /*
+    * TimeOut = BitField 15:5
+    * Key      = BitField  4:0 = 0x08
+    */
+
+    // sec * 32768 / 512 = sec * 64 = sec * 1 << 6
+    timeout = (unsigned int)(value * ( 1 << 6) );
+    timeout = timeout << 5;
+    writel((timeout | MTK_WDT_LENGTH_KEY), MTK_WDT_LENGTH);
+}
+
+static void mtk_wdt_restart(void)
+{
+    // Reset WatchDogTimer's counting value to time out value
+    // ie., keepalive()
+    writel(MTK_WDT_RESTART_KEY, MTK_WDT_RESTART);
+}
+
+static void mtk_wdt_sw_reset(void)
+{
+    printf ("UB WDT SW RESET\n");
+    //DRV_WriteReg32 (0x70025000, 0x2201);
+    //DRV_WriteReg32 (0x70025008, 0x1971);
+    //DRV_WriteReg32 (0x7002501C, 0x1209);
+    mtk_wdt_reset(1);/* NOTE here, this reset will cause by pass power key */
+
+    // system will reset
+
+    while (1) {
+        printf ("UB SW reset fail ... \n");
+    };
+}
+
+static void mtk_wdt_hw_reset(void)
+{
+    dprintf(INFO,"UB WDT_HW_Reset\n");
+
+    // 1. set WDT timeout 1 secs, 1*64*512/32768 = 1sec
+    mtk_wdt_set_time_out_value(1);
+
+    // 2. enable WDT debug reset enable, generating irq disable, ext reset disable
+    //    ext reset signal low, wdt enalbe
+    mtk_wdt_mode_config(true, false, false, false, true);
+
+    // 3. reset the watch dog timer to the value set in WDT_LENGTH register
+    mtk_wdt_restart();
+
+    // 4. system will reset
+    while (1);
+}
+
+static const char* parsing_reset_reason(unsigned int wdt_status)
+{
+	const char *rst_reason="normal";
+	switch(wdt_status)
+	{
+		case MTK_WDT_STATUS_HWWDT_RST:
+			rst_reason="hw_rst";
+			break;
+
+		case MTK_WDT_STATUS_SWWDT_RST:
+			rst_reason="sw_rst";
+			break;
+
+		case MTK_WDT_STATUS_IRQWDT_RST:
+			rst_reason="irq_rst";
+			break;
+
+		case MTK_WDT_STATUS_SECURITY_RST:
+			rst_reason="security_rst";
+			break;
+
+		case MTK_WDT_STATUS_DEBUGWDT_RST:
+			rst_reason="debug_rst";
+			break;
+
+		case MTK_WDT_STATUS_THERMAL_DIRECT_RST:
+			rst_reason="thermal_ctl_rst";
+			break;
+
+		case MTK_WDT_STATUS_SPMWDT_RST:
+			rst_reason="spm_rst";
+			break;
+
+		case MTK_WDT_STATUS_SPM_THERMAL_RST:
+			rst_reason="spm_thermal_rst";
+			break;
+
+		default:
+			break;
+	}
+
+	return rst_reason;
+}
+
+void mtk_wdt_init(void)
+{
+    /* This function will store the reset reason: Time out/ SW trigger */
+    dprintf(ALWAYS, "Watchdog Status: %x , boot from %s\n", mtk_wdt_check_status(),parsing_reset_reason(mtk_wdt_check_status()));
+
+    mtk_wdt_mode_config(false, false, false, false, false);
+
+#if (!LK_WDT_DISABLE)
+    mtk_wdt_set_time_out_value(10);
+    mtk_wdt_mode_config(true, true, true, false, true);
+    mtk_wdt_restart();
+#endif
+}
+
+static bool mtk_is_rgu_trigger_reset(void)
+{
+    if (mtk_wdt_check_status())
+        return true;
+    return false;
+}
+
+void mtk_arch_reset(char mode)
+{
+    dprintf(INFO,"UB mtk_arch_reset\n");
+
+    mtk_wdt_reset(mode);
+
+    while (1);
+}
+
+static void rgu_swsys_reset(WD_SYS_RST_TYPE reset_type)
+{
+    if (WD_MD_RST == reset_type) {
+        unsigned int wdt_dbg_ctrl;
+        wdt_dbg_ctrl = readl(MTK_WDT_SWSYSRST);
+        wdt_dbg_ctrl |= MTK_WDT_SWSYS_RST_KEY;
+        wdt_dbg_ctrl |= 0x80;// 1<<7
+        writel(MTK_WDT_SWSYSRST, wdt_dbg_ctrl);
+        spin(1000);
+        wdt_dbg_ctrl = readl(MTK_WDT_SWSYSRST);
+        wdt_dbg_ctrl |= MTK_WDT_SWSYS_RST_KEY;
+        wdt_dbg_ctrl &= (~0x80);// ~(1<<7)
+        writel(MTK_WDT_SWSYSRST, wdt_dbg_ctrl);
+        dprintf(INFO,"rgu pl md reset\n");
+    }
+}
+#else
+void mtk_wdt_init(void)
+{
+    dprintf(INFO,"UB WDT Dummy init called\n");
+}
+static bool mtk_is_rgu_trigger_reset()
+{
+    dprintf(INFO,"UB Dummy mtk_is_rgu_trigger_reset called\n");
+    return FALSE;
+}
+void mtk_arch_reset(char mode)
+{
+    dprintf(INFO,"UB WDT Dummy arch reset called\n");
+}
+
+int mtk_wdt_boot_check(void)
+{
+    dprintf(INFO,"UB WDT Dummy mtk_wdt_boot_check called\n");
+    return WDT_NOT_WDT_REBOOT;
+}
+
+void mtk_wdt_disable(void)
+{
+    dprintf(INFO,"UB WDT Dummy mtk_wdt_disable called\n");
+}
+
+static void mtk_wdt_restart(void)
+{
+    dprintf(INFO,"UB WDT Dummy mtk_wdt_restart called\n");
+}
+static void mtk_wdt_sw_reset(void)
+{
+    dprintf(INFO,"UB WDT Dummy mtk_wdt_sw_reset called\n");
+}
+
+static void mtk_wdt_hw_reset(void)
+{
+    dprintf(INFO,"UB WDT Dummy mtk_wdt_hw_reset called\n");
+}
+static void rgu_swsys_reset(WD_SYS_RST_TYPE reset_type)
+{
+    dprintf(INFO,"UB WDT Dummy rgu_swsys_reset called\n");
+}
+#endif
diff --git a/src/bsp/lk/platform/mt2701/include/platform/cust_nand.h b/src/bsp/lk/platform/mt2701/include/platform/cust_nand.h
new file mode 100644
index 0000000..bea2bfc
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/cust_nand.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef CUST_NAND_H
+#define CUST_NAND_H
+
+#include <platform/typedefs.h>
+
+#if 0
+#define RAMDOM_READ 1<<0
+#define CACHE_READ  1<<1
+
+typedef struct {
+    u16 id;          //deviceid+menuid
+    u8  addr_cycle;
+    u8  iowidth;
+    u16 totalsize;
+    u16 blocksize;
+    u16 pagesize;
+    u32 timmingsetting;
+    char devciename[14];
+    u32 advancedmode;   //
+} flashdev_info,*pflashdev_info;
+
+static const flashdev_info g_FlashTable[]= {
+    //micro
+    {0xAA2C,  5,  8,  256,  128,  2048,  0x01113,  "MT29F2G08ABD",  0},
+    {0xB12C,  4,  16, 128,  128,  2048,  0x01113,  "MT29F1G16ABC",  0},
+    {0xBA2C,  5,  16, 256,  128,  2048,  0x01113,  "MT29F2G16ABD",  0},
+    {0xAC2C,  5,  8,  512,  128,  2048,  0x01113,  "MT29F4G08ABC",  0},
+    {0xA12C,  4,  8,  128,  128,  2048,  0x01113,  "MT29F1G08ABB",  0},
+    {0xBC2C,  5,  16, 512, 128,  2048,  0x01112,  "MT29F4G16ABBDA", 0},
+    //samsung
+    {0xBAEC,  5,  16, 256,  128,  2048,  0x01123,  "K522H1GACE",    0},
+    {0xBCEC,  5,  16, 512,  128,  2048,  0x01123,  "K524G2GACB",    RAMDOM_READ},
+    {0xDAEC,  5,  8,  256,  128,  2048,  0x33222,  "K9F2G08U0A",    RAMDOM_READ},
+    {0xF1EC,  4,  8,  128,  128,  2048,  0x01123,  "K9F1G08U0A",    RAMDOM_READ},
+    {0xAAEC,  5,  8,  256,  128,  2048,  0x01123,  "K9F2G08R0A",    0},
+    //hynix
+    {0xD3AD,  5,  8,  1024, 256,  2048,  0x44333,  "HY27UT088G2A",  0},
+    {0xA1AD,  4,  8,  128,  128,  2048,  0x01123,  "H8BCSOPJOMCP",  0},
+    {0xBCAD,  5,  16, 512,  128,  2048,  0x01123,  "H8BCSOUNOMCR",  0},
+    {0xBAAD,  5,  16, 256,  128,  2048,  0x01123,  "H8BCSOSNOMCR",  0},
+    //toshiba
+    {0x9598,  5,  16, 816,  128,  2048,  0x00113,  "TY9C000000CMG", 0},
+    {0x9498,  5,  16, 375,  128,  2048,  0x00113,  "TY9C000000CMG", 0},
+    {0xBC98,  5,  16, 512, 128,  2048,  0x02113,  "TY58NYG2S8E",    0},
+    {0xC198,  4,  16, 128,  128,  2048,  0x44333,  "TC58NWGOS8C",   0},
+    {0xBA98,  5,  16, 256,  128,  2048,  0x02113,  "TC58NYG1S8C",   0},
+    //st-micro
+    {0xBA20,  5,  16, 256,  128,  2048,  0x01123,  "ND02CGR4B2DI6", 0},
+
+    // elpida
+    {0xBC20,  5,  16, 512,  128,  2048,  0x01123,  "04GR4B2DDI6",   0},
+    {0x0000,  0,  0,  0,    0,    0,     0,        "xxxxxxxxxxxxx", 0}
+};
+#endif
+
+#define NFI_DEFAULT_ACCESS_TIMING        (0x44333)
+
+//preloader only support 1 cs
+//#define NFI_CS_NUM                    (2)
+#define NFI_DEFAULT_CS              (0)
+
+//#define USE_AHB_MODE                  (0)
+
+#endif
diff --git a/src/bsp/lk/platform/mt2701/include/platform/efuse.h b/src/bsp/lk/platform/mt2701/include/platform/efuse.h
new file mode 100644
index 0000000..4255237
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/efuse.h
@@ -0,0 +1,71 @@
+/*
+ * Copyright (c) 2017 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#pragma once
+
+struct efuse_entry {
+    u32 id;
+    size_t len;
+};
+
+#define EFUEE_INFO(_id, _len) {   \
+        .id = _id,                  \
+        .len = _len, /* byte length */ \
+}
+
+/* efuse index */
+enum efuse_index {
+    SBC_PUBK0_HASH = 0,
+    SBC_PUBK1_HASH,
+    SBC_PUBK2_HASH,
+    SBC_PUBK3_HASH,
+    SBC_PUBK0_DIS,
+    SBC_PUBK1_DIS,
+    SBC_PUBK2_DIS,
+    SBC_PUBK3_DIS,
+    HRID,
+    EFUSE_MAX
+};
+
+static const struct efuse_entry efuse_info[] = {
+    EFUEE_INFO(SBC_PUBK0_HASH, 32),
+    EFUEE_INFO(SBC_PUBK1_HASH, 32),
+    EFUEE_INFO(SBC_PUBK2_HASH, 32),
+    EFUEE_INFO(SBC_PUBK3_HASH, 32),
+    EFUEE_INFO(SBC_PUBK0_DIS, 1),
+    EFUEE_INFO(SBC_PUBK1_DIS, 1),
+    EFUEE_INFO(SBC_PUBK2_DIS, 1),
+    EFUEE_INFO(SBC_PUBK3_DIS, 1),
+    EFUEE_INFO(HRID, 8),
+};
+
+/**
+ * fuse_read - read data from efuse hardware
+ *
+ * @fuse:   the efuse index
+ * @data:   the data storing the returned efuse data
+ * @len:    the data length in byte
+ *
+ * Return: Return 0 if the command is done, otherwise it is failed.
+ */
+int fuse_read(u32 fuse, u8 *data, size_t len);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/gic.h b/src/bsp/lk/platform/mt2701/include/platform/gic.h
new file mode 100644
index 0000000..b8c2251
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/gic.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <platform/mt2701.h>
+
+#define GICBASE(n)  (0x10210000)
+#define GICD_OFFSET (0x1000)
+#define GICC_OFFSET (0x2000)
+
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mmc_core.h b/src/bsp/lk/platform/mt2701/include/platform/mmc_core.h
new file mode 100644
index 0000000..7f885e8
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mmc_core.h
@@ -0,0 +1,641 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <stdbool.h>
+#include <sys/types.h>
+#include <kernel/mutex.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define MMC_BLOCK_BITS_SHFT             (9)
+#define MMC_BLOCK_SIZE                  (1 << MMC_BLOCK_BITS_SHFT)
+#define MMC_MAX_BLOCK_SIZE              (1 << MMC_BLOCK_BITS_SHFT)
+
+#define SDIO_MAX_FUNCS                  (7)
+
+#define SD_CMD_BIT                      (1 << 7)
+#define SD_CMD_APP_BIT                  (1 << 8)
+#define SD_CMD_AUTO_BIT                 (1 << 9)
+
+/* MMC command numbers */
+#define MMC_CMD_GO_IDLE_STATE           (0)              /* bc.   no response */
+#define MMC_CMD_SEND_OP_COND            (1)              /* bcr.  R3          */
+#define MMC_CMD_ALL_SEND_CID            (2)              /* bcr.  R2          */
+#define MMC_CMD_SET_RELATIVE_ADDR       (3)              /* ac.   R1          */
+#define MMC_CMD_SET_DSR                 (4)              /* bc.   no response */
+#define MMC_CMD_SLEEP_AWAKE             (5)              /* ac.   R1b         */
+#define MMC_CMD_SWITCH                  (6)              /* ac.   R1b         */
+#define MMC_CMD_SELECT_CARD             (7)              /* ac.   R1/R1b      */
+#define MMC_CMD_SEND_EXT_CSD            (8)              /* adtc. R1          */
+#define MMC_CMD_SEND_CSD                (9)              /* ac.   R2          */
+#define MMC_CMD_SEND_CID                (10)             /* ac.   R2          */
+#define MMC_CMD_READ_DAT_UNTIL_STOP     (11)             /* adtc. R1          */
+#define MMC_CMD_STOP_TRANSMISSION       (12)             /* ac.   R1/R1b      */
+#define MMC_CMD_SEND_STATUS             (13)             /* ac.   R1          */
+#define MMC_CMD_BUSTEST_R               (14)             /* adtc. R1          */
+#define MMC_CMD_GO_INACTIVE_STATE       (15)             /* ac.   no response */
+#define MMC_CMD_SET_BLOCKLEN            (16)             /* ac.   R1          */
+#define MMC_CMD_READ_SINGLE_BLOCK       (17)             /* adtc. R1          */
+#define MMC_CMD_READ_MULTIPLE_BLOCK     (18)             /* adtc. R1          */
+#define MMC_CMD_BUSTEST_W               (19)             /* adtc. R1          */
+#define MMC_CMD_WRITE_DAT_UNTIL_STOP    (20)             /* adtc. R1          */
+#define MMC_CMD21                       (21)             /* adtc. R1  Sandisk  */
+#define MMC_CMD_SET_BLOCK_COUNT         (23)             /* ac.   R1          */
+#define MMC_CMD_WRITE_BLOCK             (24)             /* adtc. R1          */
+#define MMC_CMD_WRITE_MULTIPLE_BLOCK    (25)             /* adtc. R1          */
+#define MMC_CMD_PROGRAM_CID             (26)             /* adtc. R1          */
+#define MMC_CMD_PROGRAM_CSD             (27)             /* adtc. R1          */
+
+#define MMC_CMD_SET_WRITE_PROT          (28)             /* ac.   R1b         */
+#define MMC_CMD_CLR_WRITE_PROT          (29)             /* ac.   R1b         */
+#define MMC_CMD_SEND_WRITE_PROT         (30)             /* adtc. R1          */
+#define MMC_CMD_SEND_WRITE_PROT_TYPE    (31)             /* adtc. R1          */
+#define MMC_CMD_ERASE_WR_BLK_START      (32)
+#define MMC_CMD_ERASE_WR_BLK_END        (33)
+#define MMC_CMD_ERASE_GROUP_START       (35)             /* ac.   R1          */
+#define MMC_CMD_ERASE_GROUP_END         (36)             /* ac.   R1          */
+#define MMC_CMD_ERASE                   (38)             /* ac.   R1b         */
+#define MMC_CMD_FAST_IO                 (39)             /* ac.   R4          */
+#define MMC_CMD_GO_IRQ_STATE            (40)             /* bcr.  R5          */
+#define MMC_CMD_LOCK_UNLOCK             (42)             /* adtc. R1          */
+#define MMC_CMD50                       (50)             /* adtc. R1 Sandisk */
+#define MMC_CMD_APP_CMD                 (55)             /* ac.   R1          */
+#define MMC_CMD_GEN_CMD                 (56)             /* adtc. R1          */
+
+/* SD Card command numbers */
+#define SD_CMD_SEND_RELATIVE_ADDR       (3 | SD_CMD_BIT)
+#define SD_CMD_SWITCH                   (6 | SD_CMD_BIT)
+#define SD_CMD_SEND_IF_COND             (8 | SD_CMD_BIT)
+#define SD_CMD_VOL_SWITCH               (11 | SD_CMD_BIT)
+#define SD_CMD_SEND_TUNING_BLOCK        (19 | SD_CMD_BIT)
+#define SD_CMD_SPEED_CLASS_CTRL         (20 | SD_CMD_BIT)
+
+#define SD_ACMD_SET_BUSWIDTH            (6  | SD_CMD_APP_BIT)
+#define SD_ACMD_SD_STATUS               (13 | SD_CMD_APP_BIT)
+#define SD_ACMD_SEND_NR_WR_BLOCKS       (22 | SD_CMD_APP_BIT)
+#define SD_ACMD_SET_WR_ERASE_CNT        (23 | SD_CMD_APP_BIT)
+#define SD_ACMD_SEND_OP_COND            (41 | SD_CMD_APP_BIT)
+#define SD_ACMD_SET_CLR_CD              (42 | SD_CMD_APP_BIT)
+#define SD_ACMD_SEND_SCR                (51 | SD_CMD_APP_BIT)
+
+/* SDIO Card command numbers */
+#define SD_IO_SEND_OP_COND              (5 | SD_CMD_BIT) /* bcr. R4           */
+#define SD_IO_RW_DIRECT                 (52 | SD_CMD_BIT)/* ac.  R5           */
+#define SD_IO_RW_EXTENDED               (53 | SD_CMD_BIT)/* adtc. R5          */
+
+/* platform dependent command */
+#define SD_ATOCMD_STOP_TRANSMISSION     (12 | SD_CMD_AUTO_BIT)
+#define SD_ATOCMD_SET_BLOCK_COUNT       (23 | SD_CMD_AUTO_BIT)
+
+#define MMC_VDD_145_150 0x00000001  /* VDD voltage 1.45 - 1.50 */
+#define MMC_VDD_150_155 0x00000002  /* VDD voltage 1.50 - 1.55 */
+#define MMC_VDD_155_160 0x00000004  /* VDD voltage 1.55 - 1.60 */
+#define MMC_VDD_160_165 0x00000008  /* VDD voltage 1.60 - 1.65 */
+#define MMC_VDD_165_170 0x00000010  /* VDD voltage 1.65 - 1.70 */
+#define MMC_VDD_17_18   0x00000020  /* VDD voltage 1.7 - 1.8 */
+#define MMC_VDD_18_19   0x00000040  /* VDD voltage 1.8 - 1.9 */
+#define MMC_VDD_19_20   0x00000080  /* VDD voltage 1.9 - 2.0 */
+#define MMC_VDD_20_21   0x00000100  /* VDD voltage 2.0 ~ 2.1 */
+#define MMC_VDD_21_22   0x00000200  /* VDD voltage 2.1 ~ 2.2 */
+#define MMC_VDD_22_23   0x00000400  /* VDD voltage 2.2 ~ 2.3 */
+#define MMC_VDD_23_24   0x00000800  /* VDD voltage 2.3 ~ 2.4 */
+#define MMC_VDD_24_25   0x00001000  /* VDD voltage 2.4 ~ 2.5 */
+#define MMC_VDD_25_26   0x00002000  /* VDD voltage 2.5 ~ 2.6 */
+#define MMC_VDD_26_27   0x00004000  /* VDD voltage 2.6 ~ 2.7 */
+#define MMC_VDD_27_28   0x00008000  /* VDD voltage 2.7 ~ 2.8 */
+#define MMC_VDD_28_29   0x00010000  /* VDD voltage 2.8 ~ 2.9 */
+#define MMC_VDD_29_30   0x00020000  /* VDD voltage 2.9 ~ 3.0 */
+#define MMC_VDD_30_31   0x00040000  /* VDD voltage 3.0 ~ 3.1 */
+#define MMC_VDD_31_32   0x00080000  /* VDD voltage 3.1 ~ 3.2 */
+#define MMC_VDD_32_33   0x00100000  /* VDD voltage 3.2 ~ 3.3 */
+#define MMC_VDD_33_34   0x00200000  /* VDD voltage 3.3 ~ 3.4 */
+#define MMC_VDD_34_35   0x00400000  /* VDD voltage 3.4 ~ 3.5 */
+#define MMC_VDD_35_36   0x00800000  /* VDD voltage 3.5 ~ 3.6 */
+#define MMC_CARD_BUSY   0x80000000  /* Card Power up status bit */
+#define MMC_VDD_165_195     0x00000080  /* VDD voltage 1.65 - 1.95 */ //Add this line by reference to include/linux/mmc/host.h in linux kernel
+#define MMC_VDD_27_36       0x00FF8000
+
+#define EMMC_VER_50   (50)
+#define EMMC_VER_45   (45)
+#define EMMC_VER_44   (44)
+#define EMMC_VER_43   (43)
+#define EMMC_VER_42   (42)
+#define SD_VER_10     (10)
+#define SD_VER_20     (20)
+#define SD_VER_30     (30)
+
+#define MMC_ERR_NONE          0
+#define MMC_ERR_TIMEOUT       1
+#define MMC_ERR_BADCRC        2
+#define MMC_ERR_FIFO          3
+#define MMC_ERR_FAILED        4
+#define MMC_ERR_INVALID       5
+#define MMC_ERR_CMDTUNEFAIL   6
+#define MMC_ERR_READTUNEFAIL  7
+#define MMC_ERR_WRITETUNEFAIL 8
+#define MMC_ERR_CMD_TIMEOUT   9
+#define MMC_ERR_CMD_RSPCRC    10
+#define MMC_ERR_ACMD_TIMEOUT  11
+#define MMC_ERR_ACMD_RSPCRC   12
+#define MMC_ERR_AXI_RSPCRC    13
+#define MMC_ERR_UNEXPECT      14
+
+#define MMC_POWER_OFF       0
+#define MMC_POWER_UP        1
+#define MMC_POWER_ON        2
+
+#define MMC_BUS_WIDTH_1     0
+#define MMC_BUS_WIDTH_4     2
+
+#define SD_BUS_WIDTH_1      0
+#define SD_BUS_WIDTH_4      2
+
+#define MMC_STATE_PRESENT       (1<<0)      /* present in sysfs */
+#define MMC_STATE_READONLY      (1<<1)      /* card is read-only */
+#define MMC_STATE_HIGHSPEED     (1<<2)      /* card is in high speed mode */
+#define MMC_STATE_BLOCKADDR     (1<<3)      /* card uses block-addressing */
+#define MMC_STATE_HIGHCAPS      (1<<4)
+#define MMC_STATE_UHS1          (1<<5)      /* card is in ultra high speed mode */
+#define MMC_STATE_DDR           (1<<6)      /* card is in ddr mode */
+#define MMC_STATE_HS200         (1<<7)
+#define MMC_STATE_HS400         (1<<8)
+#define MMC_STATE_BACKYARD      (1<<9)
+
+#define R1_OUT_OF_RANGE         (1UL << 31) /* er, c */
+#define R1_ADDRESS_ERROR        (1 << 30)   /* erx, c */
+#define R1_BLOCK_LEN_ERROR      (1 << 29)   /* er, c */
+#define R1_ERASE_SEQ_ERROR      (1 << 28)   /* er, c */
+#define R1_ERASE_PARAM          (1 << 27)   /* ex, c */
+#define R1_WP_VIOLATION         (1 << 26)   /* erx, c */
+#define R1_CARD_IS_LOCKED       (1 << 25)   /* sx, a */
+#define R1_LOCK_UNLOCK_FAILED   (1 << 24)   /* erx, c */
+#define R1_COM_CRC_ERROR        (1 << 23)   /* er, b */
+#define R1_ILLEGAL_COMMAND      (1 << 22)   /* er, b */
+#define R1_CARD_ECC_FAILED      (1 << 21)   /* ex, c */
+#define R1_CC_ERROR             (1 << 20)   /* erx, c */
+#define R1_ERROR                (1 << 19)   /* erx, c */
+#define R1_UNDERRUN             (1 << 18)   /* ex, c */
+#define R1_OVERRUN              (1 << 17)   /* ex, c */
+#define R1_CID_CSD_OVERWRITE    (1 << 16)   /* erx, c, CID/CSD overwrite */
+#define R1_WP_ERASE_SKIP        (1 << 15)   /* sx, c */
+#define R1_CARD_ECC_DISABLED    (1 << 14)   /* sx, a */
+#define R1_ERASE_RESET          (1 << 13)   /* sr, c */
+#define R1_STATUS(x)            (x & 0xFFFFE000)
+#define R1_CURRENT_STATE(x)     ((x & 0x00001E00) >> 9) /* sx, b (4 bits) */
+#define R1_READY_FOR_DATA       (1 << 8)    /* sx, a */
+#define R1_SWITCH_ERROR         (1 << 7)    /* ex, b */
+#define R1_URGENT_BKOPS         (1 << 6)    /* sr, a */
+#define R1_APP_CMD              (1 << 5)    /* sr, c */
+
+/*
+ * Card Command Classes (CCC)
+ */
+#define CCC_BASIC               (1<<0)  /* (0) Basic protocol functions */
+/* (CMD0,1,2,3,4,7,9,10,12,13,15) */
+#define CCC_STREAM_READ         (1<<1)  /* (1) Stream read commands */
+/* (CMD11) */
+#define CCC_BLOCK_READ          (1<<2)  /* (2) Block read commands */
+/* (CMD16,17,18) */
+#define CCC_STREAM_WRITE        (1<<3)  /* (3) Stream write commands */
+/* (CMD20) */
+#define CCC_BLOCK_WRITE         (1<<4)  /* (4) Block write commands */
+/* (CMD16,24,25,26,27) */
+#define CCC_ERASE               (1<<5)  /* (5) Ability to erase blocks */
+/* (CMD32,33,34,35,36,37,38,39) */
+#define CCC_WRITE_PROT          (1<<6)  /* (6) Able to write protect blocks */
+/* (CMD28,29,30) */
+#define CCC_LOCK_CARD           (1<<7)  /* (7) Able to lock down card */
+/* (CMD16,CMD42) */
+#define CCC_APP_SPEC            (1<<8)  /* (8) Application specific */
+/* (CMD55,56,57,ACMD*) */
+#define CCC_IO_MODE             (1<<9)  /* (9) I/O mode */
+/* (CMD5,39,40,52,53) */
+#define CCC_SWITCH              (1<<10) /* (10) High speed switch */
+/* (CMD6,34,35,36,37,50) */
+/* (11) Reserved */
+/* (CMD?) */
+
+/*
+ * CSD field definitions
+ */
+
+#define CSD_STRUCT_VER_1_0  0   /* Valid for system specification 1.0 - 1.2 */
+#define CSD_STRUCT_VER_1_1  1   /* Valid for system specification 1.4 - 2.2 */
+#define CSD_STRUCT_VER_1_2  2   /* Valid for system specification 3.1 - 3.2 - 3.31 - 4.0 - 4.1 */
+#define CSD_STRUCT_EXT_CSD  3   /* Version is coded in CSD_STRUCTURE in EXT_CSD */
+
+#define CSD_SPEC_VER_0      0   /* Implements system specification 1.0 - 1.2 */
+#define CSD_SPEC_VER_1      1   /* Implements system specification 1.4 */
+#define CSD_SPEC_VER_2      2   /* Implements system specification 2.0 - 2.2 */
+#define CSD_SPEC_VER_3      3   /* Implements system specification 3.1 - 3.2 - 3.31 */
+#define CSD_SPEC_VER_4      4   /* Implements system specification 4.0 - 4.1 */
+
+/*
+ * EXT_CSD fields
+ */
+
+#define EXT_CSD_BADBLK_MGMT             134 /* R/W */
+#define EXT_CSD_ENH_START_ADDR          136 /* R/W 4 bytes */
+#define EXT_CSD_ENH_SIZE_MULT           140 /* R/W 3 bytes */
+#define EXT_CSD_GP1_SIZE_MULT           143 /* R/W 3 bytes */
+#define EXT_CSD_GP2_SIZE_MULT           146 /* R/W 3 bytes */
+#define EXT_CSD_GP3_SIZE_MULT           149 /* R/W 3 bytes */
+#define EXT_CSD_GP4_SIZE_MULT           152 /* R/W 3 bytes */
+#define EXT_CSD_PART_SET_COMPL          155 /* R/W */
+#define EXT_CSD_PART_ATTR               156 /* R/W 3 bytes */
+#define EXT_CSD_MAX_ENH_SIZE_MULT       157 /* R/W 3 bytes */
+#define EXT_CSD_PART_SUPPORT            160 /* R */
+#define EXT_CSD_HPI_MGMT                161 /* R/W/E_P (4.41) */
+#define EXT_CSD_RST_N_FUNC              162 /* R/W */
+#define EXT_CSD_BKOPS_EN                163 /* R/W (4.41) */
+#define EXT_CSD_BKOPS_START             164 /* W/E_P (4.41) */
+#define EXT_CSD_WR_REL_PARAM            166 /* R (4.41) */
+#define EXT_CSD_WR_REL_SET              167 /* R/W (4.41) */
+#define EXT_CSD_RPMB_SIZE_MULT          168 /* R */
+#define EXT_CSD_FW_CONFIG               169 /* R/W */
+#define EXT_CSD_USR_WP                  171 /* R/W, R/W/C_P & R/W/E_P */
+#define EXT_CSD_BOOT_WP                 173 /* R/W, R/W/C_P */
+#define EXT_CSD_ERASE_GRP_DEF           175 /* R/W/E */
+#define EXT_CSD_BOOT_BUS_WIDTH          177 /* R/W/E */
+#define EXT_CSD_BOOT_CONFIG_PROT        178 /* R/W & R/W/C_P */
+#define EXT_CSD_PART_CFG                179 /* R/W/E & R/W/E_P */
+#define EXT_CSD_ERASED_MEM_CONT         181 /* R */
+#define EXT_CSD_BUS_WIDTH               183 /* R/W */
+#define EXT_CSD_HS_TIMING               185 /* R/W */
+#define EXT_CSD_PWR_CLASS               187 /* R/W/E_P */
+#define EXT_CSD_CMD_SET_REV             189 /* R */
+#define EXT_CSD_CMD_SET                 191 /* R/W/E_P */
+#define EXT_CSD_REV                     192 /* R */
+#define EXT_CSD_STRUCT                  194 /* R */
+#define EXT_CSD_CARD_TYPE               196 /* RO */
+#define EXT_CSD_OUT_OF_INTR_TIME        198 /* R (4.41) */
+#define EXT_CSD_PART_SWITCH_TIME        199 /* R (4.41) */
+#define EXT_CSD_PWR_CL_52_195           200 /* R */
+#define EXT_CSD_PWR_CL_26_195           201 /* R */
+#define EXT_CSD_PWR_CL_52_360           202 /* R */
+#define EXT_CSD_PWR_CL_26_360           203 /* R */
+#define EXT_CSD_MIN_PERF_R_4_26         205 /* R */
+#define EXT_CSD_MIN_PERF_W_4_26         206 /* R */
+#define EXT_CSD_MIN_PERF_R_8_26_4_25    207 /* R */
+#define EXT_CSD_MIN_PERF_W_8_26_4_25    208 /* R */
+#define EXT_CSD_MIN_PERF_R_8_52         209 /* R */
+#define EXT_CSD_MIN_PERF_W_8_52         210 /* R */
+#define EXT_CSD_SEC_CNT                 212 /* RO, 4 bytes */
+#define EXT_CSD_S_A_TIMEOUT             217 /* R */
+#define EXT_CSD_S_C_VCCQ                219 /* R */
+#define EXT_CSD_S_C_VCC                 220 /* R */
+#define EXT_CSD_HC_WP_GPR_SIZE          221 /* R */
+#define EXT_CSD_REL_WR_SEC_C            222 /* R */
+#define EXT_CSD_ERASE_TIMEOUT_MULT      223 /* R */
+#define EXT_CSD_HC_ERASE_GRP_SIZE       224 /* R */
+#define EXT_CSD_ACC_SIZE                225 /* R */
+#define EXT_CSD_BOOT_SIZE_MULT          226 /* R */
+#define EXT_CSD_BOOT_INFO               228 /* R */
+#define EXT_CSD_SEC_TRIM_MULT           229 /* R */
+#define EXT_CSD_SEC_ERASE_MULT          230 /* R */
+#define EXT_CSD_SEC_FEATURE_SUPPORT     231 /* R */
+#define EXT_CSD_TRIM_MULT               232 /* R */
+#define EXT_CSD_MIN_PERF_DDR_R_8_52     234 /* R */
+#define EXT_CSD_MIN_PERF_DDR_W_8_52     235 /* R */
+#define EXT_CSD_PWR_CL_DDR_52_195       238 /* R */
+#define EXT_CSD_PWR_CL_DDR_52_360       239 /* R */
+#define EXT_CSD_INI_TIMEOUT_AP          241 /* R */
+#define EXT_CSD_CORRECT_PRG_SECTS_NUM   242 /* R, 4 bytes (4.41) */
+#define EXT_CSD_BKOPS_STATUS            246 /* R (4.41) */
+#define EXT_CSD_BKOPS_SUPP              502 /* R (4.41) */
+#define EXT_CSD_HPI_FEATURE             503 /* R (4.41) */
+#define EXT_CSD_S_CMD_SET               504 /* R */
+
+/*
+ * EXT_CSD field definitions
+ */
+
+/* SEC_FEATURE_SUPPORT[231] */
+#define EXT_CSD_SEC_FEATURE_ER_EN       (1<<0)
+#define EXT_CSD_SEC_FEATURE_BD_BLK_EN   (1<<2)
+#define EXT_CSD_SEC_FEATURE_GB_CL_EN    (1<<4)
+
+/* BOOT_INFO[228] */
+#define EXT_CSD_BOOT_INFO_ALT_BOOT      (1<<0)
+#define EXT_CSD_BOOT_INFO_DDR_BOOT      (1<<1)
+#define EXT_CSD_BOOT_INFO_HS_BOOT       (1<<2)
+
+#define EXT_CSD_CMD_SET_NORMAL          (1<<0)
+#define EXT_CSD_CMD_SET_SECURE          (1<<1)
+#define EXT_CSD_CMD_SET_CPSECURE        (1<<2)
+
+#define EXT_CSD_CARD_TYPE_26            (1<<0)  /* Card can run at 26MHz */
+#define EXT_CSD_CARD_TYPE_52            (1<<1)  /* Card can run at 52MHz */
+#define EXT_CSD_CARD_TYPE_DDR_52        (1<<2)  /* Card can run at DDR 52MHz@1.8V or 3V */
+#define EXT_CSD_CARD_TYPE_DDR_52_1_2V   (1<<3)  /* Card can run at DDR 52MHz@1.2V */
+#define EXT_CSD_CARD_TYPE_HS200_1_8V    (1<<4)  /* Card can run at 200MHz@ 1.8V*/
+#define EXT_CSD_CARD_TYPE_HS200_1_2V    (1<<5)  /* Card can run at 200MHz@ 1.2V*/
+#define EXT_CSD_CARD_TYPE_HS400_1_8V    (1<<6)  /* Card can run at 200MHz@ 1.8V*/
+#define EXT_CSD_CARD_TYPE_HS400_1_2V    (1<<7)  /* Card can run at 200MHz@ 1.2V*/
+
+/* BUS_WIDTH[183] */
+#define EXT_CSD_BUS_WIDTH_1             (0) /* Card is in 1 bit mode */
+#define EXT_CSD_BUS_WIDTH_4             (1) /* Card is in 4 bit mode */
+#define EXT_CSD_BUS_WIDTH_8             (2) /* Card is in 8 bit mode */
+#define EXT_CSD_BUS_WIDTH_4_DDR         (5) /* Card is in 4 bit mode + DDR */
+#define EXT_CSD_BUS_WIDTH_8_DDR         (6) /* Card is in 8 bit mode + DDR */
+
+/* high speed timing */
+#define EXT_CSD_HS_TIMEING_BACKWARDS    (0) /* selecting backwards compatibility interface timing */
+#define EXT_CSD_HS_TIMEING_HS           (1) /* selecting high speed  interface timing */
+#define EXT_CSD_HS_TIMEING_HS200        (2) /* selecting hs200 interface timing */
+#define EXT_CSD_HS_TIMEING_HS400        (3) /* selecting hs400 interface timing */
+
+/* ERASED_MEM_CONT[181] */
+#define EXT_CSD_ERASED_MEM_CONT_0       (0)
+#define EXT_CSD_ERASED_MEM_CONT_1       (1)
+
+/* PARTITION CONFIG[179] */
+#define EXT_CSD_PART_CFG_DEFT_PART      (0)
+#define EXT_CSD_PART_CFG_BOOT_PART_1    (1)
+#define EXT_CSD_PART_CFG_BOOT_PART_2    (2)
+#define EXT_CSD_PART_CFG_RPMB_PART      (3)
+#define EXT_CSD_PART_CFG_GP_PART_1      (4)
+#define EXT_CSD_PART_CFG_GP_PART_2      (5)
+#define EXT_CSD_PART_CFG_GP_PART_3      (6)
+#define EXT_CSD_PART_CFG_GP_PART_4      (7)
+#define EXT_CSD_PART_CFG_EN_NO_BOOT     (0 << 3)
+#define EXT_CSD_PART_CFG_EN_BOOT_PART_1 (1 << 3)
+#define EXT_CSD_PART_CFG_EN_BOOT_PART_2 (2 << 3)
+#define EXT_CSD_PART_CFG_EN_USER_AREA   (7 << 3)
+#define EXT_CSD_PART_CFG_EN_NO_ACK      (0 << 6)
+#define EXT_CSD_PART_CFG_EN_ACK         (1 << 6)
+
+/* BOOT_CONFIG_PROT[178] */
+#define EXT_CSD_EN_PWR_BOOT_CFG_PROT    (1)
+#define EXT_CSD_EN_PERM_BOOT_CFG_PROT   (1<<4)  /* Carefully */
+
+/* BOOT_BUS_WIDTH[177] */
+#define EXT_CSD_BOOT_BUS_WIDTH_1        (0)
+#define EXT_CSD_BOOT_BUS_WIDTH_4        (1)
+#define EXT_CSD_BOOT_BUS_WIDTH_8        (2)
+#define EXT_CSD_BOOT_BUS_RESET          (1 << 2)
+
+#define EXT_CSD_BOOT_BUS_MODE_DEFT      (0 << 3)
+#define EXT_CSD_BOOT_BUS_MODE_HS        (1 << 3)
+#define EXT_CSD_BOOT_BUS_MODE_DDR       (2 << 3)
+
+/* ERASE_GROUP_DEF[175] */
+#define EXT_CSD_ERASE_GRP_DEF_EN        (1)
+
+/* BOOT_WP[173] */
+#define EXT_CSD_BOOT_WP_EN_PWR_WP       (1)
+#define EXT_CSD_BOOT_WP_EN_PERM_WP      (1 << 2)
+#define EXT_CSD_BOOT_WP_DIS_PERM_WP     (1 << 4)
+#define EXT_CSD_BOOT_WP_DIS_PWR_WP      (1 << 6)
+
+/* USER_WP[171] */
+#define EXT_CSD_USR_WP_EN_PWR_WP        (1)
+#define EXT_CSD_USR_WP_EN_PERM_WP       (1<<2)
+#define EXT_CSD_USR_WP_DIS_PWR_WP       (1<<3)
+#define EXT_CSD_USR_WP_DIS_PERM_WP      (1<<4)
+#define EXT_CSD_USR_WP_DIS_CD_PERM_WP   (1<<6)
+#define EXT_CSD_USR_WP_DIS_PERM_PWD     (1<<7)
+
+/* RST_n_FUNCTION[162] */
+#define EXT_CSD_RST_N_TEMP_DIS          (0)
+#define EXT_CSD_RST_N_PERM_EN           (1) /* carefully */
+#define EXT_CSD_RST_N_PERM_DIS          (2) /* carefully */
+
+/* PARTITIONING_SUPPORT[160] */
+#define EXT_CSD_PART_SUPPORT_PART_EN     (1)
+#define EXT_CSD_PART_SUPPORT_ENH_ATTR_EN (1<<1)
+
+/* PARTITIONS_ATTRIBUTE[156] */
+#define EXT_CSD_PART_ATTR_ENH_USR       (1<<0)
+#define EXT_CSD_PART_ATTR_ENH_1         (1<<1)
+#define EXT_CSD_PART_ATTR_ENH_2         (1<<2)
+#define EXT_CSD_PART_ATTR_ENH_3         (1<<3)
+#define EXT_CSD_PART_ATTR_ENH_4         (1<<4)
+
+/* PARTITION_SETTING_COMPLETED[156] */
+#define EXT_CSD_PART_SET_COMPL_BIT      (1<<0)
+
+/*
+ * MMC_SWITCH access modes
+ */
+
+#define MMC_SWITCH_MODE_CMD_SET     0x00    /* Change the command set */
+#define MMC_SWITCH_MODE_SET_BITS    0x01    /* Set bits which are 1 in value */
+#define MMC_SWITCH_MODE_CLEAR_BITS  0x02    /* Clear bits which are 1 in value */
+#define MMC_SWITCH_MODE_WRITE_BYTE  0x03    /* Set target to value */
+
+#define MMC_SWITCH_MODE_SDR12       0
+#define MMC_SWITCH_MODE_SDR25       1
+#define MMC_SWITCH_MODE_SDR50       2
+#define MMC_SWITCH_MODE_SDR104      3
+#define MMC_SWITCH_MODE_DDR50       4
+
+#define MMC_SWITCH_MODE_DRV_TYPE_B  0
+#define MMC_SWITCH_MODE_DRV_TYPE_A  1
+#define MMC_SWITCH_MODE_DRV_TYPE_C  2
+#define MMC_SWITCH_MODE_DRV_TYPE_D  3
+
+#define MMC_SWITCH_MODE_CL_200MA    0
+#define MMC_SWITCH_MODE_CL_400MA    1
+#define MMC_SWITCH_MODE_CL_600MA    2
+#define MMC_SWITCH_MODE_CL_800MA    3
+
+/*
+ * MMC_ERASE arguments
+ */
+#define MMC_ERASE_SECURE_REQ        (1 << 31)
+#define MMC_ERASE_GC_REQ            (1 << 15)
+#define MMC_ERASE_DISCARD           (3 << 0)
+#define MMC_ERASE_TRIM              (1 << 0)
+#define MMC_ERASE_NORMAL            (0)
+
+#define HOST_BUS_WIDTH_1            (1)
+#define HOST_BUS_WIDTH_4            (4)
+#define HOST_BUS_WIDTH_8            (8)
+
+#define EMMC_BOOT_PULL_CMD_MODE     (0)
+#define EMMC_BOOT_RST_CMD_MODE      (1)
+
+enum {
+    EMMC_BOOT_PWR_RESET = 0,
+    EMMC_BOOT_RST_N_SIG,
+    EMMC_BOOT_PRE_IDLE_CMD
+};
+
+enum {
+    RESP_NONE = 0,
+    RESP_R1,
+    RESP_R2,
+    RESP_R3,
+    RESP_R4,
+    RESP_R5,
+    RESP_R6,
+    RESP_R7,
+    RESP_R1B
+};
+
+struct mmc_csd {
+    unsigned char  mmca_vsn;
+    unsigned int   max_dtr;             /* max. data transfer rate */
+    unsigned int   read_blkbits;        /* max. read data block length */
+    unsigned int   capacity;            /* card capacity */
+};
+
+struct mmc_ext_csd {
+    unsigned int    sectors;
+    unsigned int    hs_max_dtr;
+    unsigned char   rev;
+    unsigned char   boot_info;
+    unsigned int    boot_part_sz;
+    unsigned char   ddr_support;
+    unsigned char   part_cfg;
+    unsigned char   sec_support;
+    unsigned char	reset_en;
+};
+
+#define MMC_CAP_4_BIT_DATA      (1 << 0) /* Can the host do 4 bit transfers */
+#define MMC_CAP_MULTIWRITE      (1 << 1) /* Can accurately report bytes sent to card on error */
+#define MMC_CAP_BYTEBLOCK       (1 << 2) /* Can do non-log2 block sizes */
+#define MMC_CAP_MMC_HIGHSPEED   (1 << 3) /* Can do MMC high-speed timing */
+#define MMC_CAP_SD_HIGHSPEED    (1 << 4) /* Can do SD high-speed timing */
+#define MMC_CAP_8_BIT_DATA      (1 << 5) /* Can the host do 8 bit transfers */
+#define MMC_CAP_SD_UHS1         (1 << 6) /* Can do SD ultra-high-speed timing */
+#define MMC_CAP_DDR             (1 << 7) /* The host support dual data rate */
+#define MMC_CAP_EMMC_HS200      (1 << 8) /* The host support dual data rate */
+#define MMC_CAP_EMMC_HS400      (1 << 9) /* The host support dual data rate */
+
+struct mmc_host {
+    struct mmc_card *card;
+    u32 max_phys_segs;
+    u32 base;         /* host base address */
+    u32 caps;         /* Host capabilities */
+    u32 f_min;        /* host min. frequency */
+    u32 f_max;        /* host max. frequency */
+    u32 clk;          /* host clock speed */
+    u32 sclk;         /* SD/MS clock speed */
+    u32 blklen;       /* block len */
+    u32 ocr;          /* current ocr */
+    u32 ocr_avail;    /* available ocr */
+    u32 timeout_ns;   /* data timeout ns */
+    u32 timeout_clks; /* data timeout clks */
+    u8  clksrc;       /* clock source */
+    u8  hclksrc;       /* clock source */
+    u8  curr_part;    /* host current working partition */
+    u32 intr_mask;    /* Interrupt mask */
+    u32  time_read;
+    void *priv;       /* private data */
+    mutex_t lock;    /* mutex lock for multi-thread */
+    int (*blk_read)(struct mmc_host *host, u8 *dst, u32 src, u32 nblks);
+    int (*blk_write)(struct mmc_host *host, u32 dst, u8 *src, u32 nblks);
+};
+
+#define MMC_TYPE_UNKNOWN    (0)          /* Unknown card */
+#define MMC_TYPE_MMC        (0x00000001) /* MMC card */
+#define MMC_TYPE_SD         (0x00000002) /* SD card */
+#define MMC_TYPE_SDIO       (0x00000004) /* SDIO card */
+
+/* MMC device */
+struct mmc_card {
+    struct mmc_host        *host;       /* the host this device belongs to */
+    unsigned int            nblks;
+    unsigned int            blklen;
+    unsigned int            ocr;
+    unsigned int            maxhz;
+    unsigned int            uhs_mode;
+    unsigned int            rca;        /* relative card address of device */
+    unsigned int            type;       /* card type */
+    unsigned int            sdio_funcs; /* number of SDIO functions */
+    unsigned short          state;      /* (our) card state */
+    unsigned short          ready;      /* card is ready or not */
+    struct mmc_csd          csd;        /* card specific */
+    struct mmc_ext_csd      ext_csd;    /* mmc v4 extended card specific */
+    u8                      version;    /* the SD card version, 1.0, 2.0, or 3.0*/
+};
+
+struct mmc_command {
+    u32 opcode;
+    u32 arg;
+    u32 rsptyp;
+    u32 resp[4];
+    u32 timeout;
+    u32 retries;    /* max number of retries */
+    u32 error;      /* command error */
+};
+
+struct mmc_data {
+    u8  *buf;
+    struct mmc_command *cmd;
+    u32  blks;
+    u32  timeout;   /* ms */
+};
+
+#define mmc_card_set_host(c,h)      ((c)->host = (h))
+#define mmc_card_set_unknown(c)     ((c)->type = MMC_TYPE_UNKNOWN)
+
+#define mmc_card_present(c)         ((c)->state & MMC_STATE_PRESENT)
+#define mmc_card_readonly(c)        ((c)->state & MMC_STATE_READONLY)
+#define mmc_card_backyard(c)        ((c)->state & MMC_STATE_BACKYARD)
+#define mmc_card_highspeed(c)       ((c)->state & MMC_STATE_HIGHSPEED)
+#define mmc_card_uhs1(c)            ((c)->state & MMC_STATE_UHS1)
+#define mmc_card_hs200(c)           ((c)->state & MMC_STATE_HS200)
+#define mmc_card_hs400(c)           ((c)->state & MMC_STATE_HS400)
+#define mmc_card_ddr(c)             ((c)->state & MMC_STATE_DDR)
+#define mmc_card_blockaddr(c)       ((c)->state & MMC_STATE_BLOCKADDR)
+#define mmc_card_highcaps(c)        ((c)->state & MMC_STATE_HIGHCAPS)
+
+#define mmc_card_set_present(c)     ((c)->state |= MMC_STATE_PRESENT)
+#define mmc_card_set_readonly(c)    ((c)->state |= MMC_STATE_READONLY)
+#define mmc_card_set_backyard(c)    ((c)->state |= MMC_STATE_BACKYARD)
+#define mmc_card_set_highspeed(c)   ((c)->state |= MMC_STATE_HIGHSPEED)
+#define mmc_card_set_uhs1(c)        ((c)->state |= MMC_STATE_UHS1)
+#define mmc_card_set_hs200(c)       ((c)->state |= MMC_STATE_HS200)
+#define mmc_card_set_hs400(c)       ((c)->state |= MMC_STATE_HS400)
+#define mmc_card_set_ddr(c)         ((c)->state |= MMC_STATE_DDR)
+#define mmc_card_set_blockaddr(c)   ((c)->state |= MMC_STATE_BLOCKADDR)
+
+#define mmc_card_clear_present(c)     ((c)->state &= ~MMC_STATE_PRESENT)
+#define mmc_card_clear_readonly(c)    ((c)->state &= ~MMC_STATE_READONLY)
+#define mmc_card_clear_highspeed(c)   ((c)->state &= ~MMC_STATE_HIGHSPEED)
+#define mmc_card_clear_uhs1(c)        ((c)->state &= ~MMC_STATE_UHS1)
+#define mmc_card_clear_hs200(c)       ((c)->state &= ~MMC_STATE_HS200)
+#define mmc_card_clear_hs400(c)       ((c)->state &= ~MMC_STATE_HS400)
+#define mmc_card_clear_ddr(c)         ((c)->state &= ~MMC_STATE_DDR)
+#define mmc_card_clear_blockaddr(c)   ((c)->state &= ~MMC_STATE_BLOCKADDR)
+
+#define mmc_card_clr_ddr(c)         ((c)->state &= ~MMC_STATE_DDR)
+#define mmc_card_clr_speed_mode(c)  ((c)->state &= ~(MMC_STATE_HS400 | MMC_STATE_HS200 | MMC_STATE_UHS1 | MMC_STATE_HIGHSPEED | MMC_STATE_BACKYARD))
+
+#define mmc_card_name(c)            ((c)->cid.prod_name)
+
+#define mmc_op_multi(op)    (((op) == MMC_CMD_READ_MULTIPLE_BLOCK) || \
+        ((op) == MMC_CMD_WRITE_MULTIPLE_BLOCK))
+
+int emmc_init(void);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mmc_rpmb.h b/src/bsp/lk/platform/mt2701/include/platform/mmc_rpmb.h
new file mode 100644
index 0000000..5ebaa54
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mmc_rpmb.h
@@ -0,0 +1,22 @@
+/*
+ * opyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
diff --git a/src/bsp/lk/platform/mt2701/include/platform/msdc.h b/src/bsp/lk/platform/mt2701/include/platform/msdc.h
new file mode 100644
index 0000000..2f668f1
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/msdc.h
@@ -0,0 +1,1402 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <reg.h>
+#include <platform/msdc_cfg.h>
+#include <platform/mt_reg_base.h>
+#include <platform/mmc_core.h>
+
+/*--------------------------------------------------------------------------*/
+/* Common Macro                                                             */
+/*--------------------------------------------------------------------------*/
+#define REG_ADDR(x)             ((volatile uint32_t *)(base + OFFSET_##x))
+
+/*--------------------------------------------------------------------------*/
+/* Common Definition                                                        */
+/*--------------------------------------------------------------------------*/
+#define MSDC_FIFO_SZ            (128)
+#define MSDC_FIFO_THD           (128)
+//#define MSDC_MAX_NUM            (2)
+
+#define MSDC_MS                 (0)
+#define MSDC_SDMMC              (1)
+
+#define MSDC_MODE_UNKNOWN       (0)
+#define MSDC_MODE_PIO           (1)
+#define MSDC_MODE_DMA_BASIC     (2)
+#define MSDC_MODE_DMA_DESC      (3)
+#define MSDC_MODE_DMA_ENHANCED  (4)
+#define MSDC_MODE_MMC_STREAM    (5)
+
+#define MSDC_BUS_1BITS          (0)
+#define MSDC_BUS_4BITS          (1)
+#define MSDC_BUS_8BITS          (2)
+
+#define MSDC_BURST_8B           (3)
+#define MSDC_BURST_16B          (4)
+#define MSDC_BURST_32B          (5)
+#define MSDC_BURST_64B          (6)
+
+#define MSDC_PIN_PULL_NONE      (0)
+#define MSDC_PIN_PULL_DOWN      (1)
+#define MSDC_PIN_PULL_UP        (2)
+#define MSDC_PIN_KEEP           (3)
+
+#ifdef FPGA_PLATFORM
+#define MSDC_OP_SCLK            (12000000)
+#define MSDC_MAX_SCLK           MSDC_OP_SCLK
+#else
+#define MSDC_OP_SCLK            (200000000)
+#define MSDC_MAX_SCLK           (400000000)
+#endif
+
+#define MSDC_MIN_SCLK           (260000)
+
+#define MSDC_350K_SCLK          (350000)
+#define MSDC_400K_SCLK          (400000)
+#define MSDC_25M_SCLK           (25000000)
+#define MSDC_26M_SCLK           (26000000)
+#define MSDC_50M_SCLK           (50000000)
+#define MSDC_52M_SCLK           (52000000)
+#define MSDC_100M_SCLK          (100000000)
+#define MSDC_179M_SCLK          (179000000)
+#define MSDC_200M_SCLK          (200000000)
+#define MSDC_208M_SCLK          (208000000)
+#define MSDC_400M_SCLK          (400000000)
+#define MSDC_800M_SCLK          (800000000)
+
+#define MSDC_AUTOCMD12          (0x0001)
+#define MSDC_AUTOCMD23          (0x0002)
+#define MSDC_AUTOCMD19          (0x0003)
+
+#define TYPE_CMD_RESP_EDGE      (0)
+#define TYPE_WRITE_CRC_EDGE     (1)
+#define TYPE_READ_DATA_EDGE     (2)
+#define TYPE_WRITE_DATA_EDGE    (3)
+
+#define START_AT_RISING                 (0x0)
+#define START_AT_FALLING                (0x1)
+#define START_AT_RISING_AND_FALLING     (0x2)
+#define START_AT_RISING_OR_FALLING      (0x3)
+
+#define MSDC_DMA_BURST_8B       (3)
+#define MSDC_DMA_BURST_16B      (4)
+#define MSDC_DMA_BURST_32B      (5)
+#define MSDC_DMA_BURST_64B      (6)
+
+/*--------------------------------------------------------------------------*/
+/* Register Offset                                                          */
+/*--------------------------------------------------------------------------*/
+#define OFFSET_MSDC_CFG                  (0x00)
+#define OFFSET_MSDC_IOCON                (0x04)
+#define OFFSET_MSDC_PS                   (0x08)
+#define OFFSET_MSDC_INT                  (0x0c)
+#define OFFSET_MSDC_INTEN                (0x10)
+#define OFFSET_MSDC_FIFOCS               (0x14)
+#define OFFSET_MSDC_TXDATA               (0x18)
+#define OFFSET_MSDC_RXDATA               (0x1c)
+#define OFFSET_SDC_CFG                   (0x30)
+#define OFFSET_SDC_CMD                   (0x34)
+#define OFFSET_SDC_ARG                   (0x38)
+#define OFFSET_SDC_STS                   (0x3c)
+#define OFFSET_SDC_RESP0                 (0x40)
+#define OFFSET_SDC_RESP1                 (0x44)
+#define OFFSET_SDC_RESP2                 (0x48)
+#define OFFSET_SDC_RESP3                 (0x4c)
+#define OFFSET_SDC_BLK_NUM               (0x50)
+#define OFFSET_SDC_VOL_CHG               (0x54)
+#define OFFSET_SDC_CSTS                  (0x58)
+#define OFFSET_SDC_CSTS_EN               (0x5c)
+#define OFFSET_SDC_DCRC_STS              (0x60)
+
+/* Only for EMMC Controller 4 registers below */
+#define OFFSET_EMMC_CFG0                 (0x70)
+#define OFFSET_EMMC_CFG1                 (0x74)
+#define OFFSET_EMMC_STS                  (0x78)
+#define OFFSET_EMMC_IOCON                (0x7c)
+
+#define OFFSET_SDC_ACMD_RESP             (0x80)
+#define OFFSET_SDC_ACMD19_TRG            (0x84)
+#define OFFSET_SDC_ACMD19_STS            (0x88)
+#define OFFSET_MSDC_DMA_HIGH4BIT         (0x8C)
+#define OFFSET_MSDC_DMA_SA               (0x90)
+#define OFFSET_MSDC_DMA_CA               (0x94)
+#define OFFSET_MSDC_DMA_CTRL             (0x98)
+#define OFFSET_MSDC_DMA_CFG              (0x9c)
+#define OFFSET_MSDC_DBG_SEL              (0xa0)
+#define OFFSET_MSDC_DBG_OUT              (0xa4)
+#define OFFSET_MSDC_DMA_LEN              (0xa8)
+#define OFFSET_MSDC_PATCH_BIT0           (0xb0)
+#define OFFSET_MSDC_PATCH_BIT1           (0xb4)
+#define OFFSET_MSDC_PATCH_BIT2           (0xb8)
+
+/* Only for SD/SDIO Controller 6 registers below */
+#define OFFSET_DAT0_TUNE_CRC             (0xc0)
+#define OFFSET_DAT1_TUNE_CRC             (0xc4)
+#define OFFSET_DAT2_TUNE_CRC             (0xc8)
+#define OFFSET_DAT3_TUNE_CRC             (0xcc)
+#define OFFSET_CMD_TUNE_CRC              (0xd0)
+#define OFFSET_SDIO_TUNE_WIND            (0xd4)
+
+#define OFFSET_MSDC_PAD_TUNE             (0xF0)
+#define OFFSET_MSDC_PAD_TUNE0            (0xF0)
+#define OFFSET_MSDC_PAD_TUNE1            (0xF4)
+#define OFFSET_MSDC_DAT_RDDLY0           (0xF8)
+#define OFFSET_MSDC_DAT_RDDLY1           (0xFC)
+
+#define OFFSET_MSDC_DAT_RDDLY2           (0x100)
+#define OFFSET_MSDC_DAT_RDDLY3           (0x104)
+
+#define OFFSET_MSDC_HW_DBG               (0x110)
+#define OFFSET_MSDC_VERSION              (0x114)
+#define OFFSET_MSDC_ECO_VER              (0x118)
+
+/* Only for EMMC 5.0 Controller 4 registers below */
+#define OFFSET_EMMC50_PAD_CTL0           (0x180)
+#define OFFSET_EMMC50_PAD_DS_CTL0        (0x184)
+#define OFFSET_EMMC50_PAD_DS_TUNE        (0x188)
+#define OFFSET_EMMC50_PAD_CMD_TUNE       (0x18c)
+#define OFFSET_EMMC50_PAD_DAT01_TUNE     (0x190)
+#define OFFSET_EMMC50_PAD_DAT23_TUNE     (0x194)
+#define OFFSET_EMMC50_PAD_DAT45_TUNE     (0x198)
+#define OFFSET_EMMC50_PAD_DAT67_TUNE     (0x19c)
+#define OFFSET_EMMC51_CFG0               (0x204)
+#define OFFSET_EMMC50_CFG0               (0x208)
+#define OFFSET_EMMC50_CFG1               (0x20c)
+#define OFFSET_EMMC50_CFG2               (0x21c)
+#define OFFSET_EMMC50_CFG3               (0x220)
+#define OFFSET_EMMC50_CFG4               (0x224)
+#define OFFSET_EMMC50_BLOCK_LENGTH       (0x228)
+/*--------------------------------------------------------------------------*/
+/* Register Address                                                         */
+/*--------------------------------------------------------------------------*/
+/* common register */
+#define MSDC_CFG                         REG_ADDR(MSDC_CFG)
+#define MSDC_IOCON                       REG_ADDR(MSDC_IOCON)
+#define MSDC_PS                          REG_ADDR(MSDC_PS)
+#define MSDC_INT                         REG_ADDR(MSDC_INT)
+#define MSDC_INTEN                       REG_ADDR(MSDC_INTEN)
+#define MSDC_FIFOCS                      REG_ADDR(MSDC_FIFOCS)
+#define MSDC_TXDATA                      REG_ADDR(MSDC_TXDATA)
+#define MSDC_RXDATA                      REG_ADDR(MSDC_RXDATA)
+
+/* sdmmc register */
+#define SDC_CFG                          REG_ADDR(SDC_CFG)
+#define SDC_CMD                          REG_ADDR(SDC_CMD)
+#define SDC_ARG                          REG_ADDR(SDC_ARG)
+#define SDC_STS                          REG_ADDR(SDC_STS)
+#define SDC_RESP0                        REG_ADDR(SDC_RESP0)
+#define SDC_RESP1                        REG_ADDR(SDC_RESP1)
+#define SDC_RESP2                        REG_ADDR(SDC_RESP2)
+#define SDC_RESP3                        REG_ADDR(SDC_RESP3)
+#define SDC_BLK_NUM                      REG_ADDR(SDC_BLK_NUM)
+#define SDC_VOL_CHG                      REG_ADDR(SDC_VOL_CHG)
+#define SDC_CSTS                         REG_ADDR(SDC_CSTS)
+#define SDC_CSTS_EN                      REG_ADDR(SDC_CSTS_EN)
+#define SDC_DCRC_STS                     REG_ADDR(SDC_DCRC_STS)
+
+/* emmc register*/
+#define EMMC_CFG0                        REG_ADDR(EMMC_CFG0)
+#define EMMC_CFG1                        REG_ADDR(EMMC_CFG1)
+#define EMMC_STS                         REG_ADDR(EMMC_STS)
+#define EMMC_IOCON                       REG_ADDR(EMMC_IOCON)
+
+/* auto command register */
+#define SDC_ACMD_RESP                    REG_ADDR(SDC_ACMD_RESP)
+#define SDC_ACMD19_TRG                   REG_ADDR(SDC_ACMD19_TRG)
+#define SDC_ACMD19_STS                   REG_ADDR(SDC_ACMD19_STS)
+
+/* dma register */
+#define MSDC_DMA_HIGH4BIT                REG_ADDR(MSDC_DMA_HIGH4BIT)
+#define MSDC_DMA_SA                      REG_ADDR(MSDC_DMA_SA)
+#define MSDC_DMA_CA                      REG_ADDR(MSDC_DMA_CA)
+#define MSDC_DMA_CTRL                    REG_ADDR(MSDC_DMA_CTRL)
+#define MSDC_DMA_CFG                     REG_ADDR(MSDC_DMA_CFG)
+
+/* debug register */
+#define MSDC_DBG_SEL                     REG_ADDR(MSDC_DBG_SEL)
+#define MSDC_DBG_OUT                     REG_ADDR(MSDC_DBG_OUT)
+#define MSDC_DMA_LEN                     REG_ADDR(MSDC_DMA_LEN)
+
+/* misc register */
+#define MSDC_PATCH_BIT0                  REG_ADDR(MSDC_PATCH_BIT0)
+#define MSDC_PATCH_BIT1                  REG_ADDR(MSDC_PATCH_BIT1)
+#define MSDC_PATCH_BIT2                  REG_ADDR(MSDC_PATCH_BIT2)
+#define DAT0_TUNE_CRC                    REG_ADDR(DAT0_TUNE_CRC)
+#define DAT1_TUNE_CRC                    REG_ADDR(DAT1_TUNE_CRC)
+#define DAT2_TUNE_CRC                    REG_ADDR(DAT2_TUNE_CRC)
+#define DAT3_TUNE_CRC                    REG_ADDR(DAT3_TUNE_CRC)
+#define CMD_TUNE_CRC                     REG_ADDR(CMD_TUNE_CRC)
+#define SDIO_TUNE_WIND                   REG_ADDR(SDIO_TUNE_WIND)
+#define MSDC_PAD_TUNE                    REG_ADDR(MSDC_PAD_TUNE)
+#define MSDC_PAD_TUNE0                   REG_ADDR(MSDC_PAD_TUNE0)
+#define MSDC_PAD_TUNE1                   REG_ADDR(MSDC_PAD_TUNE1)
+
+/* data read delay */
+#define MSDC_DAT_RDDLY0                  REG_ADDR(MSDC_DAT_RDDLY0)
+#define MSDC_DAT_RDDLY1                  REG_ADDR(MSDC_DAT_RDDLY1)
+#define MSDC_DAT_RDDLY2                  REG_ADDR(MSDC_DAT_RDDLY2)
+#define MSDC_DAT_RDDLY3                  REG_ADDR(MSDC_DAT_RDDLY3)
+
+#define MSDC_HW_DBG                      REG_ADDR(MSDC_HW_DBG)
+#define MSDC_VERSION                     REG_ADDR(MSDC_VERSION)
+#define MSDC_ECO_VER                     REG_ADDR(MSDC_ECO_VER)
+/* eMMC 5.0 register */
+#define EMMC50_PAD_CTL0                  REG_ADDR(EMMC50_PAD_CTL0)
+#define EMMC50_PAD_DS_CTL0               REG_ADDR(EMMC50_PAD_DS_CTL0)
+#define EMMC50_PAD_DS_TUNE               REG_ADDR(EMMC50_PAD_DS_TUNE)
+#define EMMC50_PAD_CMD_TUNE              REG_ADDR(EMMC50_PAD_CMD_TUNE)
+#define EMMC50_PAD_DAT01_TUNE            REG_ADDR(EMMC50_PAD_DAT01_TUNE)
+#define EMMC50_PAD_DAT23_TUNE            REG_ADDR(EMMC50_PAD_DAT23_TUNE)
+#define EMMC50_PAD_DAT45_TUNE            REG_ADDR(EMMC50_PAD_DAT45_TUNE)
+#define EMMC50_PAD_DAT67_TUNE            REG_ADDR(EMMC50_PAD_DAT67_TUNE)
+#define EMMC51_CFG0                      REG_ADDR(EMMC51_CFG0)
+#define EMMC50_CFG0                      REG_ADDR(EMMC50_CFG0)
+#define EMMC50_CFG1                      REG_ADDR(EMMC50_CFG1)
+#define EMMC50_CFG2                      REG_ADDR(EMMC50_CFG2)
+#define EMMC50_CFG3                      REG_ADDR(EMMC50_CFG3)
+#define EMMC50_CFG4                      REG_ADDR(EMMC50_CFG4)
+#define EMMC50_BLOCK_LENGTH              REG_ADDR(EMMC50_BLOCK_LENGTH)
+
+/*--------------------------------------------------------------------------*/
+/* Register Mask                                                            */
+/*--------------------------------------------------------------------------*/
+
+/* MSDC_CFG mask */
+#define MSDC_CFG_MODE           (0x1   <<  0)     /* RW */
+#define MSDC_CFG_CKPDN          (0x1   <<  1)     /* RW */
+#define MSDC_CFG_RST            (0x1   <<  2)     /* A0 */
+#define MSDC_CFG_PIO            (0x1   <<  3)     /* RW */
+#define MSDC_CFG_CKDRVEN        (0x1   <<  4)     /* RW */
+#define MSDC_CFG_BV18SDT        (0x1   <<  5)     /* RW */
+#define MSDC_CFG_BV18PSS        (0x1   <<  6)     /* R  */
+#define MSDC_CFG_CKSTB          (0x1   <<  7)     /* R  */
+#define MSDC_CFG_CKDIV          (0xFFF <<  8)     /* RW   !!! MT2701 change 0xFF ->0xFFF*/
+#define MSDC_CFG_CKMOD          (0x3   << 20)     /* W1C  !!! MT2701 change 16 ->21 only for eMCC 5.0*/
+#define MSDC_CFG_CKMOD_HS400    (0x1   << 22)     /* RW   !!! MT2701 change 18 ->22 only for eMCC 5.0*/
+#define MSDC_CFG_START_BIT      (0x3   << 23)     /* RW   !!! MT2701 change 19 ->23 only for eMCC 5.0*/
+#define MSDC_CFG_SCLK_STOP_DDR  (0x1   << 25)     /* RW   !!! MT2701 change 21 ->25 */
+
+/* MSDC_IOCON mask */
+#define MSDC_IOCON_SDR104CKS    (0x1   <<  0)     /* RW */
+#define MSDC_IOCON_RSPL         (0x1   <<  1)     /* RW */
+#define MSDC_IOCON_R_D_SMPL     (0x1   <<  2)     /* RW */
+#define MSDC_IOCON_DSPL          MSDC_IOCON_R_D_SMPL /* alias */
+
+#define MSDC_IOCON_DDLSEL       (0x1   <<  3)     /* RW */
+#define MSDC_IOCON_DDR50CKD     (0x1   <<  4)     /* RW */
+#define MSDC_IOCON_R_D_SMPL_SEL (0x1   <<  5)     /* RW */
+#define MSDC_IOCON_W_D_SMPL     (0x1   <<  8)     /* RW */
+#define MSDC_IOCON_W_D_SMPL_SEL (0x1   <<  9)     /* RW */
+#define MSDC_IOCON_W_D0SPL      (0x1   << 10)     /* RW */
+#define MSDC_IOCON_W_D1SPL      (0x1   << 11)     /* RW */
+#define MSDC_IOCON_W_D2SPL      (0x1   << 12)     /* RW */
+#define MSDC_IOCON_W_D3SPL      (0x1   << 13)     /* RW */
+
+#define MSDC_IOCON_R_D0SPL      (0x1   << 16)     /* RW */
+#define MSDC_IOCON_R_D1SPL      (0x1   << 17)     /* RW */
+#define MSDC_IOCON_R_D2SPL      (0x1   << 18)     /* RW */
+#define MSDC_IOCON_R_D3SPL      (0x1   << 19)     /* RW */
+#define MSDC_IOCON_R_D4SPL      (0x1   << 20)     /* RW */
+#define MSDC_IOCON_R_D5SPL      (0x1   << 21)     /* RW */
+#define MSDC_IOCON_R_D6SPL      (0x1   << 22)     /* RW */
+#define MSDC_IOCON_R_D7SPL      (0x1   << 23)     /* RW */
+//#define MSDC_IOCON_RISCSZ       (0x3   << 24)     /* RW  !!! MT2701  remove*/
+
+/* MSDC_PS mask */
+#define MSDC_PS_CDEN            (0x1   <<  0)     /* RW */
+#define MSDC_PS_CDSTS           (0x1   <<  1)     /* RU  */
+
+#define MSDC_PS_CDDEBOUNCE      (0xF   << 12)     /* RW */
+#define MSDC_PS_DAT             (0xFF  << 16)     /* RU */
+#define MSDC_PS_DAT8PIN         (0xFF  << 16)     /* RU */
+#define MSDC_PS_DAT4PIN         (0xF   << 16)     /* RU */
+#define MSDC_PS_DAT0            (0x1   << 16)     /* RU */
+
+#define MSDC_PS_CMD             (0x1   << 24)     /* RU */
+
+#define MSDC_PS_WP              (0x1   << 31)     /* RU  */
+
+/* MSDC_INT mask */
+#define MSDC_INT_MMCIRQ         (0x1   <<  0)     /* W1C */
+#define MSDC_INT_CDSC           (0x1   <<  1)     /* W1C */
+
+#define MSDC_INT_ACMDRDY        (0x1   <<  3)     /* W1C */
+#define MSDC_INT_ACMDTMO        (0x1   <<  4)     /* W1C */
+#define MSDC_INT_ACMDCRCERR     (0x1   <<  5)     /* W1C */
+#define MSDC_INT_DMAQ_EMPTY     (0x1   <<  6)     /* W1C */
+#define MSDC_INT_SDIOIRQ        (0x1   <<  7)     /* W1C Only for SD/SDIO */
+#define MSDC_INT_CMDRDY         (0x1   <<  8)     /* W1C */
+#define MSDC_INT_CMDTMO         (0x1   <<  9)     /* W1C */
+#define MSDC_INT_RSPCRCERR      (0x1   << 10)     /* W1C */
+#define MSDC_INT_CSTA           (0x1   << 11)     /* R */
+#define MSDC_INT_XFER_COMPL     (0x1   << 12)     /* W1C */
+#define MSDC_INT_DXFER_DONE     (0x1   << 13)     /* W1C */
+#define MSDC_INT_DATTMO         (0x1   << 14)     /* W1C */
+#define MSDC_INT_DATCRCERR      (0x1   << 15)     /* W1C */
+#define MSDC_INT_ACMD19_DONE    (0x1   << 16)     /* W1C */
+#define MSDC_INT_BDCSERR        (0x1   << 17)     /* W1C */
+#define MSDC_INT_GPDCSERR       (0x1   << 18)     /* W1C */
+#define MSDC_INT_DMAPRO         (0x1   << 19)     /* W1C */
+#define MSDC_INT_GOBOUND        (0x1   << 20)     /* W1C Only for SD/SDIO ACMD 53*/
+#define MSDC_INT_ACMD53_DONE    (0x1   << 21)     /* W1C Only for SD/SDIO ACMD 53*/
+#define MSDC_INT_ACMD53_FAIL    (0x1   << 22)     /* W1C Only for SD/SDIO ACMD 53*/
+#define MSDC_INT_AXI_RESP_ERR   (0x1   << 23)     /* W1C Only for eMMC 5.0*/
+
+/* MSDC_INTEN mask */
+#define MSDC_INTEN_MMCIRQ       (0x1   <<  0)     /* RW */
+#define MSDC_INTEN_CDSC         (0x1   <<  1)     /* RW */
+
+#define MSDC_INTEN_ACMDRDY      (0x1   <<  3)     /* RW */
+#define MSDC_INTEN_ACMDTMO      (0x1   <<  4)     /* RW */
+#define MSDC_INTEN_ACMDCRCERR   (0x1   <<  5)     /* RW */
+#define MSDC_INTEN_DMAQ_EMPTY   (0x1   <<  6)     /* RW */
+#define MSDC_INTEN_SDIOIRQ      (0x1   <<  7)     /* RW Only for SDIO*/
+#define MSDC_INTEN_CMDRDY       (0x1   <<  8)     /* RW */
+#define MSDC_INTEN_CMDTMO       (0x1   <<  9)     /* RW */
+#define MSDC_INTEN_RSPCRCERR    (0x1   << 10)     /* RW */
+#define MSDC_INTEN_CSTA         (0x1   << 11)     /* RW */
+#define MSDC_INTEN_XFER_COMPL   (0x1   << 12)     /* RW */
+#define MSDC_INTEN_DXFER_DONE   (0x1   << 13)     /* RW */
+#define MSDC_INTEN_DATTMO       (0x1   << 14)     /* RW */
+#define MSDC_INTEN_DATCRCERR    (0x1   << 15)     /* RW */
+#define MSDC_INTEN_ACMD19_DONE  (0x1   << 16)     /* RW */
+#define MSDC_INTEN_BDCSERR      (0x1   << 17)     /* RW */
+#define MSDC_INTEN_GPDCSERR     (0x1   << 18)     /* RW */
+#define MSDC_INTEN_DMAPRO       (0x1   << 19)     /* RW */
+#define MSDC_INTEN_GOBOUND      (0x1   << 20)     /* RW  Only for SD/SDIO ACMD 53*/
+#define MSDC_INTEN_ACMD53_DONE  (0x1   << 21)     /* RW  Only for SD/SDIO ACMD 53*/
+#define MSDC_INTEN_ACMD53_FAIL  (0x1   << 22)     /* RW  Only for SD/SDIO ACMD 53*/
+#define MSDC_INTEN_AXI_RESP_ERR (0x1   << 23)     /* RW  Only for eMMC 5.0*/
+
+#define MSDC_INTEN_DFT       (  MSDC_INTEN_MMCIRQ        |MSDC_INTEN_CDSC        | MSDC_INTEN_ACMDRDY\
+                                |MSDC_INTEN_ACMDTMO      |MSDC_INTEN_ACMDCRCERR  | MSDC_INTEN_DMAQ_EMPTY /*|MSDC_INTEN_SDIOIRQ*/\
+                                |MSDC_INTEN_CMDRDY       |MSDC_INTEN_CMDTMO      | MSDC_INTEN_RSPCRCERR   |MSDC_INTEN_CSTA\
+                                |MSDC_INTEN_XFER_COMPL   |MSDC_INTEN_DXFER_DONE  | MSDC_INTEN_DATTMO      |MSDC_INTEN_DATCRCERR\
+                                |MSDC_INTEN_BDCSERR      |MSDC_INTEN_ACMD19_DONE | MSDC_INTEN_GPDCSERR     /*|MSDC_INTEN_DMAPRO*/\
+                                /*|MSDC_INTEN_GOBOUND   |MSDC_INTEN_ACMD53_DONE |MSDC_INTEN_ACMD53_FAIL |MSDC_INTEN_AXI_RESP_ERR*/)
+
+
+/* MSDC_FIFOCS mask */
+#define MSDC_FIFOCS_RXCNT       (0xFF  <<  0)     /* R */
+#define MSDC_FIFOCS_TXCNT       (0xFF  << 16)     /* R */
+#define MSDC_FIFOCS_CLR         (0x1   << 31)     /* RW */
+
+/* SDC_CFG mask */
+#define SDC_CFG_SDIOINTWKUP     (0x1   <<  0)     /* RW */
+#define SDC_CFG_INSWKUP         (0x1   <<  1)     /* RW */
+#define SDC_CFG_BUSWIDTH        (0x3   << 16)     /* RW */
+#define SDC_CFG_SDIO            (0x1   << 19)     /* RW */
+#define SDC_CFG_SDIOIDE         (0x1   << 20)     /* RW */
+#define SDC_CFG_INTATGAP        (0x1   << 21)     /* RW */
+#define SDC_CFG_DTOC            (0xFF  << 24)     /* RW */
+
+/* SDC_CMD mask */
+#define SDC_CMD_OPC             (0x3F  <<  0)     /* RW */
+#define SDC_CMD_BRK             (0x1   <<  6)     /* RW */
+#define SDC_CMD_RSPTYP          (0x7   <<  7)     /* RW */
+#define SDC_CMD_DTYP            (0x3   << 11)     /* RW */
+#define SDC_CMD_RW              (0x1   << 13)     /* RW */
+#define SDC_CMD_STOP            (0x1   << 14)     /* RW */
+#define SDC_CMD_GOIRQ           (0x1   << 15)     /* RW */
+#define SDC_CMD_BLKLEN          (0xFFF << 16)     /* RW */
+#define SDC_CMD_AUTOCMD         (0x3   << 28)     /* RW */
+#define SDC_CMD_VOLSWTH         (0x1   << 30)     /* RW */
+#define SDC_CMD_ACMD53          (0x1   << 31)     /* RW Only for SD/SDIO ACMD 53*/
+
+/* SDC_STS mask */
+#define SDC_STS_SDCBUSY         (0x1   <<  0)     /* RW */
+#define SDC_STS_CMDBUSY         (0x1   <<  1)     /* RW */
+#define SDC_STS_CMD_WR_BUSY     (0x1   << 16)     /* RW !!! MT2701  Add*/
+#define SDC_STS_SWR_COMPL       (0x1   << 31)     /* RW */
+
+/* SDC_VOL_CHG mask */
+#define SDC_VOL_CHG_VCHGCNT     (0xFFFF<<  0)     /* RW  !!! MT2701  Add*/
+/* SDC_DCRC_STS mask */
+#define SDC_DCRC_STS_POS        (0xFF  <<  0)     /* RO */
+#define SDC_DCRC_STS_NEG        (0xFF  <<  8)     /* RO */
+
+/* EMMC_CFG0 mask */
+#define EMMC_CFG0_BOOTSTART     (0x1   <<  0)     /* WO Only for eMMC */
+#define EMMC_CFG0_BOOTSTOP      (0x1   <<  1)     /* WO Only for eMMC */
+#define EMMC_CFG0_BOOTMODE      (0x1   <<  2)     /* RW Only for eMMC */
+#define EMMC_CFG0_BOOTACKDIS    (0x1   <<  3)     /* RW Only for eMMC */
+
+#define EMMC_CFG0_BOOTWDLY      (0x7   << 12)     /* RW Only for eMMC */
+#define EMMC_CFG0_BOOTSUPP      (0x1   << 15)     /* RW Only for eMMC */
+
+/* EMMC_CFG1 mask */
+#define EMMC_CFG1_BOOTDATTMC   (0xFFFFF<<  0)     /* RW Only for eMMC */
+#define EMMC_CFG1_BOOTACKTMC    (0xFFF << 20)     /* RW Only for eMMC */
+
+/* EMMC_STS mask */
+#define EMMC_STS_BOOTCRCERR     (0x1   <<  0)     /* W1C Only for eMMC */
+#define EMMC_STS_BOOTACKERR     (0x1   <<  1)     /* W1C Only for eMMC */
+#define EMMC_STS_BOOTDATTMO     (0x1   <<  2)     /* W1C Only for eMMC */
+#define EMMC_STS_BOOTACKTMO     (0x1   <<  3)     /* W1C Only for eMMC */
+#define EMMC_STS_BOOTUPSTATE    (0x1   <<  4)     /* RU Only for eMMC */
+#define EMMC_STS_BOOTACKRCV     (0x1   <<  5)     /* W1C Only for eMMC */
+#define EMMC_STS_BOOTDATRCV     (0x1   <<  6)     /* RU Only for eMMC */
+
+/* EMMC_IOCON mask */
+#define EMMC_IOCON_BOOTRST      (0x1   <<  0)     /* RW Only for eMMC */
+
+/* SDC_ACMD19_TRG mask */
+#define SDC_ACMD19_TRG_TUNESEL  (0xF   <<  0)     /* RW */
+
+/* DMA_SA_HIGH4BIT mask */
+#define DMA_SA_HIGH4BIT_L4BITS  (0xF   <<  0)     /* RW  !!! MT2701  Add*/
+/* MSDC_DMA_CTRL mask */
+#define MSDC_DMA_CTRL_START     (0x1   <<  0)     /* WO */
+#define MSDC_DMA_CTRL_STOP      (0x1   <<  1)     /* AO */
+#define MSDC_DMA_CTRL_RESUME    (0x1   <<  2)     /* WO */
+#define MSDC_DMA_CTRL_READYM    (0x1   <<  3)     /* RO  !!! MT2701  Add*/
+
+#define MSDC_DMA_CTRL_MODE      (0x1   <<  8)     /* RW */
+#define MSDC_DMA_CTRL_ALIGN     (0x1   <<  9)     /* RW !!! MT2701  Add*/
+#define MSDC_DMA_CTRL_LASTBUF   (0x1   << 10)     /* RW */
+#define MSDC_DMA_CTRL_SPLIT1K   (0x1   << 11)     /* RW !!! MT2701  Add*/
+#define MSDC_DMA_CTRL_BURSTSZ   (0x7   << 12)     /* RW */
+// #define MSDC_DMA_CTRL_XFERSZ    (0xffffUL << 16)/* RW */
+
+/* MSDC_DMA_CFG mask */
+#define MSDC_DMA_CFG_STS              (0x1  <<  0)     /* R */
+#define MSDC_DMA_CFG_DECSEN           (0x1  <<  1)     /* RW */
+#define MSDC_DMA_CFG_LOCKDISABLE      (0x1  <<  2)     /* RW !!! MT2701  Add*/
+//#define MSDC_DMA_CFG_BDCSERR        (0x1  <<  4)     /* R */
+//#define MSDC_DMA_CFG_GPDCSERR       (0x1  <<  5)     /* R */
+#define MSDC_DMA_CFG_AHBEN            (0x3  <<  8)     /* RW */
+#define MSDC_DMA_CFG_ACTEN            (0x3  << 12)     /* RW */
+
+#define MSDC_DMA_CFG_CS12B            (0x1  << 16)     /* RW */
+#define MSDC_DMA_CFG_OUTB_STOP        (0x1  << 17)     /* RW */
+
+/* MSDC_PATCH_BIT0 mask */
+//#define MSDC_PB0_RESV1              (0x1  <<  0)
+#define MSDC_PB0_EN_8BITSUP           (0x1  <<  1)    /* RW */
+#define MSDC_PB0_DIS_RECMDWR          (0x1  <<  2)    /* RW */
+//#define MSDC_PB0_RESV2                        (0x1  <<  3)
+#define MSDC_PB0_RDDATSEL             (0x1  <<  3)    /* RW !!! MT2701 Add for SD/SDIO/eMMC 4.5*/
+#define MSDC_PB0_ACMD53_CRCINTR       (0x1  <<  4)    /* RW !!! MT2701 Add only for SD/SDIO */
+#define MSDC_PB0_ACMD53_ONESHOT       (0x1  <<  5)    /* RW !!! MT2701 Add ony for SD/SDIO */
+//#define MSDC_PB0_RESV3                        (0x1  <<  6)
+#define MSDC_PB0_DESC_UP_SEL          (0x1  <<  6)    /* RW !!! MT2701  Add*/
+#define MSDC_PB0_INT_DAT_LATCH_CK_SEL (0x7  <<  7)    /* RW */
+#define MSDC_INT_DAT_LATCH_CK_SEL      MSDC_PB0_INT_DAT_LATCH_CK_SEL  /* alias */
+
+#define MSDC_PB0_CKGEN_MSDC_DLY_SEL   (0x1F << 10)    /* RW */
+#define MSDC_CKGEN_MSDC_DLY_SEL        MSDC_PB0_CKGEN_MSDC_DLY_SEL  /* alias */
+
+
+#define MSDC_PB0_FIFORD_DIS           (0x1  << 15)    /* RW */
+//#define MSDC_PB0_SDIO_DBSSEL                  (0x1  << 16)    /* RW !!! MT2701  change*/
+#define MSDC_PB0_MSDC_BLKNUMSEL       (0x1  << 16)    /* RW !!! MT2701  change ACMD23*/
+#define MSDC_PB0_BLKNUM_SEL           MSDC_PB0_MSDC_BLKNUMSEL /* alias */
+
+#define MSDC_PB0_SDIO_INTCSEL         (0x1  << 17)    /* RW */
+#define MSDC_PB0_SDIO_BSYDLY          (0xF  << 18)    /* RW */
+#define MSDC_PB0_SDC_WDOD             (0xF  << 22)    /* RW */
+#define MSDC_PB0_CMDIDRTSEL           (0x1  << 26)    /* RW */
+#define MSDC_PB0_CMDFAILSEL           (0x1  << 27)    /* RW */
+#define MSDC_PB0_SDIO_INTDLYSEL       (0x1  << 28)    /* RW */
+#define MSDC_PB0_SPCPUSH              (0x1  << 29)    /* RW */
+#define MSDC_PB0_DETWR_CRCTMO         (0x1  << 30)    /* RW */
+#define MSDC_PB0_EN_DRVRSP            (0x1  << 31)    /* RW */
+
+/* MSDC_PATCH_BIT1 mask */
+#define MSDC_PB1_WRDAT_CRCS_TA_CNTR   (0x7  <<  0)    /* RW */
+#define MSDC_PATCH_BIT1_WRDAT_CRCS    MSDC_PB1_WRDAT_CRCS_TA_CNTR /* alias */
+
+
+#define MSDC_PB1_CMD_RSP_TA_CNTR      (0x7  <<  3)    /* RW */
+#define MSDC_PATCH_BIT1_CMD_RSP       MSDC_PB1_CMD_RSP_TA_CNTR  /* alias */
+
+//#define MSDC_PB1_RESV3                        (0x3  <<  6)
+#define MSDC_PB1_GET_BUSY_MARGIN      (0x1  <<  6)    /* RW !!! MT2701  Add */
+#define MSDC_PB1_GET_CRC_MARGIN       (0x1  <<  7)    /* RW !!! MT2701  Add */
+#define MSDC_PB1_BIAS_TUNE_28NM       (0xF  <<  8)    /* RW */
+#define MSDC_PB1_BIAS_EN18IO_28NM     (0x1  << 12)    /* RW */
+#define MSDC_PB1_BIAS_EXT_28NM        (0x1  << 13)    /* RW */
+
+//#define MSDC_PB1_RESV2                        (0x3  << 14)
+#define MSDC_PB1_RESET_GDMA           (0x1  << 15)    /* RW !!! MT2701  Add */
+//#define MSDC_PB1_RESV1                        (0x7F << 16)
+#define MSDC_PB1_EN_SINGLE_BURST      (0x1  << 16)    /* RW !!! MT2701  Add */
+#define MSDC_PB1_EN_FORCE_STOP_GDMA   (0x1  << 17)    /* RW !!! MT2701  Add  for eMMC 5.0 only*/
+#define MSDC_PB1_DCM_DIV_SEL2         (0x3  << 18)    /* RW !!! MT2701  Add  for eMMC 5.0 only*/
+#define MSDC_PB1_DCM_DIV_SEL1         (0x1  << 20)    /* RW !!! MT2701  Add */
+#define MSDC_PB1_DCM_EN               (0x1  << 21)    /* RW !!! MT2701  Add */
+#define MSDC_PB1_AXI_WRAP_CKEN        (0x1  << 22)    /* RW !!! MT2701  Add for eMMC 5.0 only*/
+#define MSDC_PB1_AHBCKEN              (0x1  << 23)    /* RW */
+#define MSDC_PB1_CKSPCEN              (0x1  << 24)    /* RW */
+#define MSDC_PB1_CKPSCEN              (0x1  << 25)    /* RW */
+#define MSDC_PB1_CKVOLDETEN           (0x1  << 26)    /* RW */
+#define MSDC_PB1_CKACMDEN             (0x1  << 27)    /* RW */
+#define MSDC_PB1_CKSDEN               (0x1  << 28)    /* RW */
+#define MSDC_PB1_CKWCTLEN             (0x1  << 29)    /* RW */
+#define MSDC_PB1_CKRCTLEN             (0x1  << 30)    /* RW */
+#define MSDC_PB1_CKSHBFFEN            (0x1  << 31)    /* RW */
+
+/* MSDC_PATCH_BIT2 mask */
+#define MSDC_PB2_ENHANCEGPD           (0x1  <<  0)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_SUPPORT64G           (0x1  <<  1)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_RESPWAIT             (0x3  <<  2)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGRDATCNT           (0x1F <<  4)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGRDAT              (0x1  <<  9)    /* RW !!! MT2701  Add */
+
+#define MSDC_PB2_INTCRESPSEL          (0x1  << 11)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGRESPCNT           (0x7  << 12)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGRESP              (0x1  << 15)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_RESPSTSENSEL         (0x7  << 16)    /* RW !!! MT2701  Add */
+
+#define MSDC_PB2_POPENCNT             (0xF  << 20)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGCRCSTSSEL         (0x1  << 24)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGCRCSTSEDGE        (0x1  << 25)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGCRCSTSCNT         (0x3  << 26)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CFGCRCSTS            (0x1  << 28)    /* RW !!! MT2701  Add */
+#define MSDC_PB2_CRCSTSENSEL          (0x7  << 29)    /* RW !!! MT2701  Add */
+
+
+/* SDIO_TUNE_WIND mask */
+#define SDIO_TUNE_WIND_TUNEWINDOW     (0x1F  <<  0)     /* RW !!! MT2701  Add for SD/SDIO only*/
+
+/* MSDC_PAD_TUNE/MSDC_PAD_TUNE0 mask */
+#define MSDC_PAD_TUNE_DATWRDLY        (0x1F  <<  0)     /* RW */
+
+#define MSDC_PAD_TUNE_DELAYEN         (0x1   <<  7)     /* RW !!! MT2701  Add*/
+#define MSDC_PAD_TUNE_DATRRDLY        (0x1F  <<  8)     /* RW */
+#define MSDC_PAD_TUNE_DATRRDLYSEL     (0x1   << 13)     /* RW !!! MT2701  Add*/
+
+#define MSDC_PAD_TUNE_RXDLYSEL        (0x1   << 15)     /* RW !!! MT2701  Add*/
+#define MSDC_PAD_TUNE_CMDRDLY         (0x1F  << 16)     /* RW */
+#define MSDC_PAD_TUNE_CMDRDLYSEL      (0x1   << 21)     /* RW !!! MT2701  Add*/
+#define MSDC_PAD_TUNE_CMDRRDLY        (0x1F  << 22)     /* RW */
+#define MSDC_PAD_TUNE_CLKTXDLY        (0x1F  << 27)     /* RW */
+
+/* MSDC_PAD_TUNE1 mask */
+
+#define MSDC_PAD_TUNE1_DATRRDLY2      (0x1F  <<  8)     /* RW  !!! MT2701  Add*/
+#define MSDC_PAD_TUNE1_DATRDLY2SEL    (0x1   << 13)     /* RW  !!! MT2701  Add*/
+
+#define MSDC_PAD_TUNE1_CMDRDLY2       (0x1F  << 16)     /* RW  !!! MT2701  Add*/
+#define MSDC_PAD_TUNE1_CMDRDLY2SEL    (0x1   << 21)     /* RW  !!! MT2701  Add*/
+
+
+/* MSDC_DAT_RDDLY0 mask */
+#define MSDC_DAT_RDDLY0_D3            (0x1F  <<  0)     /* RW */
+#define MSDC_DAT_RDDLY0_D2            (0x1F  <<  8)     /* RW */
+#define MSDC_DAT_RDDLY0_D1            (0x1F  << 16)     /* RW */
+#define MSDC_DAT_RDDLY0_D0            (0x1F  << 24)     /* RW */
+
+/* MSDC_DAT_RDDLY1 mask */
+#define MSDC_DAT_RDDLY1_D7            (0x1F  <<  0)     /* RW */
+
+#define MSDC_DAT_RDDLY1_D6            (0x1F  <<  8)     /* RW */
+
+#define MSDC_DAT_RDDLY1_D5            (0x1F  << 16)     /* RW */
+
+#define MSDC_DAT_RDDLY1_D4            (0x1F  << 24)     /* RW */
+
+/* MSDC_DAT_RDDLY2 mask */
+#define MSDC_DAT_RDDLY2_D3            (0x1F  <<  0)     /* RW !!! MT2701  Add*/
+
+#define MSDC_DAT_RDDLY2_D2            (0x1F  <<  8)     /* RW !!! MT2701  Add*/
+
+#define MSDC_DAT_RDDLY2_D1            (0x1F  << 16)     /* RW !!! MT2701  Add*/
+
+#define MSDC_DAT_RDDLY2_D0            (0x1F  << 24)     /* RW !!! MT2701  Add*/
+
+/* MSDC_DAT_RDDLY3 mask */
+#define MSDC_DAT_RDDLY3_D7            (0x1F  <<  0)     /* RW !!! MT2701  Add*/
+
+#define MSDC_DAT_RDDLY3_D6            (0x1F  <<  8)     /* RW !!! MT2701  Add*/
+
+#define MSDC_DAT_RDDLY3_D5            (0x1F  << 16)     /* RW !!! MT2701  Add*/
+
+#define MSDC_DAT_RDDLY3_D4            (0x1F  << 24)     /* RW !!! MT2701  Add*/
+
+/* MSDC_HW_DBG_SEL mask */
+#define MSDC_HW_DBG0_SEL              (0xFF  <<  0)     /* RW DBG3->DBG0 !!! MT2701  Change*/
+#define MSDC_HW_DBG1_SEL              (0x3F  <<  8)     /* RW DBG2->DBG1 !!! MT2701  Add*/
+
+#define MSDC_HW_DBG2_SEL              (0xFF  << 16)     /* RW DBG1->DBG2 !!! MT2701  Add*/
+//#define MSDC_HW_DBG_WRAPTYPE_SEL    (0x3   << 22)           /* RW !!! MT2701  Removed*/
+#define MSDC_HW_DBG3_SEL              (0x3F  << 24)     /* RW DBG0->DBG3 !!! MT2701  Add*/
+#define MSDC_HW_DBG_WRAP_SEL          (0x1   << 30)     /* RW */
+
+
+/* MSDC_EMMC50_PAD_CTL0 mask*/
+#define MSDC_EMMC50_PAD_CTL0_DCCSEL   (0x1  <<  0)     /* RW */
+#define MSDC_EMMC50_PAD_CTL0_HLSEL    (0x1  <<  1)     /* RW */
+#define MSDC_EMMC50_PAD_CTL0_DLP0     (0x3  <<  2)     /* RW */
+#define MSDC_EMMC50_PAD_CTL0_DLN0     (0x3  <<  4)     /* RW */
+#define MSDC_EMMC50_PAD_CTL0_DLP1     (0x3  <<  6)     /* RW */
+#define MSDC_EMMC50_PAD_CTL0_DLN1     (0x3  <<  8)     /* RW */
+
+/* MSDC_EMMC50_PAD_DS_CTL0 mask */
+#define MSDC_EMMC50_PAD_DS_CTL0_SR    (0x1  <<  0)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_R0    (0x1  <<  1)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_R1    (0x1  <<  2)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_PUPD  (0x1  <<  3)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_IES   (0x1  <<  4)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_SMT   (0x1  <<  5)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_RDSEL (0x3F <<  6)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_TDSEL (0xF  << 12)     /* RW */
+#define MSDC_EMMC50_PAD_DS_CTL0_DRV   (0x7  << 16)     /* RW */
+
+
+/* EMMC50_PAD_DS_TUNE mask */
+#define MSDC_EMMC50_PAD_DS_TUNE_DLYSEL  (0x1  <<  0)  /* RW */
+#define MSDC_EMMC50_PAD_DS_TUNE_DLY2SEL (0x1  <<  1)  /* RW */
+#define MSDC_EMMC50_PAD_DS_TUNE_DLY1    (0x1F <<  2)  /* RW */
+#define MSDC_EMMC50_PAD_DS_TUNE_DLY2    (0x1F <<  7)  /* RW */
+#define MSDC_EMMC50_PAD_DS_TUNE_DLY3    (0x1F << 12)  /* RW */
+
+/* EMMC50_PAD_CMD_TUNE mask */
+#define MSDC_EMMC50_PAD_CMD_TUNE_DLY3SEL (0x1  <<  0)  /* RW */
+#define MSDC_EMMC50_PAD_CMD_TUNE_RXDLY3  (0x1F <<  1)  /* RW */
+#define MSDC_EMMC50_PAD_CMD_TUNE_TXDLY   (0x1F <<  6)  /* RW */
+
+/* EMMC50_PAD_DAT01_TUNE mask */
+#define MSDC_EMMC50_PAD_DAT0_RXDLY3SEL   (0x1  <<  0)  /* RW */
+#define MSDC_EMMC50_PAD_DAT0_RXDLY3      (0x1F <<  1)  /* RW */
+#define MSDC_EMMC50_PAD_DAT0_TXDLY       (0x1F <<  6)  /* RW */
+#define MSDC_EMMC50_PAD_DAT1_RXDLY3SEL   (0x1  << 16)  /* RW */
+#define MSDC_EMMC50_PAD_DAT1_RXDLY3      (0x1F << 17)  /* RW */
+#define MSDC_EMMC50_PAD_DAT1_TXDLY       (0x1F << 22)  /* RW */
+
+/* EMMC50_PAD_DAT23_TUNE mask */
+#define MSDC_EMMC50_PAD_DAT2_RXDLY3SEL   (0x1  <<  0)  /* RW */
+#define MSDC_EMMC50_PAD_DAT2_RXDLY3      (0x1F <<  1)  /* RW */
+#define MSDC_EMMC50_PAD_DAT2_TXDLY       (0x1F <<  6)  /* RW */
+#define MSDC_EMMC50_PAD_DAT3_RXDLY3SEL   (0x1  << 16)  /* RW */
+#define MSDC_EMMC50_PAD_DAT3_RXDLY3      (0x1F << 17)  /* RW */
+#define MSDC_EMMC50_PAD_DAT3_TXDLY       (0x1F << 22)  /* RW */
+
+/* EMMC50_PAD_DAT45_TUNE mask */
+#define MSDC_EMMC50_PAD_DAT4_RXDLY3SEL   (0x1  <<  0)  /* RW */
+#define MSDC_EMMC50_PAD_DAT4_RXDLY3      (0x1F <<  1)  /* RW */
+#define MSDC_EMMC50_PAD_DAT4_TXDLY       (0x1F <<  6)  /* RW */
+#define MSDC_EMMC50_PAD_DAT5_RXDLY3SEL   (0x1  << 16)  /* RW */
+#define MSDC_EMMC50_PAD_DAT5_RXDLY3      (0x1F << 17)  /* RW */
+#define MSDC_EMMC50_PAD_DAT5_TXDLY       (0x1F << 22)  /* RW */
+
+/* EMMC50_PAD_DAT67_TUNE mask */
+#define MSDC_EMMC50_PAD_DAT6_RXDLY3SEL   (0x1  <<  0)  /* RW */
+#define MSDC_EMMC50_PAD_DAT6_RXDLY3      (0x1F <<  1)  /* RW */
+#define MSDC_EMMC50_PAD_DAT6_TXDLY       (0x1F <<  6)  /* RW */
+#define MSDC_EMMC50_PAD_DAT7_RXDLY3SEL   (0x1  << 16)  /* RW */
+#define MSDC_EMMC50_PAD_DAT7_RXDLY3      (0x1F << 17)  /* RW */
+#define MSDC_EMMC50_PAD_DAT7_TXDLY       (0x1F << 22)  /* RW */
+
+/* EMMC51_CFG0 mask */
+#define MSDC_EMMC51_CFG_CMDQ_EN          (0x1   <<  0) /* RW !!! MT2701  Add*/
+#define MSDC_EMMC51_CFG_WDAT_CNT         (0x3FF <<  1) /* RW !!! MT2701  Add*/
+#define MSDC_EMMC51_CFG_RDAT_CNT         (0x3FF << 11) /* RW !!! MT2701  Add*/
+#define MSDC_EMMC51_CFG_CMDQ_CMD_EN      (0x1   << 21) /* RW !!! MT2701  Add*/
+
+
+/* EMMC50_CFG0 mask */
+#define MSDC_EMMC50_CFG_PADCMD_LATCHCK         (0x1  <<  0)  /* RW*/
+#define MSDC_EMMC50_CFG_CRCSTS_CNT             (0x3  <<  1)  /* RW*/
+#define MSDC_EMMC50_CFG_CRCSTS_EDGE            (0x1  <<  3)  /* RW*/
+#define MSDC_EMMC50_CFG_CRC_STS_EDGE           MSDC_EMMC50_CFG_CRCSTS_EDGE /*alias */
+
+#define MSDC_EMMC50_CFG_CRCSTS_SEL             (0x1  <<  4)  /* RW*/
+#define MSDC_EMMC50_CFG_CRC_STS_SEL            MSDC_EMMC50_CFG_CRCSTS_SEL /*alias */
+
+#define MSDC_EMMC50_CFG_ENDBIT_CHKCNT          (0xF  <<  5)  /* RW*/
+#define MSDC_EMMC50_CFG_CMDRSP_SEL             (0x1  <<  9)  /* RW*/
+#define MSDC_EMMC50_CFG_CMD_RESP_SEL           MSDC_EMMC50_CFG_CMDRSP_SEL  /*alias */
+
+#define MSDC_EMMC50_CFG_CMDEDGE_SEL            (0x1  << 10)  /* RW*/
+#define MSDC_EMMC50_CFG_ENDBIT_CNT             (0x3FF<< 11)  /* RW*/
+#define MSDC_EMMC50_CFG_READDAT_CNT            (0x7  << 21)  /* RW*/
+#define MSDC_EMMC50_CFG_EMMC50_MONSEL          (0x1  << 24)  /* RW*/
+#define MSDC_EMMC50_CFG_MSDC_WRVALID           (0x1  << 25)  /* RW*/
+#define MSDC_EMMC50_CFG_MSDC_RDVALID           (0x1  << 26)  /* RW*/
+#define MSDC_EMMC50_CFG_MSDC_WRVALID_SEL       (0x1  << 27)  /* RW*/
+#define MSDC_EMMC50_CFG_MSDC_RDVALID_SEL       (0x1  << 28)  /* RW*/
+#define MSDC_EMMC50_CFG_MSDC_TXSKEW_SEL        (0x1  << 29)  /* RW*/
+//#define MSDC_EMMC50_CFG_MSDC_GDMA_RESET      (0x1  << 31)  /* RW !!! MT2701  Removed*/
+
+/* EMMC50_CFG1 mask */
+#define MSDC_EMMC50_CFG1_WRPTR_MARGIN          (0xFF <<  0)  /* RW*/
+#define MSDC_EMMC50_CFG1_CKSWITCH_CNT          (0x7  <<  8)  /* RW*/
+#define MSDC_EMMC50_CFG1_RDDAT_STOP            (0x1  << 11)  /* RW*/
+#define MSDC_EMMC50_CFG1_WAIT8CLK_CNT          (0xF  << 12)  /* RW*/
+#define MSDC_EMMC50_CFG1_EMMC50_DBG_SEL        (0xFF << 16)  /* RW*/
+#define MSDC_EMMC50_CFG1_PSH_CNT               (0x7  << 24)  /* RW !!! MT2701  Add*/
+#define MSDC_EMMC50_CFG1_PSH_PS_SEL            (0x1  << 27)  /* RW !!! MT2701  Add*/
+#define MSDC_EMMC50_CFG1_DS_CFG                (0x1  << 28)  /* RW !!! MT2701  Add*/
+
+/* EMMC50_CFG2 mask */
+//#define MSDC_EMMC50_CFG2_AXI_GPD_UP          (0x1  <<  0)  /* RW !!! MT2701  Removed*/
+#define MSDC_EMMC50_CFG2_AXI_IOMMU_WR_EMI      (0x1  <<  1) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_SHARE_EN_WR_EMI   (0x1  <<  2) /* RW*/
+
+#define MSDC_EMMC50_CFG2_AXI_IOMMU_RD_EMI      (0x1  <<  7) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_SHARE_EN_RD_EMI   (0x1  <<  8) /* RW*/
+
+#define MSDC_EMMC50_CFG2_AXI_BOUND_128B        (0x1  << 13) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_BOUND_256B        (0x1  << 14) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_BOUND_512B        (0x1  << 15) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_BOUND_1K          (0x1  << 16) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_BOUND_2K          (0x1  << 17) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_BOUND_4K          (0x1  << 18) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_RD_OUTSTANDING_NUM (0x1F << 19) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_RD_OUTS_NUM       MSDC_EMMC50_CFG2_AXI_RD_OUTSTANDING_NUM /*alias */
+
+#define MSDC_EMMC50_CFG2_AXI_SET_LET           (0xF  << 24) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_SET_LEN           MSDC_EMMC50_CFG2_AXI_SET_LET /*alias */
+
+#define MSDC_EMMC50_CFG2_AXI_RESP_ERR_TYPE     (0x3  << 28) /* RW*/
+#define MSDC_EMMC50_CFG2_AXI_BUSY              (0x1  << 30) /* RW*/
+
+
+/* EMMC50_CFG3 mask */
+#define MSDC_EMMC50_CFG3_OUTSTANDING_WR        (0x1F <<  0) /* RW*/
+#define MSDC_EMMC50_CFG3_OUTS_WR               MSDC_EMMC50_CFG3_OUTSTANDING_WR /*alias */
+
+#define MSDC_EMMC50_CFG3_ULTRA_SET_WR          (0x3F <<  5) /* RW*/
+#define MSDC_EMMC50_CFG3_PREULTRA_SET_WR       (0x3F << 11) /* RW*/
+#define MSDC_EMMC50_CFG3_ULTRA_SET_RD          (0x3F << 17) /* RW*/
+#define MSDC_EMMC50_CFG3_PREULTRA_SET_RD       (0x3F << 23) /* RW*/
+
+/* EMMC50_CFG4 mask */
+#define MSDC_EMMC50_CFG4_IMPR_ULTRA_SET_WR     (0xFF <<  0) /* RW*/
+#define MSDC_EMMC50_CFG4_IMPR_ULTRA_SET_RD     (0xFF <<  8) /* RW*/
+#define MSDC_EMMC50_CFG4_ULTRA_EN              (0x3  << 16) /* RW*/
+#define MSDC_EMMC50_CFG4_WRAP_SEL              (0x1F << 18) /* RW !!! MT2701  Add*/
+
+/* EMMC50_BLOCK_LENGTH mask */
+#define EMMC50_BLOCK_LENGTH_BLKLENGTH          (0xFF <<  0) /* RW !!! MT2701  Add*/
+
+/*
+ *  This definition is used for MT2701_GPIO_RegMap.doc by candy.li on 2014-9-03
+ *  Zhanyong.Wang
+ *  2014/11/28
+ */
+#define GPIO_PIN_MAX                          280
+#ifndef GPIO_BASE
+#define GPIO_BASE                             0x10005000
+#endif
+#if GPIO_BASE != 0x10005000
+#error  Please check your GPIO_BASE definition!!!!
+#endif
+
+#define REG_GPIO_BASE(x)                     ((volatile unsigned int *)(GPIO_BASE + OFFSET_##x))
+
+#define GPIO_DIR_ASS                         175
+#define GPIO_DIR_PIN2REG(n)                  ((volatile unsigned int *)(GPIO_BASE + (((n) <= GPIO_DIR_ASS) ? 0 : 0x10) +  (((n) % GPIO_PIN_MAX) & 0x1F0))) /* 0: input, 1: output */
+
+#define GPIO_DATOUT_PIN2REG(n)               ((volatile unsigned int *)(GPIO_BASE +  0x500 +  (((n) % GPIO_PIN_MAX) & 0x1F0)))
+#define GPIO_DATIN_PIN2REG(n)                ((volatile unsigned int *)(GPIO_BASE +  0x630 +  (((n) % GPIO_PIN_MAX) & 0x1F0)))
+
+#define GPIO_PULLEN_PIN2REG(n)               ((volatile unsigned int *)(GPIO_BASE +  0x150 +  (((n) % GPIO_PIN_MAX) & 0x1F0))) /* 0: disable, 1: enable */
+#define GPIO_PULLSEL_PIN2REG(n)              ((volatile unsigned int *)(GPIO_BASE +  0x280 +  (((n) % GPIO_PIN_MAX) & 0x1F0))) /* 0: pull down, 1: pull up */
+
+#define BIT_GPIOMASK(n)                      (1 << (((n) % GPIO_PIN_MAX) & 0x0F))
+
+
+#define GPIO_MODE_PIN2REG(n)                 ((volatile unsigned int *)(GPIO_BASE +  0x760 +  ((((n) % GPIO_PIN_MAX)/5) << 4)))
+
+#define BIT_MODE_GPIOMASK(n)                 (0x07 << (3*(((n) % GPIO_PIN_MAX)%5)))
+#define BV_GPIO_MSDC_MODE                    1
+
+#define BIT_MSDC_CLK_R0R1PUPD                (0x07 << 8)
+#define BIT_MSDC_CMD_R0R1PUPD                (0x07 << 8)
+#define BIT_MSDC_DAT0_R0R1PUPD               (0x07 << 0)
+#define BIT_MSDC_DAT1_R0R1PUPD               (0x07 << 4)
+#define BIT_MSDC_DAT2_R0R1PUPD               (0x07 << 8)
+#define BIT_MSDC_DAT3_R0R1PUPD               (0x07 <<12)
+#define BIT_MSDC_DAT4_R0R1PUPD               (0x07 << 0)
+#define BIT_MSDC_DAT5_R0R1PUPD               (0x07 << 4)
+#define BIT_MSDC_DAT6_R0R1PUPD               (0x07 << 8)
+#define BIT_MSDC_DAT7_R0R1PUPD               (0x07 <<12)
+
+#define BIT_MSDC_RSTB_R0R1PUPD               (0x07 << 0)
+
+#define BIT_MSDC_DSL_R0R1PUPD                (0x07 << 4)
+#define BIT_MSDC_RCLK_R0R1PUPD               (0x07 << 8)
+
+#define BV_MSDCRX_PUR0R1_HIGHZ                0
+#define BV_MSDCRX_PUR0R1_50KOHM               2
+#define BV_MSDCRX_PUR0R1_10KOHM               4
+#define BV_MSDCRX_PUR0R1_08KOHM               6
+#define BV_MSDCRX_PDR0R1_HIGHZ                1
+#define BV_MSDCRX_PDR0R1_50KOHM               3
+#define BV_MSDCRX_PDR0R1_10KOHM               5
+#define BV_MSDCRX_PDR0R1_08KOHM               7
+
+#define BIT_MSDC_CLK_E8E4E2                  (0x07 << 0)
+#define BIT_MSDC_CMD_E8E4E2                  (0x07 << 0)
+#define BIT_MSDC_DAT_E8E4E2                  (0x07 << 0)
+#define BIT_MSDC_RCLK_E8E4E2                 (0x07 << 0)
+#define BIT_GPIO_MSDC_18VE8E4E2              (0x07 << 0)
+#define BV_MSDCTX_18VE8E4E2_01RD78U85MA       0
+#define BV_MSDCTX_18VE8E4E2_03RD78U85MA       1
+#define BV_MSDCTX_18VE8E4E2_04RD78U85MA       2
+#define BV_MSDCTX_18VE8E4E2_06RD78U85MA       3
+#define BV_MSDCTX_18VE8E4E2_07RD78U85MA       4
+#define BV_MSDCTX_18VE8E4E2_10RD78U85MA       5
+#define BV_MSDCTX_18VE8E4E2_11RD78U85MA       6
+#define BV_MSDCTX_18VE8E4E2_13RD78U85MA       7
+
+#define BIT_GPIO_MSDC_33VE8E4E2              (0x07 << 0)
+#define BV_MSDCTX_33VE8E4E2_01RD14U17MA       0
+#define BV_MSDCTX_33VE8E4E2_03RD14U17MA       1
+#define BV_MSDCTX_33VE8E4E2_04RD14U17MA       2
+#define BV_MSDCTX_33VE8E4E2_06RD14U17MA       3
+
+/*
+   MSDC Driving Strength Definition: specify as gear instead of driving
+*/
+#define MSDC_DRVN_GEAR0                       BV_MSDCTX_18VE8E4E2_01RD78U85MA
+#define MSDC_DRVN_GEAR1                       BV_MSDCTX_18VE8E4E2_03RD78U85MA
+#define MSDC_DRVN_GEAR2                       BV_MSDCTX_18VE8E4E2_04RD78U85MA
+#define MSDC_DRVN_GEAR3                       BV_MSDCTX_18VE8E4E2_06RD78U85MA
+#define MSDC_DRVN_GEAR4                       BV_MSDCTX_18VE8E4E2_07RD78U85MA
+#define MSDC_DRVN_GEAR5                       BV_MSDCTX_18VE8E4E2_10RD78U85MA
+#define MSDC_DRVN_GEAR6                       BV_MSDCTX_18VE8E4E2_11RD78U85MA
+#define MSDC_DRVN_GEAR7                       BV_MSDCTX_18VE8E4E2_13RD78U85MA
+#define MSDC_DRVN_DONT_CARE                   MSDC_DRVN_GEAR0
+
+/* id valid range "MSDC[0, 3]" index"[0, 5/6]" */
+#define MSDC_CTRL(id, index)                  GPIO_##id##_CTRL##index
+
+#define PIN4_CLK(id)                          PIN4##id##_CLK
+#define PIN4_CMD(id)                          PIN4##id##_CMD
+#define PIN4_RSTB(id)                         PIN4##id##_RSTB
+#define PIN4_DAT(id,line)                     PIN4##id##_##line
+#define PIN4_DSL(id)                          PIN4##id##_DSL
+#define PIN4_INS(id)                          PIN4##id##_INS
+
+/* id valid range "MSDC[0, 4]"*/
+#define MSDC_CLK_SMT(id)                      B_##id##CLK_SMT
+#define MSDC_CLK_R0(id)                       B_##id##CLK_R0
+#define MSDC_CLK_R1(id)                       B_##id##CLK_R1
+#define MSDC_CLK_PUPD(id)                     B_##id##CLK_PUPD
+#define MSDC_CLK_IES(id)                      B_##id##CLK_IES
+#define MSDC_CLK_SR(id)                       B_##id##CLK_SR
+#define MSDC_CLK_E8(id)                       B_##id##CLK_E8
+#define MSDC_CLK_E4(id)                       B_##id##CLK_E4
+#define MSDC_CLK_E2(id)                       B_##id##CLK_E2
+
+#define MSDC_CMD_SMT(id)                      B_##id##CMD_SMT
+#define MSDC_CMD_R0(id)                       B_##id##CMD_R0
+#define MSDC_CMD_R1(id)                       B_##id##CMD_R1
+#define MSDC_CMD_PUPD(id)                     B_##id##CMD_PUPD
+#define MSDC_CMD_IES(id)                      B_##id##CMD_IES
+#define MSDC_CMD_SR(id)                       B_##id##CMD_SR
+#define MSDC_CMD_E8(id)                       B_##id##CMD_E8
+#define MSDC_CMD_E4(id)                       B_##id##CMD_E4
+#define MSDC_CMD_E2(id)                       B_##id##CMD_E2
+
+#define MSDC_DAT_IES(id)                      B_##id##DAT_IES
+#define MSDC_DAT_SR(id)                       B_##id##DAT_SR
+#define MSDC_DAT_E8(id)                       B_##id##DAT_E8
+#define MSDC_DAT_E4(id)                       B_##id##DAT_E4
+#define MSDC_DAT_E2(id)                       B_##id##DAT_E2
+
+/* use line valid range "DAT[0, 7]" */
+#define MSDC_DAT_SMT(id,line)                 B_##id##line##_SMT
+#define MSDC_DAT_R0(id,line)                  B_##id##line##_R0
+#define MSDC_DAT_R1(id,line)                  B_##id##line##_R1
+#define MSDC_DAT_PUPD(id,line)                B_##id##line##_PUPD
+
+#define MSDC_DSL_SMT(id)                      B_##id##DSL_SMT
+#define MSDC_DSL_R0(id)                       B_##id##DSL_R0
+#define MSDC_DSL_R1(id)                       B_##id##DSL_R1
+#define MSDC_DSL_PUPD(id)                     B_##id##DSL_PUPD
+
+#define MSDC_RSTB_SMT(id)                     B_##id##RSTB_SMT
+#define MSDC_RSTB_R0(id)                      B_##id##RSTB_R0
+#define MSDC_RSTB_R1(id)                      B_##id##RSTB_R1
+#define MSDC_RSTB_PUPD(id)                    B_##id##RSTB_PUPD
+
+#define MSDC_PAD_RDSEL(id)                    B_##id##PAD_RDSEL
+#define MSDC_PAD_TDSEL(id)                    B_##id##PAD_TDSEL
+
+#define MSDC_INS_SMT(id)                      B_##id##INS_SMT
+/* *RCLK* Obsolete control pin by hui.zeng confirmed, fengguo.gao and zhanyong.wang present 2014/12/18*/
+#define MSDC_RCLK_SMT(id)                     B_##id##RCLK_SMT
+#define MSDC_RCLK_R0(id)                      B_##id##RCLK_R0
+#define MSDC_RCLK_R1(id)                      B_##id##RCLK_R1
+#define MSDC_RCLK_PUPD(id)                    B_##id##RCLK_PUPD
+
+#define MSDC_RCLK_IES(id)                     B_##id##RCLK_IES
+#define MSDC_RCLK_SR(id)                      B_##id##RCLK_SR
+#define MSDC_RCLK_E8(id)                      B_##id##RCLK_E8
+#define MSDC_RCLK_E4(id)                      B_##id##RCLK_E4
+#define MSDC_RCLK_E2(id)                      B_##id##RCLK_E2
+
+#define MSDC_RCLK_RDSEL(id)                   B_##id##RCLK_RDSEL
+#define MSDC_RCLK_TDSEL(id)                   B_##id##RCLK_TDSEL
+
+
+/* ******************************Don't modify these source code below ******************************/
+/*
+ * MSDC0  GPIO Control definition
+ *  DAT7   DAT6 DAT5 DAT4 RSTB CMD CLK DAT3 DAT2 DAT1 DAT0
+ * GPIO111 112  113  114  115  116 117 118  119  120  121
+ *   1      1    1    1    0    1   0   1    1    1    1
+ ***************************************************************************/
+#define PIN4MSDC0_CLK                           117
+#define PIN4MSDC0_CMD                           116
+#define PIN4MSDC0_RSTB                          115
+#define PIN4MSDC0_DAT0                          121
+#define PIN4MSDC0_DAT1                          120
+#define PIN4MSDC0_DAT2                          119
+#define PIN4MSDC0_DAT3                          118
+#define PIN4MSDC0_DAT4                          114
+#define PIN4MSDC0_DAT5                          113
+#define PIN4MSDC0_DAT6                          112
+#define PIN4MSDC0_DAT7                          111
+
+#define OFFSET_MSDC0_CTRL0                      0xCC0
+#define OFFSET_MSDC0_CTRL1                      0xCD0
+#define OFFSET_MSDC0_CTRL2                      0xCE0
+#define OFFSET_MSDC0_CTRL3                      0xCF0
+#define OFFSET_MSDC0_CTRL4                      0xD00
+#define OFFSET_MSDC0_CTRL5                      0xD10
+#define OFFSET_MSDC0_CTRL6                      0xD20
+
+#ifdef MSDC0_CTRL0
+#undef MSDC0_CTRL0
+#endif
+#ifdef MSDC0_CTRL1
+#undef MSDC0_CTRL1
+#endif
+#ifdef MSDC0_CTRL2
+#undef MSDC0_CTRL2
+#endif
+#ifdef MSDC0_CTRL3
+#undef MSDC0_CTRL3
+#endif
+#ifdef MSDC0_CTRL4
+#undef MSDC0_CTRL4
+#endif
+#ifdef MSDC0_CTRL5
+#undef MSDC0_CTRL5
+#endif
+#ifdef MSDC0_CTRL6
+#undef MSDC0_CTRL6
+#endif
+
+#define GPIO_MSDC0_CTRL0                        REG_GPIO_BASE(MSDC0_CTRL0)
+#define GPIO_MSDC0_CTRL1                        REG_GPIO_BASE(MSDC0_CTRL1)
+#define GPIO_MSDC0_CTRL2                        REG_GPIO_BASE(MSDC0_CTRL2)
+#define GPIO_MSDC0_CTRL3                        REG_GPIO_BASE(MSDC0_CTRL3)
+#define GPIO_MSDC0_CTRL4                        REG_GPIO_BASE(MSDC0_CTRL4)
+#define GPIO_MSDC0_CTRL5                        REG_GPIO_BASE(MSDC0_CTRL5)
+#define GPIO_MSDC0_CTRL6                        REG_GPIO_BASE(MSDC0_CTRL6)
+
+/* 0x10005CC0 MSDC0_CTRL0   MSDC 0 CLK pad Control Register 0x0311 */
+#define B_MSDC0CLK_BACKUP1                     (0xF << 12)
+#define B_MSDC0CLK_SMT                         (  1 << 11)
+#define B_MSDC0CLK_R0                          (  1 << 10)
+#define B_MSDC0CLK_R1                          (  1 <<  9)
+#define B_MSDC0CLK_PUPD                        (  1 <<  8)
+#define B_MSDC0CLK_BACKUP0                     (0x7 <<  5)
+#define B_MSDC0CLK_IES                         (  1 <<  4)
+#define B_MSDC0CLK_SR                          (  1 <<  3)
+#define B_MSDC0CLK_E8                          (  1 <<  2)
+#define B_MSDC0CLK_E4                          (  1 <<  1)
+#define B_MSDC0CLK_E2                          (  1 <<  0)
+
+/* 0x10005CD0 MSDC0_CTRL1   MSDC 0 CMD pad Control Register 0x0411 */
+#define B_MSDC0CMD_BACKUP1                     (0xF << 12)
+#define B_MSDC0CMD_SMT                         (  1 << 11)
+#define B_MSDC0CMD_R0                          (  1 << 10)
+#define B_MSDC0CMD_R1                          (  1 <<  9)
+#define B_MSDC0CMD_PUPD                        (  1 <<  8)
+#define B_MSDC0CMD_BACKUP0                     (0x7 <<  5)
+#define B_MSDC0CMD_IES                         (  1 <<  4)
+#define B_MSDC0CMD_SR                          (  1 <<  3)
+#define B_MSDC0CMD_E8                          (  1 <<  2)
+#define B_MSDC0CMD_E4                          (  1 <<  1)
+#define B_MSDC0CMD_E2                          (  1 <<  0)
+
+/* 0x10005CE0 MSDC0_CTRL2   MSDC 0 DAT pad Control Register 0x0011 */
+#define B_MSDC0DAT_BACKUP1                     (0x7FF<< 5)
+#define B_MSDC0DAT_IES                         (  1 <<  4)
+#define B_MSDC0DAT_SR                          (  1 <<  3)
+#define B_MSDC0DAT_E8                          (  1 <<  2)
+#define B_MSDC0DAT_E4                          (  1 <<  1)
+#define B_MSDC0DAT_E2                          (  1 <<  0)
+
+
+/* 0x10005CF0 MSDC0_CTRL3   MSDC 0 DAT pad Control Register 0x4444 */
+#define B_MSDC0DAT3_SMT                         (  1 << 15)
+#define B_MSDC0DAT3_R0                          (  1 << 14)
+#define B_MSDC0DAT3_R1                          (  1 << 13)
+#define B_MSDC0DAT3_PUPD                        (  1 << 12)
+
+#define B_MSDC0DAT2_SMT                         (  1 << 11)
+#define B_MSDC0DAT2_R0                          (  1 << 10)
+#define B_MSDC0DAT2_R1                          (  1 <<  9)
+#define B_MSDC0DAT2_PUPD                        (  1 <<  8)
+
+#define B_MSDC0DAT1_SMT                         (  1 <<  7)
+#define B_MSDC0DAT1_R0                          (  1 <<  6)
+#define B_MSDC0DAT1_R1                          (  1 <<  5)
+#define B_MSDC0DAT1_PUPD                        (  1 <<  4)
+
+#define B_MSDC0DAT0_SMT                         (  1 <<  3)
+#define B_MSDC0DAT0_R0                          (  1 <<  2)
+#define B_MSDC0DAT0_R1                          (  1 <<  1)
+#define B_MSDC0DAT0_PUPD                        (  1 <<  0)
+
+
+/* 0x10005D00 MSDC0_CTRL4   MSDC 0 DAT pad Control Register 0x4444 */
+#define B_MSDC0DAT7_SMT                         (  1 << 15)
+#define B_MSDC0DAT7_R0                          (  1 << 14)
+#define B_MSDC0DAT7_R1                          (  1 << 13)
+#define B_MSDC0DAT7_PUPD                        (  1 << 12)
+
+#define B_MSDC0DAT6_SMT                         (  1 << 11)
+#define B_MSDC0DAT6_R0                          (  1 << 10)
+#define B_MSDC0DAT6_R1                          (  1 <<  9)
+#define B_MSDC0DAT6_PUPD                        (  1 <<  8)
+
+#define B_MSDC0DAT5_SMT                         (  1 <<  7)
+#define B_MSDC0DAT5_R0                          (  1 <<  6)
+#define B_MSDC0DAT5_R1                          (  1 <<  5)
+#define B_MSDC0DAT5_PUPD                        (  1 <<  4)
+
+#define B_MSDC0DAT4_SMT                         (  1 <<  3)
+#define B_MSDC0DAT4_R0                          (  1 <<  2)
+#define B_MSDC0DAT4_R1                          (  1 <<  1)
+#define B_MSDC0DAT4_PUPD                        (  1 <<  0)
+
+/* 0x10005D10 MSDC0_CTRL5   MSDC 0 DAT pad Control Register  0x0004 */
+#define B_MSDC0DAT_BACKUP                       (0xFFF<< 4)
+#define B_MSDC0RSTB_SMT                         (  1 <<  3)
+#define B_MSDC0RSTB_R0                          (  1 <<  2)
+#define B_MSDC0RSTB_R1                          (  1 <<  1)
+#define B_MSDC0RSTB_PUPD                        (  1 <<  0)
+
+/* 0x10005D20 MSDC0_CTRL6   MSDC 0 pad Control Register      0x000A */
+#define B_MSDC0PAD_BACKUP                       (0x3F << 10)
+#define B_MSDC0PAD_RDSEL                        (0x3F <<  4)
+#define B_MSDC0PAD_TDSEL                        (0x0F <<  0)
+
+typedef enum __MSDC_PIN_STATE {
+    MSDC_PST_PU_HIGHZ = 0,
+    MSDC_PST_PD_HIGHZ,
+    MSDC_PST_PU_50KOHM,
+    MSDC_PST_PD_50KOHM,
+    MSDC_PST_PU_10KOHM,
+    MSDC_PST_PD_10KOHM,
+    MSDC_PST_PU_08KOHM,
+    MSDC_PST_PD_08KOHM,
+    MSDC_PST_MAX
+} MSDC_PIN_STATE;
+
+/*
+  *  this definition is used for MT2701_TOPCKGEN_Registers.docx by chengbin.Gao
+  *  Zhanyong.Wang
+  *  2014/12/14
+  */
+#ifndef TOPCKGEN_BASE
+#define TOPCKGEN_BASE                             0x10000000
+#endif
+#if TOPCKGEN_BASE != 0x10000000
+#error  Please check your TOPCKGEN_BASE definition!!!!
+#endif
+
+#define REG_TOPCKGEN_BASE(x)                     ((volatile unsigned int *)(TOPCKGEN_BASE + OFFSET_##x))
+
+/*valid reg name list:
+    CLKMODE, CLKCFGUPDATE,
+    CLKCFG2 /CLKCFG2_SET/CLKCFG2_CLR,
+    CLKCFG3 /CLKCFG3_SET/CLKCFG3_CLR
+    CLKCFG14/CLKCFG14_SET/CLKCFG14_CLR
+    */
+#define TOPCKGEN_CTRL(topckgenreg)               TOPCKGEN_##topckgenreg
+
+/*
+  * id valid range "MSDC[0, 3]"
+  * reg valid range "CLKCFG2/3/14"
+  */
+
+#define MSDC_CKGEN_SEL(id,reg)                    B_CKGEN_##reg##_##id##_SEL
+#define MSDC_CKGEN_INV(id,reg)                    B_CKGEN_##reg##_##id##_INV
+#define MSDC_CKGEN_PDN(id,reg)                    B_CKGEN_##reg##_##id##_PDN
+#define MSDC_CKGEN_UPDATE(id)                     B_CKGEN_CLKCFGUPDATE_##id
+
+#define MSDC_CKGENHCLK_SEL(id,reg)                B_CKGEN_##reg##_##id##HCLK_SEL
+#define MSDC_CKGENHCLK_INV(id,reg)                B_CKGEN_##reg##_##id##HCLK_INV
+//#define MSDC_CKGENHCLK_PDN(id,reg)                B_CKGEN_##reg##_##id##HCLK_PDN
+#define MSDC_CKGENHCLK_UPDATE(id)                 B_CKGEN_CLKCFGUPDATE_##id##HCLK
+
+#define OFFSET_TOPCKGEN_CLK_MODE                  0x0000
+#define OFFSET_TOPCKGEN_CLK_CFG_UPDATE            0x0004
+#define OFFSET_TOPCKGEN_CLK_CFG2                  0x0060
+#define OFFSET_TOPCKGEN_CLK_CFG2_SET              0x0064
+#define OFFSET_TOPCKGEN_CLK_CFG2_CLR              0x0068
+#define OFFSET_TOPCKGEN_CLK_CFG3                  0x0070
+#define OFFSET_TOPCKGEN_CLK_CFG3_SET              0x0074
+#define OFFSET_TOPCKGEN_CLK_CFG3_CLR              0x0078
+#define OFFSET_TOPCKGEN_CLK_CFG6                 0x00A0
+#define OFFSET_TOPCKGEN_CLK_CFG6_SET             0x00A4
+#define OFFSET_TOPCKGEN_CLK_CFG6_CLR             0x00A8
+#define OFFSET_TOPCKGEN_CLK_CFG14                 0x00E0
+#define OFFSET_TOPCKGEN_CLK_CFG14_SET             0x00E4
+#define OFFSET_TOPCKGEN_CLK_CFG14_CLR             0x00E8
+
+
+/*
+  * 0x10000000 CLK_MODE  Clock 26MHz, 32KHz PDN Control Register  0x00000000
+  *************************************************************************/
+
+#define TOPCKGEN_CLKMODE                          REG_TOPCKGEN_BASE(TOPCKGEN_CLK_MODE)
+#define B_CKGEN_CLKMODE_CGDIS                     (1 <<  0)
+#define B_CKGEN_CLKMODE_PDNMD32KHZ                (1 <<  8)
+#define B_CKGEN_CLKMODE_PDNCONN32KHZ              (1 << 10)
+
+/*
+ *
+ * 0x10000060 CLK_CFG_2  Function Clock selection Register 2  0x00000000
+ * 0x10000064 SET CLK_CFG_2  Function Clock selection Register 2  0x00000000
+ * 0x10000068 CLR CLK_CFG_2  Function Clock selection Register 2  0x00000000
+ *************************************************************************/
+#define TOPCKGEN_CLKCFG2                          REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG2)
+#define TOPCKGEN_CLKCFG2_SET                      REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG2_SET)
+#define TOPCKGEN_CLKCFG2_CLR                      REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG2_CLR)
+#define B_CKGEN_CLKCFG2_MSDC0_SEL                 (0x7 << 24)
+#define B_CKGEN_CLKCFG2_MSDC0_INV                 (  1 << 28)
+#define B_CKGEN_CLKCFG2_MSDC0_PDN                 (  1 << 31)
+
+
+/*
+ *
+ * 0x10000070 CLK_CFG_3  Function Clock selection Register 3  0x00000000
+ * 0x10000074 SET CLK_CFG_3  Function Clock selection Register 3  0x00000000
+ * 0x10000078 CLR CLK_CFG_3  Function Clock selection Register 3  0x00000000
+ *************************************************************************/
+#define TOPCKGEN_CLKCFG3                         REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG3)
+#define TOPCKGEN_CLKCFG3_SET                     REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG3_SET)
+#define TOPCKGEN_CLKCFG3_CLR                     REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG3_CLR)
+
+/*
+ *
+ * 0x100000A0 CLK_CFG_6  Function Clock selection Register 6  0x00000000
+ * 0x100000A4 SET CLK_CFG_6  Function Clock selection Register 6  0x00000000
+ * 0x100000A8 CLR CLK_CFG_6  Function Clock selection Register 6  0x00000000
+ *************************************************************************/
+#define TOPCKGEN_CLKCFG6                          REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG6)
+#define TOPCKGEN_CLKCFG6_SET                      REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG6_SET)
+#define TOPCKGEN_CLKCFG6_CLR                      REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG6_CLR)
+
+/*
+ *
+ * 0x100000E0 CLK_CFG_14  Function Clock selection Register 14  0x00000000
+ * 0x100000E4 SET CLK_CFG_14  Function Clock selection Register 14  0x00000000
+ * 0x100000E8 CLR CLK_CFG_14  Function Clock selection Register 14  0x00000000
+ *************************************************************************/
+#define TOPCKGEN_CLKCFG14                         REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG14)
+#define TOPCKGEN_CLKCFG14_SET                     REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG14_SET)
+#define TOPCKGEN_CLKCFG14_CLR                     REG_TOPCKGEN_BASE(TOPCKGEN_CLK_CFG14_CLR)
+
+/* each PLL have different gears for select
+ * software can used mux interface from clock management module to select */
+enum {
+    MSDC50_CLKSRC4HCLK_26MHZ  = 0,
+    MSDC50_CLKSRC4HCLK_273MHZ,
+    MSDC50_CLKSRC4HCLK_182MHZ,
+    MSDC50_CLKSRC4HCLK_78MHZ,
+    MSDC_DONOTCARE_HCLK,
+    MSDC50_CLKSRC4HCLK_MAX
+};
+
+enum {
+    MSDC50_CLKSRC_26MHZ  = 0,
+    MSDC50_CLKSRC_400MHZ,  /* MSDCPLL_CK */
+    MSDC50_CLKSRC_182MHZ,  /*MSDCPLL_D2 */
+    MSDC50_CLKSRC_136MHZ,
+    MSDC50_CLKSRC_156MHZ,
+    MSDC50_CLKSRC_200MHZ,  /*MSDCPLL_D4 */
+    MSDC50_CLKSRC_100MHZ,
+    MSDC50_CLKSRC_50MHZ,
+    MSDC50_CLKSRC_MAX
+};
+
+/* MSDC0/1/2
+     PLL MUX SEL List */
+enum {
+    MSDC30_CLKSRC_26MHZ   = 0,
+    MSDC30_CLKSRC_200MHZ,
+    MSDC30_CLKSRC_182MHZ,
+    MSDC30_CLKSRC_91MHZ,
+    MSDC30_CLKSRC_156MHZ,
+    MSDC30_CLKSRC_104MHZ,
+    MSDC30_CLKSRC_MAX
+};
+
+#define MSDC50_CLKSRC_DEFAULT     MSDC50_CLKSRC_200MHZ
+#define MSDC30_CLKSRC_DEFAULT     MSDC30_CLKSRC_200MHZ
+
+typedef enum MT65XX_POWER_VOL_TAG {
+    VOL_DEFAULT,
+    VOL_0900 = 900,
+    VOL_1000 = 1000,
+    VOL_1100 = 1100,
+    VOL_1200 = 1200,
+    VOL_1300 = 1300,
+    VOL_1350 = 1350,
+    VOL_1500 = 1500,
+    VOL_1800 = 1800,
+    VOL_2000 = 2000,
+    VOL_2100 = 2100,
+    VOL_2500 = 2500,
+    VOL_2800 = 2800,
+    VOL_3000 = 3000,
+    VOL_3300 = 3300,
+    VOL_3400 = 3400,
+    VOL_3500 = 3500,
+    VOL_3600 = 3600
+} MT65XX_POWER_VOLTAGE;
+
+/*--------------------------------------------------------------------------*/
+/* Descriptor Structure                                                     */
+/*--------------------------------------------------------------------------*/
+#define DMA_FLAG_NONE       (0x00000000)
+#define DMA_FLAG_EN_CHKSUM  (0x00000001)
+#define DMA_FLAG_PAD_BLOCK  (0x00000002)
+#define DMA_FLAG_PAD_DWORD  (0x00000004)
+
+#define MSDC_WRITE32(addr, data)    writel(data, addr)
+#define MSDC_READ32(addr)           readl(addr)
+#define MSDC_WRITE8(addr, data)     writeb(data, addr)
+#define MSDC_READ8(addr)            readb(addr)
+
+#define MSDC_SET_BIT32(addr,mask) \
+    do {    \
+        unsigned int tv = MSDC_READ32(addr); \
+        tv |=((u32)(mask)); \
+        MSDC_WRITE32(addr,tv); \
+    } while (0)
+#define MSDC_CLR_BIT32(addr,mask) \
+    do {    \
+        unsigned int tv = MSDC_READ32(addr); \
+        tv &= ~((u32)(mask)); \
+        MSDC_WRITE32(addr,tv); \
+    } while (0)
+
+#define MSDC_SET_FIELD(reg,field,val) \
+    do { \
+        u32 tv = MSDC_READ32(reg); \
+        tv &= ~((u32)(field)); \
+        tv |= ((val) << (__builtin_ffs((u32)(field)) - 1)); \
+        MSDC_WRITE32(reg, tv); \
+    } while (0)
+
+#define MSDC_GET_FIELD(reg,field,val) \
+    do { \
+        u32 tv = MSDC_READ32(reg); \
+        val = ((tv & (field)) >> (__builtin_ffs((u32)(field)) - 1)); \
+    } while (0)
+
+#define MSDC_RETRY(expr,retry,cnt) \
+    do { \
+        uint32_t t = cnt; \
+        uint32_t r = retry; \
+        uint32_t c = cnt; \
+        while (r) { \
+            if (!(expr)) break; \
+            if (c-- == 0) { \
+                r--; spin(200); c = t; \
+            } \
+        } \
+        if (r == 0) \
+            dprintf(CRITICAL, "%s->%d: retry %d times failed!\n", __func__, \
+                    __LINE__, retry); \
+    } while (0)
+
+#define MSDC_RESET() \
+    do { \
+        MSDC_SET_BIT32(MSDC_CFG, MSDC_CFG_RST); \
+        MSDC_RETRY(MSDC_READ32(MSDC_CFG) & MSDC_CFG_RST, 5, 1000); \
+    } while (0)
+
+#define MSDC_CLR_INT() \
+    do { \
+        volatile uint32_t val = MSDC_READ32(MSDC_INT); \
+        MSDC_WRITE32(MSDC_INT, val); \
+    } while (0)
+
+#define MSDC_CLR_FIFO() \
+    do { \
+        MSDC_SET_BIT32(MSDC_FIFOCS, MSDC_FIFOCS_CLR); \
+        MSDC_RETRY(MSDC_READ32(MSDC_FIFOCS) & MSDC_FIFOCS_CLR, 5, 1000); \
+    } while (0)
+
+#define MSDC_FIFO_WRITE32(val)  MSDC_WRITE32(MSDC_TXDATA, val)
+#define MSDC_FIFO_READ32()      MSDC_READ32(MSDC_RXDATA)
+#define MSDC_FIFO_WRITE8(val)   MSDC_WRITE8(MSDC_TXDATA, val)
+#define MSDC_FIFO_READ8()       MSDC_READ8(MSDC_RXDATA)
+
+#define MSDC_TXFIFOCNT() \
+    ((MSDC_READ32(MSDC_FIFOCS) & MSDC_FIFOCS_TXCNT) >> 16)
+#define MSDC_RXFIFOCNT() \
+    ((MSDC_READ32(MSDC_FIFOCS) & MSDC_FIFOCS_RXCNT) >> 0)
+
+#define SDC_IS_BUSY()       (MSDC_READ32(SDC_STS) & SDC_STS_SDCBUSY)
+#define SDC_IS_CMD_BUSY()   (MSDC_READ32(SDC_STS) & SDC_STS_CMDBUSY)
+
+#define SDC_SEND_CMD(cmd,arg) \
+    do { \
+        MSDC_WRITE32(SDC_ARG, (arg)); \
+        MSDC_WRITE32(SDC_CMD, (cmd)); \
+    } while (0)
+
+#define MSDC_DMA_ON     MSDC_CLR_BIT32(MSDC_CFG, MSDC_CFG_PIO);
+#define MSDC_DMA_OFF    MSDC_SET_BIT32(MSDC_CFG, MSDC_CFG_PIO);
+
+#define MSDC_RELIABLE_WRITE     (0x1 << 0)
+#define MSDC_PACKED             (0x1 << 1)
+#define MSDC_TAG_REQUEST        (0x1 << 2)
+#define MSDC_CONTEXT_ID         (0x1 << 3)
+#define MSDC_FORCED_PROG        (0x1 << 4)
+
+#ifdef FPGA_PLATFORM
+#define msdc_pin_set(...)
+#define msdc_set_smt(...)
+#define msdc_set_driving(...)
+#endif
+
+int msdc_init(struct mmc_host *host);
+void msdc_config_bus(struct mmc_host *host, u32 width);
+int msdc_dma_transfer(struct mmc_host *host, struct mmc_data *data);
+int msdc_tune_bwrite(struct mmc_host *host, u32 dst, u8 *src, u32 nblks);
+int msdc_tune_bread(struct mmc_host *host, u8 *dst, u32 src, u32 nblks);
+void msdc_reset_tune_counter(struct mmc_host *host);
+int msdc_abort_handler(struct mmc_host *host, int abort_card);
+int msdc_tune_read(struct mmc_host *host);
+void msdc_config_clock(struct mmc_host *host, int state, u32 hz);
+int msdc_cmd(struct mmc_host *host, struct mmc_command *cmd);
+void msdc_set_timeout(struct mmc_host *host, u32 ns, u32 clks);
+
diff --git a/src/bsp/lk/platform/mt2701/include/platform/msdc_cfg.h b/src/bsp/lk/platform/mt2701/include/platform/msdc_cfg.h
new file mode 100644
index 0000000..0afd634
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/msdc_cfg.h
@@ -0,0 +1,48 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+/*--------------------------------------------------------------------------*/
+/* Common Definition                                                        */
+/*--------------------------------------------------------------------------*/
+#ifdef MACH_FPGA
+#define FPGA_PLATFORM
+#endif
+
+/* HW deal with the 2K DMA boundary limitation, SW do nothing with it */
+/* Most of eMMC request in lk are sequential access, so it's no need to
+ * use descript DMA mode, I just remove relevant codes. JieWu@20160607 */
+#define MSDC_USE_DMA_MODE
+
+#define FEATURE_MMC_WR_TUNING
+#define FEATURE_MMC_RD_TUNING
+#define FEATURE_MMC_CM_TUNING
+
+/* Maybe we discard these macro definition */
+//#define MSDC_USE_PATCH_BIT2_TURNING_WITH_ASYNC
+
+/*--------------------------------------------------------------------------*/
+/* Debug Definition                                                         */
+/*--------------------------------------------------------------------------*/
+//#define KEEP_SLIENT_BUILD
+//#define ___MSDC_DEBUG___
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mt2701.h b/src/bsp/lk/platform/mt2701/include/platform/mt2701.h
new file mode 100644
index 0000000..6f94ce8
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mt2701.h
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+#include <debug.h>
+#define SRAM_BASE_PHYS (0x100000)
+#define SRAM_BASE_SIZE (0x10000)
+#define SRAM_BASE_VIRT (0x100000)
+
+#define MEMORY_BASE_PHYS     (0x200000)
+#define MEMORY_APERTURE_SIZE (0x20000L)
+
+/* map all of 0-1GB into kernel space in one shot */
+#define PERIPHERAL_BASE_PHYS (0x10000000)
+#define PERIPHERAL_BASE_SIZE (0x0d000000)
+#define PERIPHERAL_BASE_VIRT (0x10000000)
+
+/* individual peripherals in this mapping */
+#define CPUPRIV_BASE_VIRT   (PERIPHERAL_BASE_VIRT + 0x08000000)
+#define CPUPRIV_BASE_PHYS   (PERIPHERAL_BASE_PHYS + 0x08000000)
+#define CPUPRIV_SIZE        (0x00020000)
+#define UART_BASE           (PERIPHERAL_BASE_VIRT + 0x09000000)
+#define UART_SIZE           (0x00001000)
+#define FW_CFG_BASE         (PERIPHERAL_BASE_VIRT + 0x09020000)
+#define FW_CFG_SIZE         (0x00001000)
+#define NUM_VIRTIO_TRANSPORTS 32
+#define VIRTIO_BASE         (PERIPHERAL_BASE_VIRT + 0x0a000000)
+#define VIRTIO_SIZE         (NUM_VIRTIO_TRANSPORTS * 0x200)
+
+/* interrupts */
+#define ARM_GENERIC_TIMER_VIRTUAL_INT 27
+#define ARM_GENERIC_TIMER_PHYSICAL_INT 29
+#define UART0_INT   (32 + 1)
+#define VIRTIO0_INT (32 + 16)
+
+#define MAX_INT 128
+
+#define DRAM_BASE_PHY  (0x80000000U)
+#define DRAM_BASE_VIRT DRAM_BASE_PHY
+
+/* mt_mempll_init and dram calibration */
+#define DRAMC0_BASE         (PERIPHERAL_BASE_PHYS + 0x4000)
+#define DRAMC_NAO_BASE      (PERIPHERAL_BASE_PHYS + 0x20E000)
+#define DDRPHY_BASE         (PERIPHERAL_BASE_PHYS + 0xF000)
+#define SPM_BASE            (PERIPHERAL_BASE_PHYS + 0x6000)
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mt_irq.h b/src/bsp/lk/platform/mt2701/include/platform/mt_irq.h
new file mode 100644
index 0000000..dba6497
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mt_irq.h
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#define GIC_DIST_PENDING_SET            0x200
+#define GIC_DIST_ENABLE_SET             0x100
+#define GIC_DIST_ENABLE_CLEAR           0x180
+#define GIC_DIST_PENDING_CLEAR          0x280
+#define GIC_DIST_CTR                    0x004
+#define GIC_DIST_CTRL                   0x000
+#define GIC_DIST_TARGET                 0x800
+#define GIC_DIST_ACTIVE_BIT             0x300
+#define GIC_DIST_PRI                    0x400
+#define GIC_DIST_ICDISR                 0x80
+#define GIC_DIST_CONFIG                 0xc00
+#define GIC_DIST_SOFTINT                0xf00
+
+#define GIC_CPU_HIGHPRI                 0x18
+#define GIC_CPU_EOI                     0x10
+#define GIC_CPU_RUNNINGPRI              0x14
+#define GIC_CPU_BINPOINT                0x08
+#define GIC_CPU_INTACK                  0x0c
+#define GIC_CPU_CTRL                    0x00
+#define GIC_CPU_PRIMASK                 0x04
+
+/*
+ * Define IRQ code.
+ */
+#define GIC_PRIVATE_SIGNALS (32)
+
+#define GIC_PPI_OFFSET          (27)
+#define GIC_PPI_GLOBAL_TIMER    (GIC_PPI_OFFSET + 0)
+#define GIC_PPI_LEGACY_FIQ      (GIC_PPI_OFFSET + 1)
+#define GIC_PPI_PRIVATE_TIMER   (GIC_PPI_OFFSET + 2)
+#define GIC_PPI_WATCHDOG_TIMER  (GIC_PPI_OFFSET + 3)
+#define GIC_PPI_LEGACY_IRQ      (GIC_PPI_OFFSET + 4)
+
+#define MT_USB0_IRQ_ID                  (GIC_PRIVATE_SIGNALS + 32)
+#define MT_MSDC0_IRQ_ID                 (GIC_PRIVATE_SIGNALS + 39)
+#define MT_MSDC1_IRQ_ID                 (GIC_PRIVATE_SIGNALS + 40)
+#define MT_MSDC3_IRQ_ID                 (GIC_PRIVATE_SIGNALS + 131)
+#define MT_AP_HIF_IRQ_ID                (GIC_PRIVATE_SIGNALS + 43)
+#define MT_I2C0_IRQ_ID                  (GIC_PRIVATE_SIGNALS + 44)
+#define MT_I2C1_IRQ_ID                  (GIC_PRIVATE_SIGNALS + 45)
+#define MT_I2C2_IRQ_ID                  (GIC_PRIVATE_SIGNALS + 46)
+#define MT_I2C3_IRQ_ID                  (GIC_PRIVATE_SIGNALS + 47)
+#define MT_NOR_IRQ_ID                   (GIC_PRIVATE_SIGNALS + 48)
+
+#define MT_UART1_IRQ_ID                     (GIC_PRIVATE_SIGNALS + 51)
+#define MT_UART2_IRQ_ID                     (GIC_PRIVATE_SIGNALS + 52)
+#define MT_UART3_IRQ_ID                     (GIC_PRIVATE_SIGNALS + 53)
+#define MT_UART4_IRQ_ID                     (GIC_PRIVATE_SIGNALS + 54)
+#define MT_NFIECC_IRQ_ID                (GIC_PRIVATE_SIGNALS + 55)
+#define MT_NFI_IRQ_ID                   (GIC_PRIVATE_SIGNALS + 56)
+#define MT_GDMA1_IRQ_ID                     (GIC_PRIVATE_SIGNALS + 57)
+#define MT_GDMA2_IRQ_ID                     (GIC_PRIVATE_SIGNALS + 58)
+#define MT_DMA_UART0_TX_IRQ_ID              (GIC_PRIVATE_SIGNALS + 63)
+#define MT_DMA_UART0_RX_IRQ_ID              (GIC_PRIVATE_SIGNALS + 64)
+#define MT_DMA_UART1_TX_IRQ_ID              (GIC_PRIVATE_SIGNALS + 65)
+#define MT_DMA_UART1_RX_IRQ_ID              (GIC_PRIVATE_SIGNALS + 66)
+#define MT_DMA_UART2_TX_IRQ_ID              (GIC_PRIVATE_SIGNALS + 67)
+#define MT_DMA_UART2_RX_IRQ_ID              (GIC_PRIVATE_SIGNALS + 68)
+
+#define MT_SPI1_IRQ_ID                      (GIC_PRIVATE_SIGNALS + 78)
+#define MT_IRRX_IRQ_ID                       (GIC_PRIVATE_SIGNALS + 87)
+#define MT_WDT_IRQ_ID                       (GIC_PRIVATE_SIGNALS + 88)
+#define MT_APARM_DOMAIN_IRQ_ID              (GIC_PRIVATE_SIGNALS + 94)
+#define MT_APARM_DECERR_IRQ_ID              (GIC_PRIVATE_SIGNALS + 95)
+#define MT_GPT_IRQ_ID                       (GIC_PRIVATE_SIGNALS + 112)
+#define MT_EINT_IRQ_ID                      (GIC_PRIVATE_SIGNALS + 113)
+#define MT_PMIC_WRAP_IRQ_ID                 (GIC_PRIVATE_SIGNALS + 115)
+#define MT_KP_IRQ_ID                        (GIC_PRIVATE_SIGNALS + 116)
+#define MT_SPM_IRQ_ID                       (GIC_PRIVATE_SIGNALS + 117)
+#define MT_VENC_IRQ_ID                      (GIC_PRIVATE_SIGNALS + 139)
+#define MT_VDEC_IRQ_ID                      (GIC_PRIVATE_SIGNALS + 140)
+#define CAMERA_ISP_IRQ0_ID                  (GIC_PRIVATE_SIGNALS + 143)
+#define CAMERA_ISP_IRQ1_ID                  (GIC_PRIVATE_SIGNALS + 144)
+#define CAMERA_ISP_IRQ2_ID                  (GIC_PRIVATE_SIGNALS + 145)
+#define MT_JPEG_ENC_IRQ_ID                  (GIC_PRIVATE_SIGNALS + 141)
+
+#define MT_NR_PPI   (5)
+#define MT_NR_SPI   (224)
+#define NR_IRQ_LINE  (GIC_PPI_OFFSET + MT_NR_PPI + MT_NR_SPI)
+
+#define EDGE_SENSITIVE 0
+#define LEVEL_SENSITIVE 1
+
+#define MT65xx_POLARITY_LOW   0
+#define MT65xx_POLARITY_HIGH  1
+
+void mt_irq_set_sens(unsigned int irq, unsigned int sens);
+void mt_irq_set_polarity(unsigned int irq, unsigned int polarity);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mt_reg_base.h b/src/bsp/lk/platform/mt2701/include/platform/mt_reg_base.h
new file mode 100644
index 0000000..45241d3
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mt_reg_base.h
@@ -0,0 +1,219 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+#pragma once
+
+/* I/O mapping */
+#define IO_PHYS             0x10000000
+#define IO_SIZE             0x00100000
+
+
+#define APCONFIG_BASE (IO_PHYS + 0x00026000)
+
+/* IO register definitions */
+#define EFUSE_BASE          (IO_PHYS + 0x00206000)
+#define CONFIG_BASE         (IO_PHYS + 0x00001000)
+#define EMI_BASE            (IO_PHYS + 0x00203000)
+#define GPIO_BASE           (IO_PHYS + 0x00005000)
+#define TOP_RGU_BASE        (IO_PHYS + 0x00007000)
+#define PERI_CON_BASE       (IO_PHYS + 0x00003000)
+#define GIC_CPU_BASE        (IO_PHYS + 0x00212000)
+#define GIC_DIST_BASE       (IO_PHYS + 0x00211000)
+#define SLEEP_BASE          (IO_PHYS + 0x00006000)
+#define MCUSYS_CFGREG_BASE  (IO_PHYS + 0x00200000)
+#define MMSYS1_CONFIG_BASE  (IO_PHYS + 0x02080000)
+#define MMSYS2_CONFG_BASE   (IO_PHYS + 0x020C0000)
+#define AUDIO_BASE          (IO_PHYS + 0x02071000)
+#define NFI_BASE            (IO_PHYS + 0x0100D000)
+#define INT_POL_CTL0        (MCUSYS_CFGREG_BASE + 0x100)
+/**************************************************
+ *                   F002 0000                    *
+ **************************************************/
+
+#define AP_DMA_BASE         (IO_PHYS + 0x01000000)
+
+/*Hong-Rong: Modified for 6575 porting*/
+#define UART1_BASE          (IO_PHYS + 0x01002000)
+#define UART2_BASE          (IO_PHYS + 0x01003000)
+#define UART3_BASE          (IO_PHYS + 0x01004000)
+#define UART4_BASE          (IO_PHYS + 0x01005000)
+
+#define APMCU_GPTIMER_BASE  (IO_PHYS + 0x00008000)
+
+#define KP_BASE             (IO_PHYS + 0x00011000)
+#define MSDC0_BASE          (IO_PHYS + 0x01230000)
+#define MSDC1_BASE          (IO_PHYS + 0x01240000)
+#define MSDC2_BASE          (IO_PHYS + 0x01250000)
+#define MSDC3_BASE          (IO_PHYS + 0x01260000)
+#define MSDC4_BASE          (IO_PHYS + 0x01270000)
+
+
+/**************************************************
+ *                   F003 0000                    *
+ **************************************************/
+#define I2C0_BASE           (IO_PHYS + 0x01007000)
+#define I2C1_BASE           (IO_PHYS + 0x01008000)
+#define I2C2_BASE           (IO_PHYS + 0x01009000)
+#define NFI_BASE            (IO_PHYS + 0x0100D000)
+#define NFIECC_BASE         (IO_PHYS + 0x0100E000)
+#define PWM_BASE            (IO_PHYS + 0x01006000)
+#define AUXADC_BASE         (IO_PHYS + 0x01001000)
+#define PWRAP_BASE      (IO_PHYS + 0xD000)
+#define NOR_BASE            (IO_PHYS + 0x01014000)
+/**************************************************
+ *                   F004 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F006 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F007 0000                    *
+ **************************************************/
+/**************************************************
+ *                   F008 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F009 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F00A 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F00C 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F010 0000                    *
+ **************************************************/
+#define USB0_BASE           (IO_PHYS + 0x01200000)
+#define USBSIF_BASE         (IO_PHYS + 0x01210000)
+#define USB_BASE            (USB0_BASE)
+
+
+/**************************************************
+ *                   F012 0000                    *
+ **************************************************/
+#define LCD_BASE            (IO_PHYS + 0x04012000)
+
+/**************************************************
+ *                   F013 0000                    *
+ **************************************************/
+
+/**************************************************
+ *                   F014 0000                    *
+ **************************************************/
+
+#define MIPI_CONFIG_BASE    (IO_PHYS + 0x00010000)
+
+// APB Module LVDS ANA
+#define LVDS_ANA_BASE (IO_PHYS + 0x00010400)
+
+
+
+#define MMSYS2_CONFG_BASE   (IO_PHYS + 0x020C0000)
+
+
+/* disp subsys register */
+#define MMSYS_CONFIG_BASE   (IO_PHYS + 0x04000000)
+#define MDP_RDMA_BASE       (IO_PHYS + 0x04001000)
+#define MDP_RSZ0_BASE       (IO_PHYS + 0x04002000)
+#define MDP_RSZ1_BASE       (IO_PHYS + 0x04003000)
+#define MDP_WDMA_BASE       (IO_PHYS + 0x04004000)
+#define MDP_WROT_BASE       (IO_PHYS + 0x04005000)
+#define MDP_TDSHP_BASE      (IO_PHYS + 0x04006000)
+#define DISP_OVL_BASE       (IO_PHYS + 0x04007000)
+#define DISP_RDMA_BASE      (IO_PHYS + 0x04008000)
+#define DISP_WDMA_BASE      (IO_PHYS + 0x04009000)
+#define DISP_BLS_BASE       (IO_PHYS + 0x0400A000)
+#define DISP_COLOR_BASE     (IO_PHYS + 0x0400B000)
+#define DSI_BASE            (IO_PHYS + 0x0400C000)
+#define DPI_BASE            (IO_PHYS + 0x0400D000)
+#define DPI1_BASE            (IO_PHYS + 0x04014000)
+
+// LVDS TX
+#define LVDS_TX_BASE (IO_PHYS + 0x04016200)
+
+#define MM_MUTEX_BASE       (IO_PHYS + 0x0400E000)
+#define MM_CMDQ_BASE        (IO_PHYS + 0x0400F000)
+
+#define DISPSYS_BASE        (MMSYS_CONFIG_BASE)
+
+/* hardware version register */
+#define VER_BASE 0x08000000
+#define APHW_CODE           (VER_BASE)
+#define APHW_SUBCODE        (VER_BASE + 0x04)
+#define APHW_VER            (VER_BASE + 0x08)
+#define APSW_VER            (VER_BASE + 0x0C)
+
+/* EMI Registers */
+#define EMI_CON0                    (EMI_BASE+0x0000) /* Bank 0 configuration */
+#define EMI_CON1                    (EMI_BASE+0x0004) /* Bank 1 configuration */
+#define EMI_CON2                    (EMI_BASE+0x0008) /* Bank 2 configuration */
+#define EMI_CON3                    (EMI_BASE+0x000C) /* Bank 3 configuration */
+#define EMI_CON4                    (EMI_BASE+0x0010) /* Boot Mapping config  */
+#define EMI_CON5                    (EMI_BASE+0x0014)
+
+//----------------------------------------------------------------------------
+/* Powerdown module watch dog timer registers */
+#define REG_RW_WATCHDOG_EN          0x0100      // Watchdog Timer Control Register
+#define REG_RW_WATCHDOG_TMR         0x0104      // Watchdog Timer Register
+#define REG_RW_WAKE_RSTCNT          0x0108      // Wakeup Reset Counter Register
+
+/* MT6575 EMI freq. definition */
+#define EMI_52MHZ                   52000000
+#define EMI_58_5MHZ                 58500000
+#define EMI_104MHZ                  104000000
+#define EMI_117MHZ                  117000000
+#define EMI_130MHZ                  130000000
+
+/* MT6575 storage boot type definitions */
+#define NON_BOOTABLE                0
+#define RAW_BOOT                    1
+#define FAT_BOOT                    2
+
+#define CONFIG_STACKSIZE        (128*1024)    /* regular stack */
+
+// xuecheng, define this because we use zlib for boot logo compression
+#define CONFIG_ZLIB     1
+
+// =======================================================================
+// UBOOT DEBUG CONTROL
+// =======================================================================
+#define UBOOT_DEBUG_TRACER          (0)
+
+/*MTK Memory layout configuration*/
+#define MAX_NR_BANK    4
+
+#define DRAM_PHY_ADDR   0x80000000
+
+#define TZ_SIZE    0x400000
+
+#define RIL_SIZE 0
+
+#define CFG_BOOTIMG_LOAD_ADDR           (DRAM_PHY_ADDR + 0x8000)
+
+#define CFG_BOOTARGS_ADDR               (DRAM_PHY_ADDR + 0x3000000)
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mt_uart.h b/src/bsp/lk/platform/mt2701/include/platform/mt_uart.h
new file mode 100644
index 0000000..f151098
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mt_uart.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+#pragma once
+
+bool check_uart_enter(void);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mt_usb.h b/src/bsp/lk/platform/mt2701/include/platform/mt_usb.h
new file mode 100644
index 0000000..1742f05
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mt_usb.h
@@ -0,0 +1,273 @@
+/*
+ * Copyright (c) 2012 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+/* USB PHY registers */
+#define USB20_PHY_BASE  (USBSIF_BASE + 0x0800)
+#define USB11_PHY_BASE  (USBSIF_BASE + 0x0900)
+
+/* hardware spec */
+#define MT_EP_NUM 4
+#define MT_CHAN_NUM 4
+#define MT_EP0_FIFOSIZE 64
+
+#define FIFO_ADDR_START  512
+
+#define MT_BULK_MAXP 512
+#define MT_INT_MAXP  1024
+
+/* USB common registers */
+#define FADDR    (USB_BASE + 0x0000)    /* Function Address Register */
+#define POWER    (USB_BASE + 0x0001)    /* Power Management Register */
+#define INTRTX   (USB_BASE + 0x0002)    /* TX Interrupt Status Register */
+#define INTRRX   (USB_BASE + 0x0004)    /* RX Interrupt Status Register */
+#define INTRTXE  (USB_BASE + 0x0006)    /* TX Interrupt Status Enable Register */
+#define INTRRXE  (USB_BASE + 0x0008)    /* RX Interrupt Status Enable Register */
+#define INTRUSB  (USB_BASE + 0x000a)    /* Common USB Interrupt Register */
+#define INTRUSBE (USB_BASE + 0x000b)    /* Common USB Interrupt Enable Register */
+#define FRAME    (USB_BASE + 0x000c)    /* Frame Number Register */
+#define INDEX    (USB_BASE + 0x000e)    /* Endpoint Selecting Index Register */
+#define TESTMODE (USB_BASE + 0x000f)    /* Test Mode Enable Register */
+
+/* POWER fields */
+#define PWR_ISO_UPDATE       (1<<7)
+#define PWR_SOFT_CONN        (1<<6)
+#define PWR_HS_ENAB          (1<<5)
+#define PWR_HS_MODE          (1<<4)
+#define PWR_RESET            (1<<3)
+#define PWR_RESUME           (1<<2)
+#define PWR_SUSPEND_MODE     (1<<1)
+#define PWR_ENABLE_SUSPENDM  (1<<0)
+
+/* INTRUSB fields */
+#define INTRUSB_VBUS_ERROR (1<<7)
+#define INTRUSB_SESS_REQ   (1<<6)
+#define INTRUSB_DISCON     (1<<5)
+#define INTRUSB_CONN       (1<<4)
+#define INTRUSB_SOF        (1<<3)
+#define INTRUSB_RESET      (1<<2)
+#define INTRUSB_RESUME     (1<<1)
+#define INTRUSB_SUSPEND    (1<<0)
+
+/* DMA control registers */
+#define USB_DMA_INTR (USB_BASE + 0x0200)
+#define USB_DMA_INTR_UNMASK_SET_OFFSET (24)
+
+#define USB_DMA_CNTL(chan)  (USB_BASE + 0x0204 + 0x10*(chan-1))
+#define USB_DMA_ADDR(chan)  (USB_BASE + 0x0208 + 0x10*(chan-1))
+#define USB_DMA_COUNT(chan) (USB_BASE + 0x020c + 0x10*(chan-1))
+
+/* Endpoint Control/Status Registers */
+#define IECSR (USB_BASE + 0x0010)
+/* for EP0 */
+#define CSR0         0x2        /* EP0 Control Status Register */
+/* For Host Mode, it would be 0x2 */
+#define COUNT0       0x8        /* EP0 Received Bytes Register */
+#define NAKLIMIT0    0xB        /* NAK Limit Register */
+#define CONFIGDATA   0xF        /* Core Configuration Register */
+/* for other endpoints */
+#define TXMAP        0x0        /* TXMAP Register: Max Packet Size for TX */
+#define TXCSR        0x2        /* TXCSR Register: TX Control Status Register */
+#define RXMAP        0x4        /* RXMAP Register: Max Packet Size for RX */
+#define RXCSR        0x6        /* RXCSR Register: RX Control Status Register */
+#define RXCOUNT      0x8        /* RXCOUNT Register */
+#define TXTYPE       0xa        /* TX Type Register */
+#define TXINTERVAL   0xb        /* TX Interval Register */
+#define RXTYPE       0xc        /* RX Type Register */
+#define RXINTERVAL   0xd        /* RX Interval Register */
+#define FIFOSIZE     0xf        /* configured FIFO size register */
+
+/* control status register fields */
+/* CSR0_DEV */
+#define EP0_FLUSH_FIFO           (1<<8)
+#define EP0_SERVICE_SETUP_END    (1<<7)
+#define EP0_SERVICED_RXPKTRDY    (1<<6)
+#define EP0_SENDSTALL            (1<<5)
+#define EP0_SETUPEND             (1<<4)
+#define EP0_DATAEND              (1<<3)
+#define EP0_SENTSTALL            (1<<2)
+#define EP0_TXPKTRDY             (1<<1)
+#define EP0_RXPKTRDY             (1<<0)
+
+/* TXCSR_DEV */
+#define EPX_TX_AUTOSET           (1<<15)
+#define EPX_TX_ISO               (1<<14)
+#define EPX_TX_MODE              (1<<13)
+#define EPX_TX_DMAREQEN          (1<<12)
+#define EPX_TX_FRCDATATOG        (1<<11)
+#define EPX_TX_DMAREQMODE        (1<<10)
+#define EPX_TX_AUTOSETEN_SPKT    (1<<9)
+#define EPX_TX_INCOMPTX          (1<<7)
+#define EPX_TX_CLRDATATOG        (1<<6)
+#define EPX_TX_SENTSTALL         (1<<5)
+#define EPX_TX_SENDSTALL         (1<<4)
+#define EPX_TX_FLUSHFIFO         (1<<3)
+#define EPX_TX_UNDERRUN          (1<<2)
+#define EPX_TX_FIFONOTEMPTY      (1<<1)
+#define EPX_TX_TXPKTRDY          (1<<0)
+
+/* RXCSR_DEV */
+#define EPX_RX_AUTOCLEAR         (1<<15)
+#define EPX_RX_ISO               (1<<14)
+#define EPX_RX_DMAREQEN          (1<<13)
+#define EPX_RX_DISNYET           (1<<12)
+#define EPX_RX_PIDERR            (1<<12)
+#define EPX_RX_DMAREQMODE        (1<<11)
+#define EPX_RX_AUTOCLRENSPKT     (1<<10)
+#define EPX_RX_INCOMPRXINTREN    (1<<9)
+#define EPX_RX_INCOMPRX          (1<<8)
+#define EPX_RX_CLRDATATOG        (1<<7)
+#define EPX_RX_SENTSTALL         (1<<6)
+#define EPX_RX_SENDSTALL         (1<<5)
+#define EPX_RX_FLUSHFIFO         (1<<4)
+#define EPX_RX_DATAERR           (1<<3)
+#define EPX_RX_OVERRUN           (1<<2)
+#define EPX_RX_FIFOFULL          (1<<1)
+#define EPX_RX_RXPKTRDY          (1<<0)
+
+/* CONFIGDATA fields */
+#define MP_RXE         (1<<7)
+#define MP_TXE         (1<<6)
+#define BIGENDIAN      (1<<5)
+#define HBRXE          (1<<4)
+#define HBTXE          (1<<3)
+#define DYNFIFOSIZING  (1<<2)
+#define SOFTCONE       (1<<1)
+#define UTMIDATAWIDTH  (1<<0)
+
+/* FIFO register */
+/*
+ * for endpint 1 ~ 4, writing to these addresses = writing to the
+ * corresponding TX FIFO, reading from these addresses = reading from
+ * corresponding RX FIFO
+ */
+
+#define FIFO(ep_num)     (USB_BASE + 0x0020 + ep_num*0x0004)
+
+/* ============================ */
+/* additional control registers */
+/* ============================ */
+
+#define DEVCTL       (USB_BASE + 0x0060)        /* OTG Device Control Register */
+#define PWRUPCNT     (USB_BASE + 0x0061)        /* Power Up Counter Register */
+#define TXFIFOSZ     (USB_BASE + 0x0062)        /* TX FIFO Size Register */
+#define RXFIFOSZ     (USB_BASE + 0x0063)        /* RX FIFO Size Register */
+#define TXFIFOADD    (USB_BASE + 0x0064)        /* TX FIFO Address Register */
+#define RXFIFOADD    (USB_BASE + 0x0066)        /* RX FIFO Address Register */
+#define HWVERS       (USB_BASE + 0x006c)        /* H/W Version Register */
+#define SWRST        (USB_BASE + 0x0074)        /* Software Reset Register */
+#define EPINFO       (USB_BASE + 0x0078)        /* TX and RX Information Register */
+#define RAM_DMAINFO  (USB_BASE + 0x0079)        /* RAM and DMA Information Register */
+#define LINKINFO     (USB_BASE + 0x007a)        /* Delay Time Information Register */
+#define VPLEN        (USB_BASE + 0x007b)        /* VBUS Pulse Charge Time Register */
+#define HSEOF1       (USB_BASE + 0x007c)        /* High Speed EOF1 Register */
+#define FSEOF1       (USB_BASE + 0x007d)        /* Full Speed EOF1 Register */
+#define LSEOF1       (USB_BASE + 0x007e)        /* Low Speed EOF1 Register */
+#define RSTINFO      (USB_BASE + 0x007f)        /* Reset Information Register */
+
+/* FIFO size register fields and available packet size values */
+#define DOUBLE_BUF  1
+#define FIFOSZ_DPB  (1 << 4)
+#define PKTSZ       0x0f
+
+#define PKTSZ_8     (1<<3)
+#define PKTSZ_16    (1<<4)
+#define PKTSZ_32    (1<<5)
+#define PKTSZ_64    (1<<6)
+#define PKTSZ_128   (1<<7)
+#define PKTSZ_256   (1<<8)
+#define PKTSZ_512   (1<<9)
+#define PKTSZ_1024  (1<<10)
+
+#define FIFOSZ_8    (0x0)
+#define FIFOSZ_16   (0x1)
+#define FIFOSZ_32   (0x2)
+#define FIFOSZ_64   (0x3)
+#define FIFOSZ_128  (0x4)
+#define FIFOSZ_256  (0x5)
+#define FIFOSZ_512  (0x6)
+#define FIFOSZ_1024 (0x7)
+#define FIFOSZ_2048 (0x8)
+#define FIFOSZ_4096 (0x9)
+#define FIFOSZ_3072 (0xF)
+
+/* SWRST fields */
+#define SWRST_PHY_RST         (1<<7)
+#define SWRST_PHYSIG_GATE_HS  (1<<6)
+#define SWRST_PHYSIG_GATE_EN  (1<<5)
+#define SWRST_REDUCE_DLY      (1<<4)
+#define SWRST_UNDO_SRPFIX     (1<<3)
+#define SWRST_FRC_VBUSVALID   (1<<2)
+#define SWRST_SWRST           (1<<1)
+#define SWRST_DISUSBRESET     (1<<0)
+
+/* DMA_CNTL */
+#define USB_DMA_CNTL_ENDMAMODE2            (1 << 13)
+#define USB_DMA_CNTL_PP_RST                (1 << 12)
+#define USB_DMA_CNTL_PP_EN                 (1 << 11)
+#define USB_DMA_BURST_MODE_MASK            (3 << 9)
+#define USB_DMA_BURST_MODE_0               (0 << 9)
+#define USB_DMA_BURST_MODE_1               (0x1 << 9)
+#define USB_DMA_BURST_MODE_2               (0x2 << 9)
+#define USB_DMA_BURST_MODE_3               (0x3 << 9)
+#define USB_DMA_BUS_ERROR                  (0x1 << 8)
+#define USB_DMA_ENDPNT_MASK                (0xf << 4)
+#define USB_DMA_ENDPNT_OFFSET              (4)
+#define USB_DMA_INTEN                      (1 << 3)
+#define USB_DMA_DMAMODE                    (1 << 2)
+#define USB_DMA_DIR                        (1 << 1)
+#define USB_DMA_EN                         (1 << 0)
+
+/* USB level 1 interrupt registers */
+
+#define USB_L1INTS (USB_BASE + 0xa0)  /* USB level 1 interrupt status register */
+#define USB_L1INTM (USB_BASE + 0xa4)  /* USB level 1 interrupt mask register  */
+#define USB_L1INTP (USB_BASE + 0xa8)  /* USB level 1 interrupt polarity register  */
+
+#define TX_INT_STATUS       (1 << 0)
+#define RX_INT_STATUS       (1 << 1)
+#define USBCOM_INT_STATUS   (1 << 2)
+#define DMA_INT_STATUS      (1 << 3)
+#define PSR_INT_STATUS      (1 << 4)
+#define QINT_STATUS     (1 << 5)
+#define QHIF_INT_STATUS     (1 << 6)
+#define DPDM_INT_STATUS     (1 << 7)
+#define VBUSVALID_INT_STATUS    (1 << 8)
+#define IDDIG_INT_STATUS    (1 << 9)
+#define DRVVBUS_INT_STATUS  (1 << 10)
+#define POWERDWN_INT_STATUS (1 << 11)
+
+#define VBUSVALID_INT_POL   (1 << 8)
+#define IDDIG_INT_POL       (1 << 9)
+#define DRVVBUS_INT_POL     (1 << 10)
+
+/* mt_usb defines */
+typedef enum {
+    EP0_IDLE = 0,
+    EP0_RX,
+    EP0_TX,
+} EP0_STATE;
+
+/* some macros */
+#define EPMASK(X)   (1 << X)
+#define CHANMASK(X) (1 << X)
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mtk_key.h b/src/bsp/lk/platform/mt2701/include/platform/mtk_key.h
new file mode 100644
index 0000000..d89f5a1
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mtk_key.h
@@ -0,0 +1,26 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#pragma once
+
+bool check_download_key(void);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mtk_nor.h b/src/bsp/lk/platform/mt2701/include/platform/mtk_nor.h
new file mode 100644
index 0000000..920c7bf
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mtk_nor.h
@@ -0,0 +1,3 @@
+#pragma once
+
+void nor_init_device(void);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mtk_serial_key.h b/src/bsp/lk/platform/mt2701/include/platform/mtk_serial_key.h
new file mode 100644
index 0000000..a01cc2a
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mtk_serial_key.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+/* serial key */
+#define SERIAL_KEY_HI    (270557508U)
+#define SERIAL_KEY_LO    (270557504U)
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mtk_timer.h b/src/bsp/lk/platform/mt2701/include/platform/mtk_timer.h
new file mode 100644
index 0000000..0ba81da
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mtk_timer.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <sys/types.h>
+#include <stdbool.h>
+#include <reg.h>
+
+void mtk_timer_init(void);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/mtk_wdt.h b/src/bsp/lk/platform/mt2701/include/platform/mtk_wdt.h
new file mode 100644
index 0000000..c72aeeb
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/mtk_wdt.h
@@ -0,0 +1,108 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+#include <platform/mt_reg_base.h>
+#include <stdbool.h>
+
+#define ENABLE_WDT_MODULE       (1) /* Module switch */
+#define LK_WDT_DISABLE          (1)
+
+#define MTK_WDT_BASE            TOP_RGU_BASE
+
+#define MTK_WDT_MODE            (MTK_WDT_BASE+0x0000)
+#define MTK_WDT_LENGTH          (MTK_WDT_BASE+0x0004)
+#define MTK_WDT_RESTART         (MTK_WDT_BASE+0x0008)
+#define MTK_WDT_STATUS          (MTK_WDT_BASE+0x000C)
+#define MTK_WDT_INTERVAL        (MTK_WDT_BASE+0x0010)
+#define MTK_WDT_SWRST           (MTK_WDT_BASE+0x0014)
+#define MTK_WDT_SWSYSRST        (MTK_WDT_BASE+0x0018)
+#define MTK_WDT_NONRST_REG      (MTK_WDT_BASE+0x0020)
+#define MTK_WDT_NONRST_REG2     (MTK_WDT_BASE+0x0024)
+#define MTK_WDT_REQ_MODE        (MTK_WDT_BASE+0x0030)
+#define MTK_WDT_REQ_IRQ_EN      (MTK_WDT_BASE+0x0034)
+#define MTK_WDT_DRAMC_CTL       (MTK_WDT_BASE+0x0040)
+
+/*WDT_MODE*/
+#define MTK_WDT_MODE_KEYMASK        (0xff00)
+#define MTK_WDT_MODE_KEY        (0x22000000)
+
+#define MTK_WDT_MODE_DUAL_MODE  (0x0040)
+#define MTK_WDT_MODE_IN_DIS     (0x0020) /* Reserved */
+#define MTK_WDT_MODE_AUTO_RESTART   (0x0010) /* Reserved */
+#define MTK_WDT_MODE_IRQ        (0x0008)
+#define MTK_WDT_MODE_EXTEN      (0x0004)
+#define MTK_WDT_MODE_EXT_POL        (0x0002)
+#define MTK_WDT_MODE_ENABLE     (0x0001)
+
+/*WDT_LENGTH*/
+#define MTK_WDT_LENGTH_TIME_OUT     (0xffe0)
+#define MTK_WDT_LENGTH_KEYMASK      (0x001f)
+#define MTK_WDT_LENGTH_KEY      (0x0008)
+
+/*WDT_RESTART*/
+#define MTK_WDT_RESTART_KEY     (0x1971)
+
+/*WDT_STATUS*/
+#define MTK_WDT_STATUS_HWWDT_RST    (0x80000000)
+#define MTK_WDT_STATUS_SWWDT_RST    (0x40000000)
+#define MTK_WDT_STATUS_IRQWDT_RST   (0x20000000)
+#define MTK_WDT_STATUS_SECURITY_RST (1<<28)
+#define MTK_WDT_STATUS_DEBUGWDT_RST (0x00080000)
+#define MTK_WDT_STATUS_THERMAL_DIRECT_RST   (1<<18)
+#define MTK_WDT_STATUS_SPMWDT_RST          (0x0002)
+#define MTK_WDT_STATUS_SPM_THERMAL_RST     (0x0001)
+
+/*WDT_INTERVAL*/
+#define MTK_WDT_INTERVAL_MASK       (0x0fff)
+
+/*WDT_SWRST*/
+#define MTK_WDT_SWRST_KEY       (0x1209)
+
+/*WDT_SWSYSRST*/
+#define MTK_WDT_SWSYS_RST_PWRAP_SPI_CTL_RST (0x0800)
+#define MTK_WDT_SWSYS_RST_APMIXED_RST   (0x0400)
+#define MTK_WDT_SWSYS_RST_MD_LITE_RST   (0x0200)
+#define MTK_WDT_SWSYS_RST_INFRA_AO_RST  (0x0100)
+#define MTK_WDT_SWSYS_RST_MD_RST    (0x0080)
+#define MTK_WDT_SWSYS_RST_DDRPHY_RST    (0x0040)
+#define MTK_WDT_SWSYS_RST_IMG_RST   (0x0020)
+#define MTK_WDT_SWSYS_RST_VDEC_RST  (0x0010)
+#define MTK_WDT_SWSYS_RST_VENC_RST  (0x0008)
+#define MTK_WDT_SWSYS_RST_MFG_RST   (0x0004)
+#define MTK_WDT_SWSYS_RST_DISP_RST  (0x0002)
+#define MTK_WDT_SWSYS_RST_INFRA_RST (0x0001)
+
+#define MTK_WDT_SWSYS_RST_KEY       (0x88000000)
+
+typedef enum wd_swsys_reset_type {
+    WD_MD_RST,
+} WD_SYS_RST_TYPE;
+
+void set_clr_fastboot_mode(bool flag);
+void set_clr_recovery_mode(bool flag);
+bool check_fastboot_mode(void);
+bool check_recovery_mode(void);
+void mtk_wdt_init(void);
+void mtk_wdt_disable(void);
+void mtk_arch_reset(char mode);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/nand.h b/src/bsp/lk/platform/mt2701/include/platform/nand.h
new file mode 100644
index 0000000..ac06633
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/nand.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef __MTK_NAND_H__
+#define __MTK_NAND_H__
+
+#include <platform/typedefs.h>
+
+
+#define MTK_COMBO_NAND_SUPPORT
+#define MTK_SLC_NAND_SUPPORT
+
+//#define TEST_ECC
+//#define REDUCE_NAND_PL_SIZE //to reduce nand preloader size
+
+#ifndef REDUCE_NAND_PL_SIZE
+u32 nand_ecc_test(void);
+#endif
+
+extern u32 nand_init_device(void);
+
+#endif                          /* __NAND_COMMON_INTER_H__ */
diff --git a/src/bsp/lk/platform/mt2701/include/platform/nand_core.h b/src/bsp/lk/platform/mt2701/include/platform/nand_core.h
new file mode 100644
index 0000000..1659a1e
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/nand_core.h
@@ -0,0 +1,737 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _MTK_NAND_CORE_H
+#define _MTK_NAND_CORE_H
+
+#include <platform/typedefs.h>
+#ifndef MTK_EMMC_SUPPORT
+#include "nand_device_list.h"
+#endif
+
+#define CHIPVER_ECO_1               (0x8a00)
+#define CHIPVER_ECO_2               (0x8a01)
+
+/**************************************************************************
+*  SIZE DEFINITION
+**************************************************************************/
+#define NFI_BUF_MAX_SIZE             (0x880)
+
+/**************************************************************************
+ * Buffer address define for NAND modules
+ *************************************************************************/
+#define MAX_MAIN_SIZE                (0x4000)
+#define MAX_SPAR_SIZE                (0x400)
+
+#define BMT_DAT_BUFFER              bmt_dat_buf
+#define BMT_DAT_BUFFER_SIZE         (MAX_MAIN_SIZE + MAX_SPAR_SIZE)
+
+#define PMT_DAT_BUFFER              pmt_dat_buf
+#define PMT_DAT_BUFFER_SIZE         (MAX_MAIN_SIZE + MAX_SPAR_SIZE)
+
+#define PMT_READ_BUFFER             pmt_read_buf
+#define PMT_READ_BUFFER_SIZE        (MAX_MAIN_SIZE)
+
+#define NAND_NFI_BUFFER             nand_nfi_buf
+#define NAND_NFI_BUFFER_SIZE        (NFI_BUF_MAX_SIZE)
+
+/******************************************************************************
+*
+* NFI & ECC Configuration
+
+*******************************************************************************/
+//#define NFI_BASE                  (0x80032000)
+//#define NFIECC_BASE               (0x80038000)
+
+/******************************************************************************
+*
+* NFI Register Definition
+
+******************************************************************************/
+
+#define NFI_CNFG_REG16              (NFI_BASE+0x0000)
+#define NFI_PAGEFMT_REG16           (NFI_BASE+0x0004)
+#define NFI_CON_REG16               (NFI_BASE+0x0008)
+#define NFI_ACCCON_REG32            (NFI_BASE+0x000C)
+#define NFI_INTR_EN_REG16           (NFI_BASE+0x0010)
+#define NFI_INTR_REG16              (NFI_BASE+0x0014)
+
+#define NFI_CMD_REG16               (NFI_BASE+0x0020)
+
+#define NFI_ADDRNOB_REG16           (NFI_BASE+0x0030)
+#define NFI_COLADDR_REG32           (NFI_BASE+0x0034)
+#define NFI_ROWADDR_REG32           (NFI_BASE+0x0038)
+
+#define NFI_STRDATA_REG16           (NFI_BASE+0x0040)
+#define NFI_CNRNB_REG16             (NFI_BASE+0x0044)
+
+#define NFI_DATAW_REG32             (NFI_BASE+0x0050)
+#define NFI_DATAR_REG32             (NFI_BASE+0x0054)
+#define NFI_PIO_DIRDY_REG16         (NFI_BASE+0x0058)
+
+#define NFI_STA_REG32               (NFI_BASE+0x0060)
+#define NFI_FIFOSTA_REG16           (NFI_BASE+0x0064)
+//#define NFI_LOCKSTA_REG16           (NFI_BASE+0x0068)
+
+#define NFI_ADDRCNTR_REG16          (NFI_BASE+0x0070)
+
+#define NFI_STRADDR_REG32           (NFI_BASE+0x0080)
+#define NFI_BYTELEN_REG16           (NFI_BASE+0x0084)
+
+#define NFI_CSEL_REG16              (NFI_BASE+0x0090)
+#define NFI_IOCON_REG16             (NFI_BASE+0x0094)
+
+#define NFI_FDM0L_REG32             (NFI_BASE+0x00A0)
+#define NFI_FDM0M_REG32             (NFI_BASE+0x00A4)
+
+#define NFI_LOCK_REG16              (NFI_BASE+0x0100)
+#define NFI_LOCKCON_REG32           (NFI_BASE+0x0104)
+#define NFI_LOCKANOB_REG16          (NFI_BASE+0x0108)
+#define NFI_LOCK00ADD_REG32         (NFI_BASE+0x0110)
+#define NFI_LOCK00FMT_REG32         (NFI_BASE+0x0114)
+#define NFI_LOCK01ADD_REG32         (NFI_BASE+0x0118)
+#define NFI_LOCK01FMT_REG32         (NFI_BASE+0x011C)
+#define NFI_LOCK02ADD_REG32         (NFI_BASE+0x0120)
+#define NFI_LOCK02FMT_REG32         (NFI_BASE+0x0124)
+#define NFI_LOCK03ADD_REG32         (NFI_BASE+0x0128)
+#define NFI_LOCK03FMT_REG32         (NFI_BASE+0x012C)
+#define NFI_LOCK04ADD_REG32         (NFI_BASE+0x0130)
+#define NFI_LOCK04FMT_REG32         (NFI_BASE+0x0134)
+#define NFI_LOCK05ADD_REG32         (NFI_BASE+0x0138)
+#define NFI_LOCK05FMT_REG32         (NFI_BASE+0x013C)
+#define NFI_LOCK06ADD_REG32         (NFI_BASE+0x0140)
+#define NFI_LOCK06FMT_REG32         (NFI_BASE+0x0144)
+#define NFI_LOCK07ADD_REG32         (NFI_BASE+0x0148)
+#define NFI_LOCK07FMT_REG32         (NFI_BASE+0x014C)
+#define NFI_LOCK08ADD_REG32         (NFI_BASE+0x0150)
+#define NFI_LOCK08FMT_REG32         (NFI_BASE+0x0154)
+#define NFI_LOCK09ADD_REG32         (NFI_BASE+0x0158)
+#define NFI_LOCK09FMT_REG32         (NFI_BASE+0x015C)
+#define NFI_LOCK10ADD_REG32         (NFI_BASE+0x0160)
+#define NFI_LOCK10FMT_REG32         (NFI_BASE+0x0164)
+#define NFI_LOCK11ADD_REG32         (NFI_BASE+0x0168)
+#define NFI_LOCK11FMT_REG32         (NFI_BASE+0x016C)
+#define NFI_LOCK12ADD_REG32         (NFI_BASE+0x0170)
+#define NFI_LOCK12FMT_REG32         (NFI_BASE+0x0174)
+#define NFI_LOCK13ADD_REG32         (NFI_BASE+0x0178)
+#define NFI_LOCK13FMT_REG32         (NFI_BASE+0x017C)
+#define NFI_LOCK14ADD_REG32         (NFI_BASE+0x0180)
+#define NFI_LOCK14FMT_REG32         (NFI_BASE+0x0184)
+#define NFI_LOCK15ADD_REG32         (NFI_BASE+0x0188)
+#define NFI_LOCK15FMT_REG32         (NFI_BASE+0x018C)
+
+#define NFI_FIFODATA0_REG32         (NFI_BASE+0x0190)
+#define NFI_FIFODATA1_REG32         (NFI_BASE+0x0194)
+#define NFI_FIFODATA2_REG32         (NFI_BASE+0x0198)
+#define NFI_FIFODATA3_REG32         (NFI_BASE+0x019C)
+
+#define NFI_MASTERSTA_REG16         (NFI_BASE+0x0224)
+#define NFI_RANDOM_CNFG_REG32 ((volatile P_U32)(NFI_BASE+0x0238))
+#define NFI_ENMPTY_THRESH_REG32 ((volatile P_U32)(NFI_BASE+0x023C))
+#define NFI_NAND_TYPE_CNFG_REG32 ((volatile P_U32)(NFI_BASE+0x0240))
+#define NFI_ACCCON1_REG3    ((volatile P_U32)(NFI_BASE+0x0244))
+#define NFI_DLYCTRL_REG32    ((volatile P_U32)(NFI_BASE+0x0248))
+
+#ifndef REDUCE_NAND_PL_SIZE
+#define NFI_RANDOM_ENSEED01_TS_REG32 ((volatile P_U32)(NFI_BASE+0x024C))
+#define NFI_RANDOM_ENSEED02_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0250))
+#define NFI_RANDOM_ENSEED03_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0254))
+#define NFI_RANDOM_ENSEED04_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0258))
+#define NFI_RANDOM_ENSEED05_TS_REG32 ((volatile P_U32)(NFI_BASE+0x025C))
+#define NFI_RANDOM_ENSEED06_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0260))
+
+#define NFI_RANDOM_DESEED01_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0264))
+#define NFI_RANDOM_DESEED02_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0268))
+#define NFI_RANDOM_DESEED03_TS_REG32 ((volatile P_U32)(NFI_BASE+0x026C))
+#define NFI_RANDOM_DESEED04_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0270))
+#define NFI_RANDOM_DESEED05_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0274))
+#define NFI_RANDOM_DESEED06_TS_REG32 ((volatile P_U32)(NFI_BASE+0x0278))
+#endif
+
+/******************************************************************************
+*
+* NFI Register Field Definition
+
+******************************************************************************/
+
+/* NFI_CNFG */
+#define CNFG_AHB                    (0x0001)
+#define CNFG_READ_EN                (0x0002)
+#define CNFG_BURST_EN               (0x0004)
+#define CNFG_BYTE_RW                (0x0040)
+#define CNFG_HW_ECC_EN              (0x0100)
+#define CNFG_AUTO_FMT_EN            (0x0200)
+#define CNFG_OP_IDLE                (0x0000)
+#define CNFG_OP_READ                (0x1000)
+#define CNFG_OP_SRD                 (0x2000)
+#define CNFG_OP_PRGM                (0x3000)
+#define CNFG_OP_ERASE               (0x4000)
+#define CNFG_OP_RESET               (0x5000)
+#define CNFG_OP_CUST                (0x6000)
+
+#define CNFG_OP_MODE_MASK           (0x7000)
+#define CNFG_OP_MODE_SHIFT          (12)
+
+/* NFI_CON */
+#define CON_FIFO_FLUSH              (0x0001)
+#define CON_NFI_RST                 (0x0002)
+#define CON_NFI_SRD                 (0x0010)
+
+#define CON_NOB_MASK                (0x00E0)
+#define CON_NOB_SHIFT               (5)
+
+#define CON_NFI_BRD                 (0x0100)
+#define CON_NFI_BWR                 (0x0200)
+
+#define CON_SEC_NUM_MASK            (0x1F000)
+#define CON_NFI_SEC_SHIFT           (12)
+
+
+/*NFI_RANDOM_CFG*/
+#define SEED_MASK             (0x7FFF)
+#define EN_SEED_SHIFT         (0x1)
+#define DE_SEED_SHIFT         (0x11)
+#define CNFG_RAN_SEC         (0x0010)
+#define CNFG_RAN_SEL         (0x0020)
+#define RAN_CNFG_ENCODE_EN (1 << 0)
+#define RAN_CNFG_DECODE_EN (1 << 16)
+#define RAN_CNFG_ENCODE_SEED(x) (((U32)(x) & SEED_MASK) << 1)
+#define RAN_CNFG_DECODE_SEED(x) (((U32)(x) & SEED_MASK) << 17)
+
+/* NFI_INTR_EN */
+#define RD_DONE_EN                  (0x0001)
+#define WR_DONE_EN                  (0x0002)
+#define RST_DONE_EN                 (0x0004)
+#define ERASE_DONE_EN               (0x0008)
+#define BSY_RTN_EN                  (0x0010)
+#define ACC_LOCK_EN                 (0x0020)
+#define AHB_DONE_EN                 (0x0040)
+#define ALL_INTR_DE                 (0x0000)
+#define ALL_INTR_EN                 (0x007F)
+
+/* NFI_INTR */
+#define RD_COMPLETE                 (0x0001)
+#define WR_COMPLETE                 (0x0002)
+#define RESET_COMPLETE              (0x0004)
+#define ERASE_COMPLETE              (0x0008)
+#define BUSY_RETURN                 (0x0010)
+#define ACCESS_LOCK_LOCK            (0x0020)
+#define AHB_DONE                    (0x0040)
+
+/* NFI_ADDRNOB */
+#define ADDR_COL_NOB_MASK           (0x0007)
+#define ADDR_COL_NOB_SHIFT          (0)
+//#define ADDR_ROW_NOB_MASK           (0x0070)
+#define ADDR_ROW_NOB_SHIFT          (4)
+
+/* NFI_FIFO */
+#define FIFO_RD_EMPTY               (0x0040)
+#define FIFO_RD_FULL                (0x0080)
+#define FIFO_WR_FULL                (0x8000)
+#define FIFO_WR_EMPTY               (0x4000)
+#define FIFO_RD_REMAIN(x)           (0x1F&(x))
+#define FIFO_WR_REMAIN(x)           ((0x1F00&(x))>>8)
+
+/* NFI_ADDRCNTR */
+#define ADDRCNTR_CNTR(x)            ((0x1F000&(x))>>12)
+#define ADDRCNTR_OFFSET(x)          (0x0FFF&(x))
+
+/* NFI_LOCK */
+#define NFI_LOCK_ON                 (0x0001)
+
+/* NFI_LOCKANOB */
+#define PROG_RADD_NOB_MASK          (0x7000)
+#define PROG_RADD_NOB_SHIFT         (12)
+#define PROG_CADD_NOB_MASK          (0x0300)
+#define PROG_CADD_NOB_SHIFT         (8)
+#define ERASE_RADD_NOB_MASK         (0x0070)
+#define ERASE_RADD_NOB_SHIFT        (4)
+#define ERASE_CADD_NOB_MASK         (0x0007)
+#define ERASE_CADD_NOB_SHIFT        (0)
+
+/* Nand flash command */
+#define RD_1ST_CMD                  0x00
+#define RANDOM_RD_1ST_CMD           0x05
+#define RD_2ND_HALF_CMD             0x01    // only for 512 bytes page-size
+#define RD_SPARE_CMD                0x50    // only for 512 bytes page-size
+#define RD_2ND_CYCLE_CMD            0x30    // only for 2K  bytes page-size
+#define RANDOM_RD_2ND_CMD           0xE0
+#define RD_FOR_COPYBACK             0x35
+#define COPY_1ST_CMD                0x85
+#define COPY_2ND_CMD                0x10
+#define COPY_PROGRAM                0x8A
+#define INPUT_DATA_CMD              0x80
+#define PLANE_INPUT_DATA_CMD        0x81
+#define RANDOM_INPUT_DATA_CMD       0x85
+#define PROG_DATA_CMD               0x10
+#define PLANE_PROG_DATA_CMD         0x11
+#define CACHE_PROG_CMD              0x15
+#define BLOCK_ERASE1_CMD            0x60
+#define BLOCK_ERASE2_CMD            0xD0
+#define RD_ID_CMD                   0x90
+#define RD_STATUS_CMD               0x70
+#define RESET_CMD                   0xFF
+
+/* NFI_PAGEFMT */
+#define PAGEFMT_512          (0x0000)
+#define PAGEFMT_2K           (0x0001)
+#define PAGEFMT_4K           (0x0002)
+#define PAGEFMT_2K_1KS       (0x0000)
+#define PAGEFMT_4K_1KS       (0x0001)
+#define PAGEFMT_8K_1KS       (0x0002)
+#define PAGEFMT_16K_1KS      (0x0003)
+
+#define PAGEFMT_PAGE_MASK           (0x0003)
+
+#define PAGEFMT_SECTOR_SEL          (0x0004)
+
+#define PAGEFMT_DBYTE_EN            (0x0008)
+
+#define PAGEFMT_SPARE_16     (0x0000)
+#define PAGEFMT_SPARE_26     (0x0001)
+#define PAGEFMT_SPARE_27     (0x0002)
+#define PAGEFMT_SPARE_28     (0x0003)
+#define PAGEFMT_SPARE_32     (0x0004)
+#define PAGEFMT_SPARE_36     (0x0005)
+#define PAGEFMT_SPARE_40     (0x0006)
+#define PAGEFMT_SPARE_44     (0x0007)
+#define PAGEFMT_SPARE_48     (0x0008)
+#define PAGEFMT_SPARE_49     (0x0009)
+#define PAGEFMT_SPARE_50     (0x000A)
+#define PAGEFMT_SPARE_51     (0x000B)
+#define PAGEFMT_SPARE_52     (0x000C)
+#define PAGEFMT_SPARE_62     (0x000D)
+#define PAGEFMT_SPARE_63     (0x000E)
+#define PAGEFMT_SPARE_64     (0x000F)
+
+#define PAGEFMT_SPARE_32_1KS (0x0000)
+#define PAGEFMT_SPARE_52_1KS (0x0001)
+#define PAGEFMT_SPARE_54_1KS (0x0002)
+#define PAGEFMT_SPARE_56_1KS (0x0003)
+#define PAGEFMT_SPARE_64_1KS (0x0004)
+#define PAGEFMT_SPARE_72_1KS (0x0005)
+#define PAGEFMT_SPARE_80_1KS (0x0006)
+#define PAGEFMT_SPARE_88_1KS (0x0007)
+#define PAGEFMT_SPARE_96_1KS (0x0008)
+#define PAGEFMT_SPARE_98_1KS (0x0009)
+#define PAGEFMT_SPARE_100_1KS (0x000A)
+#define PAGEFMT_SPARE_102_1KS (0x000B)
+#define PAGEFMT_SPARE_104_1KS (0x000C)
+#define PAGEFMT_SPARE_124_1KS (0x000D)
+#define PAGEFMT_SPARE_126_1KS (0x000E)
+#define PAGEFMT_SPARE_128_1KS (0x000F)
+
+#define PAGEFMT_SPARE_MASK   (0x00F0)
+#define PAGEFMT_SPARE_SHIFT  (4)
+
+#define PAGEFMT_FDM_MASK            (0x0F00)
+#define PAGEFMT_FDM_SHIFT           (8)
+
+#define PAGEFMT_FDM_ECC_MASK        (0xF000)
+#define PAGEFMT_FDM_ECC_SHIFT       (12)
+
+
+#define NFI_BYPASS        0x8000
+#define ECC_BYPASS        0x1
+
+#define PAD_MACRO_RST     2
+
+/******************************************************************************
+*
+* ECC Register Definition
+*
+*******************************************************************************/
+#define ECC_ENCCON_REG16    ((volatile P_U16)(NFIECC_BASE+0x0000))
+#define ECC_ENCCNFG_REG32   ((volatile P_U32)(NFIECC_BASE+0x0004))
+#define ECC_ENCDIADDR_REG32 ((volatile P_U32)(NFIECC_BASE+0x0008))
+#define ECC_ENCIDLE_REG32   ((volatile P_U32)(NFIECC_BASE+0x000C))
+#define ECC_ENCPAR0_REG32   ((volatile P_U32)(NFIECC_BASE+0x0010))
+#define ECC_ENCPAR1_REG32   ((volatile P_U32)(NFIECC_BASE+0x0014))
+#define ECC_ENCPAR2_REG32   ((volatile P_U32)(NFIECC_BASE+0x0018))
+#define ECC_ENCPAR3_REG32   ((volatile P_U32)(NFIECC_BASE+0x001C))
+#define ECC_ENCPAR4_REG32   ((volatile P_U32)(NFIECC_BASE+0x0020))
+#define ECC_ENCPAR5_REG32   ((volatile P_U32)(NFIECC_BASE+0x0024))
+#define ECC_ENCPAR6_REG32   ((volatile P_U32)(NFIECC_BASE+0x0028))
+#define ECC_ENCSTA_REG32    ((volatile P_U32)(NFIECC_BASE+0x007C))
+#define ECC_ENCIRQEN_REG16  ((volatile P_U16)(NFIECC_BASE+0x0080))
+#define ECC_ENCIRQSTA_REG16 ((volatile P_U16)(NFIECC_BASE+0x0084))
+
+#define ECC_DECCON_REG16    ((volatile P_U16)(NFIECC_BASE+0x0100))
+#define ECC_DECCNFG_REG32   ((volatile P_U32)(NFIECC_BASE+0x0104))
+#define ECC_DECDIADDR_REG32 ((volatile P_U32)(NFIECC_BASE+0x0108))
+#define ECC_DECIDLE_REG16   ((volatile P_U16)(NFIECC_BASE+0x010C))
+#define ECC_DECFER_REG16    ((volatile P_U16)(NFIECC_BASE+0x0110))
+#define ECC_DECENUM0_REG32   ((volatile P_U32)(NFIECC_BASE+0x0114))
+#define ECC_DECENUM1_REG32   ((volatile P_U32)(NFIECC_BASE+0x0118))
+#define ECC_DECDONE_REG16   ((volatile P_U16)(NFIECC_BASE+0x0124))
+#define ECC_DECEL0_REG32    ((volatile P_U32)(NFIECC_BASE+0x0128))
+#define ECC_DECEL1_REG32    ((volatile P_U32)(NFIECC_BASE+0x012C))
+#define ECC_DECEL2_REG32    ((volatile P_U32)(NFIECC_BASE+0x0130))
+#define ECC_DECEL3_REG32    ((volatile P_U32)(NFIECC_BASE+0x0134))
+#define ECC_DECEL4_REG32    ((volatile P_U32)(NFIECC_BASE+0x0138))
+#define ECC_DECEL5_REG32    ((volatile P_U32)(NFIECC_BASE+0x013C))
+#define ECC_DECEL6_REG32    ((volatile P_U32)(NFIECC_BASE+0x0140))
+#define ECC_DECEL7_REG32    ((volatile P_U32)(NFIECC_BASE+0x0144))
+#define ECC_DECIRQEN_REG16  ((volatile P_U16)(NFIECC_BASE+0x0200))
+#define ECC_DECIRQSTA_REG16 ((volatile P_U16)(NFIECC_BASE+0x0204))
+//#define ECC_FDMADDR_REG32   ((volatile P_U32)(NFIECC_BASE+0x0148))
+#define ECC_DECFSM_REG32    ((volatile P_U32)(NFIECC_BASE+0x0208))
+#define ECC_BYPASS_REG32    ((volatile P_U32)(NFIECC_BASE+0x020C))
+
+//#define ECC_SYNSTA_REG32    ((volatile P_U32)(NFIECC_BASE+0x0150))
+//#define ECC_DECNFIDI_REG32  ((volatile P_U32)(NFIECC_BASE+0x0154))
+//#define ECC_SYN0_REG32      ((volatile P_U32)(NFIECC_BASE+0x0158))
+
+/******************************************************************************
+*
+* ECC register definition
+*
+*******************************************************************************
+*/
+/* ECC_ENCON */
+#define ECC_PARITY_BIT          (14)
+
+#define ENC_EN                  (0x0001)
+#define ENC_DE                  (0x0000)
+
+/* ECC_ENCCNFG */
+#define ECC_CNFG_ECC4          (0x0000)
+#define ECC_CNFG_ECC6          (0x0001)
+#define ECC_CNFG_ECC8          (0x0002)
+#define ECC_CNFG_ECC10         (0x0003)
+#define ECC_CNFG_ECC12         (0x0004)
+#define ECC_CNFG_ECC14         (0x0005)
+#define ECC_CNFG_ECC16         (0x0006)
+#define ECC_CNFG_ECC18         (0x0007)
+#define ECC_CNFG_ECC20         (0x0008)
+#define ECC_CNFG_ECC22         (0x0009)
+#define ECC_CNFG_ECC24         (0x000A)
+#define ECC_CNFG_ECC28         (0x000B)
+#define ECC_CNFG_ECC32         (0x000C)
+#define ECC_CNFG_ECC36         (0x000D)
+#define ECC_CNFG_ECC40         (0x000E)
+#define ECC_CNFG_ECC44         (0x000F)
+#define ECC_CNFG_ECC48         (0x0010)
+#define ECC_CNFG_ECC52         (0x0011)
+#define ECC_CNFG_ECC56         (0x0012)
+#define ECC_CNFG_ECC60         (0x0013)
+
+#define ECC_CNFG_ECC_MASK       (0x0000001F)
+
+#define ENC_CNFG_NFI            (0x0020)
+#define ENC_CNFG_MODE_MASK      (0x0060)
+
+#define ENC_CNFG_META6          (0x10300000)
+#define ENC_CNFG_META8          (0x10400000)
+
+#define ENC_CNFG_MSG_MASK   (0x3FFF0000)
+#define ENC_CNFG_MSG_SHIFT      (0x10)
+
+/* ECC_ENCIDLE */
+#define ENC_IDLE                (0x0001)
+
+/* ECC_ENCSTA */
+#define STA_FSM             (0x0007)
+#define STA_COUNT_PS            (0xFF10)
+#define STA_COUNT_MS            (0x3FFF0000)
+
+/* ECC_ENCIRQEN */
+#define ENC_IRQEN               (0x0001)
+
+/* ECC_ENCIRQSTA */
+#define ENC_IRQSTA              (0x0001)
+
+/* ECC_DECCON */
+#define DEC_EN                  (0x0001)
+#define DEC_DE                  (0x0000)
+
+/* ECC_ENCCNFG */
+#define DEC_CNFG_ECC4          (0x0000)
+#define DEC_CNFG_ECC6          (0x0001)
+#define DEC_CNFG_ECC12         (0x0002)
+#define DEC_CNFG_NFI           (0x0020)
+//#define DEC_CNFG_META6         (0x10300000)
+//#define DEC_CNFG_META8         (0x10400000)
+
+#define DEC_CNFG_FER           (0x01000)
+#define DEC_CNFG_EL            (0x02000)
+#define DEC_CNFG_CORRECT       (0x03000)
+#define DEC_CNFG_TYPE_MASK     (0x03000)
+
+#define DEC_CNFG_EMPTY_EN      (0x80000000)
+
+#define DEC_CNFG_CODE_MASK     (0x3FFF0000)
+#define DEC_CNFG_CODE_SHIFT    (0x10)
+
+/* ECC_DECIDLE */
+#define DEC_IDLE                (0x0001)
+
+/* ECC_DECFSM */
+#define ECC_DECFSM_IDLE         (0x01010101)
+
+
+/* ECC_DECFER */
+#define DEC_FER0               (0x0001)
+#define DEC_FER1               (0x0002)
+#define DEC_FER2               (0x0004)
+#define DEC_FER3               (0x0008)
+#define DEC_FER4               (0x0010)
+#define DEC_FER5               (0x0020)
+#define DEC_FER6               (0x0040)
+#define DEC_FER7               (0x0080)
+
+/* ECC_DECENUM */
+#define ERR_NUM0               (0x0000003F)
+#define ERR_NUM1               (0x00003F00)
+#define ERR_NUM2               (0x003F0000)
+#define ERR_NUM3               (0x3F000000)
+#define ERR_NUM4               (0x0000003F)
+#define ERR_NUM5               (0x00003F00)
+#define ERR_NUM6               (0x003F0000)
+#define ERR_NUM7               (0x3F000000)
+
+/* ECC_DECDONE */
+#define DEC_DONE0               (0x0001)
+#define DEC_DONE1               (0x0002)
+#define DEC_DONE2               (0x0004)
+#define DEC_DONE3               (0x0008)
+#define DEC_DONE4               (0x0010)
+#define DEC_DONE5               (0x0020)
+#define DEC_DONE6               (0x0040)
+#define DEC_DONE7               (0x0080)
+
+/* ECC_DECIRQEN */
+#define DEC_IRQEN               (0x0001)
+
+/* ECC_DECIRQSTA */
+#define DEC_IRQSTA              (0x0001)
+
+/******************************************************************************
+*
+* NFI Register Field Definition
+*
+*******************************************************************************/
+
+/* NFI_ACCCON */
+#define ACCCON_SETTING       ()
+
+/* NFI_INTR_EN */
+#define INTR_RD_DONE_EN      (0x0001)
+#define INTR_WR_DONE_EN      (0x0002)
+#define INTR_RST_DONE_EN     (0x0004)
+#define INTR_ERASE_DONE_EN   (0x0008)
+#define INTR_BSY_RTN_EN      (0x0010)
+#define INTR_ACC_LOCK_EN     (0x0020)
+#define INTR_AHB_DONE_EN     (0x0040)
+#define INTR_ALL_INTR_DE     (0x0000)
+#define INTR_ALL_INTR_EN     (0x007F)
+
+/* NFI_INTR */
+#define INTR_RD_DONE         (0x0001)
+#define INTR_WR_DONE         (0x0002)
+#define INTR_RST_DONE        (0x0004)
+#define INTR_ERASE_DONE      (0x0008)
+#define INTR_BSY_RTN         (0x0010)
+#define INTR_ACC_LOCK        (0x0020)
+#define INTR_AHB_DONE        (0x0040)
+
+/* NFI_ADDRNOB */
+//#define ADDR_COL_NOB_MASK    (0x0003)
+#define ADDR_COL_NOB_SHIFT   (0)
+//#define ADDR_ROW_NOB_MASK    (0x0030)
+#define ADDR_ROW_NOB_SHIFT   (4)
+
+/* NFI_STA */
+#define STA_READ_EMPTY       (0x00001000)
+#define STA_ACC_LOCK         (0x00000010)
+#define STA_CMD_STATE        (0x00000001)
+#define STA_ADDR_STATE       (0x00000002)
+#define STA_DATAR_STATE      (0x00000004)
+#define STA_DATAW_STATE      (0x00000008)
+#define STA_FLASH_MACRO_IDLE (0x00000020)
+
+#define STA_NAND_FSM_MASK    (0x1F000000)
+#define STA_NAND_BUSY        (0x00000100)
+#define STA_NAND_BUSY_RETURN (0x00000200)
+#define STA_NFI_FSM_MASK     (0x000F0000)
+#define STA_NFI_OP_MASK      (0x0000000F)
+
+
+/******************************************************************************
+*
+* MTD command
+*
+******************************************************************************/
+#define NAND_CMD_READ0              0
+#define NAND_CMD_READ1              1
+#define NAND_CMD_PAGEPROG           0x10
+#define NAND_CMD_READOOB            0x50
+#define NAND_CMD_ERASE1             0x60
+#define NAND_CMD_STATUS             0x70
+#define NAND_CMD_SEQIN              0x80
+#define NAND_CMD_READID             0x90
+#define NAND_CMD_ERASE2             0xd0
+#define NAND_CMD_RESET              0xff
+#define NAND_CMD_READSTART          0x30
+#define NAND_CMD_CACHEDPROG         0x15
+
+/* Options */
+
+#define NAND_NO_AUTOINCR    0x00000001
+
+#define NAND_BUSW_16        0x00000002
+
+#define NAND_NO_PADDING     0x00000004
+
+#define NAND_CACHEPROGRAM   0x00000008
+
+#define NAND_COPYBACK       0x00000010
+
+#define NAND_IS_AND         0x00000020
+
+#define NAND_4PAGE_ARRAY    0x00000040
+
+#define NAND_SS_LOWPOWER_OPTIONS \
+    (NAND_NO_PADDING | NAND_CACHEPROGRAM | NAND_COPYBACK)
+
+//#define _DEBUG_
+
+/* Debug message event */
+#define DBG_EVT_NONE        0x00000000  /* No event */
+#define DBG_EVT_ERR         0x00000001  /* DMA related event */
+#define DBG_EVT_CMD         0x00000002  /* NAND CMD related event */
+#define DBG_EVT_BAD         0x00000004  /* NAND BAD BLOCK handling event */
+#define DBG_EVT_INIT        0x00000008  /* NAND INT event */
+#define DBG_EVT_READ        0x00000010  /* NAND READ event */
+#define DBG_EVT_WRITE       0x00000020  /* NAND WRITE event */
+#define DBG_EVT_ERASE       0x00000040  /* NAND ERASE event */
+
+#define DBG_EVT_ALL         0xffffffff
+
+#define DBG_EVT_MASK       (DBG_EVT_INIT | DBG_EVT_BAD )
+
+#ifdef _DEBUG_
+#define MSG(evt, fmt, args...) \
+    do {    \
+    if ((DBG_EVT_##evt) & DBG_EVT_MASK) { \
+    dprintf(fmt, ##args); \
+    } \
+    } while(0)
+
+#define MSG_FUNC_ENTRY(f)   MSG(FUC, "<FUN_ENT>: %s\n", __FUNCTION__)
+#else
+#define MSG(evt, fmt, args...) do{}while(0)
+#define MSG_FUNC_ENTRY(f)      do{}while(0)
+#define dbg_print(a,...)
+#endif
+
+struct nand_chip {
+    u32 page_shift;
+    u32 page_size;
+    char ChipID;                /* Type of DiskOnChip */
+    char *chips_name;
+    u64 chipsize;
+    unsigned long erasesize;
+    unsigned long mfr;          /* Flash IDs - only one type of flash per device */
+    unsigned long id;
+    char *name;
+    int numchips;
+    int oobblock;               /* Size of OOB blocks (e.g. 512) */
+    int oobsize;                /* Amount of OOB data per block (e.g. 16) */
+    int eccsize;
+    int bus16;
+    int nand_ecc_mode;
+    u32 sector_size;
+    u32 sector_shift;
+    u32 phys_erase_shift;
+};
+
+struct nand_flash_device {
+    char *name;
+    int nand_id;
+    unsigned long page_size;
+    unsigned long chip_size;
+    unsigned long erase_size;
+    unsigned long options;
+};
+
+/**
+* struct nand_manufacturers - NAND Flash Manufacturer ID Structure
+* @name:    Manufacturer name
+* @id:      manufacturer ID code of device.
+*/
+struct nand_manufacturers {
+    int id;
+    char *name;
+};
+/*
+struct nand_oobinfo
+{
+    u32 useecc;
+    u32 eccbytes;
+    u32 oobfree[8][2];
+    u32 eccpos[32];
+};
+*/
+
+struct nand_oobfree {
+    u32 offset;
+    u32 length;
+};
+
+#define MTD_MAX_OOBFREE_ENTRIES 16
+
+struct nand_ecclayout {
+    u32 eccbytes;
+    u32 eccpos[64];
+    u32 oobavail;
+    struct nand_oobfree oobfree[MTD_MAX_OOBFREE_ENTRIES];
+};
+
+/* ECC byte placement */
+#define MTD_NANDECC_OFF         0   /* Switch off ECC (Not recommended) */
+#define MTD_NANDECC_PLACE       1   /* Use the given placement in the structure (YAFFS1 legacy mode) */
+#define MTD_NANDECC_AUTOPLACE   2   /* Use the default placement scheme */
+#define MTD_NANDECC_PLACEONLY   3   /* Use the given placement in the structure (Do not store ecc result on read) */
+#define MTD_NANDECC_AUTOPL_USR  4   /* Use the given autoplacement scheme rather than using the default */
+
+/*
+* NAND Flash Manufacturer ID Codes
+*/
+#define NAND_MANFR_TOSHIBA  0x98
+#define NAND_MANFR_SAMSUNG  0xec
+#define NAND_MANFR_FUJITSU  0x04
+#define NAND_MANFR_NATIONAL 0x8f
+#define NAND_MANFR_RENESAS  0x07
+#define NAND_MANFR_STMICRO  0x20
+#define NAND_MANFR_HYNIX    0xad
+#define NAND_MANFR_MICRON   0x2c
+#define NAND_MANFR_AMD      0x01
+
+/******************************************************************************
+*
+* NAND APIs
+*
+******************************************************************************/
+//extern void nand_device_init(void);
+#endif
diff --git a/src/bsp/lk/platform/mt2701/include/platform/nand_device_list.h b/src/bsp/lk/platform/mt2701/include/platform/nand_device_list.h
new file mode 100644
index 0000000..da32023
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/nand_device_list.h
@@ -0,0 +1,134 @@
+
+#ifndef __NAND_DEVICE_LIST_H__
+#define __NAND_DEVICE_LIST_H__
+
+#define NAND_ABTC_ATAG
+#define ATAG_FLASH_NUMBER_INFO       0x54430006
+#define ATAG_FLASH_INFO       0x54430007
+
+#define MAX_FLASH 20 //modify this define if device list is more than 20 later .xiaolei
+#define NAND_MAX_ID     6
+#define CHIP_CNT        20
+#define P_SIZE      16384
+#define P_PER_BLK       256
+#define C_SIZE      8192
+#define RAMDOM_READ     (1<<0)
+#define CACHE_READ      (1<<1)
+#define RAND_TYPE_SAMSUNG 0
+#define RAND_TYPE_TOSHIBA 1
+#define RAND_TYPE_NONE 2
+
+#define READ_RETRY_MAX 10
+struct gFeature {
+    u32 address;
+    u32 feature;
+};
+
+enum readRetryType {
+    RTYPE_MICRON,
+    RTYPE_SANDISK,
+    RTYPE_SANDISK_19NM,
+    RTYPE_TOSHIBA,
+    RTYPE_HYNIX,
+    RTYPE_HYNIX_16NM
+};
+
+struct gFeatureSet {
+    u8 sfeatureCmd;
+    u8 gfeatureCmd;
+    u8 readRetryPreCmd;
+    u8 readRetryCnt;
+    u32 readRetryAddress;
+    u32 readRetryDefault;
+    u32 readRetryStart;
+    enum readRetryType rtype;
+    struct gFeature Interface;
+    struct gFeature Async_timing;
+};
+
+struct gRandConfig {
+    u8 type;
+    u32 seed[6];
+};
+
+enum pptbl {
+    MICRON_8K,
+    HYNIX_8K,
+    SANDISK_16K,
+};
+
+struct MLC_feature_set {
+    enum pptbl ptbl_idx;
+    struct gFeatureSet   FeatureSet;
+    struct gRandConfig   randConfig;
+};
+
+enum flashdev_vendor {
+    VEND_SAMSUNG,
+    VEND_MICRON,
+    VEND_TOSHIBA,
+    VEND_HYNIX,
+    VEND_SANDISK,
+    VEND_BIWIN,
+    VEND_SPANSION,
+    VEND_MXIC,
+    VEND_WINBOND,
+    VEND_NONE,
+};
+
+enum flashdev_IOWidth {
+    IO_8BIT = 8,
+    IO_16BIT = 16,
+    IO_TOGGLEDDR = 9,
+    IO_TOGGLESDR = 10,
+    IO_ONFI = 12,
+};
+
+typedef struct {
+    u8 id[NAND_MAX_ID];
+    u8 id_length;
+    u8 addr_cycle;
+    enum flashdev_IOWidth iowidth;
+    u16 totalsize;
+    u16 blocksize;
+    u16 pagesize;
+    u16 sparesize;
+    u32 timmingsetting;
+    u32 s_acccon;
+    u32 s_acccon1;
+    u32 freq;
+    enum flashdev_vendor vendor;
+    u16 sectorsize;
+    u8 devciename[30];
+    u32 advancedmode;
+    struct MLC_feature_set feature_set;
+} flashdev_info,*pflashdev_info;
+
+static const flashdev_info gen_FlashTable[]= {
+    {{0xEF,0xF1,0x00,0x95,0x00, 0x00}, 5,5,IO_8BIT,128,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_WINBOND,1024, "W29N01HV",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0xEF,0xF1,0x80,0x95,0x00, 0x00}, 5,5,IO_8BIT,128,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_WINBOND,1024, "W29N01GV",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0xEF,0xDA,0x90,0x95,0x04, 0x00}, 5,5,IO_8BIT,256,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_WINBOND,1024, "W29N02GV",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0xEF,0xDC,0x90,0x95,0x54, 0x00}, 5,5,IO_8BIT,512,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_WINBOND,1024, "W29N04GV",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0xC2,0xF1,0x80,0x95,0x02,0x00}, 5,4,IO_8BIT,128,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_MXIC,1024, "MX30LF1G18AC",0 ,{SANDISK_16K, NULL,NULL}},
+
+    {{0xC2,0xDA,0x90,0x95,0x06,0x00}, 5,5,IO_8BIT,256,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_MXIC,1024, "MX30LF2G18AC",0 , {SANDISK_16K, NULL, NULL}},
+
+    {{0xC2,0xDC,0x90,0x95,0x56,0x00}, 5,5,IO_8BIT,512,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_MXIC,1024, "MX30LF4G18AC",0 , {SANDISK_16K, NULL, NULL}},
+
+    {{0xC2,0xD3,0xD1,0x95,0x5A,0x00}, 5,5,IO_8BIT,1024,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_MXIC,1024, "MX60LF8G18AC",0 , {SANDISK_16K, NULL, NULL}},
+
+    {{0x1,0xF1,0x80,0x1D,0x01,0xF1}, 5,5,IO_8BIT,128,128,2048,64,0x30c77fff, 0xC03222,0x101,80,VEND_SPANSION,1024, "S34ML01G200TFI",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0x1,0xDA,0x90,0x95,0x46,0x00}, 5,5,IO_8BIT,256,128,2048,128,0x30c77fff, 0xC03222,0x101,80,VEND_SPANSION,1024, "S34ML02G200TFI",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0x1,0xDC,0x90,0x95,0x56,0x00}, 5,5,IO_8BIT,512,128,2048,128,0x30c77fff, 0xC03222,0x101,80,VEND_SPANSION,1024, "S34ML04G200TFI",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0x1,0xD3,0xD1,0x95,0x5a,0x00}, 5,5,IO_8BIT,1024,128,2048,128,0x30c77fff, 0xC03222,0x101,80,VEND_SPANSION,1024, "S34ML08G201TFI",0 ,{SANDISK_16K, NULL, NULL}},
+
+    {{0x2C,0x38,0x00,0x26,0x85,0x00}, 5,5,IO_8BIT,1024,512,4096,224,0x30c77fff, 0xC03222,0x101,80,VEND_SPANSION,1024, "MT29F8G08ABABA",0 ,{SANDISK_16K, NULL, NULL}},
+};
+#endif
diff --git a/src/bsp/lk/platform/mt2701/include/platform/pll.h b/src/bsp/lk/platform/mt2701/include/platform/pll.h
new file mode 100644
index 0000000..d5b97f8
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/pll.h
@@ -0,0 +1,254 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+#pragma once
+
+/* APMIXEDSYS Register */
+#define AP_PLL_CON0             (0x10209000)
+#define AP_PLL_CON1             (0x10209004)
+#define AP_PLL_CON2             (0x10209008)
+#define AP_PLL_CON3             (0x1020900C)
+#define AP_PLL_CON4             (0x10209010)
+#define PLL_HP_CON0             (0x10209014)
+#define CLKSQ_STB_CON0          (0x10209018)
+#define PLL_ISO_CON0            (0x10209024)
+#define PLL_TEST_CON0           (0x10209038)
+#define ARMPLL_CON0             (0x10209200)
+#define ARMPLL_CON1             (0x10209204)
+#define ARMPLL_PWR_CON0         (0x1020920C)
+#define MAINPLL_CON0            (0x10209210)
+#define MAINPLL_CON1            (0x10209214)
+#define MAINPLL_PWR_CON0        (0x1020921C)
+#define UNIVPLL_CON0            (0x10209220)
+#define UNIVPLL_CON1            (0x10209224)
+#define UNIVPLL_PWR_CON0        (0x1020922C)
+#define MMPLL_CON0              (0x10209230)
+#define MMPLL_CON1              (0x10209234)
+#define MMPLL_PWR_CON0          (0x1020923C)
+#define MSDCPLL_CON0            (0x10209240)
+#define MSDCPLL_CON1            (0x10209244)
+#define MSDCPLL_PWR_CON0        (0x1020924C)
+#define TVDPLL_CON0             (0x10209250)
+#define TVDPLL_CON1             (0x10209254)
+#define TVDPLL_CON2             (0x10209258)
+#define TVDPLL_PWR_CON0         (0x1020925C)
+#define TVDPLL_SSC_CON0         (0x10209260)
+#define TVDPLL_SSC_CON1         (0x10209264)
+#define TVDPLL_SSC_CON2         (0x10209268)
+#define AUD1PLL_CON0            (0x10209270)
+#define AUD1PLL_CON1            (0x10209274)
+#define AUD1PLL_CON2            (0x10209278)
+#define AUD1PLL_PWR_CON0        (0x1020927C)
+#define TRGPLL_CON0             (0x10209280)
+#define TRGPLL_CON1             (0x10209284)
+#define TRGPLL_CON2             (0x10209288)
+#define TRGPLL_PWR_CON0         (0x1020928C)
+#define ETHPLL_CON0             (0x10209290)
+#define ETHPLL_CON1             (0x10209294)
+#define ETHPLL_CON2             (0x10209298)
+#define ETHPLL_PWR_CON0         (0x1020929C)
+#define VDECPLL_CON0            (0x102092A0)
+#define VDECPLL_CON1            (0x102092A4)
+#define VDECPLL_CON2            (0x102092A8)
+#define VDECPLL_PWR_CON0        (0x102092AC)
+#define HADDS2PLL_CON0          (0x102092B0)
+#define HADDS2PLL_CON1          (0x102092B4)
+#define HADDS2PLL_CON2          (0x102092B8)
+#define HADDS2PLL_PWR_CON0      (0x102092BC)
+#define AUD2PLL_CON0            (0x102092C0)
+#define AUD2PLL_CON1            (0x102092C4)
+#define AUD2PLL_CON2            (0x102092C8)
+#define AUD2PLL_PWR_CON0        (0x102092CC)
+#define TVD2PLL_CON0            (0x102092D0)
+#define TVD2PLL_CON1            (0x102092D4)
+#define TVD2PLL_CON2            (0x102092D8)
+#define TVD2PLL_PWR_CON0        (0x102092DC)
+#define TVD2PLL_SSC_CON0        (0x102092F0)
+#define TVD2PLL_SSC_CON1        (0x102092F4)
+#define TVD2PLL_SSC_CON2        (0x102092F8)
+#define AP_AUXADC_CON0          (0x10209400)
+#define AP_AUXADC_CON1          (0x10209404)
+#define TS_CON0                 (0x10209600)
+#define TS_CON1                 (0x10209604)
+//#define AP_ABIST_MON_CON0       (0x10209E00)
+//#define AP_ABIST_MON_CON1       (0x10209E04)
+//#define AP_ABIST_MON_CON2       (0x10209E08)
+//#define AP_ABIST_MON_CON3       (0x10209E0C)
+//#define AP_ACIF_WR_PATH_CON0    (0x10209E10)
+//#define AP_ACIF_WR_PATH_CON1    (0x10209E14)
+//#define CLKDIV_CON0             (0x10209E1C)
+
+#define VENCPLL_CON0            (0x1000F800)
+#define VENCPLL_CON1            (0x1000F804)
+#define VENCPLL_PWR_CON0        (0x1000F80C)
+
+/* TOPCKGEN Register */
+#define CLK_MODE                (0x10000000)
+#define DCM_CFG                 (0x10000004)
+#define TST_SEL_0               (0x10000020)
+#define TST_SEL_1               (0x10000024)
+#define TST_SEL_2               (0x10000028)
+#define CLK_CFG_0               (0x10000040)
+#define CLK_CFG_1               (0x10000050)
+#define CLK_CFG_2               (0x10000060)
+#define CLK_CFG_3               (0x10000070)
+#define CLK_CFG_4               (0x10000080)
+#define CLK_CFG_5               (0x10000090)
+#define CLK_CFG_6               (0x100000A0)
+#define CLK_CFG_7               (0x100000B0)
+#define CLK_CFG_8               (0x10000100)
+#define CLK_CFG_9               (0x10000104)
+#define CLK_CFG_10              (0x10000108)
+#define CLK_CFG_11              (0x1000010C)
+#define CLK_CFG_12              (0x100000C0)
+#define CLK_CFG_13              (0x100000D0)
+#define CLK_CFG_14              (0x100000E0)
+#define CLK_CFG_15              (0x100000F0)
+#define CLK_SCP_CFG_0           (0x10000200)
+#define CLK_SCP_CFG_1           (0x10000204)
+#define CLK_MISC_CFG_0          (0x10000210)
+#define CLK_MISC_CFG_1          (0x10000214)
+#define CLK26CALI_0             (0x10000220)
+#define CLK26CALI_1             (0x10000224)
+#define CLK26CALI_2             (0x10000228)
+#define CKSTA_REG               (0x1000022C)
+#define TEST_MODE_CFG           (0x10000230)
+#define MBIST_CFG_0             (0x10000308)
+#define MBIST_CFG_1             (0x1000030C)
+#define MBIST_CFG_2             (0x10000310)
+#define MBIST_CFG_3             (0x10000314)
+
+/* INFRASYS Register */
+#define TOP_CKMUXSEL            (0x10001000)
+#define TOP_CKDIV1              (0x10001008)
+#define TOP_DCMCTL              (0x10001010)
+#define TOP_DCMDBC              (0x10001014)
+
+/* MCUSS Register */
+#define ACLKEN_DIV              (0x10200060)
+
+/* DISP Register*/
+#define DISP_CG_CON0        (0x14000100)
+#define DISP_CG_SET0        (0x14000104)
+#define DISP_CG_CLR0        (0x14000108)
+#define DISP_CG_CON1        (0x14000110)
+#define DISP_CG_SET1        (0x14000114)
+#define DISP_CG_CLR1        (0x14000118)
+
+/* APB Module smi_larb */
+#define SMI_LARB0_BASE (0x14010000)
+#define SMI_LARB0_STAT  ((volatile unsigned int *)(SMI_LARB0_BASE+0x000))
+#define SMI_LARB0_OSTD_CTRL_EN  ((volatile unsigned int *)(SMI_LARB0_BASE+0x064))
+/*****************************Mempll*************************************/
+enum {
+    DDR533   = 533,
+    DDR667   = 667,
+    DDR800   = 800,
+    DDR1066  = 1066,
+    DDR1333  = 1333,
+    DDR1466  = 1466,
+    DDR1600  = 1600
+};
+
+enum {
+    PLL_MODE_1  = 1,
+    PLL_MODE_2  = 2,
+    PLL_MODE_3  = 3,
+};
+
+/*
+ * mempll init function
+ * you can change dram data rate by this function in emi.c as below:
+ * mt_mempll_init(DDR533, DDR_PLL_MODE); <--- data rate is 533Mbps
+ * mt_mempll_init(DDR1333, DDR_PLL_MODE); <--- data rate is 1333Mbps
+ * DDRXXX can be DDR533/DDR667/DDR800/DDR1066/DDR1333
+ */
+void mt_mempll_init(int freq, int mode);
+void mt_pll_init(void);
+
+/*For improve MEMPLL cali*/
+//#define MEMPLL_CALI_V1_0
+
+/********************************************
+  * Async or Sync mode,
+  * if use Async mode, need to cali DATLAT after exit self-refresh
+  * Now, we always use sync mode
+*********************************************/
+//#define DRAMC_ASYNC
+
+/********************************************
+  * PLL mode select
+*********************************************/
+#define DDRPHY_3PLL_MODE
+#ifdef DDRPHY_3PLL_MODE
+#define DDRPHY_2PLL
+#define PHYSYNC_MODE
+#endif
+
+#if defined (DDRPHY_3PLL_MODE)
+#ifdef DDRPHY_2PLL
+#define DDR_PLL_MODE PLL_MODE_2
+#else
+#define DDR_PLL_MODE PLL_MODE_3
+#endif
+#else
+#define DDR_PLL_MODE PLL_MODE_1
+#endif
+
+/******************************************************************/
+
+/* for MTCMOS */
+#define STA_POWER_DOWN  0
+#define STA_POWER_ON    1
+
+#define VDE_PWR_STA_MASK    (0x1 << 7)
+#define IFR_PWR_STA_MASK    (0x1 << 6)
+#define ISP_PWR_STA_MASK    (0x1 << 5)
+#define DIS_PWR_STA_MASK    (0x1 << 3)
+#define MFG_PWR_STA_MASK    (0x1 << 4)
+#define DPY_PWR_STA_MASK    (0x1 << 2)
+#define CONN_PWR_STA_MASK    (0x1 << 1)
+#define MD1_PWR_STA_MASK    (0x1 << 0)
+
+#define PWR_RST_B           (0x1 << 0)
+#define PWR_ISO             (0x1 << 1)
+#define PWR_ON              (0x1 << 2)
+#define PWR_ON_S            (0x1 << 3)
+#define PWR_CLK_DIS         (0x1 << 4)
+
+#define SRAM_PDN            (0xf << 8)
+
+#define MD_SRAM_PDN         (0x1 << 8)
+
+#define VDE_SRAM_ACK        (0x1 << 12)
+#define IFR_SRAM_ACK        (0xf << 12)
+#define ISP_SRAM_ACK        (0x3 << 12)
+#define DIS_SRAM_ACK        (0xf << 12)
+#define MFG_SRAM_ACK        (0x1 << 12)
+
+#define TOPAXI_PROT_EN      (INFRACFG_BASE + 0x0220)
+#define TOPAXI_PROT_STA1    (INFRACFG_BASE + 0x0228)
+
+#define MD1_PROT_MASK   0x00B8
+#define CONN_PROT_MASK   0x0104
+
+int spm_mtcmos_ctrl_disp(int state);
diff --git a/src/bsp/lk/platform/mt2701/include/platform/spm.h b/src/bsp/lk/platform/mt2701/include/platform/spm.h
new file mode 100644
index 0000000..3922096
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/spm.h
@@ -0,0 +1,193 @@
+#pragma once
+
+#include "mt2701.h"
+#include <reg.h>
+#include <arch/arm.h>
+
+/*
+ * for SPM register control
+ */
+#define SPM_POWERON_CONFIG_SET      (SPM_BASE + 0x0000)
+#define SPM_POWER_ON_VAL0           (SPM_BASE + 0x0010)
+#define SPM_POWER_ON_VAL1           (SPM_BASE + 0x0014)
+#define SPM_CLK_SETTLE              (SPM_BASE + 0x0100)
+#define SPM_FC0_PWR_CON             (SPM_BASE + 0x0200)
+#define SPM_CPU_PWR_CON             (SPM_BASE + 0x0208)
+#define SPM_VDE_PWR_CON             (SPM_BASE + 0x0210)
+#define SPM_MFG_PWR_CON             (SPM_BASE + 0x0214)
+#define SPM_FC1_PWR_CON             (SPM_BASE + 0x0218)
+#define SPM_FC2_PWR_CON             (SPM_BASE + 0x021c)
+#define SPM_FC3_PWR_CON             (SPM_BASE + 0x0220)
+#define SPM_IFR_PWR_CON             (SPM_BASE + 0x0234)
+#define SPM_ISP_PWR_CON             (SPM_BASE + 0x0238)
+#define SPM_DIS_PWR_CON             (SPM_BASE + 0x023c)
+#define SPM_DPY_PWR_CON             (SPM_BASE + 0x0240)
+#define SPM_CPU_L2_DAT_PDN          (SPM_BASE + 0x0244)
+#define SPM_CPU_L2_DAT_SLEEP_B      (SPM_BASE + 0x0248)
+#define SPM_MP_CORE0_AUX            (SPM_BASE + 0x024c)
+#define SPM_MP_CORE1_AUX            (SPM_BASE + 0x0250)
+#define SPM_MP_CORE2_AUX            (SPM_BASE + 0x0254)
+#define SPM_MP_CORE3_AUX            (SPM_BASE + 0x0258)
+#define SPM_CPU_FC0_L1_PDN          (SPM_BASE + 0x025c)
+#define SPM_CPU_FC1_L1_PDN          (SPM_BASE + 0x0264)
+#define SPM_CPU_FC2_L1_PDN          (SPM_BASE + 0x026c)
+#define SPM_CPU_FC3_L1_PDN          (SPM_BASE + 0x0274)
+#define SPM_IFR_FH_SRAM_CTRL        (SPM_BASE + 0x027c)
+#define SPM_CONN_PWR_CON            (SPM_BASE + 0x0280)
+#define SPM_MD_PWR_CON              (SPM_BASE + 0x0284)
+#define SPM_MCU_PWR_CON             (SPM_BASE + 0x0290)
+#define SPM_IFR_SRAMROM_CON         (SPM_BASE + 0x0294)
+#define SPM_PCM_CON0                (SPM_BASE + 0x0310)
+#define SPM_PCM_CON1                (SPM_BASE + 0x0314)
+#define SPM_PCM_IM_PTR              (SPM_BASE + 0x0318)
+#define SPM_PCM_IM_LEN              (SPM_BASE + 0x031c)
+#define SPM_PCM_REG_DATA_INI        (SPM_BASE + 0x0320)
+#define SPM_PCM_EVENT_VECTOR0       (SPM_BASE + 0x0340)
+#define SPM_PCM_EVENT_VECTOR1       (SPM_BASE + 0x0344)
+#define SPM_PCM_EVENT_VECTOR2       (SPM_BASE + 0x0348)
+#define SPM_PCM_EVENT_VECTOR3       (SPM_BASE + 0x034c)
+#define SPM_PCM_MAS_PAUSE_MASK      (SPM_BASE + 0x0354)
+#define SPM_PCM_PWR_IO_EN           (SPM_BASE + 0x0358)
+#define SPM_PCM_TIMER_VAL           (SPM_BASE + 0x035c)
+#define SPM_PCM_TIMER_OUT           (SPM_BASE + 0x0360)
+#define SPM_PCM_REG0_DATA           (SPM_BASE + 0x0380)
+#define SPM_PCM_REG1_DATA           (SPM_BASE + 0x0384)
+#define SPM_PCM_REG2_DATA           (SPM_BASE + 0x0388)
+#define SPM_PCM_REG3_DATA           (SPM_BASE + 0x038c)
+#define SPM_PCM_REG4_DATA           (SPM_BASE + 0x0390)
+#define SPM_PCM_REG5_DATA           (SPM_BASE + 0x0394)
+#define SPM_PCM_REG6_DATA           (SPM_BASE + 0x0398)
+#define SPM_PCM_REG7_DATA           (SPM_BASE + 0x039c)
+#define SPM_PCM_REG8_DATA           (SPM_BASE + 0x03a0)
+#define SPM_PCM_REG9_DATA           (SPM_BASE + 0x03a4)
+#define SPM_PCM_REG10_DATA          (SPM_BASE + 0x03a8)
+#define SPM_PCM_REG11_DATA          (SPM_BASE + 0x03ac)
+#define SPM_PCM_REG12_DATA          (SPM_BASE + 0x03b0)
+#define SPM_PCM_REG13_DATA          (SPM_BASE + 0x03b4)
+#define SPM_PCM_REG14_DATA          (SPM_BASE + 0x03b8)
+#define SPM_PCM_REG15_DATA          (SPM_BASE + 0x03bc)
+#define SPM_PCM_EVENT_REG_STA       (SPM_BASE + 0x03c0)
+#define SPM_PCM_FSM_STA             (SPM_BASE + 0x03c4)
+#define SPM_PCM_IM_HOST_RW_PTR      (SPM_BASE + 0x03c8)
+#define SPM_PCM_IM_HOST_RW_DAT      (SPM_BASE + 0x03cc)
+#define SPM_PCM_EVENT_VECTOR4       (SPM_BASE + 0x03d0)
+#define SPM_PCM_EVENT_VECTOR5       (SPM_BASE + 0x03d4)
+#define SPM_PCM_EVENT_VECTOR6       (SPM_BASE + 0x03d8)
+#define SPM_PCM_EVENT_VECTOR7       (SPM_BASE + 0x03dc)
+#define SPM_PCM_SW_INT_SEL          (SPM_BASE + 0x03e0)
+#define SPM_PCM_SW_INT_CLEAR        (SPM_BASE + 0x03e4)
+#define SPM_CLK_CON                 (SPM_BASE + 0x0400)
+#define SPM_APMCU_PWRCTL            (SPM_BASE + 0x0600)
+#define SPM_AP_DVFS_CON_SET         (SPM_BASE + 0x0604)
+#define SPM_AP_STANBY_CON           (SPM_BASE + 0x0608)
+#define SPM_PWR_STATUS              (SPM_BASE + 0x060c)
+#define SPM_PWR_STATUS_S            (SPM_BASE + 0x0610)
+#define SPM_SLEEP_TIMER_STA         (SPM_BASE + 0x0720)
+#define SPM_SLEEP_TWAM_CON          (SPM_BASE + 0x0760)
+#define SPM_SLEEP_TWAM_STATUS0      (SPM_BASE + 0x0764)
+#define SPM_SLEEP_TWAM_STATUS1      (SPM_BASE + 0x0768)
+#define SPM_SLEEP_TWAM_STATUS2      (SPM_BASE + 0x076c)
+#define SPM_SLEEP_TWAM_STATUS3      (SPM_BASE + 0x0770)
+#define SPM_SLEEP_WAKEUP_EVENT_MASK (SPM_BASE + 0x0810)
+#define SPM_SLEEP_CPU_WAKEUP_EVENT  (SPM_BASE + 0x0814)
+#define SPM_PCM_WDT_TIMER_VAL       (SPM_BASE + 0x0824)
+#define SPM_SLEEP_ISR_MASK          (SPM_BASE + 0x0900)
+#define SPM_SLEEP_ISR_STATUS        (SPM_BASE + 0x0904)
+#define SPM_SLEEP_ISR_RAW_STA       (SPM_BASE + 0x0910)
+#define SPM_PCM_RESERVE             (SPM_BASE + 0x0b00)
+#define SPM_PCM_SRC_REQ             (SPM_BASE + 0x0b04)
+#define SPM_SLEEP_CPU_IRQ_MASK      (SPM_BASE + 0x0b10)
+#define SPM_PCM_DEBUG_CON           (SPM_BASE + 0x0b20)
+#define SPM_CORE0_WFI_SEL           (SPM_BASE + 0x0f00)
+#define SPM_CORE1_WFI_SEL           (SPM_BASE + 0x0f04)
+#define SPM_CORE2_WFI_SEL           (SPM_BASE + 0x0f08)
+#define SPM_CORE3_WFI_SEL           (SPM_BASE + 0x0f0c)
+
+#define SPM_PROJECT_CODE            0xb16
+
+#define CON0_PCM_KICK               (1U << 0)
+#define CON0_IM_KICK                (1U << 1)
+#define CON0_IM_SLEEP_DVS           (1U << 3)
+#define CON0_EVENT_VEC0_EN          (1U << 4)
+#define CON0_EVENT_VEC1_EN          (1U << 5)
+#define CON0_EVENT_VEC2_EN          (1U << 6)
+#define CON0_EVENT_VEC3_EN          (1U << 7)
+#define CON0_EVENT_VEC4_EN          (1U << 8)
+#define CON0_EVENT_VEC5_EN          (1U << 9)
+#define CON0_EVENT_VEC6_EN          (1U << 10)
+#define CON0_EVENT_VEC7_EN          (1U << 11)
+#define CON0_PCM_SW_RESET           (1U << 15)
+#define CON0_CFG_KEY                (SPM_PROJECT_CODE << 16)
+
+#define CON1_IM_SLAVE               (1U << 0)
+#define CON1_MIF_APBEN              (1U << 3)
+#define CON1_PCM_TIMER_EN           (1U << 5)
+#define CON1_IM_NONRP_EN            (1U << 6)
+#define CON1_PCM_WDT_EN             (1U << 8)
+#define CON1_PCM_WDT_WAKE_MODE      (1U << 9)
+#define CON1_SPM_SRAM_SLP_B         (1U << 10)
+#define CON1_SPM_SRAM_ISO_B         (1U << 11)
+#define CON1_CFG_KEY                (SPM_PROJECT_CODE << 16)
+
+#define PCM_PWRIO_EN_R0             (1U << 0)
+#define PCM_PWRIO_EN_R7             (1U << 7)
+#define PCM_RF_SYNC_R0              (1U << 16)
+#define PCM_RF_SYNC_R7              (1U << 23)
+
+#define R7_UART_CLK_OFF_REQ         (1U << 0)
+#define R7_WDT_KICK_P               (1U << 22)
+
+#define R13_CONN_SRCCLKENI          (1U << 1)
+#define R13_MD_SRCCLKENI            (1U << 4)
+#define R13_UART_CLK_OFF_ACK        (1U << 20)
+
+#define CC_SYSCLK0_EN_0             (1U << 0)
+#define CC_SYSCLK0_EN_1             (1U << 1)
+#define CC_SYSSETTLE_SEL            (1U << 4)
+#define CC_LOCK_INFRA_DCM           (1U << 5)
+#define CC_SRCLKENA_MASK            (1U << 6)
+#define CC_CXO32K_RM_EN_MD          (1U << 9)
+#define CC_CXO32K_RM_EN_CONN        (1U << 10)
+#define CC_CLKSQ0_SEL               (1U << 11)
+#define CC_DISABLE_SODI             (1U << 13)
+#define CC_DISABLE_DORM_PWR         (1U << 14)
+#define CC_DISABLE_INFRA_PWR        (1U << 15)
+#define CC_SRCLKEN0_EN              (1U << 16)
+
+#define WAKE_SRC_TS                 (1U << 1)
+#define WAKE_SRC_KP                 (1U << 2)
+#define WAKE_SRC_WDT                (1U << 3)
+#define WAKE_SRC_GPT                (1U << 4)
+#define WAKE_SRC_EINT               (1U << 5)
+#define WAKE_SRC_CONN_WDT           (1U << 6)
+#define WAKE_SRC_CCIF_MD            (1U << 8)
+#define WAKE_SRC_LOW_BAT            (1U << 9)
+#define WAKE_SRC_CONN               (1U << 10)
+#define WAKE_SRC_USB_CD             (1U << 14)
+#define WAKE_SRC_USB_PDN            (1U << 16)
+#define WAKE_SRC_DBGSYS             (1U << 18)
+#define WAKE_SRC_UART0              (1U << 19)
+#define WAKE_SRC_AFE                (1U << 20)
+#define WAKE_SRC_THERM              (1U << 21)
+#define WAKE_SRC_CIRQ               (1U << 22)
+#define WAKE_SRC_SYSPWREQ           (1U << 24)
+#define WAKE_SRC_MD_WDT             (1U << 25)
+#define WAKE_SRC_CPU0_IRQ           (1U << 26)
+#define WAKE_SRC_CPU1_IRQ           (1U << 27)
+#define WAKE_SRC_CPU2_IRQ           (1U << 28)
+#define WAKE_SRC_CPU3_IRQ           (1U << 29)
+
+#define EVENT_VEC(event, resume, imme, pc)  \
+    (((event) << 0) | ((resume) << 5) | ((imme) << 6) | ((pc) << 16))
+
+#define spm_read(addr)              (*(volatile u32 *)(addr))
+
+#define sync_writel(v, a) \
+        do {    \
+            writel(v, a); \
+            DSB; \
+        } while (0)
+
+#define spm_write(addr, val)        sync_writel(val, addr)
+
+#define write_r(a, v) writel(v, a) /* need to fix it */
diff --git a/src/bsp/lk/platform/mt2701/include/platform/typedefs.h b/src/bsp/lk/platform/mt2701/include/platform/typedefs.h
new file mode 100644
index 0000000..569cb13
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/typedefs.h
@@ -0,0 +1,193 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ */
+
+#ifndef _TYPEDEFS_H_
+#define _TYPEDEFS_H_
+
+
+#ifndef FALSE
+#define FALSE   0
+#endif
+
+#ifndef TRUE
+#define TRUE    1
+#endif
+
+#ifndef NULL
+#define NULL    0
+#endif
+
+
+#ifndef BOOL
+typedef unsigned char BOOL;
+#endif
+
+typedef enum {
+    KAL_FALSE = 0,
+    KAL_TRUE = 1,
+} kal_bool;
+
+
+typedef unsigned long ulong;
+typedef unsigned char uchar;
+typedef unsigned int uint;
+typedef signed char int8;
+typedef signed short int16;
+typedef signed long int32;
+typedef signed int intx;
+typedef unsigned char uint8;
+typedef unsigned short uint16;
+typedef unsigned long uint32;
+typedef unsigned int uintx;
+
+//------------------------------------------------------------------
+
+typedef volatile unsigned char *P_kal_uint8;
+typedef volatile unsigned short *P_kal_uint16;
+typedef volatile unsigned int *P_kal_uint32;
+
+typedef long LONG;
+typedef unsigned char UBYTE;
+typedef short SHORT;
+
+typedef signed char kal_int8;
+typedef signed short kal_int16;
+typedef signed int kal_int32;
+typedef long long kal_int64;
+typedef unsigned char kal_uint8;
+typedef unsigned short kal_uint16;
+typedef unsigned int kal_uint32;
+typedef unsigned long long kal_uint64;
+typedef char kal_char;
+
+typedef unsigned int *UINT32P;
+typedef volatile unsigned short *UINT16P;
+typedef volatile unsigned char *UINT8P;
+typedef unsigned char *U8P;
+
+typedef volatile unsigned char *P_U8;
+typedef volatile signed char *P_S8;
+typedef volatile unsigned short *P_U16;
+typedef volatile signed short *P_S16;
+typedef volatile unsigned int *P_U32;
+typedef volatile signed int *P_S32;
+typedef unsigned long long *P_U64;
+typedef signed long long *P_S64;
+
+typedef unsigned char U8;
+typedef signed char S8;
+typedef unsigned short U16;
+typedef signed short S16;
+typedef unsigned int U32;
+typedef signed int S32;
+typedef unsigned long long U64;
+typedef signed long long S64;
+
+typedef unsigned char UINT8;
+typedef unsigned short UINT16;
+typedef unsigned int UINT32;
+typedef unsigned short USHORT;
+typedef signed char INT8;
+typedef signed short INT16;
+typedef signed int INT32;
+typedef signed int DWORD;
+typedef void VOID;
+typedef unsigned char BYTE;
+typedef float FLOAT;
+
+typedef char *LPCSTR;
+typedef short *LPWSTR;
+
+
+typedef char __s8;
+typedef unsigned char __u8;
+typedef short __s16;
+typedef unsigned short __u16;
+typedef int __s32;
+typedef unsigned int __u32;
+typedef long long __s64;
+typedef unsigned long long __u64;
+typedef signed char s8;
+typedef unsigned char u8;
+typedef signed short s16;
+typedef unsigned short u16;
+typedef signed int s32;
+typedef unsigned int u32;
+typedef signed long long s64;
+typedef unsigned long long u64;
+
+
+/*==== EXPORTED MACRO ===================================================*/
+
+#define MAXIMUM(A,B)                (((A)>(B))?(A):(B))
+#define MINIMUM(A,B)                (((A)<(B))?(A):(B))
+
+#define READ_REGISTER_UINT32(reg) \
+        (*(volatile UINT32 * const)(reg))
+
+#define WRITE_REGISTER_UINT32(reg, val) \
+        (*(volatile UINT32 * const)(reg)) = (val)
+
+#define READ_REGISTER_UINT16(reg) \
+        (*(volatile UINT16 * const)(reg))
+
+#define WRITE_REGISTER_UINT16(reg, val) \
+        (*(volatile UINT16 * const)(reg)) = (val)
+
+#define READ_REGISTER_UINT8(reg) \
+        (*(volatile UINT8 * const)(reg))
+
+#define WRITE_REGISTER_UINT8(reg, val) \
+        (*(volatile UINT8 * const)(reg)) = (val)
+
+#define DRV_Reg8(addr)              READ_REGISTER_UINT8((UINT8*)(addr))
+#define DRV_WriteReg8(addr, data)   WRITE_REGISTER_UINT8((UINT8*)(addr), (UINT8)(data))
+#define DRV_SetReg8(addr, data)     WRITE_REGISTER_UINT8((UINT8*)(addr), (UINT8)(READ_REGISTER_UINT8((UINT8*)(addr))|(data)))
+#define DRV_ClrReg8(addr, data)     WRITE_REGISTER_UINT8((UINT8*)(addr), (UINT8)(READ_REGISTER_UINT8((UINT8*)(addr))&~(data)))
+
+#define DRV_Reg16(addr)             READ_REGISTER_UINT16((UINT16*)(addr))
+#define DRV_WriteReg16(addr, data)  WRITE_REGISTER_UINT16((UINT16*)(addr),(UINT16)(data))
+#define DRV_SetReg16(addr, data)    WRITE_REGISTER_UINT16((UINT16*)(addr), (UINT16)(READ_REGISTER_UINT16((UINT16*)(addr))|(data)))
+#define DRV_ClrReg16(addr, data)    WRITE_REGISTER_UINT16((UINT16*)(addr), (UINT16)(READ_REGISTER_UINT16((UINT16*)(addr))&~(data)))
+
+#define DRV_Reg32(addr)             READ_REGISTER_UINT32((UINT32*)(addr))
+#define DRV_WriteReg32(addr, data)  WRITE_REGISTER_UINT32((UINT32*)(addr), (UINT32)(data))
+#define DRV_SetReg32(addr, data)    WRITE_REGISTER_UINT32((UINT32*)(addr), (UINT32)(READ_REGISTER_UINT32((UINT32*)(addr))|(data)))
+#define DRV_ClrReg32(addr, data)    WRITE_REGISTER_UINT32((UINT32*)(addr), (UINT32)(READ_REGISTER_UINT32((UINT32*)(addr))&~(data)))
+
+// !!! DEPRECATED, WILL BE REMOVED LATER !!!
+#define DRV_Reg(addr)               DRV_Reg16(addr)
+#define DRV_WriteReg(addr, data)    DRV_WriteReg16(addr, data)
+#define DRV_SetReg(addr, data)      DRV_SetReg16(addr, data)
+#define DRV_ClrReg(addr, data)      DRV_ClrReg16(addr, data)
+
+
+// compile time assert
+#define COMPILE_ASSERT(condition) ((void)sizeof(char[1 - 2*!!!(condition)]))
+
+#define printf          print
+#define BUG_ON(expr)    ASSERT(!(expr))
+
+
+#endif
+
diff --git a/src/bsp/lk/platform/mt2701/include/platform/udc-common.h b/src/bsp/lk/platform/mt2701/include/platform/udc-common.h
new file mode 100644
index 0000000..45c5ce8
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/include/platform/udc-common.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2009, Google Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+ * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+#pragma once
+
+#define GET_STATUS           0
+#define CLEAR_FEATURE        1
+#define SET_FEATURE          3
+#define SET_ADDRESS          5
+#define GET_DESCRIPTOR       6
+#define SET_DESCRIPTOR       7
+#define GET_CONFIGURATION    8
+#define SET_CONFIGURATION    9
+#define GET_INTERFACE        10
+#define SET_INTERFACE        11
+#define SYNCH_FRAME          12
+#define SET_SEL              48
+
+#define TYPE_DEVICE          1
+#define TYPE_CONFIGURATION   2
+#define TYPE_STRING          3
+#define TYPE_INTERFACE       4
+#define TYPE_ENDPOINT        5
+#define TYPE_DEV_QUALIFIER   6
+#define TYPE_OTHER_SPEEDCONF 7
+#define TYPE_IFACE_POWER     8
+#define TYPE_OTG             9
+#define TYPE_DEBUG           10
+#define TYPE_IFACE_ASSOC     11
+#define TYPE_BOS             15
+#define TYPE_DEVICE_CAP      16
+#define TYPE_SS_EP_COMP      48
+
+#define DEVICE_READ          0x80
+#define DEVICE_WRITE         0x00
+#define INTERFACE_READ       0x81
+#define INTERFACE_WRITE      0x01
+#define ENDPOINT_READ        0x82
+#define ENDPOINT_WRITE       0x02
+
+#define TEST_SE0_NAK         0x0300
+#define TEST_PACKET          0x0400
+#define PORTSC_PTC           (0xF << 16)
+#define PORTSC_PTC_SE0_NAK   (0x03 << 16)
+#define PORTSC_PTC_TST_PKT   (0x4 << 16)
+
+#define USB3_U1_ENABLE       48
+#define USB3_U2_ENABLE       49
+
+#define UDC_TYPE_BULK_IN    1
+#define UDC_TYPE_BULK_OUT   2
+
+struct setup_packet {
+    unsigned char type;
+    unsigned char request;
+    unsigned short value;
+    unsigned short index;
+    unsigned short length;
+} __attribute__ ((packed));
diff --git a/src/bsp/lk/platform/mt2701/interrupts.c b/src/bsp/lk/platform/mt2701/interrupts.c
new file mode 100644
index 0000000..3c4e061
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/interrupts.c
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2008, Google Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *  * Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ *  * Neither the name of Google, Inc. nor the names of its contributors
+ *    may be used to endorse or promote products derived from this
+ *    software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
+ * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <arch/arm.h>
+#include <reg.h>
+#include <kernel/thread.h>
+#include <platform/interrupts.h>
+#include <platform/spm.h>
+#include <platform/mt_reg_base.h>
+#include <platform/mt_irq.h>
+#include <debug.h>
+
+/* set for mt_gic */
+void mt_irq_set_polarity(unsigned int irq, unsigned int polarity)
+{
+    unsigned int offset;
+    unsigned int reg_index;
+    unsigned int value;
+
+    /* peripheral device's IRQ line is using GIC's SPI, and line ID >= GIC_PRIVATE_SIGNALS */
+    if (irq < GIC_PRIVATE_SIGNALS) {
+        dprintf(SPEW, "The Interrupt ID < 32, please check!\n");
+        return;
+    }
+
+    offset = (irq - GIC_PRIVATE_SIGNALS) & 0x1F;
+    reg_index = (irq - GIC_PRIVATE_SIGNALS) >> 5;
+    if (polarity == 0) {
+        value = readl(INT_POL_CTL0 + (reg_index * 4));
+        value |= (1 << offset); /* always invert the incoming IRQ's polarity */
+        write_r((INT_POL_CTL0 + (reg_index * 4)), value);
+    } else {
+        value = readl(INT_POL_CTL0 + (reg_index * 4));
+        value &= ~(0x1 << offset);
+        write_r(INT_POL_CTL0 + (reg_index * 4), value);
+    }
+}
+
+/* set for arm gic */
+void mt_irq_set_sens(unsigned int irq, unsigned int sens)
+{
+    unsigned int config;
+
+    if (sens == EDGE_SENSITIVE) {
+        config = readl(GIC_DIST_BASE + GIC_DIST_CONFIG + (irq / 16) * 4);
+        config |= (0x2 << (irq % 16) * 2);
+        write_r(GIC_DIST_BASE + GIC_DIST_CONFIG + (irq / 16) * 4, config);
+    } else {
+        config = readl(GIC_DIST_BASE + GIC_DIST_CONFIG + (irq / 16) * 4);
+        config &= ~(0x2 << (irq % 16) * 2);
+        write_r( GIC_DIST_BASE + GIC_DIST_CONFIG + (irq / 16) * 4, config);
+    }
+    DSB;
+}
diff --git a/src/bsp/lk/platform/mt2701/platform.c b/src/bsp/lk/platform/mt2701/platform.c
new file mode 100644
index 0000000..5e9917c
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/platform.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2016 MediaTek Inc.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining
+ * a copy of this software and associated documentation files
+ * (the "Software"), to deal in the Software without restriction,
+ * including without limitation the rights to use, copy, modify, merge,
+ * publish, distribute, sublicense, and/or sell copies of the Software,
+ * and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be
+ * included in all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
+ * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
+ * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
+ * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
+ * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
+ * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
+ * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include <arch.h>
+#include <dev/interrupt/arm_gic.h>
+#include <dev/uart.h>
+#include <dev/timer/arm_generic.h>
+#include <err.h>
+#include <kernel/vm.h>
+#include <kernel/spinlock.h>
+#include <lib/mempool.h>
+#include <platform.h>
+#include <platform/emi.h>
+#include <platform/memory.h>
+#include <platform/gic.h>
+#include <platform/memory.h>
+#include <platform/mtk_timer.h>
+#include <platform/mtk_wdt.h>
+#include <platform/pll.h>
+
+#if RAMBASE
+#define RSV_PAGE RAMBASE
+#else
+#define RSV_PAGE 0
+#endif
+
+#define L2C_MAPPING_IDX 0
+#define SRAM_MAPPING_IDX 1
+#define PERIPHERAL_MAPPING_IDX 2
+#define DRAM_MAPPING_IDX 3
+
+/* initial memory mappings. parsed by start.S */
+struct mmu_initial_mapping mmu_initial_mappings[] = {
+    {
+        .phys = SRAM_BASE_PHYS,
+        .virt = SRAM_BASE_VIRT,
+        .size = SRAM_BASE_SIZE,
+        .flags = 0,
+        .name = "sram"
+    },
+    {
+        .phys = MEMORY_BASE_PHYS,
+        .virt = KERNEL_BASE,
+        .size = MEMORY_APERTURE_SIZE,
+        .flags = 0,
+        .name = "memory"
+    },
+    {
+        .phys = PERIPHERAL_BASE_PHYS,
+        .virt = PERIPHERAL_BASE_VIRT,
+        .size = PERIPHERAL_BASE_SIZE,
+        .flags = MMU_INITIAL_MAPPING_FLAG_DEVICE,
+        .name = "peripherals"
+    },
+    /* reserved for dram */
+    { 0 },
+
+    /* null entry to terminate the list */
+    { 0 }
+};
+
+static pmm_arena_t arena = {
+    .name = "sdram",
+    .base = SRAM_BASE_PHYS,
+    .size = (SRAM_BASE_SIZE - RSV_PAGE),
+    .flags = PMM_ARENA_FLAG_KMAP,
+};
+
+static void arch_enable_mmu(void)
+{
+    arm_write_sctlr(arm_read_sctlr() | (1<<0)); // mmu enable
+}
+
+static void smp_mode_enable(void)
+{
+    uint32_t actlr = arm_read_actlr();
+
+    actlr |= (1<<6);
+    arm_write_actlr(actlr);
+}
+
+void platform_early_init(void)
+{
+    uart_init_early();
+#ifdef WITH_KERNEL_VM
+    pmm_add_arena(&arena);
+#endif
+    /* initialize the interrupt controller */
+    arm_gic_init();
+    /* init GPT6 */
+    mtk_timer_init();
+    /* initialize the timer block */
+    arm_generic_timer_init(ARM_GENERIC_TIMER_PHYSICAL_INT, 13000000);
+
+    arch_disable_cache(DCACHE);
+    arch_disable_mmu();
+    /* init pll */
+    mt_pll_init();
+
+    /* init AP watchdog and set timeout to 10 secs */
+    mtk_wdt_init();
+
+    /* init memory */
+    mt_mem_init();
+
+#if WITH_KERNEL_VM
+    /* add DRAM to mmu_initial_mappings for physical-to-virtual translation */
+    mmu_initial_mappings[DRAM_MAPPING_IDX].phys = DRAM_BASE_PHY;
+    mmu_initial_mappings[DRAM_MAPPING_IDX].virt = DRAM_BASE_PHY;
+    mmu_initial_mappings[DRAM_MAPPING_IDX].size = memory_size();
+    mmu_initial_mappings[DRAM_MAPPING_IDX].flags = 0;
+    mmu_initial_mappings[DRAM_MAPPING_IDX].name = "dram";
+#endif
+
+    arch_enable_mmu();
+    arch_enable_cache(DCACHE);
+    smp_mode_enable();
+}
+
+void platform_init(void)
+{
+    int ret;
+    unsigned int dram_size = 0;
+
+    /* map for app */
+    dram_size = memory_size();
+    arch_mmu_map(DRAM_PHY_ADDR, DRAM_PHY_ADDR, dram_size / PAGE_SIZE, 0);
+
+    ret = mempool_init((void *)CACHED_MEMPOOL_ADDR, CACHED_MEMPOOL_SIZE,
+                 MEMPOOL_CACHE);
+    if (ret != NO_ERROR)
+        platform_halt(HALT_ACTION_REBOOT, HALT_REASON_SW_PANIC);
+}
diff --git a/src/bsp/lk/platform/mt2701/rules.mk b/src/bsp/lk/platform/mt2701/rules.mk
new file mode 100644
index 0000000..62c9de7
--- /dev/null
+++ b/src/bsp/lk/platform/mt2701/rules.mk
@@ -0,0 +1,51 @@
+LOCAL_DIR := $(GET_LOCAL_DIR)
+
+MODULE := $(LOCAL_DIR)
+COMMON_PLAT := $(LOCAL_DIR)/../mediatek/common
+
+ARCH := arm
+ARM_CPU := cortex-a7
+WITH_SMP ?= 0
+
+LK_HEAP_IMPLEMENTATION ?= miniheap
+
+GLOBAL_INCLUDES += -I$(LK_TOP_DIR)/include \
+
+MODULE_SRCS += \
+    $(COMMON_PLAT)/boot_mode.c \
+    $(LOCAL_DIR)/cpu_early_init.S \
+    $(LOCAL_DIR)/debug.c \
+    $(LOCAL_DIR)/platform.c \
+    $(LOCAL_DIR)/interrupts.c \
+
+KERNEL_BASE ?= 0x200000
+KERNEL_LOAD_OFFSET ?= 0x0400
+MEMBASE ?= 0x200000
+MEMSIZE ?= 0x00020000   # 128K
+RAMBASE ?= 0x1000 # 4k alignment
+MACH_TYPE := 2701
+
+MODULE_DEPS += \
+    dev/interrupt/arm_gic \
+    dev/timer/arm_generic \
+    lib/bio \
+    lib/partition \
+    lib/fdt \
+    lib/cksum \
+    lib/mempool \
+
+GLOBAL_DEFINES += \
+    MEMBASE=$(MEMBASE) \
+    MEMSIZE=$(MEMSIZE) \
+    RAMBASE=$(RAMBASE) \
+    MACH_TYPE=$(MACH_TYPE) \
+    PLATFORM_SUPPORTS_PANIC_SHELL=1 \
+    WITH_CPU_EARLY_INIT=1 \
+    WITH_NO_FP=1 \
+
+LINKER_SCRIPT += \
+    $(BUILDDIR)/system-onesegment.ld
+
+ARM_CPU := cortex-a9-neon
+
+include make/module.mk $(LOCAL_DIR)/drivers/rules.mk