Merge tag 'mips-pull-2019-10-25' of git://git.denx.de/u-boot-mips

- bmips: add BCRM NAND support for BCM6368, BCM6328, BCM6362 and BCM63268 SoCs
- bmips: various small fixes
- mtmips: add new drivers for clock, reset-controller and pinctrl
- mtmips: add support for high speed UART
- mtmips: update/enhance drivers for SPI and ethernet
- mtmips: add support for MMC

26 files changed, 1812 insertions, 436 deletions

diff --git a/drivers/clk/Makefile b/drivers/clk/Makefile
index 8de6777468..06131edb9f 100644
--- a/drivers/clk/Makefile
+++ b/drivers/clk/Makefile
@@ -16,6 +16,7 @@ obj-y += imx/
 obj-y += tegra/
 obj-$(CONFIG_ARCH_ASPEED) += aspeed/
 obj-$(CONFIG_ARCH_MEDIATEK) += mediatek/
+obj-$(CONFIG_ARCH_MTMIPS) += mtmips/
 obj-$(CONFIG_ARCH_MESON) += meson/
 obj-$(CONFIG_ARCH_ROCKCHIP) += rockchip/
 obj-$(CONFIG_ARCH_SOCFPGA) += altera/
diff --git a/drivers/clk/mtmips/Makefile b/drivers/clk/mtmips/Makefile
new file mode 100644
index 0000000000..e1938418da
--- /dev/null
+++ b/drivers/clk/mtmips/Makefile
@@ -0,0 +1,3 @@
+# SPDX-License-Identifier: GPL-2.0
+
+obj-$(CONFIG_SOC_MT7628) += clk-mt7628.o
diff --git a/drivers/clk/mtmips/clk-mt7628.c b/drivers/clk/mtmips/clk-mt7628.c
new file mode 100644
index 0000000000..35780de8c4
--- /dev/null
+++ b/drivers/clk/mtmips/clk-mt7628.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <common.h>
+#include <clk-uclass.h>
+#include <dm.h>
+#include <dt-bindings/clock/mt7628-clk.h>
+#include <linux/bitops.h>
+#include <linux/io.h>
+
+/* SYSCFG0 */
+#define XTAL_40M_SEL			BIT(6)
+
+/* CLKCFG0 */
+#define CLKCFG0_REG			0x0
+#define PERI_CLK_FROM_XTAL_SEL		BIT(4)
+#define CPU_PLL_FROM_BBP		BIT(1)
+#define CPU_PLL_FROM_XTAL		BIT(0)
+
+/* CLKCFG1 */
+#define CLKCFG1_REG			0x4
+
+#define CLK_SRC_CPU			-1
+#define CLK_SRC_CPU_D2			-2
+#define CLK_SRC_SYS			-3
+#define CLK_SRC_XTAL			-4
+#define CLK_SRC_PERI			-5
+
+struct mt7628_clk_priv {
+	void __iomem *base;
+	int cpu_clk;
+	int sys_clk;
+	int xtal_clk;
+};
+
+static const int mt7628_clks[] = {
+	[CLK_SYS] = CLK_SRC_SYS,
+	[CLK_CPU] = CLK_SRC_CPU,
+	[CLK_XTAL] = CLK_SRC_XTAL,
+	[CLK_PWM] = CLK_SRC_PERI,
+	[CLK_MIPS_CNT] = CLK_SRC_CPU_D2,
+	[CLK_UART2] = CLK_SRC_PERI,
+	[CLK_UART1] = CLK_SRC_PERI,
+	[CLK_UART0] = CLK_SRC_PERI,
+	[CLK_SPI] = CLK_SRC_SYS,
+	[CLK_I2C] = CLK_SRC_PERI,
+};
+
+static ulong mt7628_clk_get_rate(struct clk *clk)
+{
+	struct mt7628_clk_priv *priv = dev_get_priv(clk->dev);
+	u32 val;
+
+	if (clk->id >= ARRAY_SIZE(mt7628_clks))
+		return 0;
+
+	switch (mt7628_clks[clk->id]) {
+	case CLK_SRC_CPU:
+		return priv->cpu_clk;
+	case CLK_SRC_CPU_D2:
+		return priv->cpu_clk / 2;
+	case CLK_SRC_SYS:
+		return priv->sys_clk;
+	case CLK_SRC_XTAL:
+		return priv->xtal_clk;
+	case CLK_SRC_PERI:
+		val = readl(priv->base + CLKCFG0_REG);
+		if (val & PERI_CLK_FROM_XTAL_SEL)
+			return priv->xtal_clk;
+		else
+			return 40000000;
+	default:
+		return mt7628_clks[clk->id];
+	}
+}
+
+static int mt7628_clk_enable(struct clk *clk)
+{
+	struct mt7628_clk_priv *priv = dev_get_priv(clk->dev);
+
+	if (clk->id > 31)
+		return -1;
+
+	setbits_32(priv->base + CLKCFG1_REG, BIT(clk->id));
+
+	return 0;
+}
+
+static int mt7628_clk_disable(struct clk *clk)
+{
+	struct mt7628_clk_priv *priv = dev_get_priv(clk->dev);
+
+	if (clk->id > 31)
+		return -1;
+
+	clrbits_32(priv->base + CLKCFG1_REG, BIT(clk->id));
+
+	return 0;
+}
+
+const struct clk_ops mt7628_clk_ops = {
+	.enable = mt7628_clk_enable,
+	.disable = mt7628_clk_disable,
+	.get_rate = mt7628_clk_get_rate,
+};
+
+static int mt7628_clk_probe(struct udevice *dev)
+{
+	struct mt7628_clk_priv *priv = dev_get_priv(dev);
+	void __iomem *syscfg_base;
+	u32 val;
+
+	priv->base = (void __iomem *)dev_remap_addr_index(dev, 0);
+	if (!priv->base)
+		return -EINVAL;
+
+	syscfg_base = (void __iomem *)dev_remap_addr_index(dev, 1);
+	if (!syscfg_base)
+		return -EINVAL;
+
+	val = readl(syscfg_base);
+	if (val & XTAL_40M_SEL)
+		priv->xtal_clk = 40000000;
+	else
+		priv->xtal_clk = 25000000;
+
+	val = readl(priv->base + CLKCFG0_REG);
+	if (val & CPU_PLL_FROM_BBP)
+		priv->cpu_clk = 480000000;
+	else if (val & CPU_PLL_FROM_XTAL)
+		priv->cpu_clk = priv->xtal_clk;
+	else if (priv->xtal_clk == 40000000)
+		priv->cpu_clk = 580000000;	/* (xtal_freq / 2) * 29 */
+	else
+		priv->cpu_clk = 575000000;	/* xtal_freq * 23 */
+
+	priv->sys_clk = priv->cpu_clk / 3;
+
+	return 0;
+}
+
+static const struct udevice_id mt7628_clk_ids[] = {
+	{ .compatible = "mediatek,mt7628-clk" },
+	{ }
+};
+
+U_BOOT_DRIVER(mt7628_clk) = {
+	.name = "mt7628-clk",
+	.id = UCLASS_CLK,
+	.of_match = mt7628_clk_ids,
+	.probe = mt7628_clk_probe,
+	.priv_auto_alloc_size = sizeof(struct mt7628_clk_priv),
+	.ops = &mt7628_clk_ops,
+};
diff --git a/drivers/mmc/Kconfig b/drivers/mmc/Kconfig
index 18af0762a8..85fd1906bd 100644
--- a/drivers/mmc/Kconfig
+++ b/drivers/mmc/Kconfig
@@ -698,7 +698,7 @@ config FTSDC010_SDIO
 
 config MMC_MTK
 	bool "MediaTek SD/MMC Card Interface support"
-	depends on ARCH_MEDIATEK
+	depends on ARCH_MEDIATEK || ARCH_MTMIPS
 	depends on BLK && DM_MMC
 	depends on OF_CONTROL
 	help
