MLK-20559-6 f_sdp: Support searching and loading FIT or container image

Add support to f_sdp to search and load iMX8 container image or iMX8M
FIT image by new UUU command SDPV.

When using the SDPV, the uuu will continue to send out data after first
level boot loader used by ROM. This means uuu won't skip to the offset
of the second boot loader, and the padding data before second boot loader
will be sent out. So we have to search the FIT header or container header
in the buffer that SDP received.

The new BCD value is used by uuu to distinguish if the SPL supports the
SDPV.

Signed-off-by: Ye Li <ye.li@nxp.com>

3 files changed, 87 insertions, 3 deletions

diff --git a/arch/arm/mach-imx/imx8/parser.c b/arch/arm/mach-imx/imx8/parser.c
index 1a170fb63e3..f7315cbf289 100644
--- a/arch/arm/mach-imx/imx8/parser.c
+++ b/arch/arm/mach-imx/imx8/parser.c
@@ -2,7 +2,6 @@
 /*
  * Copyright 2018 NXP
  */
-
 #include <common.h>
 #include <spl.h>
 #include <errno.h>
@@ -17,6 +16,7 @@
 
 #define MMC_DEV		0
 #define QSPI_DEV	1
+#define RAM_DEV	3
 
 #define SEC_SECURE_RAM_BASE			(0x31800000UL)
 #define SEC_SECURE_RAM_END_BASE			(SEC_SECURE_RAM_BASE + 0xFFFFUL)
@@ -35,11 +35,11 @@ static int current_dev_type = MMC_DEV;
 static int start_offset;
 static void *device;
 
-static int read(int start, int len, void *load_addr)
+static int read(u32 start, u32 len, void *load_addr)
 {
 	int ret = -ENODEV;
 
-	if (!device) {
+	if (!device && current_dev_type != RAM_DEV) {
 		debug("No device selected\n");
 		return ret;
 	}
@@ -74,6 +74,11 @@ static int read(int start, int len, void *load_addr)
 	}
 #endif
 
+	if (current_dev_type == RAM_DEV) {
+		memcpy(load_addr, (const void *)(ulong)start, len);
+		ret = 0;
+	}
+
 	return ret;
 }
 
@@ -204,6 +209,7 @@ static int read_auth_container(struct spl_image_info *spl_image)
 
 		if (!image) {
 			ret = -EINVAL;
+			sc_misc_seco_authenticate(ipcHndl, SC_MISC_REL_CONTAINER, 0);
 			goto out;
 		}
 
@@ -258,3 +264,18 @@ int spi_load_image_parse_container(struct spl_image_info *spl_image,
 
 	return ret;
 }
+
+int sdp_load_image_parse_container(struct spl_image_info *spl_image,
+				   unsigned long offset)
+{
+	int ret = 0;
+
+	current_dev_type = RAM_DEV;
+	device = NULL;
+
+	start_offset = offset;
+
+	ret = read_auth_container(spl_image);
+
+	return ret;
+}
diff --git a/arch/arm/mach-imx/spl.c b/arch/arm/mach-imx/spl.c
index 2715b3b3d4f..3a668687ef6 100644
--- a/arch/arm/mach-imx/spl.c
+++ b/arch/arm/mach-imx/spl.c
@@ -159,6 +159,12 @@ int g_dnl_bind_fixup(struct usb_device_descriptor *dev, const char *name)
 
 	return 0;
 }
+
+#define SDPV_BCD_DEVICE 0x500
+int g_dnl_get_board_bcd_device_number(int gcnum)
+{
+	return SDPV_BCD_DEVICE;
+}
 #endif
 
 #if defined(CONFIG_SPL_MMC_SUPPORT)
