tools: kwboot: Add support for parsing SATA images with non-512 block size

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

@@ -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: