arm: imx: hab: Optimise flow of authenticate_image on hab_entry fail

The current code disjoins an entire block of code on hab_entry pass/fail
resulting in a large chunk of authenticate_image being offset to the right.

Fix this by checking hab_entry() pass/failure and exiting the function
directly if in an error state.

Signed-off-by: Bryan O'Donoghue <bryan.odonoghue@linaro.org>
Cc: Stefano Babic <sbabic@denx.de>
Cc: Fabio Estevam <fabio.estevam@nxp.com>
Cc: Peng Fan <peng.fan@nxp.com>
Cc: Albert Aribaud <albert.u.boot@aribaud.net>
Cc: Sven Ebenfeld <sven.ebenfeld@gmail.com>
Cc: George McCollister <george.mccollister@gmail.com>
Cc: Breno Matheus Lima <brenomatheus@gmail.com>
Tested-by: Breno Lima <breno.lima@nxp.com>
Reviewed-by: Fabio Estevam <fabio.estevam@nxp.com>

arch/arm/mach-imx/hab.c

@@ -438,75 +438,77 @@ int authenticate_image(uint32_t ddr_start, uint32_t image_size)
 
 	hab_caam_clock_enable(1);
 
-	if (hab_rvt_entry() == HAB_SUCCESS) {
-		/* If not already aligned, Align to ALIGN_SIZE */
-		ivt_offset = (image_size + ALIGN_SIZE - 1) &
-				~(ALIGN_SIZE - 1);
+	if (hab_rvt_entry() != HAB_SUCCESS) {
+		puts("hab entry function fail\n");
+		goto hab_caam_clock_disable;
+	}
 
-		start = ddr_start;
-		bytes = ivt_offset + IVT_SIZE + CSF_PAD_SIZE;
+	/* If not already aligned, Align to ALIGN_SIZE */
+	ivt_offset = (image_size + ALIGN_SIZE - 1) &
+			~(ALIGN_SIZE - 1);
+
+	start = ddr_start;
+	bytes = ivt_offset + IVT_SIZE + CSF_PAD_SIZE;
 #ifdef DEBUG
-		printf("\nivt_offset = 0x%x, ivt addr = 0x%x\n",
-		       ivt_offset, ddr_start + ivt_offset);
-		puts("Dumping IVT\n");
-		print_buffer(ddr_start + ivt_offset,
-			     (void *)(ddr_start + ivt_offset),
-			     4, 0x8, 0);
-
-		puts("Dumping CSF Header\n");
-		print_buffer(ddr_start + ivt_offset + IVT_SIZE,
-			     (void *)(ddr_start + ivt_offset + IVT_SIZE),
-			     4, 0x10, 0);
+	printf("\nivt_offset = 0x%x, ivt addr = 0x%x\n",
+	       ivt_offset, ddr_start + ivt_offset);
+	puts("Dumping IVT\n");
+	print_buffer(ddr_start + ivt_offset,
+		     (void *)(ddr_start + ivt_offset),
+		     4, 0x8, 0);
+
+	puts("Dumping CSF Header\n");
+	print_buffer(ddr_start + ivt_offset + IVT_SIZE,
+		     (void *)(ddr_start + ivt_offset + IVT_SIZE),
+		     4, 0x10, 0);
 
 #if  !defined(CONFIG_SPL_BUILD)
-		get_hab_status();
+	get_hab_status();
 #endif
 
-		puts("\nCalling authenticate_image in ROM\n");
-		printf("\tivt_offset = 0x%x\n", ivt_offset);
-		printf("\tstart = 0x%08lx\n", start);
-		printf("\tbytes = 0x%x\n", bytes);
+	puts("\nCalling authenticate_image in ROM\n");
+	printf("\tivt_offset = 0x%x\n", ivt_offset);
+	printf("\tstart = 0x%08lx\n", start);
+	printf("\tbytes = 0x%x\n", bytes);
 #endif
-		/*
-		 * If the MMU is enabled, we have to notify the ROM
-		 * code, or it won't flush the caches when needed.
-		 * This is done, by setting the "pu_irom_mmu_enabled"
-		 * word to 1. You can find its address by looking in
-		 * the ROM map. This is critical for
-		 * authenticate_image(). If MMU is enabled, without
-		 * setting this bit, authentication will fail and may
-		 * crash.
-		 */
-		/* Check MMU enabled */
-		if (is_soc_type(MXC_SOC_MX6) && get_cr() & CR_M) {
-			if (is_mx6dq()) {
-				/*
-				 * This won't work on Rev 1.0.0 of
-				 * i.MX6Q/D, since their ROM doesn't
-				 * do cache flushes. don't think any
-				 * exist, so we ignore them.
-				 */
-				if (!is_mx6dqp())
-					writel(1, MX6DQ_PU_IROM_MMU_EN_VAR);
-			} else if (is_mx6sdl()) {
-				writel(1, MX6DLS_PU_IROM_MMU_EN_VAR);
-			} else if (is_mx6sl()) {
-				writel(1, MX6SL_PU_IROM_MMU_EN_VAR);
-			}
+	/*
+	 * If the MMU is enabled, we have to notify the ROM
+	 * code, or it won't flush the caches when needed.
+	 * This is done, by setting the "pu_irom_mmu_enabled"
+	 * word to 1. You can find its address by looking in
+	 * the ROM map. This is critical for
+	 * authenticate_image(). If MMU is enabled, without
+	 * setting this bit, authentication will fail and may
+	 * crash.
+	 */
+	/* Check MMU enabled */
+	if (is_soc_type(MXC_SOC_MX6) && get_cr() & CR_M) {
+		if (is_mx6dq()) {
+			/*
+			 * This won't work on Rev 1.0.0 of
+			 * i.MX6Q/D, since their ROM doesn't
+			 * do cache flushes. don't think any
+			 * exist, so we ignore them.
+			 */
+			if (!is_mx6dqp())
+				writel(1, MX6DQ_PU_IROM_MMU_EN_VAR);
+		} else if (is_mx6sdl()) {
+			writel(1, MX6DLS_PU_IROM_MMU_EN_VAR);
+		} else if (is_mx6sl()) {
+			writel(1, MX6SL_PU_IROM_MMU_EN_VAR);
 		}
+	}
 
-		load_addr = (uint32_t)hab_rvt_authenticate_image(
-				HAB_CID_UBOOT,
-				ivt_offset, (void **)&start,
-				(size_t *)&bytes, NULL);
-		if (hab_rvt_exit() != HAB_SUCCESS) {
-			puts("hab exit function fail\n");
-			load_addr = 0;
-		}
-	} else {
-		puts("hab entry function fail\n");
+	load_addr = (uint32_t)hab_rvt_authenticate_image(
+			HAB_CID_UBOOT,
+			ivt_offset, (void **)&start,
+			(size_t *)&bytes, NULL);
+	if (hab_rvt_exit() != HAB_SUCCESS) {
+		puts("hab exit function fail\n");
+		load_addr = 0;
 	}
 
+hab_caam_clock_disable:
 	hab_caam_clock_enable(0);
 
 #if !defined(CONFIG_SPL_BUILD)