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: Peng Fan <peng.fan@nxp.com>
diff --git a/drivers/usb/gadget/f_sdp.c b/drivers/usb/gadget/f_sdp.c
index eec7560..9b73728 100644
--- a/drivers/usb/gadget/f_sdp.c
+++ b/drivers/usb/gadget/f_sdp.c
@@ -71,6 +71,8 @@
#define SDP_COMMAND_LEN 16
+#define SDP_EXIT 1
+
struct sdp_command {
u16 cmd;
u32 addr;
@@ -667,19 +669,43 @@
}
#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 @@
/* 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 @@
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 @@
default:
break;
};
+
+ return 0;
}
#ifndef CONFIG_SPL_BUILD
@@ -777,6 +824,7 @@
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 @@
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
}
}