[PATCH 2/4] tools: kwbimage: Align BIN header executable code to 128-bit boundary

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

ARM executable code inside the BIN header on some mvebu platforms
(e.g. A370, AXP) must always be aligned with the 128-bit boundary. This
requirement can be met by inserting dummy arguments into BIN header.

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

diff --git a/tools/kwbimage.c b/tools/kwbimage.c
index 77bf4dd8865e..abc88d01b9d8 100644
--- a/tools/kwbimage.c
+++ b/tools/kwbimage.c
@@ -932,6 +932,12 @@ static size_t image_headersz_v1(int *hasext)
 	 */
 	headersz = sizeof(struct main_hdr_v1);
 
+	if (image_get_csk_index() >= 0) {
+		headersz += sizeof(struct secure_hdr_v1);
+		if (hasext)
+			*hasext = 1;
+	}
+
 	count = image_count_options(IMAGE_CFG_DATA);
 	if (count > 0)
 		headersz += sizeof(struct register_set_hdr_v1) + 8 * count + 4;
@@ -963,15 +969,10 @@ static size_t image_headersz_v1(int *hasext)
 			return 0;
 		}
 
-		headersz += sizeof(struct opt_hdr_v1) +
-			ALIGN(s.st_size, 4) +
-			(binarye->binary.nargs + 2) * sizeof(uint32_t);
-		if (hasext)
-			*hasext = 1;
-	}
-
-	if (image_get_csk_index() >= 0) {
-		headersz += sizeof(struct secure_hdr_v1);
+		headersz += sizeof(struct opt_hdr_v1) + sizeof(uint32_t) +
+			(binarye->binary.nargs) * sizeof(uint32_t);
+		headersz = ALIGN(headersz, 16);
+		headersz += ALIGN(s.st_size, 4) + sizeof(uint32_t);
 		if (hasext)
 			*hasext = 1;
 	}
@@ -984,9 +985,12 @@ static size_t image_headersz_v1(int *hasext)
 }
 
 int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
-			 struct image_cfg_element *binarye)
+			 struct image_cfg_element *binarye,
+			 struct main_hdr_v1 *main_hdr)
 {
 	struct opt_hdr_v1 *hdr = (struct opt_hdr_v1 *)*cur;
+	uint32_t add_args;
+	uint32_t offset;
 	uint32_t *args;
 	size_t binhdrsz;
 	struct stat s;
@@ -1009,12 +1013,6 @@ int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
 		goto err_close;
 	}
 
-	binhdrsz = sizeof(struct opt_hdr_v1) +
-		(binarye->binary.nargs + 2) * sizeof(uint32_t) +
-		ALIGN(s.st_size, 4);
-	hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
-	hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
-
 	*cur += sizeof(struct opt_hdr_v1);
 
 	args = (uint32_t *)*cur;
@@ -1025,6 +1023,19 @@ int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
 
 	*cur += (binarye->binary.nargs + 1) * sizeof(uint32_t);
 
+	/*
+	 * ARM executable code inside the BIN header on some mvebu platforms
+	 * (e.g. A370, AXP) must always be aligned with the 128-bit boundary.
+	 * This requirement can be met by inserting dummy arguments into
+	 * BIN header, if needed.
+	 */
+	offset = *cur - (uint8_t *)main_hdr;
+	add_args = ((16 - offset % 16) % 16) / sizeof(uint32_t);
+	if (add_args) {
+		*(args - 1) = cpu_to_le32(binarye->binary.nargs + add_args);
+		*cur += add_args * sizeof(uint32_t);
+	}
+
 	ret = fread(*cur, s.st_size, 1, bin);
 	if (ret != 1) {
 		fprintf(stderr,
@@ -1043,6 +1054,12 @@ int add_binary_header_v1(uint8_t **cur, uint8_t **next_ext,
 
 	*cur += sizeof(uint32_t);
 
+	binhdrsz = sizeof(struct opt_hdr_v1) +
+		(binarye->binary.nargs + add_args + 2) * sizeof(uint32_t) +
+		ALIGN(s.st_size, 4);
+	hdr->headersz_lsb = cpu_to_le16(binhdrsz & 0xFFFF);
+	hdr->headersz_msb = (binhdrsz & 0xFFFF0000) >> 16;
+
 	return 0;
 
 err_close:
@@ -1299,7 +1316,7 @@ static void *image_create_v1(size_t *imagesz, struct image_tool_params *params,
 		if (e->type != IMAGE_CFG_BINARY)
 			continue;
 
-		if (add_binary_header_v1(&cur, &next_ext, e))
+		if (add_binary_header_v1(&cur, &next_ext, e, main_hdr))
 			return NULL;
 	}
 
-- 
2.20.1