[U-Boot] [PATCH 4/5] usb: gadget: f_sdp: Allow SPL to load and boot FIT via SDP

[Sjoerd Simons]

From: Frieder Schrempf <frieder.schrempf at kontron.de>

Add support for loading u-boot FIT images over the USB SPD protocol in
the SPL

[Small fixes to build]
Signed-off-by: Sjoerd Simons <sjoerd.simons at collabora.co.uk>
---

 common/spl/spl_sdp.c       | 11 ++++++++--
 drivers/usb/gadget/f_sdp.c | 41 ++++++++++++++++++++++++++++++++------
 include/sdp.h              |  4 +++-
 3 files changed, 47 insertions(+), 9 deletions(-)

diff --git a/common/spl/spl_sdp.c b/common/spl/spl_sdp.c
index 807256e908..dc57171966 100644
--- a/common/spl/spl_sdp.c
+++ b/common/spl/spl_sdp.c
@@ -26,9 +26,16 @@ static int spl_sdp_load_image(struct spl_image_info *spl_image,
 	}
 
 	/* This command typically does not return but jumps to an image */
-	sdp_handle(controller_index);
+	sdp_handle(controller_index, spl_image);
 	pr_err("SDP ended\n");
+	/*
+	 * This command either loads a legacy image, jumps and never returns,
+	 * or it loads a FIT image and returns it to be handled by the SPL
+	 * code.
+	 */
+	ret = sdp_handle(controller_index, spl_image);
+	debug("SDP ended\n");
 
-	return -EINVAL;
+	return ret;
 }
 SPL_LOAD_IMAGE_METHOD("USB SDP", 0, BOOT_DEVICE_BOARD, spl_sdp_load_image);
diff --git a/drivers/usb/gadget/f_sdp.c b/drivers/usb/gadget/f_sdp.c
index ae97ab2b49..2a23160d91 100644
--- a/drivers/usb/gadget/f_sdp.c
+++ b/drivers/usb/gadget/f_sdp.c
@@ -638,7 +638,18 @@ static u32 sdp_jump_imxheader(void *address)
 	return 0;
 }
 
-static void sdp_handle_in_ep(void)
+#ifdef CONFIG_SPL_LOAD_FIT
+static ulong sdp_fit_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 void sdp_handle_in_ep(struct spl_image_info *spl_image)
 {
 	u8 *data = sdp_func->in_req->buf;
 	u32 status;
@@ -689,11 +700,26 @@ static void sdp_handle_in_ep(void)
 
 		/* If imx header fails, try some U-Boot specific headers */
 		if (status) {
+			image_header_t *header =
+				sdp_ptr(sdp_func->jmp_address);
 #ifdef CONFIG_SPL_BUILD
+#ifdef CONFIG_SPL_LOAD_FIT
+			if (image_get_magic(header) == FDT_MAGIC) {
+				struct spl_load_info load;
+
+				debug("Found FIT\n");
+				load.dev = header;
+				load.bl_len = 1;
+				load.read = sdp_fit_read;
+				spl_load_simple_fit(spl_image, &load, 0,
+						    header);
+
+				return;
+			}
+#endif
 			/* In SPL, allow jumps to U-Boot images */
 			struct spl_image_info spl_image = {};
-			spl_parse_image_header(&spl_image,
-				(struct image_header *)sdp_func->jmp_address);
+			spl_parse_image_header(&spl_image, header);
 			jump_to_image_no_args(&spl_image);
 #else
 			/* In U-Boot, allow jumps to scripts */
@@ -715,19 +741,22 @@ static void sdp_handle_in_ep(void)
 	};
 }
 
-void sdp_handle(int controller_index)
+int sdp_handle(int controller_index, struct spl_image_info *spl_image)
 {
 	printf("SDP: handle requests...\n");
 	while (1) {
 		if (ctrlc()) {
 			puts("\rCTRL+C - Operation aborted.\n");
-			return;
+			return -EINVAL;
 		}
 
+		if (spl_image->flags & SPL_FIT_FOUND)
+			return 0;
+
 		WATCHDOG_RESET();
 		usb_gadget_handle_interrupts(controller_index);
 
-		sdp_handle_in_ep();
+		sdp_handle_in_ep(spl_image);
 	}
 }
 
diff --git a/include/sdp.h b/include/sdp.h
index f6252d027f..f30e2bca19 100644
--- a/include/sdp.h
+++ b/include/sdp.h
@@ -9,7 +9,9 @@
 #ifndef __SDP_H_
 #define __SDP_H_
 
+#include <spl.h>
+
 int sdp_init(int controller_index);
-void sdp_handle(int controller_index);
+int sdp_handle(int controller_index, struct spl_image_info *spl_image);
 
 #endif /* __SDP_H_ */
-- 
2.20.1