diff --git a/drivers/usb/gadget/f_sdp.c b/drivers/usb/gadget/f_sdp.c
index b69e3711e89..d87877e2a80 100644
--- a/drivers/usb/gadget/f_sdp.c
+++ b/drivers/usb/gadget/f_sdp.c
@@ -102,6 +102,7 @@ struct f_sdp {
 	enum sdp_state			next_state;
 	u32				dnl_address;
 	u32				dnl_bytes_remaining;
+	u32				last_dnl_file_bytes;
 	u32				jmp_address;
 	bool				always_send_status;
 	u32				error_status;
@@ -247,6 +248,14 @@ static struct usb_gadget_strings *sdp_generic_strings[] = {
 	NULL,
 };
 
+#ifdef CONFIG_PARSE_CONTAINER
+int __weak sdp_load_image_parse_container(struct spl_image_info *spl_image,
+				   unsigned long offset)
+{
+	return -EINVAL;
+}
+#endif
+
 void __weak board_sdp_cleanup(void)
 {
 }
@@ -298,6 +307,7 @@ static void sdp_rx_command_complete(struct usb_ep *ep, struct usb_request *req)
 		sdp->state = SDP_STATE_RX_FILE_DATA;
 		sdp->dnl_address = cmd->addr ? be32_to_cpu(cmd->addr) : CONFIG_SDP_LOADADDR;
 		sdp->dnl_bytes_remaining = be32_to_cpu(cmd->cnt);
+		sdp->last_dnl_file_bytes = sdp->dnl_bytes_remaining;
 		sdp->next_state = SDP_STATE_IDLE;
 
 		printf("Downloading file of size %d to 0x%08x... ",
@@ -672,6 +682,34 @@ static ulong sdp_spl_fit_read(struct spl_load_info *load, ulong sector,
 	return count;
 }
 
+#ifdef CONFIG_SPL_BUILD
+#ifdef CONFIG_PARSE_CONTAINER
+static ulong search_container_header(ulong p, int size)
+{
+	int i = 0;
+	uint8_t *hdr;
+	for (i = 0; i < size; i += 4) {
+		hdr = (uint8_t *)(p +i);
+		if (*(hdr + 3) == 0x87 && *hdr == 0 &&
+			(*(hdr + 1) != 0 || *(hdr + 2) != 0))
+                        return p +i;
+	}
+        return 0;
+}
+#else
+static ulong search_fit_header(ulong p, int size)
+{
+	int i = 0;
+	for (i = 0; i < size; i += 4) {
+                if (genimg_get_format((const void *)(p+i)) == IMAGE_FORMAT_FIT)
+                        return p + i;
+	}
+
+        return 0;
+}
+#endif
+#endif
+
 static void sdp_handle_in_ep(void)
 {
 	u8 *data = sdp_func->in_req->buf;
@@ -727,6 +765,20 @@ static void sdp_handle_in_ep(void)
 			struct image_header *header;
 			struct spl_image_info spl_image = {};
 
+#ifdef CONFIG_PARSE_CONTAINER
+			sdp_func->jmp_address = (u32)search_container_header((ulong)sdp_func->jmp_address,
+				sdp_func->last_dnl_file_bytes);
+#else
+			if (IS_ENABLED(CONFIG_SPL_LOAD_FIT))
+				sdp_func->jmp_address = (u32)search_fit_header((ulong)sdp_func->jmp_address,
+					sdp_func->last_dnl_file_bytes);
+#endif
+			if (sdp_func->jmp_address == 0) {
+				panic("Error in search header, failed to jump\n");
+			}
+
+			printf("Found header at 0x%08x\n", sdp_func->jmp_address);
+
 			header = (struct image_header *)(ulong)(sdp_func->jmp_address);
 
 			if (IS_ENABLED(CONFIG_SPL_LOAD_FIT) &&
@@ -743,9 +795,14 @@ static void sdp_handle_in_ep(void)
 							  sdp_func->jmp_address,
 							  (void *)header);
 			} else {
+#ifdef CONFIG_PARSE_CONTAINER
+				sdp_load_image_parse_container(&spl_image,
+							     sdp_func->jmp_address);
+#else
 				/* In SPL, allow jumps to U-Boot images */
 				spl_parse_image_header(&spl_image,
 					(struct image_header *)(ulong)(sdp_func->jmp_address));
+#endif
 			}
 
 			board_sdp_cleanup();