[PATCH] mmc: rpmb: realign frames before passing to MMC driver

[Pavel Löbl]

MMC driver requires request and response frame buffers to be properly
aligned for DMA transfers.

Fix already implemented reallocation for request frame and also add
realigment for response frame if needed.

Signed-off-by: Pavel Löbl <pavel at loebl.cz>
---
 drivers/mmc/rpmb.c | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
index b68d98573c..e951f4e9bd 100644
--- a/drivers/mmc/rpmb.c
+++ b/drivers/mmc/rpmb.c
@@ -482,13 +482,14 @@ int mmc_rpmb_route_frames(struct mmc *mmc, void *req, unsigned long reqlen,
 	 */
 	void *rpmb_data = NULL;
 	int ret;
+	void *rsp_aligned = rsp;
 
 	if (reqlen % sizeof(struct s_rpmb) || rsplen % sizeof(struct s_rpmb))
 		return -EINVAL;
 
 	if (!IS_ALIGNED((uintptr_t)req, ARCH_DMA_MINALIGN)) {
 		/* Memory alignment is required by MMC driver */
-		rpmb_data = malloc(reqlen);
+		rpmb_data = memalign(ARCH_DMA_MINALIGN, reqlen);
 		if (!rpmb_data)
 			return -ENOMEM;
 
@@ -496,8 +497,20 @@ int mmc_rpmb_route_frames(struct mmc *mmc, void *req, unsigned long reqlen,
 		req = rpmb_data;
 	}
 
+	if (!IS_ALIGNED((uintptr_t)rsp_aligned, ARCH_DMA_MINALIGN)) {
+		rsp_aligned = memalign(ARCH_DMA_MINALIGN, rsplen);
+		if (!rsp_aligned)
+			return -ENOMEM;
+	}
+
 	ret = rpmb_route_frames(mmc, req, reqlen / sizeof(struct s_rpmb),
-				rsp, rsplen / sizeof(struct s_rpmb));
+				rsp_aligned, rsplen / sizeof(struct s_rpmb));
+
+	if (rsp_aligned != rsp) {
+		memcpy(rsp, rsp_aligned, rsplen);
+		free(rsp_aligned);
+	}
+
 	free(rpmb_data);
 	return ret;
 }
-- 
2.39.2