From 5d8178563bc6f3d684f1835d79cb2cb0336409d5 Mon Sep 17 00:00:00 2001
From: =?utf8?q?Pali=20Roh=C3=A1r?= <pali@kernel.org>
Date: Wed, 29 Mar 2023 21:25:57 +0200
Subject: [PATCH] tools: kwboot: Add support for parsing SATA images with
 non-512 block size
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

Currently kwboot expected that sector size for SATA image is always 512
bytes. If SATA image cannot be parsed with sector size of 512 bytes, try
larger sector sizes which are power of two and up to the 32 kB. Maximal
theoretical value is 32 kB because ATA IDENTIFY command returns sector size
as 16-bit number.

Signed-off-by: Pali Rohár <pali@kernel.org>
---
 tools/kwboot.c | 35 ++++++++++++++++++++++++++++++++++-
 1 file changed, 34 insertions(+), 1 deletion(-)

diff --git a/tools/kwboot.c b/tools/kwboot.c
index 348a3203d6..6bef4610ff 100644
--- a/tools/kwboot.c
+++ b/tools/kwboot.c
@@ -1991,6 +1991,39 @@ _inject_baudrate_change_code(void *img, size_t *size, int for_data,
 	}
 }
 
+static int
+kwboot_img_guess_sata_blksz(void *img, uint32_t blkoff, uint32_t data_size, size_t total_size)
+{
+	uint32_t sum, *ptr, *end;
+	int blksz;
+
+	/*
+	 * Try all possible sector sizes which are power of two,
+	 * at least 512 bytes and up to the 32 kB.
+	 */
+	for (blksz = 512; blksz < 0x10000; blksz *= 2) {
+		if (blkoff * blksz > total_size ||
+		    blkoff * blksz + data_size > total_size ||
+		    data_size % 4)
+			break;
+
+		/*
+		 * Calculate data checksum and if it matches
+		 * then tried blksz should be correct.
+		 */
+		ptr = img + blkoff * blksz;
+		end = (void *)ptr + data_size - 4;
+		for (sum = 0; ptr < end; ptr++)
+			sum += *ptr;
+
+		if (sum == *end)
+			return blksz;
+	}
+
+	/* Fallback to 512 bytes */
+	return 512;
+}
+
 static const char *
 kwboot_img_type(uint8_t blockid)
 {
@@ -2049,7 +2082,7 @@ kwboot_img_patch(void *img, size_t *size, int baudrate)
 
 	switch (hdr->blockid) {
 	case IBR_HDR_SATA_ID:
-		hdr->srcaddr = cpu_to_le32(srcaddr * 512);
+		hdr->srcaddr = cpu_to_le32(srcaddr * kwboot_img_guess_sata_blksz(img, srcaddr, le32_to_cpu(hdr->blocksize), *size));
 		break;
 
 	case IBR_HDR_PEX_ID:
-- 
2.39.5