[PATCH v1 5/7] ufs: rpmb: bounce unaligned frames through a DMA-aligned buffer

The UFS controller requires DMA buffers aligned to ARCH_DMA_MINALIGN,
but the RPMB supplicant may hand the transport an unaligned frame (for
example the status-result frame built on the stack). DMAing from an
unaligned address corrupts the frame and OP-TEE reports the RPMB device
as failed.

Signed-off-by: Jorge Ramirez-Ortiz <[email protected]>
---
 drivers/ufs/ufs-rpmb.c | 22 +++++++++++++++++++++-
 1 file changed, 21 insertions(+), 1 deletion(-)

diff --git a/drivers/ufs/ufs-rpmb.c b/drivers/ufs/ufs-rpmb.c
index 1434161682b..6a02803edd9 100644
--- a/drivers/ufs/ufs-rpmb.c
+++ b/drivers/ufs/ufs-rpmb.c
@@ -2,10 +2,12 @@
 #include <dm.h>
 #include <hexdump.h>
 #include <log.h>
+#include <malloc.h>
 #include <scsi.h>
 #include <ufs.h>
 #include <vsprintf.h>
 #include <u-boot/blake2.h>
+#include <asm/cache.h>
 #include <asm/unaligned.h>
 #include <linux/errno.h>
 #include <linux/string.h>
@@ -36,9 +38,21 @@ static int ufs_rpmb_secprot(struct udevice *scsi_dev, unsigned int region,
 	struct scsi_cmd pccb;
 	u32 len = nframes * RPMB_FRAME_SIZE;
 	u16 spsp = (region << 8) | UFS_RPMB_SEC_PROTOCOL_ID;
+	void *dma_buf = buf;
+	void *bounce = NULL;
 	int retries;
 	int ret = 0;
 
+	if (!IS_ALIGNED((uintptr_t)buf, ARCH_DMA_MINALIGN)) {
+		bounce = memalign(ARCH_DMA_MINALIGN,
+				  ALIGN(len, ARCH_DMA_MINALIGN));
+		if (!bounce)
+			return -ENOMEM;
+		dma_buf = bounce;
+		if (dir == DMA_TO_DEVICE)
+			memcpy(bounce, buf, len);
+	}
+
 	memset(&pccb, 0, sizeof(pccb));
 	pccb.lun = UFS_UPIU_RPMB_WLUN;
 	pccb.cmd[0] = opcode;
@@ -54,7 +68,7 @@ static int ufs_rpmb_secprot(struct udevice *scsi_dev, unsigned int region,
 	pccb.cmd[10] = 0;
 	pccb.cmd[11] = 0;
 	pccb.cmdlen = 12;
-	pccb.pdata = buf;
+	pccb.pdata = dma_buf;
 	pccb.datalen = len;
 	pccb.dma_dir = dir;
 
@@ -64,6 +78,12 @@ static int ufs_rpmb_secprot(struct udevice *scsi_dev, unsigned int region,
 			break;
 	}
 
+	if (bounce) {
+		if (!ret && dir == DMA_FROM_DEVICE)
+			memcpy(buf, bounce, len);
+		free(bounce);
+	}
+
 	return ret;
 }
 
-- 
2.54.0