[PATCH 2/2] mtd: mtk-quadspi: misuse 1_1_2 read mode for custom read opcode

[Chuanhong Guo]

1_1_1 reading mode on this controller only support 0x03 and 0x0b
as opcode, but spi-nor framework uses nor->read for SFDP reading
as well.
Add a check for opcode and if it's not supported, misuse 1_1_2
reading and extract corresponding bits from returned data.

Signed-off-by: Chuanhong Guo <gch981213@xxxxxxxxx>
---
 drivers/mtd/spi-nor/mtk-quadspi.c | 78 ++++++++++++++++++++++++++++++-
 1 file changed, 76 insertions(+), 2 deletions(-)

diff --git a/drivers/mtd/spi-nor/mtk-quadspi.c b/drivers/mtd/spi-nor/mtk-quadspi.c
index ac0e531ce80c..46bf27c0e6e8 100644
--- a/drivers/mtd/spi-nor/mtk-quadspi.c
+++ b/drivers/mtd/spi-nor/mtk-quadspi.c
@@ -357,8 +357,8 @@ static ssize_t mtk_nor_read_dma_bounce(struct mtk_nor *mtk_nor, loff_t from,
 	return length;
 }
 
-static ssize_t mtk_nor_read(struct spi_nor *nor, loff_t from, size_t length,
-			    u_char *buffer)
+static ssize_t mtk_nor_flash_read(struct spi_nor *nor, loff_t from,
+				  size_t length, u_char *buffer)
 {
 	struct mtk_nor *mtk_nor = nor->priv;
 
@@ -372,6 +372,80 @@ static ssize_t mtk_nor_read(struct spi_nor *nor, loff_t from, size_t length,
 	return mtk_nor_read_dma(mtk_nor, from, length, buffer);
 }
 
+static ssize_t mtk_nor_generic_read(struct spi_nor *nor, loff_t from,
+				    size_t length, u_char *buffer)
+{
+	struct mtk_nor *mtk_nor = nor->priv;
+	ssize_t nor_unaligned_len = from % MTK_NOR_DMA_ALIGN;
+	loff_t read_from = from & ~(MTK_NOR_DMA_ALIGN - 1);
+	ssize_t read_len;
+	u_char *buf, *bouncebuf, tmp;
+	size_t mem_unaligned_len, i;
+	dma_addr_t dma_addr;
+	int ret;
+
+	if (length > MTK_NOR_MAX_BBUF_READ / 2)
+		length = MTK_NOR_MAX_BBUF_READ / 2;
+	read_len = ((length + nor_unaligned_len) * 2 + MTK_NOR_DMA_ALIGN) &
+		   ~(MTK_NOR_DMA_ALIGN - 1);
+
+	buf = kmalloc(read_len + MTK_NOR_DMA_ALIGN, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	mem_unaligned_len = (u32)buf % MTK_NOR_DMA_ALIGN;
+	bouncebuf = (buf + MTK_NOR_DMA_ALIGN) - mem_unaligned_len;
+
+	writeb(nor->read_opcode, mtk_nor->base + MTK_NOR_PRGDATA3_REG);
+	writeb(MTK_NOR_DUAL_READ_EN, mtk_nor->base + MTK_NOR_DUAL_REG);
+	mtk_nor_set_addr_width(mtk_nor);
+
+	dma_addr = dma_map_single(mtk_nor->dev, bouncebuf, read_len,
+				  DMA_FROM_DEVICE);
+	ret = dma_mapping_error(mtk_nor->dev, dma_addr);
+	if (ret) {
+		dev_err(mtk_nor->dev, "failed to map dma buffer.");
+		goto err;
+	}
+
+	writel(read_from, mtk_nor->base + MTK_NOR_FDMA_FADR_REG);
+	writel(dma_addr, mtk_nor->base + MTK_NOR_FDMA_DADR_REG);
+	writel((u32)dma_addr + read_len,
+	       mtk_nor->base + MTK_NOR_FDMA_END_DADR_REG);
+	ret = mtk_nor_dma_exec(mtk_nor);
+	dma_unmap_single(mtk_nor->dev, dma_addr, read_len, DMA_FROM_DEVICE);
+
+	if (ret)
+		goto err;
+
+	/* extract bits from DO line */
+	for (i = 0; i < length; i++) {
+		tmp = bouncebuf[(i + nor_unaligned_len) * 2];
+		buffer[i] = (tmp & BIT(7)) | ((tmp & BIT(5)) << 1) |
+			    ((tmp & BIT(3)) << 2) | ((tmp & BIT(1)) << 3);
+		tmp = bouncebuf[(i + nor_unaligned_len) * 2 + 1];
+		buffer[i] |= (tmp & BIT(7)) >> 4 | ((tmp & BIT(5)) >> 3) |
+			     ((tmp & BIT(3)) >> 2) | ((tmp & BIT(1)) >> 1);
+	}
+	ret = length;
+err:
+	kfree(buf);
+	return ret;
+}
+
+static ssize_t mtk_nor_read(struct spi_nor *nor, loff_t from, size_t length,
+			    u_char *buffer)
+{
+	if ((nor->read_proto != SNOR_PROTO_1_1_1) ||
+	    (nor->read_opcode == SPINOR_OP_READ) ||
+	    (nor->read_opcode == SPINOR_OP_READ_FAST))
+		return mtk_nor_flash_read(nor, from, length, buffer);
+	else if (nor->read_dummy == 8)
+		return mtk_nor_generic_read(nor, from, length, buffer);
+	else
+		return -EOPNOTSUPP;
+}
+
 static int mtk_nor_write_single_byte(struct mtk_nor *mtk_nor,
 				     int addr, int length, u8 *data)
 {
-- 
2.21.0