diff --git a/drivers/mmc/mtk-sd.c b/drivers/mmc/mtk-sd.c
index f555357af2..ffd647fac7 100644
--- a/drivers/mmc/mtk-sd.c
+++ b/drivers/mmc/mtk-sd.c
@@ -217,6 +217,7 @@ struct mtk_sd_regs {
 
 struct msdc_compatible {
 	u8 clk_div_bits;
+	u8 sclk_cycle_shift;
 	bool pad_tune0;
 	bool async_fifo;
 	bool data_tune;
@@ -269,6 +270,7 @@ struct msdc_host {
 
 	/* whether to use gpio detection or built-in hw detection */
 	bool builtin_cd;
+	bool cd_active_high;
 
 	/* card detection / write protection GPIOs */
 #if CONFIG_IS_ENABLED(DM_GPIO)
@@ -664,7 +666,7 @@ static int msdc_ops_send_cmd(struct udevice *dev, struct mmc_cmd *cmd,
 
 static void msdc_set_timeout(struct msdc_host *host, u32 ns, u32 clks)
 {
-	u32 timeout, clk_ns;
+	u32 timeout, clk_ns, shift;
 	u32 mode = 0;
 
 	host->timeout_ns = ns;
@@ -673,10 +675,11 @@ static void msdc_set_timeout(struct msdc_host *host, u32 ns, u32 clks)
 	if (host->sclk == 0) {
 		timeout = 0;
 	} else {
+		shift = host->dev_comp->sclk_cycle_shift;
 		clk_ns = 1000000000UL / host->sclk;
 		timeout = (ns + clk_ns - 1) / clk_ns + clks;
 		/* unit is 1048576 sclk cycles */
-		timeout = (timeout + (0x1 << 20) - 1) >> 20;
+		timeout = (timeout + (0x1 << shift) - 1) >> shift;
 		if (host->dev_comp->clk_div_bits == 8)
 			mode = (readl(&host->base->msdc_cfg) &
 				MSDC_CFG_CKMOD_M) >> MSDC_CFG_CKMOD_S;
@@ -850,7 +853,9 @@ static int msdc_ops_get_cd(struct udevice *dev)
 
 	if (host->builtin_cd) {
 		val = readl(&host->base->msdc_ps);
-		return !(val & MSDC_PS_CDSTS);
+		val = !!(val & MSDC_PS_CDSTS);
+
+		return !val ^ host->cd_active_high;
 	}
 
 #if CONFIG_IS_ENABLED(DM_GPIO)
@@ -1301,7 +1306,7 @@ static int msdc_drv_probe(struct udevice *dev)
 
 	host->mmc = &plat->mmc;
 	host->timeout_ns = 100000000;
-	host->timeout_clks = 3 * 1048576;
+	host->timeout_clks = 3 * (1 << host->dev_comp->sclk_cycle_shift);
 
 #ifdef CONFIG_PINCTRL
 	pinctrl_select_state(dev, "default");
@@ -1353,6 +1358,7 @@ static int msdc_ofdata_to_platdata(struct udevice *dev)
 	host->latch_ck = dev_read_u32_default(dev, "latch-ck", 0);
 	host->r_smpl = dev_read_u32_default(dev, "r_smpl", 0);
 	host->builtin_cd = dev_read_u32_default(dev, "builtin-cd", 0);
+	host->cd_active_high = dev_read_bool(dev, "cd-active-high");
 
 	return 0;
 }
@@ -1374,8 +1380,20 @@ static const struct dm_mmc_ops msdc_ops = {
 #endif
 };
 
+static const struct msdc_compatible mt7620_compat = {
+	.clk_div_bits = 8,
+	.sclk_cycle_shift = 16,
+	.pad_tune0 = false,
+	.async_fifo = false,
+	.data_tune = false,
+	.busy_check = false,
+	.stop_clk_fix = false,
+	.enhance_rx = false
+};
+
 static const struct msdc_compatible mt7623_compat = {
 	.clk_div_bits = 12,
+	.sclk_cycle_shift = 20,
 	.pad_tune0 = true,
 	.async_fifo = true,
 	.data_tune = true,
@@ -1386,6 +1404,7 @@ static const struct msdc_compatible mt7623_compat = {
 
 static const struct msdc_compatible mt8516_compat = {
 	.clk_div_bits = 12,
+	.sclk_cycle_shift = 20,
 	.pad_tune0 = true,
 	.async_fifo = true,
 	.data_tune = true,
@@ -1395,6 +1414,7 @@ static const struct msdc_compatible mt8516_compat = {
 
 static const struct msdc_compatible mt8183_compat = {
 	.clk_div_bits = 12,
+	.sclk_cycle_shift = 20,
 	.pad_tune0 = true,
 	.async_fifo = true,
 	.data_tune = true,
@@ -1403,6 +1423,7 @@ static const struct msdc_compatible mt8183_compat = {
 };
 
 static const struct udevice_id msdc_ids[] = {
+	{ .compatible = "mediatek,mt7620-mmc", .data = (ulong)&mt7620_compat },
 	{ .compatible = "mediatek,mt7623-mmc", .data = (ulong)&mt7623_compat },
 	{ .compatible = "mediatek,mt8516-mmc", .data = (ulong)&mt8516_compat },
 	{ .compatible = "mediatek,mt8183-mmc", .data = (ulong)&mt8183_compat },
diff --git a/drivers/mtd/nand/raw/Kconfig b/drivers/mtd/nand/raw/Kconfig
index a129f44869..2000826c79 100644
--- a/drivers/mtd/nand/raw/Kconfig
+++ b/drivers/mtd/nand/raw/Kconfig
@@ -72,6 +72,12 @@ config NAND_BRCMNAND
 	  Enable the driver for NAND flash on platforms using a Broadcom NAND
 	  controller.
 
+config NAND_BRCMNAND_6368
+	bool "Support Broadcom NAND controller on bcm6368"
+	depends on NAND_BRCMNAND && ARCH_BMIPS
+	help
+	  Enable support for broadcom nand driver on bcm6368.
+
 config NAND_BRCMNAND_6838
        bool "Support Broadcom NAND controller on bcm6838"
        depends on NAND_BRCMNAND && ARCH_BMIPS && SOC_BMIPS_BCM6838
diff --git a/drivers/mtd/nand/raw/brcmnand/Makefile b/drivers/mtd/nand/raw/brcmnand/Makefile
index a2363cc80e..7e70b859dc 100644
--- a/drivers/mtd/nand/raw/brcmnand/Makefile
+++ b/drivers/mtd/nand/raw/brcmnand/Makefile
@@ -1,5 +1,6 @@
 # SPDX-License-Identifier: GPL-2.0+
 
+obj-$(CONFIG_NAND_BRCMNAND_6368) += bcm6368_nand.o
 obj-$(CONFIG_NAND_BRCMNAND_63158) += bcm63158_nand.o
 obj-$(CONFIG_NAND_BRCMNAND_6838) += bcm6838_nand.o
 obj-$(CONFIG_NAND_BRCMNAND_6858) += bcm6858_nand.o
diff --git a/drivers/mtd/nand/raw/brcmnand/bcm6368_nand.c b/drivers/mtd/nand/raw/brcmnand/bcm6368_nand.c
new file mode 100644
index 0000000000..e2f5452c27
--- /dev/null
+++ b/drivers/mtd/nand/raw/brcmnand/bcm6368_nand.c
@@ -0,0 +1,117 @@
+// SPDX-License-Identifier: GPL-2.0+
+
+#include <common.h>
+#include <asm/io.h>
+#include <memalign.h>
+#include <nand.h>
+#include <linux/errno.h>
+#include <linux/io.h>
+#include <linux/ioport.h>
+#include <dm.h>
+
+#include "brcmnand.h"
+
+struct bcm6368_nand_soc {
+	struct brcmnand_soc soc;
+	void __iomem *base;
+};
+
+#define soc_to_priv(_soc) container_of(_soc, struct bcm6368_nand_soc, soc)
+
+#define BCM6368_NAND_INT		0x00
+#define  BCM6368_NAND_STATUS_SHIFT	0
+#define  BCM6368_NAND_STATUS_MASK	(0xfff << BCM6368_NAND_STATUS_SHIFT)
+#define  BCM6368_NAND_ENABLE_SHIFT	16
+#define  BCM6368_NAND_ENABLE_MASK	(0xffff << BCM6368_NAND_ENABLE_SHIFT)
+
+enum {
+	BCM6368_NP_READ		= BIT(0),
+	BCM6368_BLOCK_ERASE	= BIT(1),
+	BCM6368_COPY_BACK	= BIT(2),
+	BCM6368_PAGE_PGM	= BIT(3),
+	BCM6368_CTRL_READY	= BIT(4),
+	BCM6368_DEV_RBPIN	= BIT(5),
+	BCM6368_ECC_ERR_UNC	= BIT(6),
+	BCM6368_ECC_ERR_CORR	= BIT(7),
+};
+
+static bool bcm6368_nand_intc_ack(struct brcmnand_soc *soc)
+{
+	struct bcm6368_nand_soc *priv = soc_to_priv(soc);
+	void __iomem *mmio = priv->base + BCM6368_NAND_INT;
+	u32 val = brcmnand_readl(mmio);
+
+	if (val & (BCM6368_CTRL_READY << BCM6368_NAND_STATUS_SHIFT)) {
+		/* Ack interrupt */
+		val &= ~BCM6368_NAND_STATUS_MASK;
+		val |= BCM6368_CTRL_READY << BCM6368_NAND_STATUS_SHIFT;
+		brcmnand_writel(val, mmio);
+		return true;
+	}
+
+	return false;
+}
+
+static void bcm6368_nand_intc_set(struct brcmnand_soc *soc, bool en)
+{
+	struct bcm6368_nand_soc *priv = soc_to_priv(soc);
+	void __iomem *mmio = priv->base + BCM6368_NAND_INT;
+	u32 val = brcmnand_readl(mmio);
+
+	/* Don't ack any interrupts */
+	val &= ~BCM6368_NAND_STATUS_MASK;
+
+	if (en)
+		val |= BCM6368_CTRL_READY << BCM6368_NAND_ENABLE_SHIFT;
+	else
+		val &= ~(BCM6368_CTRL_READY << BCM6368_NAND_ENABLE_SHIFT);
+
+	brcmnand_writel(val, mmio);
+}
+
+static int bcm6368_nand_probe(struct udevice *dev)
+{
+	struct bcm6368_nand_soc *priv = dev_get_priv(dev);
+	struct brcmnand_soc *soc = &priv->soc;
+
+	priv->base = dev_remap_addr_name(dev, "nand-int-base");
+	if (!priv->base)
+		return -EINVAL;
+
+	soc->ctlrdy_ack = bcm6368_nand_intc_ack;
+	soc->ctlrdy_set_enabled = bcm6368_nand_intc_set;
+
+	/* Disable and ack all interrupts  */
+	brcmnand_writel(0, priv->base + BCM6368_NAND_INT);
+	brcmnand_writel(BCM6368_NAND_STATUS_MASK,
+			priv->base + BCM6368_NAND_INT);
+
+	return brcmnand_probe(dev, soc);
+}
+
+static const struct udevice_id bcm6368_nand_dt_ids[] = {
+	{
+		.compatible = "brcm,nand-bcm6368",
+	},
+	{ /* sentinel */ }
+};
+
+U_BOOT_DRIVER(bcm6368_nand) = {
+	.name = "bcm6368-nand",
+	.id = UCLASS_MTD,
+	.of_match = bcm6368_nand_dt_ids,
+	.probe = bcm6368_nand_probe,
+	.priv_auto_alloc_size = sizeof(struct bcm6368_nand_soc),
+};
+
+void board_nand_init(void)
+{
+	struct udevice *dev;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_MTD,
+					  DM_GET_DRIVER(bcm6368_nand), &dev);
+	if (ret && ret != -ENODEV)
+		pr_err("Failed to initialize %s. (error %d)\n", dev->name,
+		       ret);
+}
diff --git a/drivers/mtd/nand/raw/brcmnand/brcmnand.c b/drivers/mtd/nand/raw/brcmnand/brcmnand.c
index faa6da42d5..0745929253 100644
--- a/drivers/mtd/nand/raw/brcmnand/brcmnand.c
+++ b/drivers/mtd/nand/raw/brcmnand/brcmnand.c
@@ -888,183 +888,131 @@ static inline bool is_hamming_ecc(struct brcmnand_controller *ctrl,
 }
 
 /*
- * Set mtd->ooblayout to the appropriate mtd_ooblayout_ops given
- * the layout/configuration.
- * Returns -ERRCODE on failure.
+ * Returns a nand_ecclayout strucutre for the given layout/configuration.
+ * Returns NULL on failure.
  */
-static int brcmnand_hamming_ooblayout_ecc(struct mtd_info *mtd, int section,
-					  struct mtd_oob_region *oobregion)
+static struct nand_ecclayout *brcmnand_create_layout(int ecc_level,
+						     struct brcmnand_host *host)
 {
-	struct nand_chip *chip = mtd_to_nand(mtd);
-	struct brcmnand_host *host = nand_get_controller_data(chip);
-	struct brcmnand_cfg *cfg = &host->hwcfg;
-	int sas = cfg->spare_area_size << cfg->sector_size_1k;
-	int sectors = cfg->page_size / (512 << cfg->sector_size_1k);
-
-	if (section >= sectors)
-		return -ERANGE;
-
-	oobregion->offset = (section * sas) + 6;
-	oobregion->length = 3;
-
-	return 0;
-}
-
-static int brcmnand_hamming_ooblayout_free(struct mtd_info *mtd, int section,
-					   struct mtd_oob_region *oobregion)
-{
-	struct nand_chip *chip = mtd_to_nand(mtd);
-	struct brcmnand_host *host = nand_get_controller_data(chip);
 	struct brcmnand_cfg *cfg = &host->hwcfg;
-	int sas = cfg->spare_area_size << cfg->sector_size_1k;
-	int sectors = cfg->page_size / (512 << cfg->sector_size_1k);
+	int i, j;
+	struct nand_ecclayout *layout;
+	int req;
+	int sectors;
+	int sas;
+	int idx1, idx2;
 
-	if (section >= sectors * 2)
-		return -ERANGE;
-
-	oobregion->offset = (section / 2) * sas;
-
-	if (section & 1) {
-		oobregion->offset += 9;
-		oobregion->length = 7;
-	} else {
-		oobregion->length = 6;
-
-		/* First sector of each page may have BBI */
-		if (!section) {
-			/*
-			 * Small-page NAND use byte 6 for BBI while large-page
-			 * NAND use byte 0.
-			 */
-			if (cfg->page_size > 512)
-				oobregion->offset++;
-			oobregion->length--;
+#ifndef __UBOOT__
+	layout = devm_kzalloc(&host->pdev->dev, sizeof(*layout), GFP_KERNEL);
+#else
+	layout = devm_kzalloc(host->pdev, sizeof(*layout), GFP_KERNEL);
+#endif
+	if (!layout)
+		return NULL;
+
+	sectors = cfg->page_size / (512 << cfg->sector_size_1k);
+	sas = cfg->spare_area_size << cfg->sector_size_1k;
+
+	/* Hamming */
+	if (is_hamming_ecc(host->ctrl, cfg)) {
+		for (i = 0, idx1 = 0, idx2 = 0; i < sectors; i++) {
+			/* First sector of each page may have BBI */
+			if (i == 0) {
+				layout->oobfree[idx2].offset = i * sas + 1;
+				/* Small-page NAND use byte 6 for BBI */
+				if (cfg->page_size == 512)
+					layout->oobfree[idx2].offset--;
+				layout->oobfree[idx2].length = 5;
+			} else {
+				layout->oobfree[idx2].offset = i * sas;
+				layout->oobfree[idx2].length = 6;
+			}
+			idx2++;
+			layout->eccpos[idx1++] = i * sas + 6;
+			layout->eccpos[idx1++] = i * sas + 7;
+			layout->eccpos[idx1++] = i * sas + 8;
+			layout->oobfree[idx2].offset = i * sas + 9;
+			layout->oobfree[idx2].length = 7;
+			idx2++;
+			/* Leave zero-terminated entry for OOBFREE */
+			if (idx1 >= MTD_MAX_ECCPOS_ENTRIES_LARGE ||
+			    idx2 >= MTD_MAX_OOBFREE_ENTRIES_LARGE - 1)
+				break;
 		}
-	}
-
-	return 0;
-}
-
-static const struct mtd_ooblayout_ops brcmnand_hamming_ooblayout_ops = {
-	.ecc = brcmnand_hamming_ooblayout_ecc,
-	.free = brcmnand_hamming_ooblayout_free,
-};
-
-static int brcmnand_bch_ooblayout_ecc(struct mtd_info *mtd, int section,
-				      struct mtd_oob_region *oobregion)
-{
-	struct nand_chip *chip = mtd_to_nand(mtd);
-	struct brcmnand_host *host = nand_get_controller_data(chip);
-	struct brcmnand_cfg *cfg = &host->hwcfg;
-	int sas = cfg->spare_area_size << cfg->sector_size_1k;
-	int sectors = cfg->page_size / (512 << cfg->sector_size_1k);
-
-	if (section >= sectors)
-		return -ERANGE;
 
-	oobregion->offset = (section * (sas + 1)) - chip->ecc.bytes;
-	oobregion->length = chip->ecc.bytes;
-
-	return 0;
-}
-
-static int brcmnand_bch_ooblayout_free_lp(struct mtd_info *mtd, int section,
-					  struct mtd_oob_region *oobregion)
-{
-	struct nand_chip *chip = mtd_to_nand(mtd);
-	struct brcmnand_host *host = nand_get_controller_data(chip);
-	struct brcmnand_cfg *cfg = &host->hwcfg;
-	int sas = cfg->spare_area_size << cfg->sector_size_1k;
-	int sectors = cfg->page_size / (512 << cfg->sector_size_1k);
-
-	if (section >= sectors)
-		return -ERANGE;
-
-	if (sas <= chip->ecc.bytes)
-		return 0;
-
-	oobregion->offset = section * sas;
-	oobregion->length = sas - chip->ecc.bytes;
-
-	if (!section) {
-		oobregion->offset++;
-		oobregion->length--;
+		return layout;
 	}
 
-	return 0;
-}
-
-static int brcmnand_bch_ooblayout_free_sp(struct mtd_info *mtd, int section,
-					  struct mtd_oob_region *oobregion)
-{
-	struct nand_chip *chip = mtd_to_nand(mtd);
-	struct brcmnand_host *host = nand_get_controller_data(chip);
-	struct brcmnand_cfg *cfg = &host->hwcfg;
-	int sas = cfg->spare_area_size << cfg->sector_size_1k;
+	/*
+	 * CONTROLLER_VERSION:
+	 *   < v5.0: ECC_REQ = ceil(BCH_T * 13/8)
+	 *  >= v5.0: ECC_REQ = ceil(BCH_T * 14/8)
+	 * But we will just be conservative.
+	 */
+	req = DIV_ROUND_UP(ecc_level * 14, 8);
+	if (req >= sas) {
+		dev_err(&host->pdev->dev,
+			"error: ECC too large for OOB (ECC bytes %d, spare sector %d)\n",
+			req, sas);
+		return NULL;
+	}
 
-	if (section > 1 || sas - chip->ecc.bytes < 6 ||
-	    (section && sas - chip->ecc.bytes == 6))
-		return -ERANGE;
+	layout->eccbytes = req * sectors;
+	for (i = 0, idx1 = 0, idx2 = 0; i < sectors; i++) {
+		for (j = sas - req; j < sas && idx1 <
+				MTD_MAX_ECCPOS_ENTRIES_LARGE; j++, idx1++)
+			layout->eccpos[idx1] = i * sas + j;
 
-	if (!section) {
-		oobregion->offset = 0;
-		oobregion->length = 5;
-	} else {
-		oobregion->offset = 6;
-		oobregion->length = sas - chip->ecc.bytes - 6;
+		/* First sector of each page may have BBI */
+		if (i == 0) {
+			if (cfg->page_size == 512 && (sas - req >= 6)) {
+				/* Small-page NAND use byte 6 for BBI */
+				layout->oobfree[idx2].offset = 0;
+				layout->oobfree[idx2].length = 5;
+				idx2++;
+				if (sas - req > 6) {
+					layout->oobfree[idx2].offset = 6;
+					layout->oobfree[idx2].length =
+						sas - req - 6;
+					idx2++;
+				}
+			} else if (sas > req + 1) {
+				layout->oobfree[idx2].offset = i * sas + 1;
+				layout->oobfree[idx2].length = sas - req - 1;
+				idx2++;
+			}
+		} else if (sas > req) {
+			layout->oobfree[idx2].offset = i * sas;
+			layout->oobfree[idx2].length = sas - req;
+			idx2++;
+		}
+		/* Leave zero-terminated entry for OOBFREE */
+		if (idx1 >= MTD_MAX_ECCPOS_ENTRIES_LARGE ||
+		    idx2 >= MTD_MAX_OOBFREE_ENTRIES_LARGE - 1)
+			break;
 	}
 
-	return 0;
+	return layout;
 }
 
-static const struct mtd_ooblayout_ops brcmnand_bch_lp_ooblayout_ops = {
-	.ecc = brcmnand_bch_ooblayout_ecc,
-	.free = brcmnand_bch_ooblayout_free_lp,
-};
-
-static const struct mtd_ooblayout_ops brcmnand_bch_sp_ooblayout_ops = {
-	.ecc = brcmnand_bch_ooblayout_ecc,
-	.free = brcmnand_bch_ooblayout_free_sp,
-};
-
-static int brcmstb_choose_ecc_layout(struct brcmnand_host *host)
+static struct nand_ecclayout *brcmstb_choose_ecc_layout(
+		struct brcmnand_host *host)
 {
+	struct nand_ecclayout *layout;
 	struct brcmnand_cfg *p = &host->hwcfg;
-	struct mtd_info *mtd = nand_to_mtd(&host->chip);
-	struct nand_ecc_ctrl *ecc = &host->chip.ecc;
 	unsigned int ecc_level = p->ecc_level;
-	int sas = p->spare_area_size << p->sector_size_1k;
-	int sectors = p->page_size / (512 << p->sector_size_1k);
 
 	if (p->sector_size_1k)
 		ecc_level <<= 1;
 
-	if (is_hamming_ecc(host->ctrl, p)) {
-		ecc->bytes = 3 * sectors;
-		mtd_set_ooblayout(mtd, &brcmnand_hamming_ooblayout_ops);
-		return 0;
-	}
-
-	/*
-	 * CONTROLLER_VERSION:
-	 *   < v5.0: ECC_REQ = ceil(BCH_T * 13/8)
-	 *  >= v5.0: ECC_REQ = ceil(BCH_T * 14/8)
-	 * But we will just be conservative.
-	 */
-	ecc->bytes = DIV_ROUND_UP(ecc_level * 14, 8);
-	if (p->page_size == 512)
-		mtd_set_ooblayout(mtd, &brcmnand_bch_sp_ooblayout_ops);
-	else
-		mtd_set_ooblayout(mtd, &brcmnand_bch_lp_ooblayout_ops);
-
-	if (ecc->bytes >= sas) {
+	layout = brcmnand_create_layout(ecc_level, host);
+	if (!layout) {
 		dev_err(&host->pdev->dev,
-			"error: ECC too large for OOB (ECC bytes %d, spare sector %d)\n",
-			ecc->bytes, sas);
-		return -EINVAL;
+				"no proper ecc_layout for this NAND cfg\n");
+		return NULL;
 	}
 
-	return 0;
+	return layout;
 }
 
 static void brcmnand_wp(struct mtd_info *mtd, int wp)
@@ -2383,9 +2331,9 @@ static int brcmnand_init_cs(struct brcmnand_host *host, ofnode dn)
 	/* only use our internal HW threshold */
 	mtd->bitflip_threshold = 1;
 
-	ret = brcmstb_choose_ecc_layout(host);
-	if (ret)
-		return ret;
+	chip->ecc.layout = brcmstb_choose_ecc_layout(host);
+	if (!chip->ecc.layout)
+		return -ENXIO;
 
 	ret = nand_scan_tail(mtd);
 	if (ret)
diff --git a/drivers/net/Kconfig b/drivers/net/Kconfig
index 2ce3092db0..eb3d7ed45f 100644
--- a/drivers/net/Kconfig
+++ b/drivers/net/Kconfig
@@ -322,6 +322,7 @@ config MACB_ZYNQ
 config MT7628_ETH
 	bool "MediaTek MT7628 Ethernet Interface"
 	depends on SOC_MT7628
+	select PHYLIB
 	help
 	  The MediaTek MT7628 ethernet interface is used on MT7628 and
 	  MT7688 based boards.
diff --git a/drivers/net/mt7628-eth.c b/drivers/net/mt7628-eth.c
index 7833b2f47a..a1d12f6902 100644
--- a/drivers/net/mt7628-eth.c
+++ b/drivers/net/mt7628-eth.c
@@ -18,23 +18,12 @@
 #include <malloc.h>
 #include <miiphy.h>
 #include <net.h>
-#include <regmap.h>
-#include <syscon.h>
+#include <reset.h>
 #include <wait_bit.h>
 #include <asm/io.h>
 #include <linux/bitfield.h>
 #include <linux/err.h>
 
-/* System controller register */
-#define MT7628_RSTCTRL_REG	0x34
-#define RSTCTRL_EPHY_RST	BIT(24)
-
-#define MT7628_AGPIO_CFG_REG	0x3c
-#define MT7628_EPHY_GPIO_AIO_EN	GENMASK(20, 17)
-#define MT7628_EPHY_P0_DIS	BIT(16)
-
-#define MT7628_GPIO2_MODE_REG	0x64
-
 /* Ethernet frame engine register */
 #define PDMA_RELATED		0x0800
 
@@ -68,6 +57,11 @@
 /* Ethernet switch register */
 #define MT7628_SWITCH_FCT0	0x0008
 #define MT7628_SWITCH_PFC1	0x0014
+#define MT7628_SWITCH_PVIDC0	0x0040
+#define MT7628_SWITCH_PVIDC1	0x0044
+#define MT7628_SWITCH_PVIDC2	0x0048
+#define MT7628_SWITCH_PVIDC3	0x004c
+#define MT7628_SWITCH_VMSC0	0x0070
 #define MT7628_SWITCH_FPA	0x0084
 #define MT7628_SWITCH_SOCPC	0x008c
 #define MT7628_SWITCH_POC0	0x0090
@@ -122,6 +116,7 @@ struct fe_tx_dma {
 
 #define NUM_RX_DESC		256
 #define NUM_TX_DESC		4
+#define NUM_PHYS		5
 
 #define PADDING_LENGTH		60
 
@@ -131,13 +126,9 @@ struct fe_tx_dma {
 #define CONFIG_DMA_STOP_TIMEOUT	100
 #define CONFIG_TX_DMA_TIMEOUT	100
 
-#define LINK_DELAY_TIME		500		/* 500 ms */
-#define LINK_TIMEOUT		10000		/* 10 seconds */
-
 struct mt7628_eth_dev {
 	void __iomem *base;		/* frame engine base address */
 	void __iomem *eth_sw_base;	/* switch base address */
-	struct regmap *sysctrl_regmap;	/* system-controller reg-map */
 
 	struct mii_dev *bus;
 
@@ -150,8 +141,16 @@ struct mt7628_eth_dev {
 	int rx_dma_idx;
 	/* Point to the next TXD in TXD Ring0 CPU wants to use */
 	int tx_dma_idx;
+
+	struct reset_ctl	rst_ephy;
+
+	struct phy_device *phy;
+
+	int wan_port;
 };
 
+static int mt7628_eth_free_pkt(struct udevice *dev, uchar *packet, int length);
+
 static int mdio_wait_read(struct mt7628_eth_dev *priv, u32 mask, bool mask_set)
 {
 	void __iomem *base = priv->eth_sw_base;
@@ -280,6 +279,9 @@ static void mt7628_ephy_init(struct mt7628_eth_dev *priv)
 static void rt305x_esw_init(struct mt7628_eth_dev *priv)
 {
 	void __iomem *base = priv->eth_sw_base;
+	void __iomem *reg;
+	u32 val = 0, pvid;
+	int i;
 
 	/*
 	 * FC_RLS_TH=200, FC_SET_TH=160
@@ -301,20 +303,28 @@ static void rt305x_esw_init(struct mt7628_eth_dev *priv)
 	/* 1us cycle number=125 (FE's clock=125Mhz) */
 	writel(0x7d000000, base + MT7628_SWITCH_BMU_CTRL);
 
-	/* Configure analog GPIO setup */
-	regmap_update_bits(priv->sysctrl_regmap, MT7628_AGPIO_CFG_REG,
-			   MT7628_EPHY_P0_DIS, MT7628_EPHY_GPIO_AIO_EN);
+	/* LAN/WAN partition, WAN port will be unusable in u-boot network */
+	if (priv->wan_port >= 0 && priv->wan_port < 6) {
+		for (i = 0; i < 8; i++) {
+			pvid = i == priv->wan_port ? 2 : 1;
+			reg = base + MT7628_SWITCH_PVIDC0 + (i / 2) * 4;
+			if (i % 2 == 0) {
+				val = pvid;
+			} else {
+				val |= (pvid << 12);
+				writel(val, reg);
+			}
+		}
 
-	/* Reset PHY */
-	regmap_update_bits(priv->sysctrl_regmap, MT7628_RSTCTRL_REG,
-			   0, RSTCTRL_EPHY_RST);
-	regmap_update_bits(priv->sysctrl_regmap, MT7628_RSTCTRL_REG,
-			   RSTCTRL_EPHY_RST, 0);
-	mdelay(10);
+		val = 0xffff407f;
+		val |= 1 << (8 + priv->wan_port);
+		val &= ~(1 << priv->wan_port);
+		writel(val, base + MT7628_SWITCH_VMSC0);
+	}
 
-	/* Set P0 EPHY LED mode */
-	regmap_update_bits(priv->sysctrl_regmap, MT7628_GPIO2_MODE_REG,
-			   0x0ffc0ffc, 0x05540554);
+	/* Reset PHY */
+	reset_assert(&priv->rst_ephy);
+	reset_deassert(&priv->rst_ephy);
 	mdelay(10);
 
 	mt7628_ephy_init(priv);
@@ -424,6 +434,7 @@ static int mt7628_eth_recv(struct udevice *dev, int flags, uchar **packetp)
 	length = FIELD_GET(RX_DMA_PLEN0, priv->rx_ring[idx].rxd2);
 	if (length == 0 || length > MTK_QDMA_PAGE_SIZE) {
 		printf("%s: invalid length (%d bytes)\n", __func__, length);
+		mt7628_eth_free_pkt(dev, NULL, 0);
 		return -EIO;
 	}
 
@@ -458,20 +469,13 @@ static int mt7628_eth_free_pkt(struct udevice *dev, uchar *packet, int length)
 	return 0;
 }
 
-static int phy_link_up(struct mt7628_eth_dev *priv)
-{
-	u32 val;
-
-	mii_mgr_read(priv, 0x00, MII_BMSR, &val);
-	return !!(val & BMSR_LSTATUS);
-}
-
 static int mt7628_eth_start(struct udevice *dev)
 {
 	struct mt7628_eth_dev *priv = dev_get_priv(dev);
 	void __iomem *base = priv->base;
 	uchar packet[MTK_QDMA_PAGE_SIZE];
 	uchar *packetp;
+	int ret;
 	int i;
 
 	for (i = 0; i < NUM_RX_DESC; i++) {
@@ -514,25 +518,13 @@ static int mt7628_eth_start(struct udevice *dev)
 	wmb();
 	eth_dma_start(priv);
 
-	/* Check if link is not up yet */
-	if (!phy_link_up(priv)) {
-		/* Wait for link to come up */
-
-		printf("Waiting for link to come up .");
-		for (i = 0; i < (LINK_TIMEOUT / LINK_DELAY_TIME); i++) {
-			mdelay(LINK_DELAY_TIME);
-			if (phy_link_up(priv)) {
-				mdelay(100);	/* Ensure all is ready */
-				break;
-			}
-
-			printf(".");
-		}
+	if (priv->phy) {
+		ret = phy_startup(priv->phy);
+		if (ret)
+			return ret;
 
-		if (phy_link_up(priv))
-			printf(" done\n");
-		else
-			printf(" timeout! Trying anyways\n");
+		if (!priv->phy->link)
+			return -EAGAIN;
 	}
 
 	/*
@@ -558,8 +550,8 @@ static void mt7628_eth_stop(struct udevice *dev)
 static int mt7628_eth_probe(struct udevice *dev)
 {
 	struct mt7628_eth_dev *priv = dev_get_priv(dev);
-	struct udevice *syscon;
 	struct mii_dev *bus;
+	int poll_link_phy;
 	int ret;
 	int i;
 
@@ -573,19 +565,15 @@ static int mt7628_eth_probe(struct udevice *dev)
 	if (IS_ERR(priv->eth_sw_base))
 		return PTR_ERR(priv->eth_sw_base);
 
-	/* Get system controller regmap */
-	ret = uclass_get_device_by_phandle(UCLASS_SYSCON, dev,
-					   "syscon", &syscon);
+	/* Reset controller */
+	ret = reset_get_by_name(dev, "ephy", &priv->rst_ephy);
 	if (ret) {
-		pr_err("unable to find syscon device\n");
+		pr_err("unable to find reset controller for ethernet PHYs\n");
 		return ret;
 	}
 
-	priv->sysctrl_regmap = syscon_get_regmap(syscon);
-	if (!priv->sysctrl_regmap) {
-		pr_err("unable to find regmap\n");
-		return -ENODEV;
-	}
+	/* WAN port will be isolated from LAN ports */
+	priv->wan_port = dev_read_u32_default(dev, "mediatek,wan-port", -1);
 
 	/* Put rx and tx rings into KSEG1 area (uncached) */
 	priv->tx_ring = (struct fe_tx_dma *)
@@ -613,6 +601,25 @@ static int mt7628_eth_probe(struct udevice *dev)
 	if (ret)
 		return ret;
 
+	poll_link_phy = dev_read_u32_default(dev, "mediatek,poll-link-phy", -1);
+	if (poll_link_phy >= 0) {
+		if (poll_link_phy >= NUM_PHYS) {
+			pr_err("invalid phy %d for poll-link-phy\n",
+			       poll_link_phy);
+			return ret;
+		}
+
+		priv->phy = phy_connect(bus, poll_link_phy, dev,
+					PHY_INTERFACE_MODE_MII);
+		if (!priv->phy) {
+			pr_err("failed to probe phy %d\n", poll_link_phy);
+			return -ENODEV;
+		}
+
+		priv->phy->advertising = priv->phy->supported;
+		phy_config(priv->phy);
+	}
+
 	/* Switch configuration */
 	rt305x_esw_init(priv);
 
diff --git a/drivers/phy/Kconfig b/drivers/phy/Kconfig
index e317373a5c..a72f34f0d4 100644
--- a/drivers/phy/Kconfig
+++ b/drivers/phy/Kconfig
@@ -200,6 +200,7 @@ config KEYSTONE_USB_PHY
 config MT76X8_USB_PHY
 	bool "MediaTek MT76x8 (7628/88) USB PHY support"
 	depends on PHY
+	depends on SOC_MT7628
 	help
           Support the USB PHY in MT76x8 SoCs
 
diff --git a/drivers/phy/mt76x8-usb-phy.c b/drivers/phy/mt76x8-usb-phy.c
index 268da8ef6c..1e7c5f334b 100644
--- a/drivers/phy/mt76x8-usb-phy.c
+++ b/drivers/phy/mt76x8-usb-phy.c
@@ -6,93 +6,185 @@
  *     Copyright (C) 2017 John Crispin <john@phrozen.org>
  */
 
+#include <clk.h>
 #include <common.h>
 #include <dm.h>
 #include <generic-phy.h>
-#include <regmap.h>
-#include <reset-uclass.h>
-#include <syscon.h>
+#include <reset.h>
 #include <asm/io.h>
-
-#define RT_SYSC_REG_SYSCFG1		0x014
-#define RT_SYSC_REG_CLKCFG1		0x030
-#define RT_SYSC_REG_USB_PHY_CFG		0x05c
+#include <linux/bitops.h>
 
 #define OFS_U2_PHY_AC0			0x800
+#define USBPLL_FBDIV_S			16
+#define USBPLL_FBDIV_M			GENMASK(22, 16)
+#define BG_TRIM_S			8
+#define BG_TRIM_M			GENMASK(11, 8)
+#define BG_RBSEL_S			6
+#define BG_RBSEL_M			GENMASK(7, 6)
+#define BG_RASEL_S			4
+#define BG_RASEL_M			GENMASK(5, 4)
+#define BGR_DIV_S			2
+#define BGR_DIV_M			GENMASK(3, 2)
+#define CHP_EN				BIT(1)
+
 #define OFS_U2_PHY_AC1			0x804
+#define VRT_VREF_SEL_S			28
+#define VRT_VREF_SEL_M			GENMASK(30, 28)
+#define TERM_VREF_SEL_S			24
+#define TERM_VREF_SEL_M			GENMASK(26, 24)
+#define USBPLL_RSVD			BIT(4)
+#define USBPLL_ACCEN			BIT(3)
+#define USBPLL_LF			BIT(2)
+
 #define OFS_U2_PHY_AC2			0x808
+
 #define OFS_U2_PHY_ACR0			0x810
-#define OFS_U2_PHY_ACR1			0x814
-#define OFS_U2_PHY_ACR2			0x818
+#define HSTX_SRCAL_EN			BIT(23)
+#define HSTX_SRCTRL_S			16
+#define HSTX_SRCTRL_M			GENMASK(18, 16)
+
 #define OFS_U2_PHY_ACR3			0x81C
-#define OFS_U2_PHY_ACR4			0x820
-#define OFS_U2_PHY_AMON0		0x824
+#define HSTX_DBIST_S			28
+#define HSTX_DBIST_M			GENMASK(31, 28)
+#define HSRX_BIAS_EN_SEL_S		20
+#define HSRX_BIAS_EN_SEL_M		GENMASK(21, 20)
+
 #define OFS_U2_PHY_DCR0			0x860
-#define OFS_U2_PHY_DCR1			0x864
+#define PHYD_RESERVE_S			8
+#define PHYD_RESERVE_M			GENMASK(23, 8)
+#define CDR_FILT_S			0
+#define CDR_FILT_M			GENMASK(3, 0)
+
 #define OFS_U2_PHY_DTM0			0x868
-#define OFS_U2_PHY_DTM1			0x86C
+#define FORCE_USB_CLKEN			BIT(25)
+
+#define OFS_FM_CR0			0xf00
+#define FREQDET_EN			BIT(24)
+#define CYCLECNT_S			0
+#define CYCLECNT_M			GENMASK(23, 0)
 
-#define RT_RSTCTRL_UDEV			BIT(25)
-#define RT_RSTCTRL_UHST			BIT(22)
-#define RT_SYSCFG1_USB0_HOST_MODE	BIT(10)
+#define OFS_FM_MONR0			0xf0c
 
-#define MT7620_CLKCFG1_UPHY0_CLK_EN	BIT(25)
-#define MT7620_CLKCFG1_UPHY1_CLK_EN	BIT(22)
-#define RT_CLKCFG1_UPHY1_CLK_EN		BIT(20)
-#define RT_CLKCFG1_UPHY0_CLK_EN		BIT(18)
+#define OFS_FM_MONR1			0xf10
+#define FRCK_EN				BIT(8)
 
-#define USB_PHY_UTMI_8B60M		BIT(1)
-#define UDEV_WAKEUP			BIT(0)
+#define U2_SR_COEF_7628			32
 
 struct mt76x8_usb_phy {
-	u32			clk;
 	void __iomem		*base;
-	struct regmap		*sysctl;
+	struct clk		cg;	/* for clock gating */
+	struct reset_ctl	rst_phy;
 };
 
-static void u2_phy_w32(struct mt76x8_usb_phy *phy, u32 val, u32 reg)
+static void phy_w32(struct mt76x8_usb_phy *phy, u32 reg, u32 val)
 {
 	writel(val, phy->base + reg);
 }
 
-static u32 u2_phy_r32(struct mt76x8_usb_phy *phy, u32 reg)
+static u32 phy_r32(struct mt76x8_usb_phy *phy, u32 reg)
 {
 	return readl(phy->base + reg);
 }
 
+static void phy_rmw32(struct mt76x8_usb_phy *phy, u32 reg, u32 clr, u32 set)
+{
+	clrsetbits_32(phy->base + reg, clr, set);
+}
+
 static void mt76x8_usb_phy_init(struct mt76x8_usb_phy *phy)
 {
-	u2_phy_r32(phy, OFS_U2_PHY_AC2);
-	u2_phy_r32(phy, OFS_U2_PHY_ACR0);
-	u2_phy_r32(phy, OFS_U2_PHY_DCR0);
-
-	u2_phy_w32(phy, 0x00ffff02, OFS_U2_PHY_DCR0);
-	u2_phy_r32(phy, OFS_U2_PHY_DCR0);
-	u2_phy_w32(phy, 0x00555502, OFS_U2_PHY_DCR0);
-	u2_phy_r32(phy, OFS_U2_PHY_DCR0);
-	u2_phy_w32(phy, 0x00aaaa02, OFS_U2_PHY_DCR0);
-	u2_phy_r32(phy, OFS_U2_PHY_DCR0);
-	u2_phy_w32(phy, 0x00000402, OFS_U2_PHY_DCR0);
-	u2_phy_r32(phy, OFS_U2_PHY_DCR0);
-	u2_phy_w32(phy, 0x0048086a, OFS_U2_PHY_AC0);
-	u2_phy_w32(phy, 0x4400001c, OFS_U2_PHY_AC1);
-	u2_phy_w32(phy, 0xc0200000, OFS_U2_PHY_ACR3);
-	u2_phy_w32(phy, 0x02000000, OFS_U2_PHY_DTM0);
+	phy_r32(phy, OFS_U2_PHY_AC2);
+	phy_r32(phy, OFS_U2_PHY_ACR0);
+	phy_r32(phy, OFS_U2_PHY_DCR0);
+
+	phy_w32(phy, OFS_U2_PHY_DCR0,
+		(0xffff << PHYD_RESERVE_S) | (2 << CDR_FILT_S));
+	phy_r32(phy, OFS_U2_PHY_DCR0);
+
+	phy_w32(phy, OFS_U2_PHY_DCR0,
+		(0x5555 << PHYD_RESERVE_S) | (2 << CDR_FILT_S));
+	phy_r32(phy, OFS_U2_PHY_DCR0);
+
+	phy_w32(phy, OFS_U2_PHY_DCR0,
+		(0xaaaa << PHYD_RESERVE_S) | (2 << CDR_FILT_S));
+	phy_r32(phy, OFS_U2_PHY_DCR0);
+
+	phy_w32(phy, OFS_U2_PHY_DCR0,
+		(4 << PHYD_RESERVE_S) | (2 << CDR_FILT_S));
+	phy_r32(phy, OFS_U2_PHY_DCR0);
+
+	phy_w32(phy, OFS_U2_PHY_AC0,
+		(0x48 << USBPLL_FBDIV_S) | (8 << BG_TRIM_S) |
+		(1 << BG_RBSEL_S) | (2 << BG_RASEL_S) | (2 << BGR_DIV_S) |
+		CHP_EN);
+
+	phy_w32(phy, OFS_U2_PHY_AC1,
+		(4 << VRT_VREF_SEL_S) | (4 << TERM_VREF_SEL_S) | USBPLL_RSVD |
+		USBPLL_ACCEN | USBPLL_LF);
+
+	phy_w32(phy, OFS_U2_PHY_ACR3,
+		(12 << HSTX_DBIST_S) | (2 << HSRX_BIAS_EN_SEL_S));
+
+	phy_w32(phy, OFS_U2_PHY_DTM0, FORCE_USB_CLKEN);
+}
+
+static void mt76x8_usb_phy_sr_calibrate(struct mt76x8_usb_phy *phy)
+{
+	u32 fmout, tmp = 4;
+	int i;
+
+	/* Enable HS TX SR calibration */
+	phy_rmw32(phy, OFS_U2_PHY_ACR0, 0, HSTX_SRCAL_EN);
+	mdelay(1);
+
+	/* Enable free run clock */
+	phy_rmw32(phy, OFS_FM_MONR1, 0, FRCK_EN);
+
+	/* Set cycle count = 0x400 */
+	phy_rmw32(phy, OFS_FM_CR0, CYCLECNT_M, 0x400 << CYCLECNT_S);
+
+	/* Enable frequency meter */
+	phy_rmw32(phy, OFS_FM_CR0, 0, FREQDET_EN);
+
+	/* Wait for FM detection done, set timeout to 10ms */
+	for (i = 0; i < 10; i++) {
+		fmout = phy_r32(phy, OFS_FM_MONR0);
+
+		if (fmout)
+			break;
+
+		mdelay(1);
+	}
+
+	/* Disable frequency meter */
+	phy_rmw32(phy, OFS_FM_CR0, FREQDET_EN, 0);
+
+	/* Disable free run clock */
+	phy_rmw32(phy, OFS_FM_MONR1, FRCK_EN, 0);
+
+	/* Disable HS TX SR calibration */
+	phy_rmw32(phy, OFS_U2_PHY_ACR0, HSTX_SRCAL_EN, 0);
+	mdelay(1);
+
+	if (fmout) {
+		/*
+		 * set reg = (1024 / FM_OUT) * 25 * 0.028
+		 * (round to the nearest digits)
+		 */
+		tmp = (((1024 * 25 * U2_SR_COEF_7628) / fmout) + 500) / 1000;
+	}
+
+	phy_rmw32(phy, OFS_U2_PHY_ACR0, HSTX_SRCTRL_M,
+		  (tmp << HSTX_SRCTRL_S) & HSTX_SRCTRL_M);
 }
 
 static int mt76x8_usb_phy_power_on(struct phy *_phy)
 {
 	struct mt76x8_usb_phy *phy = dev_get_priv(_phy->dev);
-	u32 t;
 
-	/* enable the phy */
-	regmap_update_bits(phy->sysctl, RT_SYSC_REG_CLKCFG1,
-			   phy->clk, phy->clk);
+	clk_enable(&phy->cg);
 
-	/* setup host mode */
-	regmap_update_bits(phy->sysctl, RT_SYSC_REG_SYSCFG1,
-			   RT_SYSCFG1_USB0_HOST_MODE,
-			   RT_SYSCFG1_USB0_HOST_MODE);
+	reset_deassert(&phy->rst_phy);
 
 	/*
 	 * The SDK kernel had a delay of 100ms. however on device
@@ -100,17 +192,8 @@ static int mt76x8_usb_phy_power_on(struct phy *_phy)
 	 */
 	mdelay(10);
 
-	if (phy->base)
-		mt76x8_usb_phy_init(phy);
-
-	/* print some status info */
-	regmap_read(phy->sysctl, RT_SYSC_REG_USB_PHY_CFG, &t);
-	printf("remote usb device wakeup %s\n",
-	       (t & UDEV_WAKEUP) ? "enabled" : "disabled");
-	if (t & USB_PHY_UTMI_8B60M)
-		printf("UTMI 8bit 60MHz\n");
-	else
-		printf("UTMI 16bit 30MHz\n");
+	mt76x8_usb_phy_init(phy);
+	mt76x8_usb_phy_sr_calibrate(phy);
 
 	return 0;
 }
@@ -119,9 +202,9 @@ static int mt76x8_usb_phy_power_off(struct phy *_phy)
 {
 	struct mt76x8_usb_phy *phy = dev_get_priv(_phy->dev);
 
-	/* disable the phy */
-	regmap_update_bits(phy->sysctl, RT_SYSC_REG_CLKCFG1,
-			   phy->clk, 0);
+	clk_disable(&phy->cg);
+
+	reset_assert(&phy->rst_phy);
 
 	return 0;
 }
@@ -129,15 +212,21 @@ static int mt76x8_usb_phy_power_off(struct phy *_phy)
 static int mt76x8_usb_phy_probe(struct udevice *dev)
 {
 	struct mt76x8_usb_phy *phy = dev_get_priv(dev);
-
-	phy->sysctl = syscon_regmap_lookup_by_phandle(dev, "ralink,sysctl");
-	if (IS_ERR(phy->sysctl))
-		return PTR_ERR(phy->sysctl);
+	int ret;
 
 	phy->base = dev_read_addr_ptr(dev);
 	if (!phy->base)
 		return -EINVAL;
 
+	/* clock gate */
+	ret = clk_get_by_name(dev, "cg", &phy->cg);
+	if (ret)
+		return ret;
+
+	ret = reset_get_by_name(dev, "phy", &phy->rst_phy);
+	if (ret)
+		return ret;
+
 	return 0;
 }
 
diff --git a/drivers/pinctrl/Kconfig b/drivers/pinctrl/Kconfig
index deee92411c..eadcfd6652 100644
--- a/drivers/pinctrl/Kconfig
+++ b/drivers/pinctrl/Kconfig
@@ -269,6 +269,7 @@ source "drivers/pinctrl/exynos/Kconfig"
 source "drivers/pinctrl/mediatek/Kconfig"
 source "drivers/pinctrl/meson/Kconfig"
 source "drivers/pinctrl/mscc/Kconfig"
+source "drivers/pinctrl/mtmips/Kconfig"
 source "drivers/pinctrl/mvebu/Kconfig"
 source "drivers/pinctrl/nxp/Kconfig"
 source "drivers/pinctrl/renesas/Kconfig"
diff --git a/drivers/pinctrl/Makefile b/drivers/pinctrl/Makefile
index 4b080b74dc..ce0879a2b7 100644
--- a/drivers/pinctrl/Makefile
+++ b/drivers/pinctrl/Makefile
@@ -9,6 +9,7 @@ obj-y					+= nxp/
 obj-$(CONFIG_$(SPL_)PINCTRL_ROCKCHIP)	+= rockchip/
 obj-$(CONFIG_ARCH_ASPEED) += aspeed/
 obj-$(CONFIG_ARCH_ATH79) += ath79/
+obj-$(CONFIG_ARCH_MTMIPS) += mtmips/
 obj-$(CONFIG_ARCH_RMOBILE) += renesas/
 obj-$(CONFIG_PINCTRL_SANDBOX)	+= pinctrl-sandbox.o
 
diff --git a/drivers/pinctrl/mtmips/Kconfig b/drivers/pinctrl/mtmips/Kconfig
new file mode 100644
index 0000000000..8482a38ebc
--- /dev/null
+++ b/drivers/pinctrl/mtmips/Kconfig
@@ -0,0 +1,13 @@
+
+config PINCTRL_MTMIPS
+	depends on ARCH_MTMIPS
+	bool
+
+config PINCTRL_MT7628
+	bool "MediaTek MT7628 pin control driver"
+	select PINCTRL_MTMIPS
+	depends on SOC_MT7628 && PINCTRL_GENERIC
+	help
+	  Support pin multiplexing control on MediaTek MT7628.
+	  The driver is controlled by a device tree node which contains
+	  the pin mux functions for each available pin groups.
diff --git a/drivers/pinctrl/mtmips/Makefile b/drivers/pinctrl/mtmips/Makefile
new file mode 100644
index 0000000000..3ba5c0c66d
--- /dev/null
+++ b/drivers/pinctrl/mtmips/Makefile
@@ -0,0 +1,7 @@
+# SPDX-License-Identifier: GPL-2.0
+
+# Core
+obj-$(CONFIG_PINCTRL_MTMIPS) += pinctrl-mtmips-common.o
+
+# SoC Drivers
+obj-$(CONFIG_PINCTRL_MT7628) += pinctrl-mt7628.o
diff --git a/drivers/pinctrl/mtmips/pinctrl-mt7628.c b/drivers/pinctrl/mtmips/pinctrl-mt7628.c
new file mode 100644
index 0000000000..fc9d8b5ad1
--- /dev/null
+++ b/drivers/pinctrl/mtmips/pinctrl-mt7628.c
@@ -0,0 +1,585 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <dm/pinctrl.h>
+#include <linux/bitops.h>
+#include <linux/io.h>
+
+#include "pinctrl-mtmips-common.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define AGPIO_OFS			0
+#define GPIOMODE1_OFS			0x24
+#define GPIOMODE2_OFS			0x28
+
+#define EPHY4_1_PAD_SHIFT		17
+#define EPHY4_1_PAD_MASK		0x0f
+#define EPHY0_SHIFT			16
+#define RF_OLT_MODE_SHIFT		12
+#define N9_EINT_SRC_SHIFT		9
+#define WLED_OD_SHIFT			8
+#define REF_CLKO_PAD_SHIFT		4
+#define I2S_CLK_PAD_SHIFT		3
+#define I2S_WS_PAD_SHIFT		2
+#define I2S_SDO_PAD_SHIFT		1
+#define I2S_SDI_PAD_SHIFT		0
+
+#define GM4_MASK			3
+
+#define P4LED_K_SHIFT			26
+#define P3LED_K_SHIFT			24
+#define P2LED_K_SHIFT			22
+#define P1LED_K_SHIFT			20
+#define P0LED_K_SHIFT			18
+#define WLED_K_SHIFT			16
+#define P4LED_A_SHIFT			10
+#define P3LED_A_SHIFT			8
+#define P2LED_A_SHIFT			6
+#define P1LED_A_SHIFT			4
+#define P0LED_A_SHIFT			2
+#define WLED_A_SHIFT			0
+
+#define PWM1_SHIFT			30
+#define PWM0_SHIFT			28
+#define UART2_SHIFT			26
+#define UART1_SHIFT			24
+#define I2C_SHIFT			20
+#define REFCLK_SHIFT			18
+#define PERST_SHIFT			16
+#define ESD_SHIFT			15
+#define WDT_SHIFT			14
+#define SPI_SHIFT			12
+#define SDMODE_SHIFT			10
+#define UART0_SHIFT			8
+#define I2S_SHIFT			6
+#define SPI_CS1_SHIFT			4
+#define SPIS_SHIFT			2
+#define GPIO0_SHIFT			0
+
+#define PAD_PU_G0_REG			0x00
+#define PAD_PU_G1_REG			0x04
+#define PAD_PD_G0_REG			0x10
+#define PAD_PD_G1_REG			0x14
+#define PAD_SR_G0_REG			0x20
+#define PAD_SR_G1_REG			0x24
+#define PAD_SMT_G0_REG			0x30
+#define PAD_SMT_G1_REG			0x34
+#define PAD_E2_G0_REG			0x40
+#define PAD_E2_G1_REG			0x44
+#define PAD_E4_G0_REG			0x50
+#define PAD_E4_G1_REG			0x54
+#define PAD_E8_G0_REG			0x60
+#define PAD_E8_G1_REG			0x64
+
+#define PIN_CONFIG_DRIVE_STRENGTH_28	(PIN_CONFIG_END + 1)
+#define PIN_CONFIG_DRIVE_STRENGTH_4G	(PIN_CONFIG_END + 2)
+
+struct mt7628_pinctrl_priv {
+	struct mtmips_pinctrl_priv mp;
+
+	void __iomem *pcbase;
+};
+
+#if CONFIG_IS_ENABLED(PINMUX)
+static const struct mtmips_pmx_func ephy4_1_pad_grp[] = {
+	FUNC("digital", 0xf),
+	FUNC("analog", 0),
+};
+
+static const struct mtmips_pmx_func ephy0_grp[] = {
+	FUNC("disable", 1),
+	FUNC("enable", 0),
+};
+
+static const struct mtmips_pmx_func rf_olt_grp[] = {
+	FUNC("enable", 1),
+	FUNC("disable", 0),
+};
+
+static const struct mtmips_pmx_func n9_eint_src_grp[] = {
+	FUNC("gpio", 1),
+	FUNC("utif", 0),
+};
+
+static const struct mtmips_pmx_func wlen_od_grp[] = {
+	FUNC("enable", 1),
+	FUNC("disable", 0),
+};
+
+static const struct mtmips_pmx_func ref_clko_grp[] = {
+	FUNC("digital", 1),
+	FUNC("analog", 0),
+};
+
+static const struct mtmips_pmx_func i2s_clk_grp[] = {
+	FUNC("digital", 1),
+	FUNC("analog", 0),
+};
+
+static const struct mtmips_pmx_func i2s_ws_grp[] = {
+	FUNC("digital", 1),
+	FUNC("analog", 0),
+};
+
+static const struct mtmips_pmx_func i2s_sdo_grp[] = {
+	FUNC("digital", 1),
+	FUNC("analog", 0),
+};
+
+static const struct mtmips_pmx_func i2s_sdi_grp[] = {
+	FUNC("digital", 1),
+	FUNC("analog", 0),
+};
+
+static const struct mtmips_pmx_func pwm1_grp[] = {
+	FUNC("sdxc d6", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("pwm1", 0),
+};
+
+static const struct mtmips_pmx_func pwm0_grp[] = {
+	FUNC("sdxc d7", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("pwm0", 0),
+};
+
+static const struct mtmips_pmx_func uart2_grp[] = {
+	FUNC("sdxc d5 d4", 3),
+	FUNC("pwm", 2),
+	FUNC("gpio", 1),
+	FUNC("uart2", 0),
+};
+
+static const struct mtmips_pmx_func uart1_grp[] = {
+	FUNC("sw_r", 3),
+	FUNC("pwm", 2),
+	FUNC("gpio", 1),
+	FUNC("uart1", 0),
+};
+
+static const struct mtmips_pmx_func i2c_grp[] = {
+	FUNC("-", 3),
+	FUNC("debug", 2),
+	FUNC("gpio", 1),
+	FUNC("i2c", 0),
+};
+
+static const struct mtmips_pmx_func refclk_grp[] = {
+	FUNC("gpio", 1),
+	FUNC("refclk", 0),
+};
+
+static const struct mtmips_pmx_func perst_grp[] = {
+	FUNC("gpio", 1),
+	FUNC("perst", 0),
+};
+
+static const struct mtmips_pmx_func esd_grp[] = {
+	FUNC("router", 1),
+	FUNC("iot", 0),
+};
+
+static const struct mtmips_pmx_func wdt_grp[] = {
+	FUNC("gpio", 1),
+	FUNC("wdt", 0),
+};
+
+static const struct mtmips_pmx_func spi_grp[] = {
+	FUNC("gpio", 1),
+	FUNC("spi", 0),
+};
+
+static const struct mtmips_pmx_func sd_mode_grp[] = {
+	FUNC("n9 jtag", 3),
+	FUNC("utif1", 2),
+	FUNC("gpio", 1),
+	FUNC("sdxc", 0),
+};
+
+static const struct mtmips_pmx_func uart0_grp[] = {
+	FUNC("-", 3),
+	FUNC("-", 2),
+	FUNC("gpio", 1),
+	FUNC("uart0", 0),
+};
+
+static const struct mtmips_pmx_func i2s_grp[] = {
+	FUNC("antenna", 3),
+	FUNC("pcm", 2),
+	FUNC("gpio", 1),
+	FUNC("i2s", 0),
+};
+
+static const struct mtmips_pmx_func spi_cs1_grp[] = {
+	FUNC("-", 3),
+	FUNC("refclk", 2),
+	FUNC("gpio", 1),
+	FUNC("spi cs1", 0),
+};
+
+static const struct mtmips_pmx_func spis_grp[] = {
+	FUNC("pwm_uart2", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("spis", 0),
+};
+
+static const struct mtmips_pmx_func gpio0_grp[] = {
+	FUNC("perst", 3),
+	FUNC("refclk", 2),
+	FUNC("gpio", 1),
+	FUNC("gpio0", 0),
+};
+
+static const struct mtmips_pmx_func wled_a_grp[] = {
+	FUNC("-", 3),
+	FUNC("-", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p0led_a_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("rsvd", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p1led_a_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p2led_a_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p3led_a_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p4led_a_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func wled_k_grp[] = {
+	FUNC("-", 3),
+	FUNC("-", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p0led_k_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("rsvd", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p1led_k_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p2led_k_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p3led_k_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_func p4led_k_grp[] = {
+	FUNC("jtag", 3),
+	FUNC("utif", 2),
+	FUNC("gpio", 1),
+	FUNC("led", 0),
+};
+
+static const struct mtmips_pmx_group mt7628_pinmux_data[] = {
+	GRP("ephy4_1_pad", ephy4_1_pad_grp, AGPIO_OFS, EPHY4_1_PAD_SHIFT,
+	    EPHY4_1_PAD_MASK),
+	GRP("ephy0", ephy0_grp, AGPIO_OFS, EPHY0_SHIFT, 1),
+	GRP("rf_olt", rf_olt_grp, AGPIO_OFS, RF_OLT_MODE_SHIFT, 1),
+	GRP("n9_eint_src", n9_eint_src_grp, AGPIO_OFS, N9_EINT_SRC_SHIFT, 1),
+	GRP("wlen_od", wlen_od_grp, AGPIO_OFS, WLED_OD_SHIFT, 1),
+	GRP("ref_clko_pad", ref_clko_grp, AGPIO_OFS, REF_CLKO_PAD_SHIFT, 1),
+	GRP("i2s_clk_pad", i2s_clk_grp, AGPIO_OFS, I2S_CLK_PAD_SHIFT, 1),
+	GRP("i2s_ws_pad", i2s_ws_grp, AGPIO_OFS, I2S_WS_PAD_SHIFT, 1),
+	GRP("i2s_sdo_pad", i2s_sdo_grp, AGPIO_OFS, I2S_SDO_PAD_SHIFT, 1),
+	GRP("i2s_sdi_pad", i2s_sdi_grp, AGPIO_OFS, I2S_SDI_PAD_SHIFT, 1),
+	GRP("pwm1", pwm1_grp, GPIOMODE1_OFS, PWM1_SHIFT, GM4_MASK),
+	GRP("pwm0", pwm0_grp, GPIOMODE1_OFS, PWM0_SHIFT, GM4_MASK),
+	GRP("uart2", uart2_grp, GPIOMODE1_OFS, UART2_SHIFT, GM4_MASK),
+	GRP("uart1", uart1_grp, GPIOMODE1_OFS, UART1_SHIFT, GM4_MASK),
+	GRP("i2c", i2c_grp, GPIOMODE1_OFS, I2C_SHIFT, GM4_MASK),
+	GRP("refclk", refclk_grp, GPIOMODE1_OFS, REFCLK_SHIFT, 1),
+	GRP("perst", perst_grp, GPIOMODE1_OFS, PERST_SHIFT, 1),
+	GRP("sd router", esd_grp, GPIOMODE1_OFS, ESD_SHIFT, 1),
+	GRP("wdt", wdt_grp, GPIOMODE1_OFS, WDT_SHIFT, 1),
+	GRP("spi", spi_grp, GPIOMODE1_OFS, SPI_SHIFT, 1),
+	GRP("sdmode", sd_mode_grp, GPIOMODE1_OFS, SDMODE_SHIFT, GM4_MASK),
+	GRP("uart0", uart0_grp, GPIOMODE1_OFS, UART0_SHIFT, GM4_MASK),
+	GRP("i2s", i2s_grp, GPIOMODE1_OFS, I2S_SHIFT, GM4_MASK),
+	GRP("spi cs1", spi_cs1_grp, GPIOMODE1_OFS, SPI_CS1_SHIFT, GM4_MASK),
+	GRP("spis", spis_grp, GPIOMODE1_OFS, SPIS_SHIFT, GM4_MASK),
+	GRP("gpio0", gpio0_grp, GPIOMODE1_OFS, GPIO0_SHIFT, GM4_MASK),
+	GRP("wled_a", wled_a_grp, GPIOMODE2_OFS, WLED_A_SHIFT, GM4_MASK),
+	GRP("p0led_a", p0led_a_grp, GPIOMODE2_OFS, P0LED_A_SHIFT, GM4_MASK),
+	GRP("p1led_a", p1led_a_grp, GPIOMODE2_OFS, P1LED_A_SHIFT, GM4_MASK),
+	GRP("p2led_a", p2led_a_grp, GPIOMODE2_OFS, P2LED_A_SHIFT, GM4_MASK),
+	GRP("p3led_a", p3led_a_grp, GPIOMODE2_OFS, P3LED_A_SHIFT, GM4_MASK),
+	GRP("p4led_a", p4led_a_grp, GPIOMODE2_OFS, P4LED_A_SHIFT, GM4_MASK),
+	GRP("wled_k", wled_k_grp, GPIOMODE2_OFS, WLED_K_SHIFT, GM4_MASK),
+	GRP("p0led_k", p0led_k_grp, GPIOMODE2_OFS, P0LED_K_SHIFT, GM4_MASK),
+	GRP("p1led_k", p1led_k_grp, GPIOMODE2_OFS, P1LED_K_SHIFT, GM4_MASK),
+	GRP("p2led_k", p2led_k_grp, GPIOMODE2_OFS, P2LED_K_SHIFT, GM4_MASK),
+	GRP("p3led_k", p3led_k_grp, GPIOMODE2_OFS, P3LED_K_SHIFT, GM4_MASK),
+	GRP("p4led_k", p4led_k_grp, GPIOMODE2_OFS, P4LED_K_SHIFT, GM4_MASK),
+};
+
+static int mt7628_get_groups_count(struct udevice *dev)
+{
+	return ARRAY_SIZE(mt7628_pinmux_data);
+}
+
+static const char *mt7628_get_group_name(struct udevice *dev,
+					 unsigned int selector)
+{
+	return mt7628_pinmux_data[selector].name;
+}
+#endif /* CONFIG_IS_ENABLED(PINMUX) */
+
+#if CONFIG_IS_ENABLED(PINCONF)
+static const struct pinconf_param mt7628_conf_params[] = {
+	{ "bias-disable", PIN_CONFIG_BIAS_DISABLE, 0 },
+	{ "bias-pull-up", PIN_CONFIG_BIAS_PULL_UP, 1 },
+	{ "bias-pull-down", PIN_CONFIG_BIAS_PULL_DOWN, 1 },
+	{ "input-schmitt-enable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 1 },
+	{ "input-schmitt-disable", PIN_CONFIG_INPUT_SCHMITT_ENABLE, 0 },
+	{ "drive-strength-28", PIN_CONFIG_DRIVE_STRENGTH_28, 0 },
+	{ "drive-strength-4g", PIN_CONFIG_DRIVE_STRENGTH_4G, 0 },
+	{ "slew-rate", PIN_CONFIG_SLEW_RATE, 0 },
+};
+
+static const char *const mt7628_pins[] = {
+	"i2s_sdi",
+	"i2s_sdo",
+	"i2s_ws",
+	"i2s_clk",
+	"i2s_sclk",
+	"i2c_sd",
+	"spi_cs1",
+	"spi_clk",
+	"spi_mosi",
+	"spi_miso",
+	"spi_cs0",
+	"gpio0",
+	"uart0_txd",
+	"uart0_rxd",
+	"spis_cs",
+	"spis_clk",
+	"spis_miso",
+	"spis_mosi",
+	"pwm_ch0",
+	"pwm_ch1",
+	"uart2_txd",
+	"uart2_rxd",
+	"sd_wp",
+	"sd_cd",
+	"sd_d1",
+	"sd_d0",
+	"sd_clk",
+	"sd_cmd",
+	"sd_d3",
+	"sd_d2",
+	"ephy_led4_k",
+	"ephy_led3_k",
+	"ephy_led2_k",
+	"ephy_led1_k",
+	"ephy_led0_k",
+	"wled_k",
+	"perst_n",
+	"co_clko",
+	"wdt",
+	"ephy_led4_a",
+	"ephy_led3_a",
+	"ephy_led2_a",
+	"ephy_led1_a",
+	"ephy_led0_a",
+	"wled_a",
+	"uart1_txd",
+	"uart1_rxd",
+};
+
+static const u32 mt7628_drv_strength_28_tbl[] = {2, 4, 6, 8};
+static const u32 mt7628_drv_strength_4g_tbl[] = {4, 8, 12, 16};
+
+static int mt7628_set_drv_strength(void __iomem *base, u32 val, u32 bit,
+				   const u32 tbl[], u32 reg_lo, u32 reg_hi)
+{
+	int i;
+
+	for (i = 0; i < 4; i++)
+		if (tbl[i] == val)
+			break;
+
+	if (i >= 4)
+		return -EINVAL;
+
+	clrsetbits_32(base + reg_lo, BIT(bit), (i & 1) << bit);
+	clrsetbits_32(base + reg_hi, BIT(bit), ((i >> 1) & 1) << bit);
+
+	return 0;
+}
+
+static int mt7628_get_pins_count(struct udevice *dev)
+{
+	return ARRAY_SIZE(mt7628_pins);
+}
+
+static const char *mt7628_get_pin_name(struct udevice *dev,
+				       unsigned int selector)
+{
+	return mt7628_pins[selector];
+}
+
+static int mt7628_pinconf_set(struct udevice *dev, unsigned int pin_selector,
+			      unsigned int param, unsigned int argument)
+{
+	struct mt7628_pinctrl_priv *priv = dev_get_priv(dev);
+	u32 offs, bit;
+	int ret = 0;
+
+	offs = (pin_selector / 32) * 4;
+	bit = pin_selector % 32;
+
+	switch (param) {
+	case PIN_CONFIG_BIAS_DISABLE:
+		clrbits_32(priv->pcbase + offs + PAD_PU_G0_REG, BIT(bit));
+		clrbits_32(priv->pcbase + offs + PAD_PD_G0_REG, BIT(bit));
+		break;
+	case PIN_CONFIG_BIAS_PULL_UP:
+		setbits_32(priv->pcbase + offs + PAD_PU_G0_REG, BIT(bit));
+		clrbits_32(priv->pcbase + offs + PAD_PD_G0_REG, BIT(bit));
+		break;
+	case PIN_CONFIG_BIAS_PULL_DOWN:
+		clrbits_32(priv->pcbase + offs + PAD_PU_G0_REG, BIT(bit));
+		setbits_32(priv->pcbase + offs + PAD_PD_G0_REG, BIT(bit));
+		break;
+	case PIN_CONFIG_INPUT_SCHMITT_ENABLE:
+		clrsetbits_32(priv->pcbase + offs + PAD_SMT_G0_REG,
+			      BIT(bit), (!!argument) << bit);
+		break;
+	case PIN_CONFIG_DRIVE_STRENGTH_28:
+		ret = mt7628_set_drv_strength(priv->pcbase + offs, argument,
+					      bit, mt7628_drv_strength_28_tbl,
+					      PAD_E2_G0_REG, PAD_E4_G0_REG);
+		break;
+	case PIN_CONFIG_DRIVE_STRENGTH_4G:
+		ret = mt7628_set_drv_strength(priv->pcbase + offs, argument,
+					      bit, mt7628_drv_strength_4g_tbl,
+					      PAD_E4_G0_REG, PAD_E8_G0_REG);
+		break;
+	case PIN_CONFIG_SLEW_RATE:
+		clrsetbits_32(priv->pcbase + offs + PAD_SR_G0_REG,
+			      BIT(bit), (!!argument) << bit);
+		break;
+	default:
+		ret = -EINVAL;
+	}
+
+	return ret;
+}
+#endif
+
+static int mt7628_pinctrl_probe(struct udevice *dev)
+{
+	struct mt7628_pinctrl_priv *priv = dev_get_priv(dev);
+	int ret = 0;
+
+#if CONFIG_IS_ENABLED(PINMUX)
+	ret = mtmips_pinctrl_probe(&priv->mp, ARRAY_SIZE(mt7628_pinmux_data),
+				   mt7628_pinmux_data);
+#endif /* CONFIG_IS_ENABLED(PINMUX) */
+
+	return ret;
+}
+
+static int mt7628_pinctrl_ofdata_to_platdata(struct udevice *dev)
+{
+	struct mt7628_pinctrl_priv *priv = dev_get_priv(dev);
+
+	priv->mp.base = (void __iomem *)dev_remap_addr_index(dev, 0);
+
+	if (!priv->mp.base)
+		return -EINVAL;
+
+	priv->pcbase = (void __iomem *)dev_remap_addr_index(dev, 1);
+
+	if (!priv->pcbase)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct pinctrl_ops mt7628_pinctrl_ops = {
+#if CONFIG_IS_ENABLED(PINMUX)
+	.get_groups_count = mt7628_get_groups_count,
+	.get_group_name = mt7628_get_group_name,
+	.get_functions_count = mtmips_get_functions_count,
+	.get_function_name = mtmips_get_function_name,
+	.pinmux_group_set = mtmips_pinmux_group_set,
+#endif /* CONFIG_IS_ENABLED(PINMUX) */
+#if CONFIG_IS_ENABLED(PINCONF)
+	.pinconf_num_params = ARRAY_SIZE(mt7628_conf_params),
+	.pinconf_params = mt7628_conf_params,
+	.get_pins_count = mt7628_get_pins_count,
+	.get_pin_name = mt7628_get_pin_name,
+	.pinconf_set = mt7628_pinconf_set,
+#endif /* CONFIG_IS_ENABLED(PINCONF) */
+	.set_state = pinctrl_generic_set_state,
+};
+
+static const struct udevice_id mt7628_pinctrl_ids[] = {
+	{ .compatible = "mediatek,mt7628-pinctrl" },
+	{ }
+};
+
+U_BOOT_DRIVER(mt7628_pinctrl) = {
+	.name = "mt7628-pinctrl",
+	.id = UCLASS_PINCTRL,
+	.of_match = mt7628_pinctrl_ids,
+	.ofdata_to_platdata = mt7628_pinctrl_ofdata_to_platdata,
+	.ops = &mt7628_pinctrl_ops,
+	.probe = mt7628_pinctrl_probe,
+	.priv_auto_alloc_size = sizeof(struct mt7628_pinctrl_priv),
+};
diff --git a/drivers/pinctrl/mtmips/pinctrl-mtmips-common.c b/drivers/pinctrl/mtmips/pinctrl-mtmips-common.c
new file mode 100644
index 0000000000..ee6a9d1fc8
--- /dev/null
+++ b/drivers/pinctrl/mtmips/pinctrl-mtmips-common.c
@@ -0,0 +1,87 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <dm/pinctrl.h>
+#include <linux/io.h>
+
+#include "pinctrl-mtmips-common.h"
+
+static void mtmips_pinctrl_reg_set(struct mtmips_pinctrl_priv *priv,
+				   u32 reg, u32 shift, u32 mask, u32 value)
+{
+	u32 val;
+
+	val = readl(priv->base + reg);
+	val &= ~(mask << shift);
+	val |= value << shift;
+	writel(val, priv->base + reg);
+}
+
+int mtmips_get_functions_count(struct udevice *dev)
+{
+	struct mtmips_pinctrl_priv *priv = dev_get_priv(dev);
+
+	return priv->nfuncs;
+}
+
+const char *mtmips_get_function_name(struct udevice *dev, unsigned int selector)
+{
+	struct mtmips_pinctrl_priv *priv = dev_get_priv(dev);
+
+	return priv->funcs[selector]->name;
+}
+
+int mtmips_pinmux_group_set(struct udevice *dev, unsigned int group_selector,
+			    unsigned int func_selector)
+{
+	struct mtmips_pinctrl_priv *priv = dev_get_priv(dev);
+	const struct mtmips_pmx_group *grp = &priv->groups[group_selector];
+	const struct mtmips_pmx_func *func = priv->funcs[func_selector];
+	int i;
+
+	if (!grp->nfuncs)
+		return 0;
+
+	for (i = 0; i < grp->nfuncs; i++) {
+		if (!strcmp(grp->funcs[i].name, func->name)) {
+			mtmips_pinctrl_reg_set(priv, grp->reg, grp->shift,
+					       grp->mask, grp->funcs[i].value);
+			return 0;
+		}
+	}
+
+	return -EINVAL;
+}
+
+int mtmips_pinctrl_probe(struct mtmips_pinctrl_priv *priv, u32 ngroups,
+			 const struct mtmips_pmx_group *groups)
+{
+	int i, j, n;
+
+	priv->ngroups = ngroups;
+	priv->groups = groups;
+
+	priv->nfuncs = 0;
+
+	for (i = 0; i < ngroups; i++)
+		priv->nfuncs += groups[i].nfuncs;
+
+	priv->funcs = malloc(priv->nfuncs * sizeof(*priv->funcs));
+	if (!priv->funcs)
+		return -ENOMEM;
+
+	n = 0;
+
+	for (i = 0; i < ngroups; i++) {
+		for (j = 0; j < groups[i].nfuncs; j++)
+			priv->funcs[n++] = &groups[i].funcs[j];
+	}
+
+	return 0;
+}
diff --git a/drivers/pinctrl/mtmips/pinctrl-mtmips-common.h b/drivers/pinctrl/mtmips/pinctrl-mtmips-common.h
new file mode 100644
index 0000000000..b51d8f009c
--- /dev/null
+++ b/drivers/pinctrl/mtmips/pinctrl-mtmips-common.h
@@ -0,0 +1,53 @@
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * Copyright (C) 2019 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#ifndef _PINCTRL_MTMIPS_COMMON_H_
+#define _PINCTRL_MTMIPS_COMMON_H_
+
+#include <common.h>
+
+struct mtmips_pmx_func {
+	const char *name;
+	int value;
+};
+
+struct mtmips_pmx_group {
+	const char *name;
+
+	u32 reg;
+	u32 shift;
+	char mask;
+
+	int nfuncs;
+	const struct mtmips_pmx_func *funcs;
+};
+
+struct mtmips_pinctrl_priv {
+	void __iomem *base;
+
+	u32 ngroups;
+	const struct mtmips_pmx_group *groups;
+
+	u32 nfuncs;
+	const struct mtmips_pmx_func **funcs;
+};
+
+#define FUNC(name, value)	{ name, value }
+
+#define GRP(_name, _funcs, _reg, _shift, _mask) \
+	{ .name = (_name), .reg = (_reg), .shift = (_shift), .mask = (_mask), \
+	  .funcs = (_funcs), .nfuncs = ARRAY_SIZE(_funcs) }
+
+int mtmips_get_functions_count(struct udevice *dev);
+const char *mtmips_get_function_name(struct udevice *dev,
+				     unsigned int selector);
+int mtmips_pinmux_group_set(struct udevice *dev, unsigned int group_selector,
+			    unsigned int func_selector);
+int mtmips_pinctrl_probe(struct mtmips_pinctrl_priv *priv, u32 ngroups,
+			 const struct mtmips_pmx_group *groups);
+
+#endif /* _PINCTRL_MTMIPS_COMMON_H_ */
diff --git a/drivers/reset/Kconfig b/drivers/reset/Kconfig
index 976f3a701c..cff00820e4 100644
--- a/drivers/reset/Kconfig
+++ b/drivers/reset/Kconfig
@@ -113,6 +113,13 @@ config RESET_MEDIATEK
 	help
 	  Support for reset controller on MediaTek SoCs.
 
+config RESET_MTMIPS
+	bool "Reset controller driver for MediaTek MIPS platform"
+	depends on DM_RESET && ARCH_MTMIPS
+	default y
+	help
+	  Support for reset controller on MediaTek MIPS platform.
+
 config RESET_SUNXI
 	bool "RESET support for Allwinner SoCs"
 	depends on DM_RESET && ARCH_SUNXI
diff --git a/drivers/reset/Makefile b/drivers/reset/Makefile
index f5875fce09..8102d8db29 100644
--- a/drivers/reset/Makefile
+++ b/drivers/reset/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_RESET_ROCKCHIP) += reset-rockchip.o
 obj-$(CONFIG_RESET_MESON) += reset-meson.o
 obj-$(CONFIG_RESET_SOCFPGA) += reset-socfpga.o
 obj-$(CONFIG_RESET_MEDIATEK) += reset-mediatek.o
+obj-$(CONFIG_RESET_MTMIPS) += reset-mtmips.o
 obj-$(CONFIG_RESET_SUNXI) += reset-sunxi.o
 obj-$(CONFIG_RESET_HISILICON) += reset-hisilicon.o
 obj-$(CONFIG_RESET_IMX7) += reset-imx7.o
diff --git a/drivers/reset/reset-mtmips.c b/drivers/reset/reset-mtmips.c
new file mode 100644
index 0000000000..59734565d7
--- /dev/null
+++ b/drivers/reset/reset-mtmips.c
@@ -0,0 +1,82 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Copyright (C) 2019 MediaTek Inc. All Rights Reserved.
+ *
+ * Author: Weijie Gao <weijie.gao@mediatek.com>
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <errno.h>
+#include <reset-uclass.h>
+#include <linux/io.h>
+
+struct mtmips_reset_priv {
+	void __iomem *base;
+};
+
+static int mtmips_reset_request(struct reset_ctl *reset_ctl)
+{
+	return 0;
+}
+
+static int mtmips_reset_free(struct reset_ctl *reset_ctl)
+{
+	return 0;
+}
+
+static int mtmips_reset_assert(struct reset_ctl *reset_ctl)
+{
+	struct mtmips_reset_priv *priv = dev_get_priv(reset_ctl->dev);
+
+	setbits_32(priv->base, BIT(reset_ctl->id));
+
+	return 0;
+}
+
+static int mtmips_reset_deassert(struct reset_ctl *reset_ctl)
+{
+	struct mtmips_reset_priv *priv = dev_get_priv(reset_ctl->dev);
+
+	clrbits_32(priv->base, BIT(reset_ctl->id));
+
+	return 0;
+}
+
+static const struct reset_ops mtmips_reset_ops = {
+	.request	= mtmips_reset_request,
+	.free		= mtmips_reset_free,
+	.rst_assert	= mtmips_reset_assert,
+	.rst_deassert	= mtmips_reset_deassert,
+};
+
+static int mtmips_reset_probe(struct udevice *dev)
+{
+	return 0;
+}
+
+static int mtmips_reset_ofdata_to_platdata(struct udevice *dev)
+{
+	struct mtmips_reset_priv *priv = dev_get_priv(dev);
+
+	priv->base = (void __iomem *)dev_remap_addr_index(dev, 0);
+	if (!priv->base)
+		return -EINVAL;
+
+	return 0;
+}
+
+static const struct udevice_id mtmips_reset_ids[] = {
+	{ .compatible = "mediatek,mtmips-reset" },
+	{ }
+};
+
+U_BOOT_DRIVER(mtmips_reset) = {
+	.name = "mtmips-reset",
+	.id = UCLASS_RESET,
+	.of_match = mtmips_reset_ids,
+	.ofdata_to_platdata = mtmips_reset_ofdata_to_platdata,
+	.probe = mtmips_reset_probe,
+	.priv_auto_alloc_size = sizeof(struct mtmips_reset_priv),
+	.ops = &mtmips_reset_ops,
+};
diff --git a/drivers/serial/serial.c b/drivers/serial/serial.c
index b907508dbe..bf5f39215d 100644
--- a/drivers/serial/serial.c
+++ b/drivers/serial/serial.c
@@ -124,6 +124,7 @@ serial_initfunc(ns16550_serial_initialize);
 serial_initfunc(pl01x_serial_initialize);
 serial_initfunc(pxa_serial_initialize);
 serial_initfunc(sh_serial_initialize);
+serial_initfunc(mtk_serial_initialize);
 
 /**
  * serial_register() - Register serial driver with serial driver core
@@ -177,6 +178,7 @@ void serial_initialize(void)
 	pl01x_serial_initialize();
 	pxa_serial_initialize();
 	sh_serial_initialize();
+	mtk_serial_initialize();
 
 	serial_assign(default_serial_console()->name);
 }
diff --git a/drivers/serial/serial_mtk.c b/drivers/serial/serial_mtk.c
index bce1be8227..18530a4fd1 100644
--- a/drivers/serial/serial_mtk.c
+++ b/drivers/serial/serial_mtk.c
@@ -46,6 +46,22 @@ struct mtk_serial_regs {
 
 #define UART_LSR_DR	0x01		/* Data ready */
 #define UART_LSR_THRE	0x20		/* Xmit holding register empty */
+#define UART_LSR_TEMT	0x40		/* Xmitter empty */
+
+#define UART_MCR_DTR	0x01		/* DTR   */
+#define UART_MCR_RTS	0x02		/* RTS   */
+
+#define UART_FCR_FIFO_EN	0x01	/* Fifo enable */
+#define UART_FCR_RXSR		0x02	/* Receiver soft reset */
+#define UART_FCR_TXSR		0x04	/* Transmitter soft reset */
+
+#define UART_MCRVAL (UART_MCR_DTR | \
+		     UART_MCR_RTS)
+
+/* Clear & enable FIFOs */
+#define UART_FCRVAL (UART_FCR_FIFO_EN | \
+		     UART_FCR_RXSR |	\
+		     UART_FCR_TXSR)
 
 /* the data is correct if the real baud is within 3%. */
 #define BAUD_ALLOW_MAX(baud)	((baud) + (baud) * 3 / 100)
@@ -124,6 +140,37 @@ static void _mtk_serial_setbrg(struct mtk_serial_priv *priv, int baud)
 	}
 }
 
+static int _mtk_serial_putc(struct mtk_serial_priv *priv, const char ch)
+{
+	if (!(readl(&priv->regs->lsr) & UART_LSR_THRE))
+		return -EAGAIN;
+
+	writel(ch, &priv->regs->thr);
+
+	if (ch == '\n')
+		WATCHDOG_RESET();
+
+	return 0;
+}
+
+static int _mtk_serial_getc(struct mtk_serial_priv *priv)
+{
+	if (!(readl(&priv->regs->lsr) & UART_LSR_DR))
+		return -EAGAIN;
+
+	return readl(&priv->regs->rbr);
+}
+
+static int _mtk_serial_pending(struct mtk_serial_priv *priv, bool input)
+{
+	if (input)
+		return (readl(&priv->regs->lsr) & UART_LSR_DR) ? 1 : 0;
+	else
+		return (readl(&priv->regs->lsr) & UART_LSR_THRE) ? 0 : 1;
+}
+
+#if defined(CONFIG_DM_SERIAL) && \
+	(!defined(CONFIG_SPL_BUILD) || defined(CONFIG_SPL_DM))
 static int mtk_serial_setbrg(struct udevice *dev, int baudrate)
 {
 	struct mtk_serial_priv *priv = dev_get_priv(dev);
@@ -137,35 +184,21 @@ static int mtk_serial_putc(struct udevice *dev, const char ch)
 {
 	struct mtk_serial_priv *priv = dev_get_priv(dev);
 
-	if (!(readl(&priv->regs->lsr) & UART_LSR_THRE))
-		return -EAGAIN;
-
-	writel(ch, &priv->regs->thr);
-
-	if (ch == '\n')
-		WATCHDOG_RESET();
-
-	return 0;
+	return _mtk_serial_putc(priv, ch);
 }
 
 static int mtk_serial_getc(struct udevice *dev)
 {
 	struct mtk_serial_priv *priv = dev_get_priv(dev);
 
-	if (!(readl(&priv->regs->lsr) & UART_LSR_DR))
-		return -EAGAIN;
-
-	return readl(&priv->regs->rbr);
+	return _mtk_serial_getc(priv);
 }
 
 static int mtk_serial_pending(struct udevice *dev, bool input)
 {
 	struct mtk_serial_priv *priv = dev_get_priv(dev);
 
-	if (input)
-		return (readl(&priv->regs->lsr) & UART_LSR_DR) ? 1 : 0;
-	else
-		return (readl(&priv->regs->lsr) & UART_LSR_THRE) ? 0 : 1;
+	return _mtk_serial_pending(priv, input);
 }
 
 static int mtk_serial_probe(struct udevice *dev)
@@ -175,6 +208,9 @@ static int mtk_serial_probe(struct udevice *dev)
 	/* Disable interrupt */
 	writel(0, &priv->regs->ier);
 
+	writel(UART_MCRVAL, &priv->regs->mcr);
+	writel(UART_FCRVAL, &priv->regs->fcr);
+
 	return 0;
 }
 
@@ -235,6 +271,157 @@ U_BOOT_DRIVER(serial_mtk) = {
 	.ops = &mtk_serial_ops,
 	.flags = DM_FLAG_PRE_RELOC,
 };
+#else
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#define DECLARE_HSUART_PRIV(port) \
+	static struct mtk_serial_priv mtk_hsuart##port = { \
+	.regs = (struct mtk_serial_regs *)CONFIG_SYS_NS16550_COM##port, \
+	.clock = CONFIG_SYS_NS16550_CLK \
+};
+
+#define DECLARE_HSUART_FUNCTIONS(port) \
+	static int mtk_serial##port##_init(void) \
+	{ \
+		writel(0, &mtk_hsuart##port.regs->ier); \
+		writel(UART_MCRVAL, &mtk_hsuart##port.regs->mcr); \
+		writel(UART_FCRVAL, &mtk_hsuart##port.regs->fcr); \
+		_mtk_serial_setbrg(&mtk_hsuart##port, gd->baudrate); \
+		return 0 ; \
+	} \
+	static void mtk_serial##port##_setbrg(void) \
+	{ \
+		_mtk_serial_setbrg(&mtk_hsuart##port, gd->baudrate); \
+	} \
+	static int mtk_serial##port##_getc(void) \
+	{ \
+		int err; \
+		do { \
+			err = _mtk_serial_getc(&mtk_hsuart##port); \
+			if (err == -EAGAIN) \
+				WATCHDOG_RESET(); \
+		} while (err == -EAGAIN); \
+		return err >= 0 ? err : 0; \
+	} \
+	static int mtk_serial##port##_tstc(void) \
+	{ \
+		return _mtk_serial_pending(&mtk_hsuart##port, true); \
+	} \
+	static void mtk_serial##port##_putc(const char c) \
+	{ \
+		int err; \
+		if (c == '\n') \
+			mtk_serial##port##_putc('\r'); \
+		do { \
+			err = _mtk_serial_putc(&mtk_hsuart##port, c); \
+		} while (err == -EAGAIN); \
+	} \
+	static void mtk_serial##port##_puts(const char *s) \
+	{ \
+		while (*s) { \
+			mtk_serial##port##_putc(*s++); \
+		} \
+	}
+
+/* Serial device descriptor */
+#define INIT_HSUART_STRUCTURE(port, __name) {	\
+	.name	= __name,			\
+	.start	= mtk_serial##port##_init,	\
+	.stop	= NULL,				\
+	.setbrg	= mtk_serial##port##_setbrg,	\
+	.getc	= mtk_serial##port##_getc,	\
+	.tstc	= mtk_serial##port##_tstc,	\
+	.putc	= mtk_serial##port##_putc,	\
+	.puts	= mtk_serial##port##_puts,	\
+}
+
+#define DECLARE_HSUART(port, __name) \
+	DECLARE_HSUART_PRIV(port); \
+	DECLARE_HSUART_FUNCTIONS(port); \
+	struct serial_device mtk_hsuart##port##_device = \
+		INIT_HSUART_STRUCTURE(port, __name);
+
+#if !defined(CONFIG_CONS_INDEX)
+#elif (CONFIG_CONS_INDEX < 1) || (CONFIG_CONS_INDEX > 6)
+#error	"Invalid console index value."
+#endif
+
+#if CONFIG_CONS_INDEX == 1 && !defined(CONFIG_SYS_NS16550_COM1)
+#error	"Console port 1 defined but not configured."
+#elif CONFIG_CONS_INDEX == 2 && !defined(CONFIG_SYS_NS16550_COM2)
+#error	"Console port 2 defined but not configured."
+#elif CONFIG_CONS_INDEX == 3 && !defined(CONFIG_SYS_NS16550_COM3)
+#error	"Console port 3 defined but not configured."
+#elif CONFIG_CONS_INDEX == 4 && !defined(CONFIG_SYS_NS16550_COM4)
+#error	"Console port 4 defined but not configured."
+#elif CONFIG_CONS_INDEX == 5 && !defined(CONFIG_SYS_NS16550_COM5)
+#error	"Console port 5 defined but not configured."
+#elif CONFIG_CONS_INDEX == 6 && !defined(CONFIG_SYS_NS16550_COM6)
+#error	"Console port 6 defined but not configured."
+#endif
+
+#if defined(CONFIG_SYS_NS16550_COM1)
+DECLARE_HSUART(1, "mtk-hsuart0");
+#endif
+#if defined(CONFIG_SYS_NS16550_COM2)
+DECLARE_HSUART(2, "mtk-hsuart1");
+#endif
+#if defined(CONFIG_SYS_NS16550_COM3)
+DECLARE_HSUART(3, "mtk-hsuart2");
+#endif
+#if defined(CONFIG_SYS_NS16550_COM4)
+DECLARE_HSUART(4, "mtk-hsuart3");
+#endif
+#if defined(CONFIG_SYS_NS16550_COM5)
+DECLARE_HSUART(5, "mtk-hsuart4");
+#endif
+#if defined(CONFIG_SYS_NS16550_COM6)
+DECLARE_HSUART(6, "mtk-hsuart5");
+#endif
+
+__weak struct serial_device *default_serial_console(void)
+{
+#if CONFIG_CONS_INDEX == 1
+	return &mtk_hsuart1_device;
+#elif CONFIG_CONS_INDEX == 2
+	return &mtk_hsuart2_device;
+#elif CONFIG_CONS_INDEX == 3
+	return &mtk_hsuart3_device;
+#elif CONFIG_CONS_INDEX == 4
+	return &mtk_hsuart4_device;
+#elif CONFIG_CONS_INDEX == 5
+	return &mtk_hsuart5_device;
+#elif CONFIG_CONS_INDEX == 6
+	return &mtk_hsuart6_device;
+#else
+#error "Bad CONFIG_CONS_INDEX."
+#endif
+}
+
+void mtk_serial_initialize(void)
+{
+#if defined(CONFIG_SYS_NS16550_COM1)
+	serial_register(&mtk_hsuart1_device);
+#endif
+#if defined(CONFIG_SYS_NS16550_COM2)
+	serial_register(&mtk_hsuart2_device);
+#endif
+#if defined(CONFIG_SYS_NS16550_COM3)
+	serial_register(&mtk_hsuart3_device);
+#endif
+#if defined(CONFIG_SYS_NS16550_COM4)
+	serial_register(&mtk_hsuart4_device);
+#endif
+#if defined(CONFIG_SYS_NS16550_COM5)
+	serial_register(&mtk_hsuart5_device);
+#endif
+#if defined(CONFIG_SYS_NS16550_COM6)
+	serial_register(&mtk_hsuart6_device);
+#endif
+}
+
+#endif
 
 #ifdef CONFIG_DEBUG_UART_MTK
 
@@ -248,6 +435,8 @@ static inline void _debug_uart_init(void)
 	priv.clock = CONFIG_DEBUG_UART_CLOCK;
 
 	writel(0, &priv.regs->ier);
+	writel(UART_MCRVAL, &priv.regs->mcr);
+	writel(UART_FCRVAL, &priv.regs->fcr);
 
 	_mtk_serial_setbrg(&priv, CONFIG_BAUDRATE);
 }
diff --git a/drivers/spi/mt7621_spi.c b/drivers/spi/mt7621_spi.c
index 107e58f657..90e85c6b44 100644
--- a/drivers/spi/mt7621_spi.c
+++ b/drivers/spi/mt7621_spi.c
@@ -9,18 +9,22 @@
  */
 
 #include <common.h>
+#include <clk.h>
 #include <dm.h>
 #include <spi.h>
 #include <wait_bit.h>
 #include <linux/io.h>
 
-#define SPI_MSG_SIZE_MAX	32	/* SPI message chunk size */
-/* Enough for SPI NAND page read / write with page size 2048 bytes */
-#define SPI_MSG_SIZE_OVERALL	(2048 + 16)
+#define MT7621_RX_FIFO_LEN	32
+#define MT7621_TX_FIFO_LEN	36
 
 #define MT7621_SPI_TRANS	0x00
 #define MT7621_SPI_TRANS_START	BIT(8)
 #define MT7621_SPI_TRANS_BUSY	BIT(16)
+#define TRANS_ADDR_SZ		GENMASK(20, 19)
+#define TRANS_ADDR_SZ_SHIFT	19
+#define TRANS_MOSI_BCNT		GENMASK(3, 0)
+#define TRANS_MOSI_BCNT_SHIFT	0
 
 #define MT7621_SPI_OPCODE	0x04
 #define MT7621_SPI_DATA0	0x08
@@ -38,27 +42,34 @@
 #define MASTER_RS_CLK_SEL_SHIFT	16
 #define MASTER_RS_SLAVE_SEL	GENMASK(31, 29)
 
+#define MOREBUF_CMD_CNT		GENMASK(29, 24)
+#define MOREBUF_CMD_CNT_SHIFT	24
+#define MOREBUF_MISO_CNT	GENMASK(20, 12)
+#define MOREBUF_MISO_CNT_SHIFT	12
+#define MOREBUF_MOSI_CNT	GENMASK(8, 0)
+#define MOREBUF_MOSI_CNT_SHIFT	0
+
 struct mt7621_spi {
 	void __iomem *base;
 	unsigned int sys_freq;
-	u32 data[(SPI_MSG_SIZE_OVERALL / 4) + 1];
-	int tx_len;
 };
 
-static void mt7621_spi_reset(struct mt7621_spi *rs, int duplex)
-{
-	setbits_le32(rs->base + MT7621_SPI_MASTER,
-		     MASTER_RS_SLAVE_SEL | MASTER_MORE_BUFMODE);
-}
-
 static void mt7621_spi_set_cs(struct mt7621_spi *rs, int cs, int enable)
 {
-	u32 val = 0;
-
 	debug("%s: cs#%d -> %s\n", __func__, cs, enable ? "enable" : "disable");
-	if (enable)
-		val = BIT(cs);
-	iowrite32(val, rs->base + MT7621_SPI_POLAR);
+
+	if (enable) {
+		setbits_le32(rs->base + MT7621_SPI_MASTER,
+			     MASTER_RS_SLAVE_SEL | MASTER_MORE_BUFMODE);
+		iowrite32(BIT(cs), rs->base + MT7621_SPI_POLAR);
+	} else {
+		iowrite32(0, rs->base + MT7621_SPI_POLAR);
+		iowrite32((2 << TRANS_ADDR_SZ_SHIFT) |
+			  (1 << TRANS_MOSI_BCNT_SHIFT),
+			  rs->base + MT7621_SPI_TRANS);
+		clrbits_le32(rs->base + MT7621_SPI_MASTER,
+			     MASTER_RS_SLAVE_SEL | MASTER_MORE_BUFMODE);
+	}
 }
 
 static int mt7621_spi_set_mode(struct udevice *bus, uint mode)
@@ -128,20 +139,89 @@ static inline int mt7621_spi_wait_till_ready(struct mt7621_spi *rs)
 	return ret;
 }
 
+static int mt7621_spi_read(struct mt7621_spi *rs, u8 *buf, size_t len)
+{
+	size_t rx_len;
+	int i, ret;
+	u32 val = 0;
+
+	while (len) {
+		rx_len = min_t(size_t, len, MT7621_RX_FIFO_LEN);
+
+		iowrite32((rx_len * 8) << MOREBUF_MISO_CNT_SHIFT,
+			  rs->base + MT7621_SPI_MOREBUF);
+		iowrite32(MT7621_SPI_TRANS_START, rs->base + MT7621_SPI_TRANS);
+
+		ret = mt7621_spi_wait_till_ready(rs);
+		if (ret)
+			return ret;
+
+		for (i = 0; i < rx_len; i++) {
+			if ((i % 4) == 0)
+				val = ioread32(rs->base + MT7621_SPI_DATA0 + i);
+			*buf++ = val & 0xff;
+			val >>= 8;
+		}
+
+		len -= rx_len;
+	}
+
+	return ret;
+}
+
+static int mt7621_spi_write(struct mt7621_spi *rs, const u8 *buf, size_t len)
+{
+	size_t tx_len, opcode_len, dido_len;
+	int i, ret;
+	u32 val;
+
+	while (len) {
+		tx_len = min_t(size_t, len, MT7621_TX_FIFO_LEN);
+
+		opcode_len = min_t(size_t, tx_len, 4);
+		dido_len = tx_len - opcode_len;
+
+		val = 0;
+		for (i = 0; i < opcode_len; i++) {
+			val <<= 8;
+			val |= *buf++;
+		}
+
+		iowrite32(val, rs->base + MT7621_SPI_OPCODE);
+
+		val = 0;
+		for (i = 0; i < dido_len; i++) {
+			val |= (*buf++) << ((i % 4) * 8);
+
+			if ((i % 4 == 3) || (i == dido_len - 1)) {
+				iowrite32(val, rs->base + MT7621_SPI_DATA0 +
+					  (i & ~3));
+				val = 0;
+			}
+		}
+
+		iowrite32(((opcode_len * 8) << MOREBUF_CMD_CNT_SHIFT) |
+			  ((dido_len * 8) << MOREBUF_MOSI_CNT_SHIFT),
+			  rs->base + MT7621_SPI_MOREBUF);
+		iowrite32(MT7621_SPI_TRANS_START, rs->base + MT7621_SPI_TRANS);
+
+		ret = mt7621_spi_wait_till_ready(rs);
+		if (ret)
+			return ret;
+
+		len -= tx_len;
+	}
+
+	return 0;
+}
+
 static int mt7621_spi_xfer(struct udevice *dev, unsigned int bitlen,
 			   const void *dout, void *din, unsigned long flags)
 {
 	struct udevice *bus = dev->parent;
 	struct mt7621_spi *rs = dev_get_priv(bus);
-	const u8 *tx_buf = dout;
-	u8 *ptr = (u8 *)dout;
-	u8 *rx_buf = din;
 	int total_size = bitlen >> 3;
-	int chunk_size;
-	int rx_len = 0;
-	u32 data[(SPI_MSG_SIZE_MAX / 4) + 1] = { 0 };
-	u32 val;
-	int i;
+	int ret = 0;
 
 	debug("%s: dout=%p, din=%p, len=%x, flags=%lx\n", __func__, dout, din,
 	      total_size, flags);
@@ -155,13 +235,6 @@ static int mt7621_spi_xfer(struct udevice *dev, unsigned int bitlen,
 		return -EIO;
 	}
 
-	if (dout) {
-		debug("TX-DATA: ");
-		for (i = 0; i < total_size; i++)
-			debug("%02x ", *ptr++);
-		debug("\n");
-	}
-
 	mt7621_spi_wait_till_ready(rs);
 
 	/*
@@ -171,119 +244,41 @@ static int mt7621_spi_xfer(struct udevice *dev, unsigned int bitlen,
 	if (flags & SPI_XFER_BEGIN)
 		mt7621_spi_set_cs(rs, spi_chip_select(dev), 1);
 
-	while (total_size > 0) {
-		/* Don't exceed the max xfer size */
-		chunk_size = min_t(int, total_size, SPI_MSG_SIZE_MAX);
-
-		/*
-		 * We might have some TX data buffered from the last xfer
-		 * message. Make sure, that this does not exceed the max
-		 * xfer size
-		 */
-		if (rs->tx_len > 4)
-			chunk_size -= rs->tx_len;
-		if (din)
-			rx_len = chunk_size;
-
-		if (tx_buf) {
-			/* Check if this message does not exceed the buffer */
-			if ((chunk_size + rs->tx_len) > SPI_MSG_SIZE_OVERALL) {
-				printf("TX message size too big (%d)\n",
-				       chunk_size + rs->tx_len);
-				return -EMSGSIZE;
-			}
-
-			/*
-			 * Write all TX data into internal buffer to collect
-			 * all TX messages into one buffer (might be split into
-			 * multiple calls to this function)
-			 */
-			for (i = 0; i < chunk_size; i++, rs->tx_len++) {
-				rs->data[rs->tx_len / 4] |=
-					tx_buf[i] << (8 * (rs->tx_len & 3));
-			}
-		}
-
-		if (flags & SPI_XFER_END) {
-			/* Write TX data into controller */
-			if (rs->tx_len) {
-				rs->data[0] = swab32(rs->data[0]);
-				if (rs->tx_len < 4)
-					rs->data[0] >>= (4 - rs->tx_len) * 8;
-
-				for (i = 0; i < rs->tx_len; i += 4) {
-					iowrite32(rs->data[i / 4], rs->base +
-						  MT7621_SPI_OPCODE + i);
-				}
-			}
+	if (din)
+		ret = mt7621_spi_read(rs, din, total_size);
+	else if (dout)
+		ret = mt7621_spi_write(rs, dout, total_size);
 
-			/* Write length into controller */
-			val = (min_t(int, rs->tx_len, 4) * 8) << 24;
-			if (rs->tx_len > 4)
-				val |= (rs->tx_len - 4) * 8;
-			val |= (rx_len * 8) << 12;
-			iowrite32(val, rs->base + MT7621_SPI_MOREBUF);
-
-			/* Start the xfer */
-			setbits_le32(rs->base + MT7621_SPI_TRANS,
-				     MT7621_SPI_TRANS_START);
-
-			/* Wait until xfer is finished on bus */
-			mt7621_spi_wait_till_ready(rs);
-
-			/* Reset TX length and TX buffer for next xfer */
-			rs->tx_len = 0;
-			memset(rs->data, 0, sizeof(rs->data));
-		}
-
-		for (i = 0; i < rx_len; i += 4)
-			data[i / 4] = ioread32(rs->base + MT7621_SPI_DATA0 + i);
-
-		if (rx_len) {
-			debug("RX-DATA: ");
-			for (i = 0; i < rx_len; i++) {
-				rx_buf[i] = data[i / 4] >> (8 * (i & 3));
-				debug("%02x ", rx_buf[i]);
-			}
-			debug("\n");
-		}
-
-		if (tx_buf)
-			tx_buf += chunk_size;
-		if (rx_buf)
-			rx_buf += chunk_size;
-		total_size -= chunk_size;
-	}
-
-	/* Wait until xfer is finished on bus and de-assert CS */
-	mt7621_spi_wait_till_ready(rs);
 	if (flags & SPI_XFER_END)
 		mt7621_spi_set_cs(rs, spi_chip_select(dev), 0);
 
-	return 0;
+	return ret;
 }
 
 static int mt7621_spi_probe(struct udevice *dev)
 {
 	struct mt7621_spi *rs = dev_get_priv(dev);
+	struct clk clk;
+	int ret;
 
 	rs->base = dev_remap_addr(dev);
 	if (!rs->base)
 		return -EINVAL;
 
-	/*
-	 * Read input clock via DT for now. At some point this should be
-	 * replaced by implementing a clock driver for this SoC and getting
-	 * the SPI frequency via this clock driver.
-	 */
-	rs->sys_freq = dev_read_u32_default(dev, "clock-frequency", 0);
+	ret = clk_get_by_index(dev, 0, &clk);
+	if (ret < 0) {
+		printf("Please provide a clock!\n");
+		return ret;
+	}
+
+	clk_enable(&clk);
+
+	rs->sys_freq = clk_get_rate(&clk);
 	if (!rs->sys_freq) {
-		printf("Please provide clock-frequency!\n");
+		printf("Please provide a valid clock!\n");
 		return -EINVAL;
 	}
 
-	mt7621_spi_reset(rs, 0);
-
 	return 0;
 }