From bb3a8f822ebc94353f46332c7432a248fca56965 Mon Sep 17 00:00:00 2001
From: Sugar Zhang <sugar.zhang@rock-chips.com>
Date: Thu, 30 Aug 2018 20:36:55 +0800
Subject: [PATCH] ASoC: rockchip: vad: memcpy: add support padding size

This patch add support for memcpy with padding size.
just like extend vad 6ch to 8ch buffer, so the padding
is needed.

Change-Id: Ie9777b2856d556d4934bce6e850dae9b27b078db
Signed-off-by: Sugar Zhang <sugar.zhang@rock-chips.com>
---
 sound/soc/rockchip/rockchip_vad.c | 66 +++++++++++++++++++++++--------
 1 file changed, 50 insertions(+), 16 deletions(-)

diff --git a/sound/soc/rockchip/rockchip_vad.c b/sound/soc/rockchip/rockchip_vad.c
index 1c95688b923b..062ba8b6afed 100644
--- a/sound/soc/rockchip/rockchip_vad.c
+++ b/sound/soc/rockchip/rockchip_vad.c
@@ -325,6 +325,33 @@ bool snd_pcm_vad_attached(struct snd_pcm_substream *substream)
 }
 EXPORT_SYMBOL(snd_pcm_vad_attached);
 
+static int vad_memcpy_fromio(void *to, void __iomem *from,
+			     int size, int frame_sz, int padding_sz)
+{
+	int i, step_src, step_dst, fcount;
+
+	step_src = frame_sz;
+	step_dst = frame_sz + padding_sz;
+
+	if (size % frame_sz) {
+		pr_err("%s: invalid size: %d\n", __func__, size);
+		return -EINVAL;
+	}
+
+	fcount = size / frame_sz;
+	if (padding_sz) {
+		for (i = 0; i < fcount; i++) {
+			memcpy_fromio(to, from, frame_sz);
+			to += step_dst;
+			from += step_src;
+		}
+	} else {
+		memcpy_fromio(to, from, size);
+	}
+
+	return 0;
+}
+
 /**
  * snd_pcm_vad_memcpy - Copy vad data to dst
  * @substream: PCM substream instance
@@ -340,7 +367,7 @@ snd_pcm_sframes_t snd_pcm_vad_memcpy(struct snd_pcm_substream *substream,
 	struct rockchip_vad *vad = NULL;
 	struct vad_buf *vbuf;
 	snd_pcm_uframes_t avail;
-	int bytes;
+	int bytes, vbytes, frame_sz, vframe_sz, padding_sz;
 
 	vad = substream_get_drvdata(substream);
 
@@ -356,30 +383,37 @@ snd_pcm_sframes_t snd_pcm_vad_memcpy(struct snd_pcm_substream *substream,
 	if (bytes <= 0)
 		return -EFAULT;
 
-	dev_dbg(vad->dev,
-		"begin: %p, pos: %p, end: %p, size: %d, bytes: %d\n",
-		vbuf->begin, vbuf->pos, vbuf->end, vbuf->size, bytes);
+	frame_sz = frames_to_bytes(runtime, 1);
+	vframe_sz = samples_to_bytes(runtime, vad->channels);
+	padding_sz = frame_sz - vframe_sz;
+	vbytes = vframe_sz * avail;
+
+	memset(buf, 0x0, bytes);
 	if (!vbuf->loop) {
-		memcpy_fromio(buf, vbuf->pos, bytes);
-		vbuf->pos += bytes;
+		vad_memcpy_fromio(buf, vbuf->pos, vbytes,
+				  vframe_sz, padding_sz);
+		vbuf->pos += vbytes;
 	} else {
-		if ((vbuf->pos + bytes) <= vbuf->end) {
-			memcpy_fromio(buf, vbuf->pos, bytes);
-			vbuf->pos += bytes;
+		if ((vbuf->pos + vbytes) <= vbuf->end) {
+			vad_memcpy_fromio(buf, vbuf->pos, vbytes,
+					  vframe_sz, padding_sz);
+			vbuf->pos += vbytes;
 		} else {
 			int part1 = vbuf->end - vbuf->pos;
-			int part2 = bytes - part1;
+			int part2 = vbytes - part1;
+			int offset = part1;
 
-			memcpy_fromio(buf, vbuf->pos, part1);
-			memcpy_fromio(buf + part1, vbuf->begin, part2);
+			if (padding_sz)
+				offset = part1 / vframe_sz * frame_sz;
+			vad_memcpy_fromio(buf, vbuf->pos, part1,
+					  vframe_sz, padding_sz);
+			vad_memcpy_fromio(buf + offset, vbuf->begin, part2,
+					  vframe_sz, padding_sz);
 			vbuf->pos = vbuf->begin + part2;
 		}
 	}
 
-	vbuf->size -= bytes;
-	dev_dbg(vad->dev,
-		"begin: %p, pos: %p, end: %p, size: %d, bytes: %d\n",
-		vbuf->begin, vbuf->pos, vbuf->end, vbuf->size, bytes);
+	vbuf->size -= vbytes;
 
 	return avail;
 }