1 files changed, 61 insertions, 8 deletions

diff --git a/lib/image-sparse.c b/lib/image-sparse.c
index d80fdbbf58..5ec0f94ab3 100644
--- a/lib/image-sparse.c
+++ b/lib/image-sparse.c
@@ -46,9 +46,66 @@
 #include <asm/cache.h>
 
 #include <linux/math64.h>
+#include <linux/err.h>
 
 static void default_log(const char *ignored, char *response) {}
 
+static lbaint_t write_sparse_chunk_raw(struct sparse_storage *info,
+				       lbaint_t blk, lbaint_t blkcnt,
+				       void *data,
+				       char *response)
+{
+	lbaint_t n = blkcnt, write_blks, blks = 0, aligned_buf_blks = 100;
+	uint32_t *aligned_buf = NULL;
+
+	if (CONFIG_IS_ENABLED(SYS_DCACHE_OFF)) {
+		write_blks = info->write(info, blk, n, data);
+		if (write_blks < n)
+			goto write_fail;
+
+		return write_blks;
+	}
+
+	aligned_buf = memalign(ARCH_DMA_MINALIGN, info->blksz * aligned_buf_blks);
+	if (!aligned_buf) {
+		info->mssg("Malloc failed for: CHUNK_TYPE_RAW", response);
+		return -ENOMEM;
+	}
+
+	while (blkcnt > 0) {
+		n = min(aligned_buf_blks, blkcnt);
+		memcpy(aligned_buf, data, n * info->blksz);
+
+		/* write_blks might be > n due to NAND bad-blocks */
+		write_blks = info->write(info, blk + blks, n, aligned_buf);
+		if (write_blks < n) {
+			free(aligned_buf);
+			goto write_fail;
+		}
+
+		blks += write_blks;
+		data += n * info->blksz;
+		blkcnt -= n;
+	}
+
+	free(aligned_buf);
+	return blks;
+
+write_fail:
+	if (IS_ERR_VALUE(write_blks)) {
+		printf("%s: Write failed, block #" LBAFU " [" LBAFU "] (%lld)\n",
+		       __func__, blk + blks, n, (long long)write_blks);
+		info->mssg("flash write failure", response);
+		return write_blks;
+	}
+
+	/* write_blks < n */
+	printf("%s: Write failed, block #" LBAFU " [" LBAFU "]\n",
+	       __func__, blk + blks, n);
+	info->mssg("flash write failure(incomplete)", response);
+	return -1;
+}
+
 int write_sparse_image(struct sparse_storage *info,
 		       const char *part_name, void *data, char *response)
 {
@@ -152,15 +209,11 @@ int write_sparse_image(struct sparse_storage *info,
 				return -1;
 			}
 
-			blks = info->write(info, blk, blkcnt, data);
-			/* blks might be > blkcnt (eg. NAND bad-blocks) */
-			if (blks < blkcnt) {
-				printf("%s: %s" LBAFU " [" LBAFU "]\n",
-				       __func__, "Write failed, block #",
-				       blk, blks);
-				info->mssg("flash write failure", response);
+			blks = write_sparse_chunk_raw(info, blk, blkcnt,
+						      data, response);
+			if (blks < 0)
 				return -1;
-			}
+
 			blk += blks;
 			bytes_written += ((u64)blkcnt) * info->blksz;
 			total_blocks += chunk_header->chunk_sz;