2 files changed, 89 insertions, 0 deletions

diff --git a/arch/arm/include/asm/mach-imx/hab.h b/arch/arm/include/asm/mach-imx/hab.h
index a0cb19db2f..bb732030e9 100644
--- a/arch/arm/include/asm/mach-imx/hab.h
+++ b/arch/arm/include/asm/mach-imx/hab.h
@@ -38,6 +38,12 @@ struct ivt {
 	uint32_t reserved2;	/* Reserved should be zero */
 };
 
+struct __packed hab_hdr {
+	u8 tag;              /* Tag field */
+	u8 len[2];           /* Length field in bytes (big-endian) */
+	u8 par;              /* Parameters field */
+};
+
 /* -------- start of HAB API updates ------------*/
 /* The following are taken from HAB4 SIS */
 
@@ -182,6 +188,8 @@ typedef void hapi_clock_init_t(void);
 #define HAB_CID_ROM 0 /**< ROM Caller ID */
 #define HAB_CID_UBOOT 1 /**< UBOOT Caller ID*/
 
+#define HAB_CMD_HDR          0xD4  /* CSF Header */
+
 #define IVT_SIZE			0x20
 #define CSF_PAD_SIZE			0x2000
 
diff --git a/arch/arm/mach-imx/hab.c b/arch/arm/mach-imx/hab.c
index ba6b31d490..7f66965af5 100644
--- a/arch/arm/mach-imx/hab.c
+++ b/arch/arm/mach-imx/hab.c
@@ -453,6 +453,83 @@ U_BOOT_CMD(
 
 #endif /* !defined(CONFIG_SPL_BUILD) */
 
+/* Get CSF Header length */
+static int get_hab_hdr_len(struct hab_hdr *hdr)
+{
+	return (size_t)((hdr->len[0] << 8) + (hdr->len[1]));
+}
+
+/* Check whether addr lies between start and
+ * end and is within the length of the image
+ */
+static int chk_bounds(u8 *addr, size_t bytes, u8 *start, u8 *end)
+{
+	size_t csf_size = (size_t)((end + 1) - addr);
+
+	return (addr && (addr >= start) && (addr <= end) &&
+		(csf_size >= bytes));
+}
+
+/* Get Length of each command in CSF */
+static int get_csf_cmd_hdr_len(u8 *csf_hdr)
+{
+	if (*csf_hdr == HAB_CMD_HDR)
+		return sizeof(struct hab_hdr);
+
+	return get_hab_hdr_len((struct hab_hdr *)csf_hdr);
+}
+
+/* Check if CSF is valid */
+static bool csf_is_valid(struct ivt *ivt, ulong start_addr, size_t bytes)
+{
+	u8 *start = (u8 *)start_addr;
+	u8 *csf_hdr;
+	u8 *end;
+
+	size_t csf_hdr_len;
+	size_t cmd_hdr_len;
+	size_t offset = 0;
+
+	if (bytes != 0)
+		end = start + bytes - 1;
+	else
+		end = start;
+
+	/* Verify if CSF pointer content is zero */
+	if (!ivt->csf) {
+		puts("Error: CSF pointer is NULL\n");
+		return false;
+	}
+
+	csf_hdr = (u8 *)ivt->csf;
+
+	/* Verify if CSF Header exist */
+	if (*csf_hdr != HAB_CMD_HDR) {
+		puts("Error: CSF header command not found\n");
+		return false;
+	}
+
+	csf_hdr_len = get_hab_hdr_len((struct hab_hdr *)csf_hdr);
+
+	/* Check if the CSF lies within the image bounds */
+	if (!chk_bounds(csf_hdr, csf_hdr_len, start, end)) {
+		puts("Error: CSF lies outside the image bounds\n");
+		return false;
+	}
+
+	do {
+		cmd_hdr_len = get_csf_cmd_hdr_len(&csf_hdr[offset]);
+		if (!cmd_hdr_len) {
+			puts("Error: Invalid command length\n");
+			return false;
+		}
+		offset += cmd_hdr_len;
+
+	} while (offset < csf_hdr_len);
+
+	return true;
+}
+
 bool imx_hab_is_enabled(void)
 {
 	struct imx_sec_config_fuse_t *fuse =
@@ -525,6 +602,10 @@ int imx_hab_authenticate_image(uint32_t ddr_start, uint32_t image_size,
 	start = ddr_start;
 	bytes = image_size;
 
+	/* Verify CSF */
+	if (!csf_is_valid(ivt, start, bytes))
+		goto hab_authentication_exit;
+
 	if (hab_rvt_entry() != HAB_SUCCESS) {
 		puts("hab entry function fail\n");
 		goto hab_exit_failure_print_status;