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.

Also change to more common method to exit f_sdp handler not depending on
SPL_FIT_FOUND flag because container loader won't set this.
Signed-off-by: NPeng Fan <peng.fan@nxp.com>

drivers/usb/gadget/f_sdp.c

@@ -71,6 +71,8 @@ struct hid_report {
 
 #define SDP_COMMAND_LEN		16
 
+#define SDP_EXIT 1
+
 struct sdp_command {
 	u16 cmd;
 	u32 addr;
@@ -667,19 +669,43 @@ static u32 sdp_jump_imxheader(void *address)
 }
 
 #ifdef CONFIG_SPL_BUILD
-#ifdef CONFIG_SPL_LOAD_FIT
-static ulong sdp_fit_read(struct spl_load_info *load, ulong sector,
-			  ulong count, void *buf)
+static ulong sdp_load_read(struct spl_load_info *load, ulong sector,
+			   ulong count, void *buf)
 {
 	debug("%s: sector %lx, count %lx, buf %lx\n",
 	      __func__, sector, count, (ulong)buf);
 	memcpy(buf, (void *)(load->dev + sector), count);
 	return count;
 }
-#endif
+
+static ulong search_fit_header(ulong p, int size)
+{
+	int i;
+
+	for (i = 0; i < size; i += 4) {
+		if (genimg_get_format((const void *)(p + i)) == IMAGE_FORMAT_FIT)
+			return p + i;
+	}
+
+	return 0;
+}
+
+static ulong search_container_header(ulong p, int size)
+{
+	int i;
+	u8 *hdr;
+
+	for (i = 0; i < size; i += 4) {
+		hdr = (u8 *)(p + i);
+		if (*(hdr + 3) == 0x87 && *hdr == 0)
+			if (*(hdr + 1) != 0 || *(hdr + 2) != 0)
+				return p + i;
+	}
+	return 0;
+}
 #endif
 
-static void sdp_handle_in_ep(struct spl_image_info *spl_image)
+static int sdp_handle_in_ep(struct spl_image_info *spl_image)
 {
 	u8 *data = sdp_func->in_req->buf;
 	u32 status;
@@ -731,6 +757,15 @@ static void sdp_handle_in_ep(struct spl_image_info *spl_image)
 		/* If imx header fails, try some U-Boot specific headers */
 		if (status) {
 #ifdef CONFIG_SPL_BUILD
+			if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER))
+				sdp_func->jmp_address = (u32)search_container_header((ulong)sdp_func->jmp_address, sdp_func->dnl_bytes);
+			else if (IS_ENABLED(CONFIG_SPL_LOAD_FIT))
+				sdp_func->jmp_address = (u32)search_fit_header((ulong)sdp_func->jmp_address, sdp_func->dnl_bytes);
+			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);
+
 			image_header_t *header =
 				sdp_ptr(sdp_func->jmp_address);
 #ifdef CONFIG_SPL_LOAD_FIT
@@ -740,13 +775,23 @@ static void sdp_handle_in_ep(struct spl_image_info *spl_image)
 				debug("Found FIT\n");
 				load.dev = header;
 				load.bl_len = 1;
-				load.read = sdp_fit_read;
+				load.read = sdp_load_read;
 				spl_load_simple_fit(spl_image, &load, 0,
 						    header);
 
-				return;
+				return SDP_EXIT;
 			}
 #endif
+			if (IS_ENABLED(CONFIG_SPL_LOAD_IMX_CONTAINER)) {
+				struct spl_load_info load;
+
+				load.dev = header;
+				load.bl_len = 1;
+				load.read = sdp_load_read;
+				spl_load_imx_container(spl_image, &load, 0);
+				return SDP_EXIT;
+			}
+
 			/* In SPL, allow jumps to U-Boot images */
 			struct spl_image_info spl_image = {};
 			spl_parse_image_header(&spl_image, header);
@@ -769,6 +814,8 @@ static void sdp_handle_in_ep(struct spl_image_info *spl_image)
 	default:
 		break;
 	};
+
+	return 0;
 }
 
 #ifndef CONFIG_SPL_BUILD
@@ -777,6 +824,7 @@ int sdp_handle(int controller_index)
 int spl_sdp_handle(int controller_index, struct spl_image_info *spl_image)
 #endif
 {
+	int flag = 0;
 	printf("SDP: handle requests...\n");
 	while (1) {
 		if (ctrlc()) {
@@ -784,18 +832,16 @@ int spl_sdp_handle(int controller_index, struct spl_image_info *spl_image)
 			return -EINVAL;
 		}
 
-#ifdef CONFIG_SPL_BUILD
-		if (spl_image->flags & SPL_FIT_FOUND)
+		if (flag == SDP_EXIT)
 			return 0;
-#endif
 
 		WATCHDOG_RESET();
 		usb_gadget_handle_interrupts(controller_index);
 
 #ifdef CONFIG_SPL_BUILD
-		sdp_handle_in_ep(spl_image);
+		flag = sdp_handle_in_ep(spl_image);
 #else
-		sdp_handle_in_ep(NULL);
+		flag = sdp_handle_in_ep(NULL);
 #endif
 	}
 }