ramips: mt7621: add new NAND driver

Add new NAND driver for MT7621

Signed-off-by: DENG Qingfang <dengqf6@mail2.sysu.edu.cn>

4 files changed, 1496 insertions, 2 deletions

diff --git a/target/linux/ramips/dts/mt7621.dtsi b/target/linux/ramips/dts/mt7621.dtsi
index cc387519e4..63befa1fdc 100644
--- a/target/linux/ramips/dts/mt7621.dtsi
+++ b/target/linux/ramips/dts/mt7621.dtsi
@@ -414,13 +414,23 @@
 		};
 	};
 
+	nficlock: nficlock {
+		#clock-cells = <0>;
+		compatible = "fixed-clock";
+
+		clock-frequency = <125000000>;
+	};
+
 	nand: nand@1e003000 {
 		status = "disabled";
 
-		compatible = "mtk,mt7621-nand";
-		bank-width = <2>;
+		compatible = "mediatek,mt7621-nfc";
 		reg = <0x1e003000 0x800
 			0x1e003800 0x800>;
+		reg-names = "nfi", "ecc";
+
+		clocks = <&nficlock>;
+		clock-names = "nfi_clk";
 	};
 
 	ethsys: syscon@1e000000 {
diff --git a/target/linux/ramips/mt7621/config-5.4 b/target/linux/ramips/mt7621/config-5.4
index 20fb53c69c..9fb90b8625 100644
--- a/target/linux/ramips/mt7621/config-5.4
+++ b/target/linux/ramips/mt7621/config-5.4
@@ -202,6 +202,7 @@ CONFIG_MT7621_WDT=y
 CONFIG_MTD_CMDLINE_PARTS=y
 CONFIG_MTD_NAND_CORE=y
 CONFIG_MTD_NAND_ECC_SW_HAMMING=y
+CONFIG_MTD_NAND_MT7621=y
 CONFIG_MTD_PHYSMAP=y
 CONFIG_MTD_RAW_NAND=y
 CONFIG_MTD_SPI_NOR=y
diff --git a/target/linux/ramips/patches-5.4/0300-mtd-rawnand-add-driver-support-for-MT7621-nand-flash.patch b/target/linux/ramips/patches-5.4/0300-mtd-rawnand-add-driver-support-for-MT7621-nand-flash.patch
new file mode 100644
index 0000000000..f65092afb1
--- /dev/null
+++ b/target/linux/ramips/patches-5.4/0300-mtd-rawnand-add-driver-support-for-MT7621-nand-flash.patch
@@ -0,0 +1,1398 @@
+From e84e2430ee0e483842b4ff013ae8a6e7e2fa2734 Mon Sep 17 00:00:00 2001
+From: Weijie Gao <weijie.gao@mediatek.com>
+Date: Wed, 1 Apr 2020 02:07:58 +0800
+Subject: [PATCH 1/2] mtd: rawnand: add driver support for MT7621 nand
+ flash controller
+
+This patch adds NAND flash controller driver for MediaTek MT7621 SoC.
+
+The NAND flash controller is similar with controllers described in
+mtk_nand.c, except that the controller from MT7621 doesn't support DMA
+transmission, and some registers' offset and fields are different.
+
+Signed-off-by: Weijie Gao <weijie.gao@mediatek.com>
+---
+ drivers/mtd/nand/raw/Kconfig       |    8 +
+ drivers/mtd/nand/raw/Makefile      |    1 +
+ drivers/mtd/nand/raw/mt7621_nand.c | 1348 ++++++++++++++++++++++++++++++++++++
+ 3 files changed, 1357 insertions(+)
+ create mode 100644 drivers/mtd/nand/raw/mt7621_nand.c
+
+--- a/drivers/mtd/nand/raw/Kconfig
++++ b/drivers/mtd/nand/raw/Kconfig
+@@ -391,6 +391,14 @@ config MTD_NAND_QCOM
+ 	  Enables support for NAND flash chips on SoCs containing the EBI2 NAND
+ 	  controller. This controller is found on IPQ806x SoC.
+ 
++config MTD_NAND_MT7621
++	tristate "MT7621 NAND controller"
++	depends on SOC_MT7621 || COMPILE_TEST
++	depends on HAS_IOMEM
++	help
++	  Enables support for NAND controller on MT7621 SoC.
++	  This driver uses PIO mode for data transmission instead of DMA mode.
++
+ config MTD_NAND_MTK
+ 	tristate "MTK NAND controller"
+ 	depends on ARCH_MEDIATEK || COMPILE_TEST
+--- a/drivers/mtd/nand/raw/Makefile
++++ b/drivers/mtd/nand/raw/Makefile
+@@ -52,6 +52,7 @@ obj-$(CONFIG_MTD_NAND_SUNXI)		+= sunxi_n
+ obj-$(CONFIG_MTD_NAND_HISI504)	        += hisi504_nand.o
+ obj-$(CONFIG_MTD_NAND_BRCMNAND)		+= brcmnand/
+ obj-$(CONFIG_MTD_NAND_QCOM)		+= qcom_nandc.o
++obj-$(CONFIG_MTD_NAND_MT7621)		+= mt7621_nand.o
+ obj-$(CONFIG_MTD_NAND_MTK)		+= mtk_ecc.o mtk_nand.o
+ obj-$(CONFIG_MTD_NAND_MXIC)		+= mxic_nand.o
+ obj-$(CONFIG_MTD_NAND_TEGRA)		+= tegra_nand.o
+--- /dev/null
++++ b/drivers/mtd/nand/raw/mt7621_nand.c
+@@ -0,0 +1,1348 @@
++// SPDX-License-Identifier: GPL-2.0
++/*
++ * MediaTek MT7621 NAND Flash Controller driver
++ *
++ * Copyright (C) 2020 MediaTek Inc. All Rights Reserved.
++ *
++ * Author: Weijie Gao <weijie.gao@mediatek.com>
++ */
++
++#include <linux/io.h>
++#include <linux/clk.h>
++#include <linux/init.h>
++#include <linux/errno.h>
++#include <linux/sizes.h>
++#include <linux/iopoll.h>
++#include <linux/kernel.h>
++#include <linux/module.h>
++#include <linux/mtd/mtd.h>
++#include <linux/mtd/rawnand.h>
++#include <linux/mtd/partitions.h>
++#include <linux/platform_device.h>
++#include <asm/addrspace.h>
++
++/* NFI core registers */
++#define NFI_CNFG			0x000
++#define   CNFG_OP_MODE_S		12
++#define   CNFG_OP_MODE_M		GENMASK(14, 12)
++#define     CNFG_OP_CUSTOM		6
++#define   CNFG_AUTO_FMT_EN		BIT(9)
++#define   CNFG_HW_ECC_EN		BIT(8)
++#define   CNFG_BYTE_RW			BIT(6)
++#define   CNFG_READ_MODE		BIT(1)
++
++#define NFI_PAGEFMT			0x004
++#define   PAGEFMT_FDM_ECC_S		12
++#define   PAGEFMT_FDM_ECC_M		GENMASK(15, 12)
++#define   PAGEFMT_FDM_S			8
++#define   PAGEFMT_FDM_M			GENMASK(11, 8)
++#define   PAGEFMT_SPARE_S		4
++#define   PAGEFMT_SPARE_M		GENMASK(5, 4)
++#define   PAGEFMT_PAGE_S		0
++#define   PAGEFMT_PAGE_M		GENMASK(1, 0)
++
++#define NFI_CON				0x008
++#define   CON_NFI_SEC_S			12
++#define   CON_NFI_SEC_M			GENMASK(15, 12)
++#define   CON_NFI_BWR			BIT(9)
++#define   CON_NFI_BRD			BIT(8)
++#define   CON_NFI_RST			BIT(1)
++#define   CON_FIFO_FLUSH		BIT(0)
++
++#define NFI_ACCCON			0x00c
++#define   ACCCON_POECS_S		28
++#define   ACCCON_POECS_MAX		0x0f
++#define   ACCCON_POECS_DEF		3
++#define   ACCCON_PRECS_S		22
++#define   ACCCON_PRECS_MAX		0x3f
++#define   ACCCON_PRECS_DEF		3
++#define   ACCCON_C2R_S			16
++#define   ACCCON_C2R_MAX		0x3f
++#define   ACCCON_C2R_DEF		7
++#define   ACCCON_W2R_S			12
++#define   ACCCON_W2R_MAX		0x0f
++#define   ACCCON_W2R_DEF		7
++#define   ACCCON_WH_S			8
++#define   ACCCON_WH_MAX			0x0f
++#define   ACCCON_WH_DEF			15
++#define   ACCCON_WST_S			4
++#define   ACCCON_WST_MAX		0x0f
++#define   ACCCON_WST_DEF		15
++#define   ACCCON_WST_MIN		3
++#define   ACCCON_RLT_S			0
++#define   ACCCON_RLT_MAX		0x0f
++#define   ACCCON_RLT_DEF		15
++#define   ACCCON_RLT_MIN		3
++
++#define NFI_CMD				0x020
++
++#define NFI_ADDRNOB			0x030
++#define   ADDR_ROW_NOB_S		4
++#define   ADDR_ROW_NOB_M		GENMASK(6, 4)
++#define   ADDR_COL_NOB_S		0
++#define   ADDR_COL_NOB_M		GENMASK(2, 0)
++
++#define NFI_COLADDR			0x034
++#define NFI_ROWADDR			0x038
++
++#define NFI_STRDATA			0x040
++#define   STR_DATA			BIT(0)
++
++#define NFI_CNRNB			0x044
++#define   CB2R_TIME_S			4
++#define   CB2R_TIME_M			GENMASK(7, 4)
++#define   STR_CNRNB			BIT(0)
++
++#define NFI_DATAW			0x050
++#define NFI_DATAR			0x054
++
++#define NFI_PIO_DIRDY			0x058
++#define   PIO_DIRDY			BIT(0)
++
++#define NFI_STA				0x060
++#define   STA_NFI_FSM_S			16
++#define   STA_NFI_FSM_M			GENMASK(19, 16)
++#define     STA_FSM_CUSTOM_DATA		14
++#define   STA_BUSY			BIT(8)
++#define   STA_ADDR			BIT(1)
++#define   STA_CMD			BIT(0)
++
++#define NFI_ADDRCNTR			0x070
++#define   SEC_CNTR_S			12
++#define   SEC_CNTR_M			GENMASK(15, 12)
++#define   SEC_ADDR_S			0
++#define   SEC_ADDR_M			GENMASK(9, 0)
++
++#define NFI_CSEL			0x090
++#define   CSEL_S			0
++#define   CSEL_M			GENMASK(1, 0)
++
++#define NFI_FDM0L			0x0a0
++#define NFI_FDML(n)			(0x0a0 + ((n) << 3))
++
++#define NFI_FDM0M			0x0a4
++#define NFI_FDMM(n)			(0x0a4 + ((n) << 3))
++
++#define NFI_MASTER_STA			0x210
++#define   MAS_ADDR			GENMASK(11, 9)
++#define   MAS_RD			GENMASK(8, 6)
++#define   MAS_WR			GENMASK(5, 3)
++#define   MAS_RDDLY			GENMASK(2, 0)
++
++/* ECC engine registers */
++#define ECC_ENCCON			0x000
++#define   ENC_EN			BIT(0)
++
++#define ECC_ENCCNFG			0x004
++#define   ENC_CNFG_MSG_S		16
++#define   ENC_CNFG_MSG_M		GENMASK(28, 16)
++#define   ENC_MODE_S			4
++#define   ENC_MODE_M			GENMASK(5, 4)
++#define     ENC_MODE_NFI		1
++#define   ENC_TNUM_S			0
++#define   ENC_TNUM_M			GENMASK(2, 0)
++
++#define ECC_ENCIDLE			0x00c
++#define   ENC_IDLE			BIT(0)
++
++#define ECC_DECCON			0x100
++#define   DEC_EN			BIT(0)
++
++#define ECC_DECCNFG			0x104
++#define   DEC_EMPTY_EN			BIT(31)
++#define   DEC_CS_S			16
++#define   DEC_CS_M			GENMASK(28, 16)
++#define   DEC_CON_S			12
++#define   DEC_CON_M			GENMASK(13, 12)
++#define     DEC_CON_EL			2
++#define   DEC_MODE_S			4
++#define   DEC_MODE_M			GENMASK(5, 4)
++#define     DEC_MODE_NFI		1
++#define   DEC_TNUM_S			0
++#define   DEC_TNUM_M			GENMASK(2, 0)
++
++#define ECC_DECIDLE			0x10c
++#define   DEC_IDLE			BIT(1)
++
++#define ECC_DECENUM			0x114
++#define   ERRNUM_S			2
++#define   ERRNUM_M			GENMASK(3, 0)
++
++#define ECC_DECDONE			0x118
++#define   DEC_DONE7			BIT(7)
++#define   DEC_DONE6			BIT(6)
++#define   DEC_DONE5			BIT(5)
++#define   DEC_DONE4			BIT(4)
++#define   DEC_DONE3			BIT(3)
++#define   DEC_DONE2			BIT(2)
++#define   DEC_DONE1			BIT(1)
++#define   DEC_DONE0			BIT(0)
++
++#define ECC_DECEL(n)			(0x11c + (n) * 4)
++#define   DEC_EL_ODD_S			16
++#define   DEC_EL_EVEN_S			0
++#define   DEC_EL_M			0x1fff
++#define   DEC_EL_BYTE_POS_S		3
++#define   DEC_EL_BIT_POS_M		GENMASK(3, 0)
++
++#define ECC_FDMADDR			0x13c
++
++/* ENCIDLE and DECIDLE */
++#define   ECC_IDLE			BIT(0)
++
++#define ACCTIMING(tpoecs, tprecs, tc2r, tw2r, twh, twst, trlt) \
++	((tpoecs) << ACCCON_POECS_S | (tprecs) << ACCCON_PRECS_S | \
++	(tc2r) << ACCCON_C2R_S | (tw2r) << ACCCON_W2R_S | \
++	(twh) << ACCCON_WH_S | (twst) << ACCCON_WST_S | (trlt))
++
++#define MASTER_STA_MASK			(MAS_ADDR | MAS_RD | MAS_WR | \
++					 MAS_RDDLY)
++#define NFI_RESET_TIMEOUT		1000000
++#define NFI_CORE_TIMEOUT		500000
++#define ECC_ENGINE_TIMEOUT		500000
++
++#define ECC_SECTOR_SIZE			512
++#define ECC_PARITY_BITS			13
++
++#define NFI_FDM_SIZE		8
++
++#define MT7621_NFC_NAME			"mt7621-nand"
++
++struct mt7621_nfc {
++	struct nand_controller controller;
++	struct nand_chip nand;
++	struct clk *nfi_clk;
++	struct device *dev;
++
++	void __iomem *nfi_regs;
++	void __iomem *ecc_regs;
++
++	u32 spare_per_sector;
++};
++
++static const u16 mt7621_nfi_page_size[] = { SZ_512, SZ_2K, SZ_4K };
++static const u8 mt7621_nfi_spare_size[] = { 16, 26, 27, 28 };
++static const u8 mt7621_ecc_strength[] = { 4, 6, 8, 10, 12 };
++
++static inline u32 nfi_read32(struct mt7621_nfc *nfc, u32 reg)
++{
++	return readl(nfc->nfi_regs + reg);
++}
++
++static inline void nfi_write32(struct mt7621_nfc *nfc, u32 reg, u32 val)
++{
++	writel(val, nfc->nfi_regs + reg);
++}
++
++static inline u16 nfi_read16(struct mt7621_nfc *nfc, u32 reg)
++{
++	return readw(nfc->nfi_regs + reg);
++}
++
++static inline void nfi_write16(struct mt7621_nfc *nfc, u32 reg, u16 val)
++{
++	writew(val, nfc->nfi_regs + reg);
++}
++
++static inline void ecc_write16(struct mt7621_nfc *nfc, u32 reg, u16 val)
++{
++	writew(val, nfc->ecc_regs + reg);
++}
++
++static inline u32 ecc_read32(struct mt7621_nfc *nfc, u32 reg)
++{
++	return readl(nfc->ecc_regs + reg);
++}
++
++static inline void ecc_write32(struct mt7621_nfc *nfc, u32 reg, u32 val)
++{
++	return writel(val, nfc->ecc_regs + reg);
++}
++
++static inline u8 *oob_fdm_ptr(struct nand_chip *nand, int sect)
++{
++	return nand->oob_poi + sect * NFI_FDM_SIZE;
++}
++
++static inline u8 *oob_ecc_ptr(struct mt7621_nfc *nfc, int sect)
++{
++	struct nand_chip *nand = &nfc->nand;
++
++	return nand->oob_poi + nand->ecc.steps * NFI_FDM_SIZE +
++		sect * (nfc->spare_per_sector - NFI_FDM_SIZE);
++}
++
++static inline u8 *page_data_ptr(struct nand_chip *nand, const u8 *buf,
++				int sect)
++{
++	return (u8 *)buf + sect * nand->ecc.size;
++}
++
++static int mt7621_ecc_wait_idle(struct mt7621_nfc *nfc, u32 reg)
++{
++	struct device *dev = nfc->dev;
++	u32 val;
++	int ret;
++
++	ret = readw_poll_timeout_atomic(nfc->ecc_regs + reg, val,
++					val & ECC_IDLE, 10,
++					ECC_ENGINE_TIMEOUT);
++	if (ret) {
++		dev_warn(dev, "ECC engine timed out entering idle mode\n");
++		return -EIO;
++	}
++
++	return 0;
++}
++
++static int mt7621_ecc_decoder_wait_done(struct mt7621_nfc *nfc, u32 sect)
++{
++	struct device *dev = nfc->dev;
++	u32 val;
++	int ret;
++
++	ret = readw_poll_timeout_atomic(nfc->ecc_regs + ECC_DECDONE, val,
++					val & (1 << sect), 10,
++					ECC_ENGINE_TIMEOUT);
++
++	if (ret) {
++		dev_warn(dev, "ECC decoder for sector %d timed out\n",
++			 sect);
++		return -ETIMEDOUT;
++	}
++
++	return 0;
++}
++
++static void mt7621_ecc_encoder_op(struct mt7621_nfc *nfc, bool enable)
++{
++	mt7621_ecc_wait_idle(nfc, ECC_ENCIDLE);
++	ecc_write16(nfc, ECC_ENCCON, enable ? ENC_EN : 0);
++}
++
++static void mt7621_ecc_decoder_op(struct mt7621_nfc *nfc, bool enable)
++{
++	mt7621_ecc_wait_idle(nfc, ECC_DECIDLE);
++	ecc_write16(nfc, ECC_DECCON, enable ? DEC_EN : 0);
++}
++
++static int mt7621_ecc_correct_check(struct mt7621_nfc *nfc, u8 *sector_buf,
++				   u8 *fdm_buf, u32 sect)
++{
++	struct nand_chip *nand = &nfc->nand;
++	u32 decnum, num_error_bits, fdm_end_bits;
++	u32 error_locations, error_bit_loc;
++	u32 error_byte_pos, error_bit_pos;
++	int bitflips = 0;
++	u32 i;
++
++	decnum = ecc_read32(nfc, ECC_DECENUM);
++	num_error_bits = (decnum >> (sect << ERRNUM_S)) & ERRNUM_M;
++	fdm_end_bits = (nand->ecc.size + NFI_FDM_SIZE) << 3;
++
++	if (!num_error_bits)
++		return 0;
++
++	if (num_error_bits == ERRNUM_M)
++		return -1;
++
++	for (i = 0; i < num_error_bits; i++) {
++		error_locations = ecc_read32(nfc, ECC_DECEL(i / 2));
++		error_bit_loc = (error_locations >> ((i % 2) * DEC_EL_ODD_S)) &
++				DEC_EL_M;
++		error_byte_pos = error_bit_loc >> DEC_EL_BYTE_POS_S;
++		error_bit_pos = error_bit_loc & DEC_EL_BIT_POS_M;
++
++		if (error_bit_loc < (nand->ecc.size << 3)) {
++			if (sector_buf) {
++				sector_buf[error_byte_pos] ^=
++					(1 << error_bit_pos);
++			}
++		} else if (error_bit_loc < fdm_end_bits) {
++			if (fdm_buf) {
++				fdm_buf[error_byte_pos - nand->ecc.size] ^=
++					(1 << error_bit_pos);
++			}
++		}
++
++		bitflips++;
++	}
++
++	return bitflips;
++}
++
++static int mt7621_nfc_wait_write_completion(struct mt7621_nfc *nfc,
++					    struct nand_chip *nand)
++{
++	struct device *dev = nfc->dev;
++	u16 val;
++	int ret;
++
++	ret = readw_poll_timeout_atomic(nfc->nfi_regs + NFI_ADDRCNTR, val,
++		((val & SEC_CNTR_M) >> SEC_CNTR_S) >= nand->ecc.steps, 10,
++		NFI_CORE_TIMEOUT);
++
++	if (ret) {
++		dev_warn(dev, "NFI core write operation timed out\n");
++		return -ETIMEDOUT;
++	}
++
++	return ret;
++}
++
++static void mt7621_nfc_hw_reset(struct mt7621_nfc *nfc)
++{
++	u32 val;
++	int ret;
++
++	/* reset all registers and force the NFI master to terminate */
++	nfi_write16(nfc, NFI_CON, CON_FIFO_FLUSH | CON_NFI_RST);
++
++	/* wait for the master to finish the last transaction */
++	ret = readw_poll_timeout(nfc->nfi_regs + NFI_MASTER_STA, val,
++				 !(val & MASTER_STA_MASK), 50,
++				 NFI_RESET_TIMEOUT);
++	if (ret) {
++		dev_warn(nfc->dev, "Failed to reset NFI master in %dms\n",
++			 NFI_RESET_TIMEOUT);
++	}
++
++	/* ensure any status register affected by the NFI master is reset */
++	nfi_write16(nfc, NFI_CON, CON_FIFO_FLUSH | CON_NFI_RST);
++	nfi_write16(nfc, NFI_STRDATA, 0);
++}
++
++static inline void mt7621_nfc_hw_init(struct mt7621_nfc *nfc)
++{
++	u32 acccon;
++
++	/*
++	 * CNRNB: nand ready/busy register
++	 * -------------------------------
++	 * 7:4: timeout register for polling the NAND busy/ready signal
++	 * 0  : poll the status of the busy/ready signal after [7:4]*16 cycles.
++	 */
++	nfi_write16(nfc, NFI_CNRNB, CB2R_TIME_M | STR_CNRNB);
++
++	mt7621_nfc_hw_reset(nfc);
++
++	/* Apply default access timing */
++	acccon = ACCTIMING(ACCCON_POECS_DEF, ACCCON_PRECS_DEF, ACCCON_C2R_DEF,
++			   ACCCON_W2R_DEF, ACCCON_WH_DEF, ACCCON_WST_DEF,
++			   ACCCON_RLT_DEF);
++
++	nfi_write32(nfc, NFI_ACCCON, acccon);
++}
++
++static int mt7621_nfc_send_command(struct mt7621_nfc *nfc, u8 command)
++{
++	struct device *dev = nfc->dev;
++	u32 val;
++	int ret;
++
++	nfi_write32(nfc, NFI_CMD, command);
++
++	ret = readl_poll_timeout_atomic(nfc->nfi_regs + NFI_STA, val,
++					!(val & STA_CMD), 10,
++					NFI_CORE_TIMEOUT);
++	if (ret) {
++		dev_warn(dev, "NFI core timed out entering command mode\n");
++		return -EIO;
++	}
++
++	return 0;
++}
++
++static int mt7621_nfc_send_address_byte(struct mt7621_nfc *nfc, int addr)
++{
++	struct device *dev = nfc->dev;
++	u32 val;
++	int ret;
++
++	nfi_write32(nfc, NFI_COLADDR, addr);
++	nfi_write32(nfc, NFI_ROWADDR, 0);
++	nfi_write16(nfc, NFI_ADDRNOB, 1);
++
++	ret = readl_poll_timeout_atomic(nfc->nfi_regs + NFI_STA, val,
++					!(val & STA_ADDR), 10,
++					NFI_CORE_TIMEOUT);
++	if (ret) {
++		dev_warn(dev, "NFI core timed out entering address mode\n");
++		return -EIO;
++	}
++
++	return 0;
++}
++
++static int mt7621_nfc_send_address(struct mt7621_nfc *nfc, const u8 *addr,
++				   unsigned int naddrs)
++{
++	int ret;
++
++	while (naddrs) {
++		ret = mt7621_nfc_send_address_byte(nfc, *addr);
++		if (ret)
++			return ret;
++
++		addr++;
++		naddrs--;
++	}
++
++	return 0;
++}
++
++static void mt7621_nfc_wait_pio_ready(struct mt7621_nfc *nfc)
++{
++	struct device *dev = nfc->dev;
++	int ret;
++	u16 val;
++
++	ret = readw_poll_timeout_atomic(nfc->nfi_regs + NFI_PIO_DIRDY, val,
++					val & PIO_DIRDY, 10,
++					NFI_CORE_TIMEOUT);
++	if (ret < 0)
++		dev_err(dev, "NFI core PIO mode not ready\n");
++}
++
++static u32 mt7621_nfc_pio_read(struct mt7621_nfc *nfc, bool br)
++{
++	u32 reg;
++
++	/* after each byte read, the NFI_STA reg is reset by the hardware */
++	reg = (nfi_read32(nfc, NFI_STA) & STA_NFI_FSM_M) >> STA_NFI_FSM_S;
++	if (reg != STA_FSM_CUSTOM_DATA) {
++		reg = nfi_read16(nfc, NFI_CNFG);
++		reg |= CNFG_READ_MODE | CNFG_BYTE_RW;
++		if (!br)
++			reg &= ~CNFG_BYTE_RW;
++		nfi_write16(nfc, NFI_CNFG, reg);
++
++		/*
++		 * set to max sector to allow the HW to continue reading over
++		 * unaligned accesses
++		 */
++		nfi_write16(nfc, NFI_CON, CON_NFI_SEC_M | CON_NFI_BRD);
++
++		/* trigger to fetch data */
++		nfi_write16(nfc, NFI_STRDATA, STR_DATA);
++	}
++
++	mt7621_nfc_wait_pio_ready(nfc);
++
++	return nfi_read32(nfc, NFI_DATAR);
++}
++
++static void mt7621_nfc_read_data(struct mt7621_nfc *nfc, u8 *buf, u32 len)
++{
++	while (((uintptr_t)buf & 3) && len) {
++		*buf = mt7621_nfc_pio_read(nfc, true);
++		buf++;
++		len--;
++	}
++
++	while (len >= 4) {
++		*(u32 *)buf = mt7621_nfc_pio_read(nfc, false);
++		buf += 4;
++		len -= 4;
++	}
++
++	while (len) {
++		*buf = mt7621_nfc_pio_read(nfc, true);
++		buf++;
++		len--;
++	}
++}
++
++static void mt7621_nfc_read_data_discard(struct mt7621_nfc *nfc, u32 len)
++{
++	while (len >= 4) {
++		mt7621_nfc_pio_read(nfc, false);
++		len -= 4;
++	}
++
++	while (len) {
++		mt7621_nfc_pio_read(nfc, true);
++		len--;
++	}
++}
++
++static void mt7621_nfc_pio_write(struct mt7621_nfc *nfc, u32 val, bool bw)
++{
++	u32 reg;
++
++	reg = (nfi_read32(nfc, NFI_STA) & STA_NFI_FSM_M) >> STA_NFI_FSM_S;
++	if (reg != STA_FSM_CUSTOM_DATA) {
++		reg = nfi_read16(nfc, NFI_CNFG);
++		reg &= ~(CNFG_READ_MODE | CNFG_BYTE_RW);
++		if (bw)
++			reg |= CNFG_BYTE_RW;
++		nfi_write16(nfc, NFI_CNFG, reg);
++
++		nfi_write16(nfc, NFI_CON, CON_NFI_SEC_M | CON_NFI_BWR);
++		nfi_write16(nfc, NFI_STRDATA, STR_DATA);
++	}
++
++	mt7621_nfc_wait_pio_ready(nfc);
++	nfi_write32(nfc, NFI_DATAW, val);
++}
++
++static void mt7621_nfc_write_data(struct mt7621_nfc *nfc, const u8 *buf,
++				  u32 len)
++{
++	while (((uintptr_t)buf & 3) && len) {
++		mt7621_nfc_pio_write(nfc, *buf, true);
++		buf++;
++		len--;
++	}
++
++	while (len >= 4) {
++		mt7621_nfc_pio_write(nfc, *(const u32 *)buf, false);
++		buf += 4;
++		len -= 4;
++	}
++
++	while (len) {
++		mt7621_nfc_pio_write(nfc, *buf, true);
++		buf++;
++		len--;
++	}
++}
++
++static void mt7621_nfc_write_data_empty(struct mt7621_nfc *nfc, u32 len)
++{
++	while (len >= 4) {
++		mt7621_nfc_pio_write(nfc, 0xffffffff, false);
++		len -= 4;
++	}
++
++	while (len) {
++		mt7621_nfc_pio_write(nfc, 0xff, true);
++		len--;
++	}
++}
++
++static int mt7621_nfc_dev_ready(struct mt7621_nfc *nfc,
++				unsigned int timeout_ms)
++{
++	u32 val;
++
++	return readl_poll_timeout_atomic(nfc->nfi_regs + NFI_STA, val,
++					 !(val & STA_BUSY), 10,
++					 timeout_ms * 1000);
++}
++
++static int mt7621_nfc_exec_instr(struct nand_chip *nand,
++				 const struct nand_op_instr *instr)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++
++	switch (instr->type) {
++	case NAND_OP_CMD_INSTR:
++		mt7621_nfc_hw_reset(nfc);
++		nfi_write16(nfc, NFI_CNFG, CNFG_OP_CUSTOM << CNFG_OP_MODE_S);
++		return mt7621_nfc_send_command(nfc, instr->ctx.cmd.opcode);
++	case NAND_OP_ADDR_INSTR:
++		return mt7621_nfc_send_address(nfc, instr->ctx.addr.addrs,
++					       instr->ctx.addr.naddrs);
++	case NAND_OP_DATA_IN_INSTR:
++		mt7621_nfc_read_data(nfc, instr->ctx.data.buf.in,
++				     instr->ctx.data.len);
++		return 0;
++	case NAND_OP_DATA_OUT_INSTR:
++		mt7621_nfc_write_data(nfc, instr->ctx.data.buf.out,
++				      instr->ctx.data.len);
++		return 0;
++	case NAND_OP_WAITRDY_INSTR:
++		return mt7621_nfc_dev_ready(nfc,
++					    instr->ctx.waitrdy.timeout_ms);
++	default:
++		WARN_ONCE(1, "unsupported NAND instruction type: %d\n",
++			  instr->type);
++
++		return -EINVAL;
++	}
++}
++
++static int mt7621_nfc_exec_op(struct nand_chip *nand,
++			      const struct nand_operation *op, bool check_only)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	int i, ret;
++
++	if (check_only)
++		return 0;
++
++	/* Only CS0 available */
++	nfi_write16(nfc, NFI_CSEL, 0);
++
++	for (i = 0; i < op->ninstrs; i++) {
++		ret = mt7621_nfc_exec_instr(nand, &op->instrs[i]);
++		if (ret)
++			return ret;
++	}
++
++	return 0;
++}
++
++static int mt7621_nfc_setup_data_interface(struct nand_chip *nand, int csline,
++					   const struct nand_data_interface *conf)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	const struct nand_sdr_timings *timings;
++	u32 acccon, temp, rate, tpoecs, tprecs, tc2r, tw2r, twh, twst, trlt;
++
++	if (!nfc->nfi_clk)
++		return -ENOTSUPP;
++
++	timings = nand_get_sdr_timings(conf);
++	if (IS_ERR(timings))
++		return -ENOTSUPP;
++
++	rate = clk_get_rate(nfc->nfi_clk);
++
++	/* turn clock rate into KHZ */
++	rate /= 1000;
++
++	tpoecs = max(timings->tALH_min, timings->tCLH_min) / 1000;
++	tpoecs = DIV_ROUND_UP(tpoecs * rate, 1000000);
++	tpoecs = min_t(u32, tpoecs, ACCCON_POECS_MAX);
++
++	tprecs = max(timings->tCLS_min, timings->tALS_min) / 1000;
++	tprecs = DIV_ROUND_UP(tprecs * rate, 1000000);
++	tprecs = min_t(u32, tprecs, ACCCON_PRECS_MAX);
++
++	/* sdr interface has no tCR which means CE# low to RE# low */
++	tc2r = 0;
++
++	tw2r = timings->tWHR_min / 1000;
++	tw2r = DIV_ROUND_UP(tw2r * rate, 1000000);
++	tw2r = DIV_ROUND_UP(tw2r - 1, 2);
++	tw2r = min_t(u32, tw2r, ACCCON_W2R_MAX);
++
++	twh = max(timings->tREH_min, timings->tWH_min) / 1000;
++	twh = DIV_ROUND_UP(twh * rate, 1000000) - 1;
++	twh = min_t(u32, twh, ACCCON_WH_MAX);
++
++	/* Calculate real WE#/RE# hold time in nanosecond */
++	temp = (twh + 1) * 1000000 / rate;
++	/* nanosecond to picosecond */
++	temp *= 1000;
++
++	/*
++	 * WE# low level time should be expaned to meet WE# pulse time
++	 * and WE# cycle time at the same time.
++	 */
++	if (temp < timings->tWC_min)
++		twst = timings->tWC_min - temp;
++	else
++		twst = 0;
++	twst = max(timings->tWP_min, twst) / 1000;
++	twst = DIV_ROUND_UP(twst * rate, 1000000) - 1;
++	twst = min_t(u32, twst, ACCCON_WST_MAX);
++
++	/*
++	 * RE# low level time should be expaned to meet RE# pulse time
++	 * and RE# cycle time at the same time.
++	 */
++	if (temp < timings->tRC_min)
++		trlt = timings->tRC_min - temp;
++	else
++		trlt = 0;
++	trlt = max(trlt, timings->tRP_min) / 1000;
++	trlt = DIV_ROUND_UP(trlt * rate, 1000000) - 1;
++	trlt = min_t(u32, trlt, ACCCON_RLT_MAX);
++
++	if (csline == NAND_DATA_IFACE_CHECK_ONLY) {
++		if (twst < ACCCON_WST_MIN || trlt < ACCCON_RLT_MIN)
++			return -ENOTSUPP;
++	}
++
++	acccon = ACCTIMING(tpoecs, tprecs, tc2r, tw2r, twh, twst, trlt);
++
++	dev_info(nfc->dev, "Using programmed access timing: %08x\n", acccon);
++
++	nfi_write32(nfc, NFI_ACCCON, acccon);
++
++	return 0;
++}
++
++static int mt7621_nfc_calc_ecc_strength(struct mt7621_nfc *nfc,
++					u32 avail_ecc_bytes)
++{
++	struct nand_chip *nand = &nfc->nand;
++	struct mtd_info *mtd = nand_to_mtd(nand);
++	u32 strength;
++	int i;
++
++	strength = avail_ecc_bytes * 8 / ECC_PARITY_BITS;
++
++	/* Find the closest supported ecc strength */
++	for (i = ARRAY_SIZE(mt7621_ecc_strength) - 1; i >= 0; i--) {
++		if (mt7621_ecc_strength[i] <= strength)
++			break;
++	}
++
++	if (unlikely(i < 0)) {
++		dev_err(nfc->dev, "OOB size (%u) is not supported\n",
++			mtd->oobsize);
++		return -EINVAL;
++	}
++
++	nand->ecc.strength = mt7621_ecc_strength[i];
++	nand->ecc.bytes =
++		DIV_ROUND_UP(nand->ecc.strength * ECC_PARITY_BITS, 8);
++
++	dev_info(nfc->dev, "ECC strength adjusted to %u bits\n",
++		 nand->ecc.strength);
++
++	return i;
++}
++
++static int mt7621_nfc_set_spare_per_sector(struct mt7621_nfc *nfc)
++{
++	struct nand_chip *nand = &nfc->nand;
++	struct mtd_info *mtd = nand_to_mtd(nand);
++	u32 size;
++	int i;
++
++	size = nand->ecc.bytes + NFI_FDM_SIZE;
++
++	/* Find the closest supported spare size */
++	for (i = 0; i < ARRAY_SIZE(mt7621_nfi_spare_size); i++) {
++		if (mt7621_nfi_spare_size[i] >= size)
++			break;
++	}
++
++	if (unlikely(i >= ARRAY_SIZE(mt7621_nfi_spare_size))) {
++		dev_err(nfc->dev, "OOB size (%u) is not supported\n",
++			mtd->oobsize);
++		return -EINVAL;
++	}
++
++	nfc->spare_per_sector = mt7621_nfi_spare_size[i];
++
++	return i;
++}
++
++static int mt7621_nfc_ecc_init(struct mt7621_nfc *nfc)
++{
++	struct nand_chip *nand = &nfc->nand;
++	struct mtd_info *mtd = nand_to_mtd(nand);
++	u32 spare_per_sector, encode_block_size, decode_block_size;
++	u32 ecc_enccfg, ecc_deccfg;
++	int ecc_cap;
++
++	/* Only hardware ECC mode is supported */
++	if (nand->ecc.mode != NAND_ECC_HW_SYNDROME) {
++		dev_err(nfc->dev, "Only hardware ECC mode is supported\n");
++		return -EINVAL;
++	}
++
++	nand->ecc.size = ECC_SECTOR_SIZE;
++	nand->ecc.steps = mtd->writesize / nand->ecc.size;
++
++	spare_per_sector = mtd->oobsize / nand->ecc.steps;
++
++	ecc_cap = mt7621_nfc_calc_ecc_strength(nfc,
++		spare_per_sector - NFI_FDM_SIZE);
++	if (ecc_cap < 0)
++		return ecc_cap;
++
++	/* Sector + FDM */
++	encode_block_size = (nand->ecc.size + NFI_FDM_SIZE) * 8;
++	ecc_enccfg = ecc_cap | (ENC_MODE_NFI << ENC_MODE_S) |
++		     (encode_block_size << ENC_CNFG_MSG_S);
++
++	/* Sector + FDM + ECC parity bits */
++	decode_block_size = ((nand->ecc.size + NFI_FDM_SIZE) * 8) +
++			    nand->ecc.strength * ECC_PARITY_BITS;
++	ecc_deccfg = ecc_cap | (DEC_MODE_NFI << DEC_MODE_S) |
++		     (decode_block_size << DEC_CS_S) |
++		     (DEC_CON_EL << DEC_CON_S) | DEC_EMPTY_EN;
++
++	mt7621_ecc_encoder_op(nfc, false);
++	ecc_write32(nfc, ECC_ENCCNFG, ecc_enccfg);
++
++	mt7621_ecc_decoder_op(nfc, false);
++	ecc_write32(nfc, ECC_DECCNFG, ecc_deccfg);
++
++	return 0;
++}
++
++static int mt7621_nfc_set_page_format(struct mt7621_nfc *nfc)
++{
++	struct nand_chip *nand = &nfc->nand;
++	struct mtd_info *mtd = nand_to_mtd(nand);
++	int i, spare_size;
++	u32 pagefmt;
++
++	spare_size = mt7621_nfc_set_spare_per_sector(nfc);
++	if (spare_size < 0)
++		return spare_size;
++
++	for (i = 0; i < ARRAY_SIZE(mt7621_nfi_page_size); i++) {
++		if (mt7621_nfi_page_size[i] == mtd->writesize)
++			break;
++	}
++
++	if (unlikely(i >= ARRAY_SIZE(mt7621_nfi_page_size))) {
++		dev_err(nfc->dev, "Page size (%u) is not supported\n",
++			mtd->writesize);
++		return -EINVAL;
++	}
++
++	pagefmt = i | (spare_size << PAGEFMT_SPARE_S) |
++		  (NFI_FDM_SIZE << PAGEFMT_FDM_S) |
++		  (NFI_FDM_SIZE << PAGEFMT_FDM_ECC_S);
++
++	nfi_write16(nfc, NFI_PAGEFMT, pagefmt);
++
++	return 0;
++}
++
++static int mt7621_nfc_attach_chip(struct nand_chip *nand)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	int ret;
++
++	if (nand->options & NAND_BUSWIDTH_16) {
++		dev_err(nfc->dev, "16-bit buswidth is not supported");
++		return -EINVAL;
++	}
++
++	ret = mt7621_nfc_ecc_init(nfc);
++	if (ret)
++		return ret;
++
++	return mt7621_nfc_set_page_format(nfc);
++}
++
++static const struct nand_controller_ops mt7621_nfc_controller_ops = {
++	.attach_chip = mt7621_nfc_attach_chip,
++	.exec_op = mt7621_nfc_exec_op,
++	.setup_data_interface = mt7621_nfc_setup_data_interface,
++};
++
++static int mt7621_nfc_ooblayout_free(struct mtd_info *mtd, int section,
++				     struct mtd_oob_region *oob_region)
++{
++	struct nand_chip *nand = mtd_to_nand(mtd);
++
++	if (section >= nand->ecc.steps)
++		return -ERANGE;
++
++	oob_region->length = NFI_FDM_SIZE - 1;
++	oob_region->offset = section * NFI_FDM_SIZE + 1;
++
++	return 0;
++}
++
++static int mt7621_nfc_ooblayout_ecc(struct mtd_info *mtd, int section,
++				    struct mtd_oob_region *oob_region)
++{
++	struct nand_chip *nand = mtd_to_nand(mtd);
++
++	if (section)
++		return -ERANGE;
++
++	oob_region->offset = NFI_FDM_SIZE * nand->ecc.steps;
++	oob_region->length = mtd->oobsize - oob_region->offset;
++
++	return 0;
++}
++
++static const struct mtd_ooblayout_ops mt7621_nfc_ooblayout_ops = {
++	.free = mt7621_nfc_ooblayout_free,
++	.ecc = mt7621_nfc_ooblayout_ecc,
++};
++
++static void mt7621_nfc_write_fdm(struct mt7621_nfc *nfc)
++{
++	struct nand_chip *nand = &nfc->nand;
++	u32 vall, valm;
++	u8 *oobptr;
++	int i, j;
++
++	for (i = 0; i < nand->ecc.steps; i++) {
++		vall = 0;
++		valm = 0;
++		oobptr = oob_fdm_ptr(nand, i);
++
++		for (j = 0; j < 4; j++)
++			vall |= (u32)oobptr[j] << (j * 8);
++
++		for (j = 0; j < 4; j++)
++			valm |= (u32)oobptr[j + 4] << ((j - 4) * 8);
++
++		nfi_write32(nfc, NFI_FDML(i), vall);
++		nfi_write32(nfc, NFI_FDMM(i), valm);
++	}
++}
++
++static void mt7621_nfc_read_sector_fdm(struct mt7621_nfc *nfc, u32 sect)
++{
++	struct nand_chip *nand = &nfc->nand;
++	u32 vall, valm;
++	u8 *oobptr;
++	int i;
++
++	vall = nfi_read32(nfc, NFI_FDML(sect));
++	valm = nfi_read32(nfc, NFI_FDMM(sect));
++	oobptr = oob_fdm_ptr(nand, sect);
++
++	for (i = 0; i < 4; i++)
++		oobptr[i] = (vall >> (i * 8)) & 0xff;
++
++	for (i = 0; i < 4; i++)
++		oobptr[i + 4] = (valm >> (i * 8)) & 0xff;
++}
++
++static int mt7621_nfc_read_page_hwecc(struct nand_chip *nand, uint8_t *buf,
++				      int oob_required, int page)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	struct mtd_info *mtd = nand_to_mtd(nand);
++	int bitflips = 0;
++	int rc, i;
++
++	nand_read_page_op(nand, page, 0, NULL, 0);
++
++	nfi_write16(nfc, NFI_CNFG, (CNFG_OP_CUSTOM << CNFG_OP_MODE_S) |
++		    CNFG_READ_MODE | CNFG_AUTO_FMT_EN | CNFG_HW_ECC_EN);
++
++	mt7621_ecc_decoder_op(nfc, true);
++
++	nfi_write16(nfc, NFI_CON,
++		    CON_NFI_BRD | (nand->ecc.steps << CON_NFI_SEC_S));
++
++	for (i = 0; i < nand->ecc.steps; i++) {
++		if (buf)
++			mt7621_nfc_read_data(nfc, page_data_ptr(nand, buf, i),
++					     nand->ecc.size);
++		else
++			mt7621_nfc_read_data_discard(nfc, nand->ecc.size);
++
++		rc = mt7621_ecc_decoder_wait_done(nfc, i);
++
++		mt7621_nfc_read_sector_fdm(nfc, i);
++
++		if (rc < 0) {
++			bitflips = -EIO;
++			continue;
++		}
++
++		rc = mt7621_ecc_correct_check(nfc,
++			buf ? page_data_ptr(nand, buf, i) : NULL,
++			oob_fdm_ptr(nand, i), i);
++
++		if (rc < 0) {
++			dev_warn(nfc->dev,
++				 "Uncorrectable ECC error at page %d.%d\n",
++				 page, i);
++			bitflips = -EBADMSG;
++			mtd->ecc_stats.failed++;
++		} else if (bitflips >= 0) {
++			bitflips += rc;
++			mtd->ecc_stats.corrected += rc;
++		}
++	}
++
++	mt7621_ecc_decoder_op(nfc, false);
++
++	nfi_write16(nfc, NFI_CON, 0);
++
++	return bitflips;
++}
++
++static int mt7621_nfc_read_page_raw(struct nand_chip *nand, uint8_t *buf,
++				    int oob_required, int page)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	int i;
++
++	nand_read_page_op(nand, page, 0, NULL, 0);
++
++	nfi_write16(nfc, NFI_CNFG, (CNFG_OP_CUSTOM << CNFG_OP_MODE_S) |
++		    CNFG_READ_MODE);
++
++	nfi_write16(nfc, NFI_CON,
++		    CON_NFI_BRD | (nand->ecc.steps << CON_NFI_SEC_S));
++
++	for (i = 0; i < nand->ecc.steps; i++) {
++		/* Read data */
++		if (buf)
++			mt7621_nfc_read_data(nfc, page_data_ptr(nand, buf, i),
++					     nand->ecc.size);
++		else
++			mt7621_nfc_read_data_discard(nfc, nand->ecc.size);
++
++		/* Read FDM */
++		mt7621_nfc_read_data(nfc, oob_fdm_ptr(nand, i), NFI_FDM_SIZE);
++
++		/* Read ECC parity data */
++		mt7621_nfc_read_data(nfc, oob_ecc_ptr(nfc, i),
++				     nfc->spare_per_sector - NFI_FDM_SIZE);
++	}
++
++	nfi_write16(nfc, NFI_CON, 0);
++
++	return 0;
++}
++
++static int mt7621_nfc_read_oob_hwecc(struct nand_chip *nand, int page)
++{
++	return mt7621_nfc_read_page_hwecc(nand, NULL, 1, page);
++}
++
++static int mt7621_nfc_read_oob_raw(struct nand_chip *nand, int page)
++{
++	return mt7621_nfc_read_page_raw(nand, NULL, 1, page);
++}
++
++static int mt7621_nfc_check_empty_page(struct nand_chip *nand, const u8 *buf)
++{
++	struct mtd_info *mtd = nand_to_mtd(nand);
++	uint32_t i, j;
++	u8 *oobptr;
++
++	for (i = 0; i < mtd->writesize; i++)
++		if (buf[i] != 0xff)
++			return 0;
++
++	for (i = 0; i < nand->ecc.steps; i++) {
++		oobptr = oob_fdm_ptr(nand, i);
++		for (j = 0; j < NFI_FDM_SIZE; j++)
++			if (oobptr[j] != 0xff)
++				return 0;
++	}
++
++	return 1;
++}
++
++static int mt7621_nfc_write_page_hwecc(struct nand_chip *nand,
++				       const uint8_t *buf, int oob_required,
++				       int page)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	struct mtd_info *mtd = nand_to_mtd(nand);
++
++	if (mt7621_nfc_check_empty_page(nand, buf)) {
++		/*
++		 * MT7621 ECC engine always generates parity code for input
++		 * pages, even for empty pages. Doing so will write back ECC
++		 * parity code to the oob region, which means such pages will
++		 * no longer be empty pages.
++		 *
++		 * To avoid this, stop write operation if current page is an
++		 * empty page.
++		 */
++		return 0;
++	}
++
++	nand_prog_page_begin_op(nand, page, 0, NULL, 0);
++
++	nfi_write16(nfc, NFI_CNFG, (CNFG_OP_CUSTOM << CNFG_OP_MODE_S) |
++		   CNFG_AUTO_FMT_EN | CNFG_HW_ECC_EN);
++
++	mt7621_ecc_encoder_op(nfc, true);
++
++	mt7621_nfc_write_fdm(nfc);
++
++	nfi_write16(nfc, NFI_CON,
++		    CON_NFI_BWR | (nand->ecc.steps << CON_NFI_SEC_S));
++
++	if (buf)
++		mt7621_nfc_write_data(nfc, buf, mtd->writesize);
++	else
++		mt7621_nfc_write_data_empty(nfc, mtd->writesize);
++
++	mt7621_nfc_wait_write_completion(nfc, nand);
++
++	mt7621_ecc_encoder_op(nfc, false);
++
++	nfi_write16(nfc, NFI_CON, 0);
++
++	return nand_prog_page_end_op(nand);
++}
++
++static int mt7621_nfc_write_page_raw(struct nand_chip *nand,
++				     const uint8_t *buf, int oob_required,
++				     int page)
++{
++	struct mt7621_nfc *nfc = nand_get_controller_data(nand);
++	int i;
++
++	nand_prog_page_begin_op(nand, page, 0, NULL, 0);
++
++	nfi_write16(nfc, NFI_CNFG, (CNFG_OP_CUSTOM << CNFG_OP_MODE_S));
++
++	nfi_write16(nfc, NFI_CON,
++		    CON_NFI_BWR | (nand->ecc.steps << CON_NFI_SEC_S));
++
++	for (i = 0; i < nand->ecc.steps; i++) {
++		/* Write data */
++		if (buf)
++			mt7621_nfc_write_data(nfc, page_data_ptr(nand, buf, i),
++					      nand->ecc.size);
++		else
++			mt7621_nfc_write_data_empty(nfc, nand->ecc.size);
++
++		/* Write FDM */
++		mt7621_nfc_write_data(nfc, oob_fdm_ptr(nand, i),
++				      NFI_FDM_SIZE);
++
++		/* Write dummy ECC parity data */
++		mt7621_nfc_write_data_empty(nfc, nfc->spare_per_sector -
++					    NFI_FDM_SIZE);
++	}
++
++	mt7621_nfc_wait_write_completion(nfc, nand);
++
++	nfi_write16(nfc, NFI_CON, 0);
++
++	return nand_prog_page_end_op(nand);
++}
++
++static int mt7621_nfc_write_oob_hwecc(struct nand_chip *nand, int page)
++{
++	return mt7621_nfc_write_page_hwecc(nand, NULL, 1, page);
++}
++
++static int mt7621_nfc_write_oob_raw(struct nand_chip *nand, int page)
++{
++	return mt7621_nfc_write_page_raw(nand, NULL, 1, page);
++}
++
++static int mt7621_nfc_init_chip(struct mt7621_nfc *nfc)
++{
++	struct nand_chip *nand = &nfc->nand;
++	struct mtd_info *mtd;
++	int ret;
++
++	nand->controller = &nfc->controller;
++	nand_set_controller_data(nand, (void *)nfc);
++	nand_set_flash_node(nand, nfc->dev->of_node);
++
++	nand->options |= NAND_USE_BOUNCE_BUFFER | NAND_NO_SUBPAGE_WRITE;
++	if (!nfc->nfi_clk)
++		nand->options |= NAND_KEEP_TIMINGS;
++
++	nand->ecc.mode = NAND_ECC_HW_SYNDROME;
++	nand->ecc.read_page = mt7621_nfc_read_page_hwecc;
++	nand->ecc.read_page_raw = mt7621_nfc_read_page_raw;
++	nand->ecc.write_page = mt7621_nfc_write_page_hwecc;
++	nand->ecc.write_page_raw = mt7621_nfc_write_page_raw;
++	nand->ecc.read_oob = mt7621_nfc_read_oob_hwecc;
++	nand->ecc.read_oob_raw = mt7621_nfc_read_oob_raw;
++	nand->ecc.write_oob = mt7621_nfc_write_oob_hwecc;
++	nand->ecc.write_oob_raw = mt7621_nfc_write_oob_raw;
++
++	mtd = nand_to_mtd(nand);
++	mtd->owner = THIS_MODULE;
++	mtd->dev.parent = nfc->dev;
++	mtd->name = MT7621_NFC_NAME;
++	mtd_set_ooblayout(mtd, &mt7621_nfc_ooblayout_ops);
++
++	mt7621_nfc_hw_init(nfc);
++
++	ret = nand_scan(nand, 1);
++	if (ret)
++		return ret;
++
++	ret = mtd_device_register(mtd, NULL, 0);
++	if (ret) {
++		dev_err(nfc->dev, "Failed to register MTD: %d\n", ret);
++		nand_release(nand);
++		return ret;
++	}
++
++	return 0;
++}
++
++static int mt7621_nfc_probe(struct platform_device *pdev)
++{
++	struct device *dev = &pdev->dev;
++	struct mt7621_nfc *nfc;
++	struct resource *res;
++	int ret;
++
++	nfc = devm_kzalloc(dev, sizeof(*nfc), GFP_KERNEL);
++	if (!nfc)
++		return -ENOMEM;
++
++	nand_controller_init(&nfc->controller);
++	nfc->controller.ops = &mt7621_nfc_controller_ops;
++	nfc->dev = dev;
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "nfi");
++	nfc->nfi_regs = devm_ioremap_resource(dev, res);
++	if (IS_ERR(nfc->nfi_regs)) {
++		ret = PTR_ERR(nfc->nfi_regs);
++		return ret;
++	}
++
++	res = platform_get_resource_byname(pdev, IORESOURCE_MEM, "ecc");
++	nfc->ecc_regs = devm_ioremap_resource(dev, res);
++	if (IS_ERR(nfc->ecc_regs)) {
++		ret = PTR_ERR(nfc->ecc_regs);
++		return ret;
++	}
++
++	nfc->nfi_clk = devm_clk_get(dev, "nfi_clk");
++	if (IS_ERR(nfc->nfi_clk)) {
++		dev_warn(dev, "nfi clk not provided\n");
++		nfc->nfi_clk = NULL;
++	} else {
++		ret = clk_prepare_enable(nfc->nfi_clk);
++		if (ret) {
++			dev_err(dev, "Failed to enable nfi core clock\n");
++			return ret;
++		}
++	}
++
++	platform_set_drvdata(pdev, nfc);
++
++	ret = mt7621_nfc_init_chip(nfc);
++	if (ret) {
++		dev_err(dev, "Failed to initialize nand chip\n");
++		goto clk_disable;
++	}
++
++	return 0;
++
++clk_disable:
++	clk_disable_unprepare(nfc->nfi_clk);
++
++	return ret;
++}
++
++static int mt7621_nfc_remove(struct platform_device *pdev)
++{
++	struct mt7621_nfc *nfc = platform_get_drvdata(pdev);
++
++	nand_release(&nfc->nand);
++	clk_disable_unprepare(nfc->nfi_clk);
++
++	return 0;
++}
++
++static const struct of_device_id mt7621_nfc_id_table[] = {
++	{ .compatible = "mediatek,mt7621-nfc" },
++	{ },
++};
++MODULE_DEVICE_TABLE(of, match);
++
++static struct platform_driver mt7621_nfc_driver = {
++	.probe = mt7621_nfc_probe,
++	.remove = mt7621_nfc_remove,
++	.driver = {
++		.name = MT7621_NFC_NAME,
++		.owner = THIS_MODULE,
++		.of_match_table = mt7621_nfc_id_table,
++	},
++};
++module_platform_driver(mt7621_nfc_driver);
++
++MODULE_LICENSE("GPL");
++MODULE_AUTHOR("Weijie Gao <weijie.gao@mediatek.com>");
++MODULE_DESCRIPTION("MediaTek MT7621 NAND Flash Controller driver");
diff --git a/target/linux/ramips/patches-5.4/0301-dt-bindings-add-documentation-for-mt7621-nand-driver.patch b/target/linux/ramips/patches-5.4/0301-dt-bindings-add-documentation-for-mt7621-nand-driver.patch
new file mode 100644
index 0000000000..3d122c10c0
--- /dev/null
+++ b/target/linux/ramips/patches-5.4/0301-dt-bindings-add-documentation-for-mt7621-nand-driver.patch
@@ -0,0 +1,85 @@
+From 3d5f4da8296b23eb3abf8b13122b0d06a215e79c Mon Sep 17 00:00:00 2001
+From: Weijie Gao <weijie.gao@mediatek.com>
+Date: Wed, 1 Apr 2020 02:07:59 +0800
+Subject: [PATCH 2/2] dt-bindings: add documentation for mt7621-nand driver
+
+This patch adds documentation for MediaTek MT7621 NAND flash controller
+driver.
+
+Signed-off-by: Weijie Gao <weijie.gao@mediatek.com>
+---
+ .../bindings/mtd/mediatek,mt7621-nfc.yaml          | 68 ++++++++++++++++++++++
+ 1 file changed, 68 insertions(+)
+ create mode 100644 Documentation/devicetree/bindings/mtd/mediatek,mt7621-nfc.yaml
+
+--- /dev/null
++++ b/Documentation/devicetree/bindings/mtd/mediatek,mt7621-nfc.yaml
+@@ -0,0 +1,68 @@
++# SPDX-License-Identifier: GPL-2.0
++%YAML 1.2
++---
++$id: http://devicetree.org/schemas/mtd/mediatek,mt7621-nfc.yaml#
++$schema: http://devicetree.org/meta-schemas/core.yaml#
++
++title: MediaTek MT7621 SoC NAND Flash Controller (NFC) DT binding
++
++maintainers:
++  - Weijie Gao <weijie.gao@mediatek.com>
++
++description: |
++  This driver uses a single node to describe both NAND Flash controller
++  interface (NFI) and ECC engine for MT7621 SoC.
++  MT7621 supports only one chip select.
++
++properties:
++  "#address-cells": false
++  "#size-cells": false
++
++  compatible:
++    enum:
++      - mediatek,mt7621-nfc
++
++  reg:
++    items:
++      - description: Register base of NFI core
++      - description: Register base of ECC engine
++
++  reg-names:
++    items:
++      - const: nfi
++      - const: ecc
++
++  clocks:
++    items:
++      - description: Source clock for NFI core, fixed 125MHz
++
++  clock-names:
++    items:
++      - const: nfi_clk
++
++required:
++  - compatible
++  - reg
++  - reg-names
++  - clocks
++  - clock-names
++
++examples:
++  - |
++    nficlock: nficlock {
++    	#clock-cells = <0>;
++    	compatible = "fixed-clock";
++
++    	clock-frequency = <125000000>;
++    };
++
++    nand@1e003000 {
++    	compatible = "mediatek,mt7621-nfc";
++
++    	reg = <0x1e003000 0x800
++    	       0x1e003800 0x800>;
++    	reg-names = "nfi", "ecc";
++
++    	clocks = <&nficlock>;
++    	clock-names = "nfi_clk";
++    };