[PATCH 2/3] dmaengine: pl330: Add DMAADDH instruction

[Aatif Mushtaq <aatif4.m@xxxxxxxxxxx>]

Add support for emitting DMAADDH instruction.
Add halfword instruction adds an immediate 16-bit value to the source
address register or destination address register for the DMA channel
thread. This enables the DMAC to support 2D DMA operations.

Signed-off-by: Aatif Mushtaq <aatif4.m@xxxxxxxxxxx>
---
 drivers/dma/pl330.c | 26 ++++++++++++++++++++++++++
 1 file changed, 26 insertions(+)

diff --git a/drivers/dma/pl330.c b/drivers/dma/pl330.c
index 60c4de8dac1d..546ea442044e 100644
--- a/drivers/dma/pl330.c
+++ b/drivers/dma/pl330.c
@@ -323,6 +323,8 @@ struct pl330_xfer {
 	u32 dst_addr;
 	/* Size to xfer */
 	u32 bytes;
+	u16 src_imm;
+	u16 dst_imm;
 };
 
 /* The xfer callbacks are made with one of these arguments. */
@@ -623,6 +625,22 @@ static inline u32 _emit_LD(unsigned dry_run, u8 buf[],	enum pl330_cond cond)
 	return SZ_DMALD;
 }
 
+static inline u32 _emit_DMAADDH(unsigned dry_run, u8 buf[], enum pl330_dst ra, u16 imm)
+{
+	if (dry_run)
+		return SZ_DMAADDH;
+
+	buf[0] = CMD_DMAADDH;
+	buf[0] |= (ra << 1);
+	buf[1] = imm;
+	buf[2] = imm >> 8;
+
+	PL330_DBGCMD_DUMP(SZ_DMAADDH, "\tDMAADDH %s %u\n",
+			ra == 0 ? "SA" : "DA", imm);
+
+	return SZ_DMAADDH;
+}
+
 static inline u32 _emit_LDP(unsigned dry_run, u8 buf[],
 		enum pl330_cond cond, u8 peri)
 {
@@ -1097,6 +1115,7 @@ static inline int _ldst_memtomem(unsigned dry_run, u8 buf[],
 {
 	int off = 0;
 	struct pl330_config *pcfg = pxs->desc->rqcfg.pcfg;
+	struct pl330_xfer *x = &pxs->desc->px;
 
 	/* check lock-up free version */
 	if (get_revision(pcfg->periph_id) >= PERIPH_REV_R1P0) {
@@ -1113,6 +1132,11 @@ static inline int _ldst_memtomem(unsigned dry_run, u8 buf[],
 		}
 	}
 
+	if (x->src_imm)
+		off += _emit_DMAADDH(dry_run, &buf[off], SRC, x->src_imm);
+	if (x->dst_imm)
+		off += _emit_DMAADDH(dry_run, &buf[off], DST, x->dst_imm);
+
 	return off;
 }
 
@@ -2633,6 +2657,8 @@ static inline void fill_px(struct pl330_xfer *px,
 	px->bytes = len;
 	px->dst_addr = dst;
 	px->src_addr = src;
+	px->src_imm = 0;
+	px->dst_imm = 0;
 }
 
 static struct dma_pl330_desc *
-- 
2.17.1