diff --git a/target/linux/mediatek/files-5.4/drivers/mtd/mtk-snand/mtk-snand.c b/target/linux/mediatek/files-5.4/drivers/mtd/mtk-snand/mtk-snand.c
index 6d57d56..922454d 100644
--- a/target/linux/mediatek/files-5.4/drivers/mtd/mtk-snand/mtk-snand.c
+++ b/target/linux/mediatek/files-5.4/drivers/mtd/mtk-snand/mtk-snand.c
@@ -57,8 +57,15 @@
 #define FIFO_WR_REMAIN_S		8
 #define FIFO_RD_REMAIN_S		0
 
+#define NFI_ADDRCNTR			0x070
+#define SEC_CNTR			GENMASK(16, 12)
+#define	SEC_CNTR_S			12
+
 #define NFI_STRADDR			0x080
 
+#define NFI_BYTELEN			0x084
+#define	BUS_SEC_CNTR(val)		(((val) & SEC_CNTR) >> SEC_CNTR_S)
+
 #define NFI_FDM0L			0x0a0
 #define NFI_FDM0M			0x0a4
 #define NFI_FDML(n)			(NFI_FDM0L + (n) * 8)
@@ -715,7 +722,7 @@
 
 static int mtk_snand_read_cache(struct mtk_snand *snf, uint32_t page, bool raw)
 {
-	uint32_t coladdr, rwbytes, mode, len;
+	uint32_t coladdr, rwbytes, mode, len, val;
 	uintptr_t dma_addr;
 	int ret;
 
@@ -783,6 +790,26 @@
 		goto cleanup;
 	}
 
+	/* Wait for BUS_SEC_CNTR returning expected value */
+	ret = read32_poll_timeout(snf->nfi_base + NFI_BYTELEN, val,
+				  BUS_SEC_CNTR(val) >= snf->ecc_steps,
+				  0, SNFI_POLL_INTERVAL);
+	if (ret) {
+		snand_log_nfi(snf->pdev,
+			      "Timed out waiting for BUS_SEC_CNTR\n");
+		goto cleanup;
+	}
+
+	/* Wait for bus becoming idle */
+	ret = read32_poll_timeout(snf->nfi_base + NFI_MASTERSTA, val,
+				  !(val & snf->nfi_soc->mastersta_mask),
+				  0, SNFI_POLL_INTERVAL);
+	if (ret) {
+		snand_log_nfi(snf->pdev,
+			      "Timed out waiting for bus becoming idle\n");
+		goto cleanup;
+	}
+
 	if (!raw) {
 		ret = mtk_ecc_wait_decoder_done(snf);
 		if (ret)
@@ -941,7 +968,7 @@
 static int mtk_snand_program_load(struct mtk_snand *snf, uint32_t page,
 				  bool raw)
 {
-	uint32_t coladdr, rwbytes, mode, len;
+	uint32_t coladdr, rwbytes, mode, len, val;
 	uintptr_t dma_addr;
 	int ret;
 
@@ -1011,6 +1038,16 @@
 		goto cleanup;
 	}
 
+	/* Wait for BUS_SEC_CNTR returning expected value */
+	ret = read32_poll_timeout(snf->nfi_base + NFI_BYTELEN, val,
+				  BUS_SEC_CNTR(val) >= snf->ecc_steps,
+				  0, SNFI_POLL_INTERVAL);
+	if (ret) {
+		snand_log_nfi(snf->pdev,
+			      "Timed out waiting for BUS_SEC_CNTR\n");
+		goto cleanup;
+	}
+
 	if (!raw)
 		mtk_snand_ecc_encoder_stop(snf);