[PATCH u-boot-marvell 3/7] tools: kwbimage: Add support for dumping extended and binary v0 headers

["Pali Rohár" <pali@kernel.org>]

dumpimage is now able to successfully parse and dump content of the Dove
bootloader image.

Note that support for generating these extended parts of v0 images is not
included yet.

Signed-off-by: Pali Rohár <pali@kernel.org>
---
 tools/kwbimage.c | 127 ++++++++++++++++++++++++++++++++++++++++++-----
 tools/kwbimage.h |  51 +++++++++++++++++++
 2 files changed, 166 insertions(+), 12 deletions(-)

diff --git a/tools/kwbimage.c b/tools/kwbimage.c
index 99d38cd1cfb2..22e6a44c561c 100644
--- a/tools/kwbimage.c
+++ b/tools/kwbimage.c
@@ -17,6 +17,8 @@
 #include <stdint.h>
 #include "kwbimage.h"
 
+#include <asm-generic/bitops/fls.h>
+
 #include <openssl/bn.h>
 #include <openssl/rsa.h>
 #include <openssl/pem.h>
@@ -1898,6 +1900,7 @@ static void kwbimage_set_header(void *ptr, struct stat *sbuf, int ifd,
 static void kwbimage_print_header(const void *ptr)
 {
 	struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
+	struct bin_hdr_v0 *bhdr;
 	struct opt_hdr_v1 *ohdr;
 
 	printf("Image Type:   MVEBU Boot from %s Image\n",
@@ -1915,6 +1918,13 @@ static void kwbimage_print_header(const void *ptr)
 		}
 	}
 
+	for_each_bin_hdr_v0(bhdr, mhdr) {
+		printf("BIN Img Size: ");
+		genimg_print_size(le32_to_cpu(bhdr->size));
+		printf("BIN Img Addr: %08x\n", le32_to_cpu(bhdr->destaddr));
+		printf("BIN Img Entr: %08x\n", le32_to_cpu(bhdr->execaddr));
+	}
+
 	printf("Data Size:    ");
 	genimg_print_size(mhdr->blocksize - sizeof(uint32_t));
 	printf("Load Address: %08x\n", mhdr->destaddr);
@@ -1947,15 +1957,31 @@ static int kwbimage_verify_header(unsigned char *ptr, int image_size,
 	/* Only version 0 extended header has checksum */
 	if (kwbimage_version(ptr) == 0) {
 		struct main_hdr_v0 *mhdr = (struct main_hdr_v0 *)ptr;
+		struct ext_hdr_v0 *ext_hdr;
+		struct bin_hdr_v0 *bhdr;
 
-		if (mhdr->ext) {
-			struct ext_hdr_v0 *ext_hdr = (void *)(mhdr + 1);
-
+		for_each_ext_hdr_v0(ext_hdr, ptr) {
 			csum = image_checksum8(ext_hdr, sizeof(*ext_hdr) - 1);
 			if (csum != ext_hdr->checksum)
 				return -FDT_ERR_BADSTRUCTURE;
 		}
 
+		for_each_bin_hdr_v0(bhdr, ptr) {
+			csum = image_checksum8(bhdr, (uint8_t *)&bhdr->checksum - (uint8_t *)bhdr - 1);
+			if (csum != bhdr->checksum)
+				return -FDT_ERR_BADSTRUCTURE;
+
+			if (bhdr->offset > sizeof(*bhdr) || bhdr->offset % 4 != 0)
+				return -FDT_ERR_BADSTRUCTURE;
+
+			if (bhdr->offset + bhdr->size + 4 > sizeof(*bhdr) || bhdr->size % 4 != 0)
+				return -FDT_ERR_BADSTRUCTURE;
+
+			if (image_checksum32((uint8_t *)bhdr + bhdr->offset, bhdr->size) !=
+			    *(uint32_t *)((uint8_t *)bhdr + bhdr->offset + bhdr->size))
+				return -FDT_ERR_BADSTRUCTURE;
+		}
+
 		blockid = mhdr->blockid;
 		offset = le32_to_cpu(mhdr->srcaddr);
 		size = le32_to_cpu(mhdr->blocksize);
@@ -2130,8 +2156,11 @@ static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
 	struct register_set_hdr_v1 *regset_hdr;
 	struct ext_hdr_v0_reg *regdata;
 	struct ext_hdr_v0 *ehdr0;
+	struct bin_hdr_v0 *bhdr0;
 	struct opt_hdr_v1 *ohdr;
+	int params_count;
 	unsigned offset;
+	int is_v0_ext;
 	int cur_idx;
 	int version;
 	FILE *f;
@@ -2145,6 +2174,14 @@ static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
 
 	version = kwbimage_version(ptr);
 
+	is_v0_ext = 0;
+	if (version == 0) {
+		if (mhdr0->ext > 1 || mhdr0->bin ||
+		    ((ehdr0 = ext_hdr_v0_first(ptr)) &&
+		     (ehdr0->match_addr || ehdr0->match_mask || ehdr0->match_value)))
+			is_v0_ext = 1;
+	}
+
 	if (version != 0)
 		fprintf(f, "VERSION %d\n", version);
 
@@ -2156,10 +2193,11 @@ static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
 	if (mhdr->blockid == IBR_HDR_NAND_ID)
 		fprintf(f, "NAND_PAGE_SIZE 0x%x\n", (unsigned)mhdr->nandpagesize);
 
-	if (version != 0 && mhdr->blockid == IBR_HDR_NAND_ID) {
+	if (version != 0 && mhdr->blockid == IBR_HDR_NAND_ID)
 		fprintf(f, "NAND_BLKSZ 0x%x\n", (unsigned)mhdr->nandblocksize);
+
+	if (mhdr->blockid == IBR_HDR_NAND_ID && (mhdr->nandbadblklocation != 0 || is_v0_ext))
 		fprintf(f, "NAND_BADBLK_LOCATION 0x%x\n", (unsigned)mhdr->nandbadblklocation);
-	}
 
 	if (version == 0 && mhdr->blockid == IBR_HDR_SATA_ID)
 		fprintf(f, "SATA_PIO_MODE %u\n", (unsigned)mhdr0->satapiomode);
@@ -2222,8 +2260,39 @@ static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
 		}
 	}
 
-	if (version == 0 && mhdr0->ext) {
-		ehdr0 = (struct ext_hdr_v0 *)(mhdr0 + 1);
+	if (version == 0 && !is_v0_ext && le16_to_cpu(mhdr0->ddrinitdelay))
+		fprintf(f, "DDR_INIT_DELAY %u\n", (unsigned)le16_to_cpu(mhdr0->ddrinitdelay));
+
+	for_each_ext_hdr_v0(ehdr0, ptr) {
+		if (is_v0_ext) {
+			fprintf(f, "\nMATCH ADDRESS 0x%08x MASK 0x%08x VALUE 0x%08x\n",
+				le32_to_cpu(ehdr0->match_addr),
+				le32_to_cpu(ehdr0->match_mask),
+				le32_to_cpu(ehdr0->match_value));
+			if (ehdr0->rsvd1[0] || ehdr0->rsvd1[1] || ehdr0->rsvd1[2] ||
+			    ehdr0->rsvd1[3] || ehdr0->rsvd1[4] || ehdr0->rsvd1[5] ||
+			    ehdr0->rsvd1[6] || ehdr0->rsvd1[7])
+				fprintf(f, "#DDR_RSVD1 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+					ehdr0->rsvd1[0], ehdr0->rsvd1[1], ehdr0->rsvd1[2],
+					ehdr0->rsvd1[3], ehdr0->rsvd1[4], ehdr0->rsvd1[5],
+					ehdr0->rsvd1[6], ehdr0->rsvd1[7]);
+			if (ehdr0->rsvd2[0] || ehdr0->rsvd2[1] || ehdr0->rsvd2[2] ||
+			    ehdr0->rsvd2[3] || ehdr0->rsvd2[4] || ehdr0->rsvd2[5] ||
+			    ehdr0->rsvd2[6])
+				fprintf(f, "#DDR_RSVD2 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x 0x%x\n",
+					ehdr0->rsvd2[0], ehdr0->rsvd2[1], ehdr0->rsvd2[2],
+					ehdr0->rsvd2[3], ehdr0->rsvd2[4], ehdr0->rsvd2[5],
+					ehdr0->rsvd2[6]);
+			if (ehdr0->ddrwritetype)
+				fprintf(f, "DDR_WRITE_TYPE %u\n", (unsigned)ehdr0->ddrwritetype);
+			if (ehdr0->ddrresetmpp)
+				fprintf(f, "DDR_RESET_MPP 0x%x\n", (unsigned)ehdr0->ddrresetmpp);
+			if (ehdr0->ddrclkenmpp)
+				fprintf(f, "DDR_CLKEN_MPP 0x%x\n", (unsigned)ehdr0->ddrclkenmpp);
+			if (ehdr0->ddrinitdelay)
+				fprintf(f, "DDR_INIT_DELAY %u\n", (unsigned)ehdr0->ddrinitdelay);
+		}
+
 		if (ehdr0->offset) {
 			for (regdata = (struct ext_hdr_v0_reg *)((uint8_t *)ptr + ehdr0->offset);
 			     (uint8_t *)regdata < (uint8_t *)ptr + header_size &&
@@ -2234,10 +2303,38 @@ static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
 			if ((uint8_t *)regdata != (uint8_t *)ptr + ehdr0->offset)
 				fprintf(f, "DATA 0x0 0x0\n");
 		}
+
+		if (le32_to_cpu(ehdr0->enddelay))
+			fprintf(f, "DATA_DELAY %u\n", le32_to_cpu(ehdr0->enddelay));
+		else if (is_v0_ext)
+			fprintf(f, "DATA_DELAY SDRAM_SETUP\n");
 	}
 
-	if (version == 0 && le16_to_cpu(mhdr0->ddrinitdelay))
-		fprintf(f, "DDR_INIT_DELAY %u\n", (unsigned)le16_to_cpu(mhdr0->ddrinitdelay));
+	cur_idx = 1;
+	for_each_bin_hdr_v0(bhdr0, ptr) {
+		fprintf(f, "\nMATCH ADDRESS 0x%08x MASK 0x%08x VALUE 0x%08x\n",
+			le32_to_cpu(bhdr0->match_addr),
+			le32_to_cpu(bhdr0->match_mask),
+			le32_to_cpu(bhdr0->match_value));
+
+		fprintf(f, "BINARY binary%d.bin", cur_idx);
+		params_count = fls(bhdr0->params_flags & 0xF);
+		for (i = 0; i < params_count; i++)
+			fprintf(f, " 0x%x", (bhdr0->params[i] & (1 << i)) ? bhdr0->params[i] : 0);
+		fprintf(f, " LOAD_ADDRESS 0x%08x", le32_to_cpu(bhdr0->destaddr));
+		fprintf(f, " EXEC_ADDRESS 0x%08x", le32_to_cpu(bhdr0->execaddr));
+		fprintf(f, "\n");
+
+		fprintf(f, "#BINARY_OFFSET 0x%x\n", le32_to_cpu(bhdr0->offset));
+		fprintf(f, "#BINARY_SIZE 0x%x\n", le32_to_cpu(bhdr0->size));
+
+		if (bhdr0->rsvd1)
+			fprintf(f, "#BINARY_RSVD1 0x%x\n", (unsigned)bhdr0->rsvd1);
+		if (bhdr0->rsvd2)
+			fprintf(f, "#BINARY_RSVD2 0x%x\n", (unsigned)bhdr0->rsvd2);
+
+		cur_idx++;
+	}
 
 	/* Undocumented reserved fields */
 
@@ -2245,9 +2342,6 @@ static int kwbimage_generate_config(void *ptr, struct image_tool_params *params)
 		fprintf(f, "#RSVD1 0x%x 0x%x 0x%x\n", (unsigned)mhdr0->rsvd1[0],
 			(unsigned)mhdr0->rsvd1[1], (unsigned)mhdr0->rsvd1[2]);
 
-	if (version == 0 && mhdr0->rsvd3)
-		fprintf(f, "#RSVD3 0x%x\n", (unsigned)mhdr0->rsvd3);
-
 	if (version == 0 && le16_to_cpu(mhdr0->rsvd2))
 		fprintf(f, "#RSVD2 0x%x\n", (unsigned)le16_to_cpu(mhdr0->rsvd2));
 
@@ -2266,6 +2360,7 @@ static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params
 {
 	struct main_hdr_v1 *mhdr = (struct main_hdr_v1 *)ptr;
 	size_t header_size = kwbheader_size(ptr);
+	struct bin_hdr_v0 *bhdr;
 	struct opt_hdr_v1 *ohdr;
 	int idx = params->pflag;
 	int cur_idx;
@@ -2312,6 +2407,14 @@ static int kwbimage_extract_subimage(void *ptr, struct image_tool_params *params
 
 			++cur_idx;
 		}
+		for_each_bin_hdr_v0(bhdr, ptr) {
+			if (idx == cur_idx) {
+				image = (ulong)bhdr + bhdr->offset;
+				size = bhdr->size;
+				break;
+			}
+			++cur_idx;
+		}
 
 		if (!image) {
 			fprintf(stderr, "Argument -p %d is invalid\n", idx);
diff --git a/tools/kwbimage.h b/tools/kwbimage.h
index 502b6d503305..505522332bd4 100644
--- a/tools/kwbimage.h
+++ b/tools/kwbimage.h
@@ -270,6 +270,57 @@ static inline size_t kwbheader_size_for_csum(const void *header)
 		return kwbheader_size(header);
 }
 
+static inline struct ext_hdr_v0 *ext_hdr_v0_first(void *img)
+{
+	struct main_hdr_v0 *mhdr;
+
+	if (kwbimage_version(img) != 0)
+		return NULL;
+
+	mhdr = img;
+	if (mhdr->ext)
+		return (struct ext_hdr_v0 *)(mhdr + 1);
+	else
+		return NULL;
+}
+
+static inline void *_ext_hdr_v0_end(struct main_hdr_v0 *mhdr)
+{
+	return (uint8_t *)mhdr + kwbheader_size(mhdr) - mhdr->bin * sizeof(struct bin_hdr_v0);
+}
+
+static inline struct ext_hdr_v0 *ext_hdr_v0_next(void *img, struct ext_hdr_v0 *cur)
+{
+	if ((void *)(cur + 1) < _ext_hdr_v0_end(img))
+		return (struct ext_hdr_v0 *)((uint8_t *)(cur + 1) + 0x20);
+	else
+		return NULL;
+}
+
+#define for_each_ext_hdr_v0(ehdr, img)			\
+	for ((ehdr) = ext_hdr_v0_first((img));		\
+	     (ehdr) != NULL;				\
+	     (ehdr) = ext_hdr_v0_next((img), (ehdr)))
+
+static inline struct bin_hdr_v0 *bin_hdr_v0_first(void *img)
+{
+	struct main_hdr_v0 *mhdr;
+
+	if (kwbimage_version(img) != 0)
+		return NULL;
+
+	mhdr = img;
+	if (mhdr->bin)
+		return _ext_hdr_v0_end(mhdr);
+	else
+		return NULL;
+}
+
+#define for_each_bin_hdr_v0(bhdr, img)							\
+	for ((bhdr) = bin_hdr_v0_first((img));						\
+	     (bhdr) && (void *)(bhdr) < (void *)((uint8_t *)img + kwbheader_size(img));	\
+	     (bhdr) = (struct bin_hdr_v0 *)((bhdr))+1)
+
 static inline uint32_t opt_hdr_v1_size(const struct opt_hdr_v1 *ohdr)
 {
 	return (ohdr->headersz_msb << 16) | le16_to_cpu(ohdr->headersz_lsb);
-- 
2.20.1