diff --git a/drivers/mtd/spi-nor/mtk-quadspi.c b/drivers/mtd/spi-nor/mtk-quadspi.c
index 33a8dc56c20f..70dd62c7f989 100644
--- a/drivers/mtd/spi-nor/mtk-quadspi.c
+++ b/drivers/mtd/spi-nor/mtk-quadspi.c
@@ -101,7 +101,12 @@
 #define MTK_NOR_FAST_READ		0x1
 
 #define SFLASH_WRBUF_SIZE		128
-#define get_nth_byte(d, n)		((d >> (8 * n)) & 0xff)
+
+/* Can shift up to 48 bits (6 bytes) of TX/RX */
+#define MTK_NOR_MAX_SHIFT		6
+/* Helpers for accessing the program data / shift data registers */
+#define MTK_NOR_PRG_REG(n)		(MTK_NOR_PRGDATA0_REG + 4 * (n))
+#define MTK_NOR_SHREG(n)		(MTK_NOR_SHREG0_REG + 4 * (n))
 
 struct mt8173_nor {
 	struct spi_nor nor;
@@ -147,47 +152,54 @@ static int mt8173_nor_execute_cmd(struct mt8173_nor *mt8173_nor, u8 cmdval)
 				  !(reg & val), 100, 10000);
 }
 
-static int mt8173_nor_do_tx(struct mt8173_nor *mt8173_nor, u8 op, u8 *buf,
-			    int len)
+static int mt8173_nor_do_tx_rx(struct mt8173_nor *mt8173_nor, u8 op,
+			       u8 *tx, int txlen, u8 *rx, int rxlen)
 {
-	int i;
+	int len = 1 + txlen + rxlen;
+	int i, ret, idx;
 
-	if (len > 5)
+	if (len > MTK_NOR_MAX_SHIFT)
 		return -EINVAL;
 
-	writeb(op, mt8173_nor->base + MTK_NOR_PRGDATA5_REG);
+	writeb(len * 8, mt8173_nor->base + MTK_NOR_CNT_REG);
 
-	for (i = 0; i < len; i++)
-		writeb(buf[len - 1 - i], mt8173_nor->base +
-			       MTK_NOR_PRGDATA0_REG + 4 * (4 - i));
-	writeb((len + 1) * 8, mt8173_nor->base + MTK_NOR_CNT_REG);
-	return mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PRG_CMD);
-}
+	/* start at PRGDATA5, go down to PRGDATA0 */
+	idx = MTK_NOR_MAX_SHIFT - 1;
 
-/*
- * this function is used to execute special read commands.
- * such as SPINOR_OP_RDFSR, SPINOR_OP_RDCR, SPINOR_OP_RD_EVCR and so on.
- * len is no more than 1.
- */
-static int mt8173_nor_do_rx(struct mt8173_nor *mt8173_nor, u8 op, u8 *buf,
-			    int len)
-{
-	if (len > 1)
-		return -EINVAL;
+	/* opcode */
+	writeb(op, mt8173_nor->base + MTK_NOR_PRG_REG(idx));
+	idx--;
 
-	writeb(op, mt8173_nor->base + MTK_NOR_PRGDATA5_REG);
+	/* program TX data */
+	for (i = 0; i < txlen; i++, idx--)
+		writeb(tx[i], mt8173_nor->base + MTK_NOR_PRG_REG(idx));
 
-	writeb(8, mt8173_nor->base + MTK_NOR_CNT_REG);
-	return mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PRG_CMD);
+	/* clear out rest of TX registers */
+	while (idx >= 0) {
+		writeb(0, mt8173_nor->base + MTK_NOR_PRG_REG(idx));
+		idx--;
+	}
+
+	ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PRG_CMD);
+	if (ret)
+		return ret;
+
+	/* restart at first RX byte */
+	idx = MTK_NOR_MAX_SHIFT - 2 - txlen;
+
+	/* read out RX data */
+	for (i = 0; i < rxlen; i++, idx--)
+		rx[i] = readb(mt8173_nor->base + MTK_NOR_SHREG(idx));
+
+	return 0;
 }
 
-/* cmd1 sent to nor flash, cmd2 write to nor controller */
-static int mt8173_nor_set_para(struct mt8173_nor *mt8173_nor, int cmd1,
-			       int cmd2)
+/* Do a WRSR (Write Status Register) command */
+static int mt8173_nor_wr_sr(struct mt8173_nor *mt8173_nor, u8 sr)
 {
-	writeb(cmd1, mt8173_nor->base + MTK_NOR_PRGDATA5_REG);
+	writeb(sr, mt8173_nor->base + MTK_NOR_PRGDATA5_REG);
 	writeb(8, mt8173_nor->base + MTK_NOR_CNT_REG);
-	return mt8173_nor_execute_cmd(mt8173_nor, cmd2);
+	return mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_WRSR_CMD);
 }
 
 static int mt8173_nor_write_buffer_enable(struct mt8173_nor *mt8173_nor)
@@ -213,19 +225,16 @@ static int mt8173_nor_write_buffer_disable(struct mt8173_nor *mt8173_nor)
 				  10000);
 }
 
-static int mt8173_nor_erase_sector(struct spi_nor *nor, loff_t offset)
+static void mt8173_nor_set_addr(struct mt8173_nor *mt8173_nor, u32 addr)
 {
-	u8 buf[4], i = 0;
-	struct mt8173_nor *mt8173_nor = nor->priv;
+	int i;
 
-	while (i < 4) {
-		buf[i] = get_nth_byte(offset, i);
-		i++;
+	for (i = 0; i < 3; i++) {
+		writeb(addr & 0xff, mt8173_nor->base + MTK_NOR_RADR0_REG + i * 4);
+		addr >>= 8;
 	}
-	if (nor->mtd.size <= 0x1000000)
-		return mt8173_nor_do_tx(mt8173_nor, SPINOR_OP_BE_4K, buf, 3);
-	else
-		return mt8173_nor_do_tx(mt8173_nor, SPINOR_OP_BE_4K, buf, 4);
+	/* Last register is non-contiguous */
+	writeb(addr & 0xff, mt8173_nor->base + MTK_NOR_RADR3_REG);
 }
 
 static int mt8173_nor_read(struct spi_nor *nor, loff_t from, size_t length,
@@ -238,10 +247,7 @@ static int mt8173_nor_read(struct spi_nor *nor, loff_t from, size_t length,
 
 	/* set mode for fast read mode ,dual mode or quad mode */
 	mt8173_nor_set_read_mode(mt8173_nor);
-	writeb((addr >> 24), mt8173_nor->base + MTK_NOR_RADR3_REG);
-	writeb((addr >> 16), mt8173_nor->base + MTK_NOR_RADR2_REG);
-	writeb((addr >> 8), mt8173_nor->base + MTK_NOR_RADR1_REG);
-	writeb(addr, mt8173_nor->base + MTK_NOR_RADR0_REG);
+	mt8173_nor_set_addr(mt8173_nor, addr);
 
 	for (i = 0; i < length; i++, (*retlen)++) {
 		ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PIO_READ_CMD);
@@ -257,10 +263,7 @@ static int mt8173_nor_write_single_byte(struct mt8173_nor *mt8173_nor,
 {
 	int i, ret;
 
-	writeb((addr >> 24), mt8173_nor->base + MTK_NOR_RADR3_REG);
-	writeb((addr >> 16), mt8173_nor->base + MTK_NOR_RADR2_REG);
-	writeb((addr >> 8), mt8173_nor->base + MTK_NOR_RADR1_REG);
-	writeb(addr, mt8173_nor->base + MTK_NOR_RADR0_REG);
+	mt8173_nor_set_addr(mt8173_nor, addr);
 
 	for (i = 0; i < length; i++) {
 		ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_PIO_WR_CMD);
@@ -276,10 +279,7 @@ static int mt8173_nor_write_buffer(struct mt8173_nor *mt8173_nor, int addr,
 {
 	int i, bufidx, data;
 
-	writeb((addr >> 24), mt8173_nor->base + MTK_NOR_RADR3_REG);
-	writeb((addr >> 16), mt8173_nor->base + MTK_NOR_RADR2_REG);
-	writeb((addr >> 8), mt8173_nor->base + MTK_NOR_RADR1_REG);
-	writeb(addr, mt8173_nor->base + MTK_NOR_RADR0_REG);
+	mt8173_nor_set_addr(mt8173_nor, addr);
 
 	bufidx = 0;
 	for (i = 0; i < SFLASH_WRBUF_SIZE; i += 4) {
@@ -330,28 +330,23 @@ static int mt8173_nor_read_reg(struct spi_nor *nor, u8 opcode, u8 *buf, int len)
 
 	/* mtk nor controller doesn't supoort SPINOR_OP_RDCR */
 	switch (opcode) {
-	case SPINOR_OP_RDID:
-		/* read JEDEC ID need 4 bytes commands */
-		memset(buf, 0x0, 4);
-		ret = mt8173_nor_do_tx(mt8173_nor, opcode, buf, 3);
-		if (ret < 0)
-			return ret;
-
-		/* mtk nor flash controller only support 3 bytes IDs */
-		buf[2] = readb(mt8173_nor->base + MTK_NOR_SHREG0_REG);
-		buf[1] = readb(mt8173_nor->base + MTK_NOR_SHREG1_REG);
-		buf[0] = readb(mt8173_nor->base + MTK_NOR_SHREG2_REG);
-		break;
 	case SPINOR_OP_RDSR:
 		ret = mt8173_nor_execute_cmd(mt8173_nor, MTK_NOR_RDSR_CMD);
 		if (ret < 0)
 			return ret;
 		*buf = readb(mt8173_nor->base + MTK_NOR_RDSR_REG);
 		break;
+	case SPINOR_OP_RDID:
+		if (len > MTK_NOR_MAX_SHIFT - 1) {
+			int i;
+			/* HACK */
+			for (i = MTK_NOR_MAX_SHIFT - 1; i < len; i++)
+				buf[i] = 0;
+			len = MTK_NOR_MAX_SHIFT - 1;
+		}
+		/* fall through */
 	default:
-		/* read other register of nor flash */
-		ret = mt8173_nor_do_rx(mt8173_nor, opcode, buf, len);
-		*buf = readb(mt8173_nor->base + MTK_NOR_SHREG2_REG);
+		ret = mt8173_nor_do_tx_rx(mt8173_nor, opcode, NULL, 0, buf, len);
 		break;
 	}
 	return ret;
@@ -365,41 +360,40 @@ static int mt8173_nor_write_reg(struct spi_nor *nor, u8 opcode, u8 *buf,
 
 	switch (opcode) {
 	case SPINOR_OP_WRSR:
-		ret = mt8173_nor_set_para(mt8173_nor, *buf,
-					  MTK_NOR_WRSR_CMD);
-		break;
-	case SPINOR_OP_CHIP_ERASE:
-		ret = mt8173_nor_set_para(mt8173_nor, opcode,
-					  MTK_NOR_PRG_CMD);
+		/* We only handle 1 byte */
+		ret = mt8173_nor_wr_sr(mt8173_nor, *buf);
 		break;
 	default:
-		ret = mt8173_nor_do_tx(mt8173_nor, opcode, NULL, 0);
+		ret = mt8173_nor_do_tx_rx(mt8173_nor, opcode, buf, len, NULL, 0);
 		if (ret)
-			dev_warn(mt8173_nor->dev, "set write enable fail!\n");
+			dev_warn(mt8173_nor->dev, "write reg failure!\n");
 		break;
 	}
 	return ret;
 }
 
 static int __init mtk_nor_init(struct mt8173_nor *mt8173_nor,
-			       struct mtd_part_parser_data *ppdata)
+			       struct device_node *flash_node)
 {
+	struct mtd_part_parser_data ppdata = {
+		.of_node = flash_node,
+	};
 	int ret;
 	struct spi_nor *nor;
 
+	/* initialize controller to accept commands */
 	writel(MTK_NOR_ENABLE_SF_CMD, mt8173_nor->base + MTK_NOR_WRPROT_REG);
 
 	nor = &mt8173_nor->nor;
 	nor->dev = mt8173_nor->dev;
 	nor->priv = mt8173_nor;
-	nor->flash_node = ppdata->of_node;
+	nor->flash_node = flash_node;
 
 	/* fill the hooks to spi nor */
 	nor->read = mt8173_nor_read;
 	nor->read_reg = mt8173_nor_read_reg;
 	nor->write = mt8173_nor_write;
 	nor->write_reg = mt8173_nor_write_reg;
-	nor->erase = mt8173_nor_erase_sector;
 	nor->mtd.owner = THIS_MODULE;
 	nor->mtd.name = "mtk_nor";
 	/* initialized with NULL */
@@ -407,13 +401,12 @@ static int __init mtk_nor_init(struct mt8173_nor *mt8173_nor,
 	if (ret)
 		return ret;
 
-	return mtd_device_parse_register(&nor->mtd, NULL, ppdata, NULL, 0);
+	return mtd_device_parse_register(&nor->mtd, NULL, &ppdata, NULL, 0);
 }
 
 static int mtk_nor_drv_probe(struct platform_device *pdev)
 {
 	struct device_node *flash_np;
-	struct mtd_part_parser_data ppdata;
 	struct resource *res;
 	int ret;
 	struct mt8173_nor *mt8173_nor;
@@ -449,14 +442,14 @@ static int mtk_nor_drv_probe(struct platform_device *pdev)
 	clk_prepare_enable(mt8173_nor->spi_clk);
 	clk_prepare_enable(mt8173_nor->nor_clk);
 
+	/* only support one attached flash */
 	flash_np = of_get_next_available_child(pdev->dev.of_node, NULL);
 	if (!flash_np) {
 		dev_err(&pdev->dev, "no SPI flash device to configure\n");
 		ret = -ENODEV;
 		goto nor_free;
 	}
-	ppdata.of_node = flash_np;
-	ret = mtk_nor_init(mt8173_nor, &ppdata);
+	ret = mtk_nor_init(mt8173_nor, flash_np);
 
 nor_free:
 	return ret;
