Merge branch 'u-boot-imx/master' into 'u-boot-arm/master'

Conflicts: drivers/serial/serial.c The conflict above was a trivial case of adding one init function in each branch, and manually resolved in merge.

Merge branch 'u-boot-imx/master' into 'u-boot-arm/master'
Conflicts: drivers/serial/serial.c The conflict above was a trivial case of adding one init function in each branch, and manually resolved in merge.
Albert ARIBAUD
2 parents e62d5fb0da 8467faef7f
Showing 33 changed files Side-by-side Diff
arch/arm/cpu/arm926ejs/mxs/mxsimage.mx23.cfg
arch/arm/cpu/arm926ejs/mxs/mxsimage.mx28.cfg
arch/arm/cpu/arm926ejs/mxs/spl_power_init.c
arch/arm/cpu/armv7/mx6/Makefile
arch/arm/cpu/armv7/mx6/hab.c
arch/arm/cpu/armv7/mx6/soc.c
arch/arm/include/asm/arch-mx6/hab.h
arch/arm/include/asm/arch-mx6/imx-regs.h
arch/arm/include/asm/arch-mx6/sys_proto.h
arch/arm/include/asm/arch-mxs/regs-uartapp.h
board/boundary/nitrogen6x/nitrogen6x.c
board/congatec/cgtqmx6eval/README
boards.cfg
common/image.c
config.mk
doc/README.imximage
doc/README.mxc_hab
doc/README.mxsimage
drivers/mmc/mxsmmc.c
drivers/net/fec_mxc.c
+SECTION 0x0 BOOTABLE
+ TAG LAST
+ LOAD     0x0        spl/u-boot-spl.bin
+ CALL     0x14       0x0
+ LOAD     0x40000100 u-boot.bin
+ CALL     0x40000100 0x0
+SECTION 0x0 BOOTABLE
+ TAG LAST
+ LOAD     0x0        spl/u-boot-spl.bin
+ LOAD IVT 0x8000     0x14
+ CALL HAB 0x8000     0x0
+ LOAD     0x40000100 u-boot.bin
+ LOAD IVT 0x8000     0x40000100
+ CALL HAB 0x8000     0x0
@@ -36,7 +36,7 @@
 			CLKCTRL_CLKSEQ_BYPASS_CPU);
 }
  
-static void mxs_power_clear_auto_restart(void)
+static void mxs_power_set_auto_restart(void)
 {
 	struct mxs_rtc_regs *rtc_regs =
 		(struct mxs_rtc_regs *)MXS_RTC_BASE;
@@ -49,10 +49,7 @@
 	while (readl(&rtc_regs->hw_rtc_ctrl) & RTC_CTRL_CLKGATE)
 		;
  
-	/*
-	 * Due to the hardware design bug of mx28 EVK-A
-	 * we need to set the AUTO_RESTART bit.
-	 */
+	/* Do nothing if flag already set */
 	if (readl(&rtc_regs->hw_rtc_persistent0) & RTC_PERSISTENT0_AUTO_RESTART)
 		return;
  
@@ -911,7 +908,7 @@
 	mxs_ungate_power();
  
 	mxs_power_clock2xtal();
-	mxs_power_clear_auto_restart();
+	mxs_power_set_auto_restart();
 	mxs_power_set_linreg();
 	mxs_power_setup_5v_detect();
  
@@ -11,10 +11,11 @@
  
 LIB	= $(obj)lib$(SOC).o
  
-COBJS	= soc.o clock.o
+COBJS-y	= soc.o clock.o
+COBJS-$(CONFIG_SECURE_BOOT)	+= hab.o
  
-SRCS	:= $(SOBJS:.o=.S) $(COBJS:.o=.c)
-OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS))
+SRCS	:= $(SOBJS:.o=.S) $(COBJS-y:.o=.c)
+OBJS	:= $(addprefix $(obj),$(SOBJS) $(COBJS-y))
  
 all:	$(obj).depend $(LIB)
  
+/*
+ * Copyright (C) 2010-2013 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:    GPL-2.0+
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/hab.h>
+
+/* -------- start of HAB API updates ------------*/
+#define hab_rvt_report_event ((hab_rvt_report_event_t *)HAB_RVT_REPORT_EVENT)
+#define hab_rvt_report_status ((hab_rvt_report_status_t *)HAB_RVT_REPORT_STATUS)
+#define hab_rvt_authenticate_image \
+	((hab_rvt_authenticate_image_t *)HAB_RVT_AUTHENTICATE_IMAGE)
+#define hab_rvt_entry ((hab_rvt_entry_t *)HAB_RVT_ENTRY)
+#define hab_rvt_exit ((hab_rvt_exit_t *)HAB_RVT_EXIT)
+#define hab_rvt_clock_init HAB_RVT_CLOCK_INIT
+
+bool is_hab_enabled(void)
+{
+	struct ocotp_regs *ocotp = (struct ocotp_regs *)OCOTP_BASE_ADDR;
+	struct fuse_bank *bank = &ocotp->bank[0];
+	struct fuse_bank0_regs *fuse =
+		(struct fuse_bank0_regs *)bank->fuse_regs;
+	uint32_t reg = readl(&fuse->cfg5);
+
+	return (reg & 0x2) == 0x2;
+}
+
+void display_event(uint8_t *event_data, size_t bytes)
+{
+	uint32_t i;
+
+	if (!(event_data && bytes > 0))
+		return;
+
+	for (i = 0; i < bytes; i++) {
+		if (i == 0)
+			printf("\t0x%02x", event_data[i]);
+		else if ((i % 8) == 0)
+			printf("\n\t0x%02x", event_data[i]);
+		else
+			printf(" 0x%02x", event_data[i]);
+	}
+}
+
+int get_hab_status(void)
+{
+	uint32_t index = 0; /* Loop index */
+	uint8_t event_data[128]; /* Event data buffer */
+	size_t bytes = sizeof(event_data); /* Event size in bytes */
+	enum hab_config config = 0;
+	enum hab_state state = 0;
+
+	if (is_hab_enabled())
+		puts("\nSecure boot enabled\n");
+	else
+		puts("\nSecure boot disabled\n");
+
+	/* Check HAB status */
+	if (hab_rvt_report_status(&config, &state) != HAB_SUCCESS) {
+		printf("\nHAB Configuration: 0x%02x, HAB State: 0x%02x\n",
+		       config, state);
+
+		/* Display HAB Error events */
+		while (hab_rvt_report_event(HAB_FAILURE, index, event_data,
+					&bytes) == HAB_SUCCESS) {
+			puts("\n");
+			printf("--------- HAB Event %d -----------------\n",
+			       index + 1);
+			puts("event data:\n");
+			display_event(event_data, bytes);
+			puts("\n");
+			bytes = sizeof(event_data);
+			index++;
+		}
+	}
+	/* Display message if no HAB events are found */
+	else {
+		printf("\nHAB Configuration: 0x%02x, HAB State: 0x%02x\n",
+		       config, state);
+		puts("No HAB Events Found!\n\n");
+	}
+	return 0;
+}
+
+int do_hab_status(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	if ((argc != 1)) {
+		cmd_usage(cmdtp);
+		return 1;
+	}
+
+	get_hab_status();
+
+	return 0;
+}
+
+U_BOOT_CMD(
+		hab_status, CONFIG_SYS_MAXARGS, 1, do_hab_status,
+		"display HAB status",
+		""
+	  );
@@ -213,6 +213,34 @@
  
 void s_init(void)
 {
+	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
+	int is_6q = is_cpu_type(MXC_CPU_MX6Q);
+	u32 mask480;
+	u32 mask528;
+
+	/* Due to hardware limitation, on MX6Q we need to gate/ungate all PFDs
+	 * to make sure PFD is working right, otherwise, PFDs may
+	 * not output clock after reset, MX6DL and MX6SL have added 396M pfd
+	 * workaround in ROM code, as bus clock need it
+	 */
+
+	mask480 = ANATOP_PFD_CLKGATE_MASK(0) |
+		ANATOP_PFD_CLKGATE_MASK(1) |
+		ANATOP_PFD_CLKGATE_MASK(2) |
+		ANATOP_PFD_CLKGATE_MASK(3);
+	mask528 = ANATOP_PFD_CLKGATE_MASK(0) |
+		ANATOP_PFD_CLKGATE_MASK(1) |
+		ANATOP_PFD_CLKGATE_MASK(3);
+
+	/*
+	 * Don't reset PFD2 on DL/S
+	 */
+	if (is_6q)
+		mask528 |= ANATOP_PFD_CLKGATE_MASK(2);
+	writel(mask480, &anatop->pfd_480_set);
+	writel(mask528, &anatop->pfd_528_set);
+	writel(mask480, &anatop->pfd_480_clr);
+	writel(mask528, &anatop->pfd_528_clr);
 }
  
 #ifdef CONFIG_IMX_HDMI
+/*
+ * Copyright (C) 2012 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * SPDX-License-Identifier:    GPL-2.0+
+ *
+*/
+
+#ifndef __SECURE_MX6Q_H__
+#define __SECURE_MX6Q_H__
+
+#include <linux/types.h>
+
+/* -------- start of HAB API updates ------------*/
+/* The following are taken from HAB4 SIS */
+
+/* Status definitions */
+enum hab_status {
+	HAB_STS_ANY = 0x00,
+	HAB_FAILURE = 0x33,
+	HAB_WARNING = 0x69,
+	HAB_SUCCESS = 0xf0
+};
+
+/* Security Configuration definitions */
+enum hab_config {
+	HAB_CFG_RETURN = 0x33, /**< Field Return IC */
+	HAB_CFG_OPEN = 0xf0, /**< Non-secure IC */
+	HAB_CFG_CLOSED = 0xcc /**< Secure IC */
+};
+
+/* State definitions */
+enum hab_state {
+	HAB_STATE_INITIAL = 0x33, /**< Initialising state (transitory) */
+	HAB_STATE_CHECK = 0x55, /**< Check state (non-secure) */
+	HAB_STATE_NONSECURE = 0x66, /**< Non-secure state */
+	HAB_STATE_TRUSTED = 0x99, /**< Trusted state */
+	HAB_STATE_SECURE = 0xaa, /**< Secure state */
+	HAB_STATE_FAIL_SOFT = 0xcc, /**< Soft fail state */
+	HAB_STATE_FAIL_HARD = 0xff, /**< Hard fail state (terminal) */
+	HAB_STATE_NONE = 0xf0, /**< No security state machine */
+	HAB_STATE_MAX
+};
+
+/*Function prototype description*/
+typedef enum hab_status hab_rvt_report_event_t(enum hab_status, uint32_t,
+		uint8_t* , size_t*);
+typedef enum hab_status hab_rvt_report_status_t(enum hab_config *,
+		enum hab_state *);
+typedef enum hab_status hab_loader_callback_f_t(void**, size_t*, const void*);
+typedef enum hab_status hab_rvt_entry_t(void);
+typedef enum hab_status hab_rvt_exit_t(void);
+typedef void *hab_rvt_authenticate_image_t(uint8_t, ptrdiff_t,
+		void **, size_t *, hab_loader_callback_f_t);
+typedef void hapi_clock_init_t(void);
+
+#define HAB_RVT_REPORT_EVENT (*(uint32_t *)0x000000B4)
+#define HAB_RVT_REPORT_STATUS (*(uint32_t *)0x000000B8)
+#define HAB_RVT_AUTHENTICATE_IMAGE (*(uint32_t *)0x000000A4)
+#define HAB_RVT_ENTRY (*(uint32_t *)0x00000098)
+#define HAB_RVT_EXIT (*(uint32_t *)0x0000009C)
+#define HAB_RVT_CLOCK_INIT ((hapi_clock_init_t *)0x0000024D)
+
+#define HAB_CID_ROM 0 /**< ROM Caller ID */
+#define HAB_CID_UBOOT 1 /**< UBOOT Caller ID*/
+/* ----------- end of HAB API updates ------------*/
+
+#endif
@@ -456,7 +456,13 @@
 	u32	uid_low;
 	u32	rsvd1[3];
 	u32	uid_high;
-	u32	rsvd2[0x17];
+	u32	rsvd2[3];
+	u32	rsvd3[4];
+	u32	rsvd4[4];
+	u32	rsvd5[4];
+	u32	cfg5;
+	u32	rsvd6[3];
+	u32	rsvd7[4];
 };
  
 struct fuse_bank4_regs {
@@ -629,29 +635,12 @@
 	u32	digprog_sololite;	/* 0x280 */
 };
  
-#define ANATOP_PFD_480_PFD0_FRAC_SHIFT		0
-#define ANATOP_PFD_480_PFD0_FRAC_MASK		(0x3f<<ANATOP_PFD_480_PFD0_FRAC_SHIFT)
-#define ANATOP_PFD_480_PFD0_STABLE_SHIFT	6
-#define ANATOP_PFD_480_PFD0_STABLE_MASK		(1<<ANATOP_PFD_480_PFD0_STABLE_SHIFT)
-#define ANATOP_PFD_480_PFD0_CLKGATE_SHIFT	7
-#define ANATOP_PFD_480_PFD0_CLKGATE_MASK	(1<<ANATOP_PFD_480_PFD0_CLKGATE_SHIFT)
-#define ANATOP_PFD_480_PFD1_FRAC_SHIFT		8
-#define ANATOP_PFD_480_PFD1_FRAC_MASK		(0x3f<<ANATOP_PFD_480_PFD1_FRAC_SHIFT)
-#define ANATOP_PFD_480_PFD1_STABLE_SHIFT	14
-#define ANATOP_PFD_480_PFD1_STABLE_MASK		(1<<ANATOP_PFD_480_PFD1_STABLE_SHIFT)
-#define ANATOP_PFD_480_PFD1_CLKGATE_SHIFT	15
-#define ANATOP_PFD_480_PFD1_CLKGATE_MASK	(0x3f<<ANATOP_PFD_480_PFD1_CLKGATE_SHIFT)
-#define ANATOP_PFD_480_PFD2_FRAC_SHIFT		16
-#define ANATOP_PFD_480_PFD2_FRAC_MASK		(1<<ANATOP_PFD_480_PFD2_FRAC_SHIFT)
-#define ANATOP_PFD_480_PFD2_STABLE_SHIFT	22
-#define ANATOP_PFD_480_PFD2_STABLE_MASK	(1<<ANATOP_PFD_480_PFD2_STABLE_SHIFT)
-#define ANATOP_PFD_480_PFD2_CLKGATE_SHIFT	23
-#define ANATOP_PFD_480_PFD2_CLKGATE_MASK	(0x3f<<ANATOP_PFD_480_PFD2_CLKGATE_SHIFT)
-#define ANATOP_PFD_480_PFD3_FRAC_SHIFT		24
-#define ANATOP_PFD_480_PFD3_FRAC_MASK		(1<<ANATOP_PFD_480_PFD3_FRAC_SHIFT)
-#define ANATOP_PFD_480_PFD3_STABLE_SHIFT	30
-#define ANATOP_PFD_480_PFD3_STABLE_MASK		(1<<ANATOP_PFD_480_PFD3_STABLE_SHIFT)
-#define ANATOP_PFD_480_PFD3_CLKGATE_SHIFT	31
+#define ANATOP_PFD_FRAC_SHIFT(n)	((n)*8)
+#define ANATOP_PFD_FRAC_MASK(n)	(0x3f<<ANATOP_PFD_FRAC_SHIFT(n))
+#define ANATOP_PFD_STABLE_SHIFT(n)	(6+((n)*8))
+#define ANATOP_PFD_STABLE_MASK(n)	(1<<ANATOP_PFD_STABLE_SHIFT(n))
+#define ANATOP_PFD_CLKGATE_SHIFT(n)	(7+((n)*8))
+#define ANATOP_PFD_CLKGATE_MASK(n)	(1<<ANATOP_PFD_CLKGATE_SHIFT(n))
  
 struct iomuxc_base_regs {
 	u32     gpr[14];        /* 0x000 */
@@ -19,6 +19,13 @@
  
 #define is_soc_rev(rev)	((get_cpu_rev() & 0xFF) - rev)
 u32 get_cpu_rev(void);
+
+/* returns MXC_CPU_ value */
+#define cpu_type(rev) (((rev) >> 12)&0xff)
+
+/* use with MXC_CPU_ constants */
+#define is_cpu_type(cpu) (cpu_type(get_cpu_rev()) == cpu)
+
 const char *get_imx_type(u32 imxtype);
 unsigned imx_ddr_size(void);
  
+/*
+ * Freescale MXS UARTAPP Register Definitions
+ *
+ * Copyright (C) 2013 Andreas Wass <andreas.wass@dalelven.com>
+ *
+ * Based on code from LTIB:
+ * Copyright 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __ARCH_ARM___MXS_UARTAPP_H
+#define __ARCH_ARM___MXS_UARTAPP_H
+
+#include <asm/imx-common/regs-common.h>
+
+#ifndef __ASSEMBLY__
+struct mxs_uartapp_regs {
+	mxs_reg_32(hw_uartapp_ctrl0)
+	mxs_reg_32(hw_uartapp_ctrl1)
+	mxs_reg_32(hw_uartapp_ctrl2)
+	mxs_reg_32(hw_uartapp_linectrl)
+	mxs_reg_32(hw_uartapp_linectrl2)
+	mxs_reg_32(hw_uartapp_intr)
+	mxs_reg_32(hw_uartapp_data)
+	mxs_reg_32(hw_uartapp_stat)
+	mxs_reg_32(hw_uartapp_debug)
+	mxs_reg_32(hw_uartapp_version)
+	mxs_reg_32(hw_uartapp_autobaud)
+};
+#endif
+
+#define UARTAPP_CTRL0_SFTRST_MASK				(1 << 31)
+#define UARTAPP_CTRL0_CLKGATE_MASK			(1 << 30)
+#define UARTAPP_CTRL0_RUN_MASK				(1 << 29)
+#define UARTAPP_CTRL0_RX_SOURCE_MASK			(1 << 28)
+#define UARTAPP_CTRL0_RXTO_ENABLE_MASK			(1 << 27)
+#define UARTAPP_CTRL0_RXTIMEOUT_OFFSET			16
+#define UARTAPP_CTRL0_RXTIMEOUT_MASK			(0x7FF << 16)
+#define UARTAPP_CTRL0_XFER_COUNT_OFFSET			0
+#define UARTAPP_CTRL0_XFER_COUNT_MASK			0xFFFF
+
+#define UARTAPP_CTRL1_RUN_MASK				(1 << 28)
+
+#define UARTAPP_CTRL1_XFER_COUNT_OFFSET			0
+#define UARTAPP_CTRL1_XFER_COUNT_MASK			0xFFFF
+
+#define UARTAPP_CTRL2_INVERT_RTS_MASK			(1 << 31)
+#define UARTAPP_CTRL2_INVERT_CTS_MASK			(1 << 30)
+#define UARTAPP_CTRL2_INVERT_TX_MASK			(1 << 29)
+#define UARTAPP_CTRL2_INVERT_RX_MASK			(1 << 28)
+#define UARTAPP_CTRL2_RTS_SEMAPHORE_MASK			(1 << 27)
+#define UARTAPP_CTRL2_DMAONERR_MASK			(1 << 26)
+#define UARTAPP_CTRL2_TXDMAE_MASK				(1 << 25)
+#define UARTAPP_CTRL2_RXDMAE_MASK				(1 << 24)
+#define UARTAPP_CTRL2_RXIFLSEL_OFFSET			20
+#define UARTAPP_CTRL2_RXIFLSEL_MASK			(0x7 << 20)
+
+#define UARTAPP_CTRL2_RXIFLSEL_NOT_EMPTY		(0x0 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_ONE_QUARTER		(0x1 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_ONE_HALF		(0x2 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_THREE_QUARTERS		(0x3 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_SEVEN_EIGHTHS		(0x4 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_INVALID5		(0x5 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_INVALID6		(0x6 << 20)
+#define UARTAPP_CTRL2_RXIFLSEL_INVALID7		(0x7 << 20)
+#define UARTAPP_CTRL2_TXIFLSEL_OFFSET			16
+#define UARTAPP_CTRL2_TXIFLSEL_MASK			(0x7 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_EMPTY			(0x0 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_ONE_QUARTER		(0x1 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_ONE_HALF		(0x2 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_THREE_QUARTERS		(0x3 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_SEVEN_EIGHTHS		(0x4 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_INVALID5		(0x5 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_INVALID6		(0x6 << 16)
+#define UARTAPP_CTRL2_TXIFLSEL_INVALID7		(0x7 << 16)
+#define UARTAPP_CTRL2_CTSEN_MASK				(1 << 15)
+#define UARTAPP_CTRL2_RTSEN_MASK				(1 << 14)
+#define UARTAPP_CTRL2_OUT2_MASK				(1 << 13)
+#define UARTAPP_CTRL2_OUT1_MASK				(1 << 12)
+#define UARTAPP_CTRL2_RTS_MASK				(1 << 11)
+#define UARTAPP_CTRL2_DTR_MASK				(1 << 10)
+#define UARTAPP_CTRL2_RXE_MASK				(1 << 9)
+#define UARTAPP_CTRL2_TXE_MASK				(1 << 8)
+#define UARTAPP_CTRL2_LBE_MASK				(1 << 7)
+#define UARTAPP_CTRL2_USE_LCR2_MASK			(1 << 6)
+
+#define UARTAPP_CTRL2_SIRLP_MASK				(1 << 2)
+#define UARTAPP_CTRL2_SIREN_MASK				(1 << 1)
+#define UARTAPP_CTRL2_UARTEN_MASK				0x01
+
+#define UARTAPP_LINECTRL_BAUD_DIVINT_OFFSET			16
+#define UARTAPP_LINECTRL_BAUD_DIVINT_MASK			(0xFFFF << 16)
+#define UARTAPP_LINECTRL_EXTRACT_BAUD_DIVINT_OFFSET		6
+
+#define UARTAPP_LINECTRL_BAUD_DIVFRAC_OFFSET		8
+#define UARTAPP_LINECTRL_BAUD_DIVFRAC_MASK		(0x3F << 8)
+#define UARTAPP_LINECTRL_EXTRACT_BAUD_DIVFRAC_MASK	0x3F
+
+#define UARTAPP_LINECTRL_SPS_MASK				(1 << 7)
+#define UARTAPP_LINECTRL_WLEN_OFFSET			5
+#define UARTAPP_LINECTRL_WLEN_MASK			(0x03 << 5)
+#define UARTAPP_LINECTRL_WLEN_5BITS			(0x00 << 5)
+#define UARTAPP_LINECTRL_WLEN_6BITS			(0x01 << 5)
+#define UARTAPP_LINECTRL_WLEN_7BITS			(0x02 << 5)
+#define UARTAPP_LINECTRL_WLEN_8BITS			(0x03 << 5)
+
+#define UARTAPP_LINECTRL_FEN_MASK				(1 << 4)
+#define UARTAPP_LINECTRL_STP2_MASK			(1 << 3)
+#define UARTAPP_LINECTRL_EPS_MASK				(1 << 2)
+#define UARTAPP_LINECTRL_PEN_MASK				(1 << 1)
+#define UARTAPP_LINECTRL_BRK_MASK				1
+
+#define UARTAPP_LINECTRL2_BAUD_DIVINT_OFFSET		16
+#define UARTAPP_LINECTRL2_BAUD_DIVINT_MASK		(0xFFFF << 16)
+#define UARTAPP_LINECTRL2_EXTRACT_BAUD_DIVINT_OFFSET	6
+
+#define UARTAPP_LINECTRL2_BAUD_DIVFRAC_OFFSET		8
+#define UARTAPP_LINECTRL2_BAUD_DIVFRAC_MASK		(0x3F << 8)
+#define UARTAPP_LINECTRL2_EXTRACT_BAUD_DIVFRAC_MASK	0x3F
+
+#define UARTAPP_LINECTRL2_SPS_MASK			(1 << 7)
+#define UARTAPP_LINECTRL2_WLEN_OFFSET			5
+#define UARTAPP_LINECTRL2_WLEN_MASK			(0x03 << 5)
+#define UARTAPP_LINECTRL2_WLEN_5BITS			(0x00 << 5)
+#define UARTAPP_LINECTRL2_WLEN_6BITS			(0x01 << 5)
+#define UARTAPP_LINECTRL2_WLEN_7BITS			(0x02 << 5)
+#define UARTAPP_LINECTRL2_WLEN_8BITS			(0x03 << 5)
+
+#define UARTAPP_LINECTRL2_FEN_MASK			(1 << 4)
+#define UARTAPP_LINECTRL2_STP2_MASK			(1 << 3)
+#define UARTAPP_LINECTRL2_EPS_MASK			(1 << 2)
+#define UARTAPP_LINECTRL2_PEN_MASK			(1 << 1)
+
+#define UARTAPP_INTR_ABDIEN_MASK				(1 << 27)
+#define UARTAPP_INTR_OEIEN_MASK				(1 << 26)
+#define UARTAPP_INTR_BEIEN_MASK				(1 << 25)
+#define UARTAPP_INTR_PEIEN_MASK				(1 << 24)
+#define UARTAPP_INTR_FEIEN_MASK				(1 << 23)
+#define UARTAPP_INTR_RTIEN_MASK				(1 << 22)
+#define UARTAPP_INTR_TXIEN_MASK				(1 << 21)
+#define UARTAPP_INTR_RXIEN_MASK				(1 << 20)
+#define UARTAPP_INTR_DSRMIEN_MASK				(1 << 19)
+#define UARTAPP_INTR_DCDMIEN_MASK				(1 << 18)
+#define UARTAPP_INTR_CTSMIEN_MASK				(1 << 17)
+#define UARTAPP_INTR_RIMIEN_MASK				(1 << 16)
+
+#define UARTAPP_INTR_ABDIS_MASK				(1 << 11)
+#define UARTAPP_INTR_OEIS_MASK				(1 << 10)
+#define UARTAPP_INTR_BEIS_MASK				(1 << 9)
+#define UARTAPP_INTR_PEIS_MASK				(1 << 8)
+#define UARTAPP_INTR_FEIS_MASK				(1 << 7)
+#define UARTAPP_INTR_RTIS_MASK				(1 << 6)
+#define UARTAPP_INTR_TXIS_MASK				(1 << 5)
+#define UARTAPP_INTR_RXIS_MASK				(1 << 4)
+#define UARTAPP_INTR_DSRMIS_MASK				(1 << 3)
+#define UARTAPP_INTR_DCDMIS_MASK				(1 << 2)
+#define UARTAPP_INTR_CTSMIS_MASK				(1 << 1)
+#define UARTAPP_INTR_RIMIS_MASK				0x1
+
+#define UARTAPP_DATA_DATA_OFFSET				0
+#define UARTAPP_DATA_DATA_MASK				0xFFFFFFFF
+#define UARTAPP_STAT_PRESENT_MASK				(1 << 31)
+#define UARTAPP_STAT_PRESENT_UNAVAILABLE		(0x0 << 31)
+#define UARTAPP_STAT_PRESENT_AVAILABLE			(0x1 << 31)
+
+#define UARTAPP_STAT_HISPEED_MASK				(1 << 30)
+#define UARTAPP_STAT_HISPEED_UNAVAILABLE		(0x0 << 30)
+#define UARTAPP_STAT_HISPEED_AVAILABLE			(0x1 << 30)
+
+#define UARTAPP_STAT_BUSY_MASK				(1 << 29)
+#define UARTAPP_STAT_CTS_MASK				(1 << 28)
+#define UARTAPP_STAT_TXFE_MASK				(1 << 27)
+#define UARTAPP_STAT_RXFF_MASK				(1 << 26)
+#define UARTAPP_STAT_TXFF_MASK				(1 << 25)
+#define UARTAPP_STAT_RXFE_MASK				(1 << 24)
+#define UARTAPP_STAT_RXBYTE_INVALID_OFFSET			20
+#define UARTAPP_STAT_RXBYTE_INVALID_MASK		(0xF << 20)
+
+#define UARTAPP_STAT_OERR_MASK				(1 << 19)
+#define UARTAPP_STAT_BERR_MASK				(1 << 18)
+#define UARTAPP_STAT_PERR_MASK				(1 << 17)
+#define UARTAPP_STAT_FERR_MASK				(1 << 16)
+#define UARTAPP_STAT_RXCOUNT_OFFSET				0
+#define UARTAPP_STAT_RXCOUNT_MASK				0xFFFF
+
+#define UARTAPP_DEBUG_RXIBAUD_DIV_OFFSET			16
+#define UARTAPP_DEBUG_RXIBAUD_DIV_MASK				(0xFFFF << 16)
+
+#define UARTAPP_DEBUG_RXFBAUD_DIV_OFFSET			10
+#define UARTAPP_DEBUG_RXFBAUD_DIV_MASK				(0x3F << 10)
+
+#define UARTAPP_DEBUG_TXDMARUN_MASK			(1 << 5)
+#define UARTAPP_DEBUG_RXDMARUN_MASK			(1 << 4)
+#define UARTAPP_DEBUG_TXCMDEND_MASK			(1 << 3)
+#define UARTAPP_DEBUG_RXCMDEND_MASK			(1 << 2)
+#define UARTAPP_DEBUG_TXDMARQ_MASK			(1 << 1)
+#define UARTAPP_DEBUG_RXDMARQ_MASK			0x01
+
+#define UARTAPP_VERSION_MAJOR_OFFSET			24
+#define UARTAPP_VERSION_MAJOR_MASK			(0xFF << 24)
+
+#define UARTAPP_VERSION_MINOR_OFFSET			16
+#define UARTAPP_VERSION_MINOR_MASK			(0xFF << 16)
+
+#define UARTAPP_VERSION_STEP_OFFSET				0
+#define UARTAPP_VERSION_STEP_MASK				0xFFFF
+
+#define UARTAPP_AUTOBAUD_REFCHAR1_OFFSET			24
+#define UARTAPP_AUTOBAUD_REFCHAR1_MASK				(0xFF << 24)
+
+#define UARTAPP_AUTOBAUD_REFCHAR0_OFFSET			16
+#define UARTAPP_AUTOBAUD_REFCHAR0_MASK				(0xFF << 16)
+
+#define UARTAPP_AUTOBAUD_UPDATE_TX_MASK			(1 << 4)
+#define UARTAPP_AUTOBAUD_TWO_REF_CHARS_MASK		(1 << 3)
+#define UARTAPP_AUTOBAUD_START_WITH_RUNBIT_MASK		(1 << 2)
+#define UARTAPP_AUTOBAUD_START_BAUD_DETECT_MASK		(1 << 1)
+#define UARTAPP_AUTOBAUD_BAUD_DETECT_ENABLE_MASK		0x01
+#endif /* __ARCH_ARM___UARTAPP_H */
@@ -622,7 +622,6 @@
 static void setup_display(void)
 {
 	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
-	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
 	struct iomuxc *iomux = (struct iomuxc *)IOMUXC_BASE_ADDR;
 	int reg;
  
@@ -633,10 +632,6 @@
 	reg |=  MXC_CCM_CCGR3_LDB_DI0_MASK;
 	writel(reg, &mxc_ccm->CCGR3);
  
-	/* set PFD1_FRAC to 0x13 == 455 MHz (480*18)/0x13 */
-	writel(ANATOP_PFD_480_PFD1_FRAC_MASK, &anatop->pfd_480_clr);
-	writel(0x13<<ANATOP_PFD_480_PFD1_FRAC_SHIFT, &anatop->pfd_480_set);
-
 	/* set LDB0, LDB1 clk select to 011/011 */
 	reg = readl(&mxc_ccm->cs2cdr);
 	reg &= ~(MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_MASK
@@ -666,7 +661,8 @@
 	writel(reg, &iomux->gpr[2]);
  
 	reg = readl(&iomux->gpr[3]);
-	reg = (reg & ~IOMUXC_GPR3_LVDS0_MUX_CTL_MASK)
+	reg = (reg & ~(IOMUXC_GPR3_LVDS0_MUX_CTL_MASK
+			|IOMUXC_GPR3_HDMI_MUX_CTL_MASK))
 	    | (IOMUXC_GPR3_MUX_SRC_IPU1_DI0
 	       <<IOMUXC_GPR3_LVDS0_MUX_CTL_OFFSET);
 	writel(reg, &iomux->gpr[3]);
@@ -7,8 +7,7 @@
 1. Boot source, boot from SD card
 ---------------------------------
  
-This version of u-boot works only on the SD card. By default, the
-Congatec board can boot only from the SPI-NOR.
+By default, the Congatec board can boot only from the SPI-NOR.
 But, with the u-boot version provided with the board you can write boot
 registers to force the board to reboot and boot from the SD slot. If
 "bmode" command is not available from your pre-installed u-boot, these
@@ -204,6 +204,7 @@
 m28evk                       arm         arm926ejs   m28evk              denx           mxs		m28evk
 mx28evk                      arm         arm926ejs   mx28evk             freescale      mxs		mx28evk:ENV_IS_IN_MMC
 mx28evk_nand                 arm         arm926ejs   mx28evk             freescale      mxs		mx28evk:ENV_IS_IN_NAND
+mx28evk_auart_console        arm         arm926ejs   mx28evk             freescale      mxs		mx28evk:MXS_AUART,MXS_AUART_BASE=MXS_UARTAPP3_BASE,ENV_IS_IN_MMC
 sc_sps_1                     arm         arm926ejs   sc_sps_1            schulercontrol mxs
 nhk8815                      arm         arm926ejs   nhk8815             st             nomadik
 nhk8815_onenand              arm         arm926ejs   nhk8815             st             nomadik       nhk8815:BOOT_ONENAND
@@ -135,6 +135,7 @@
 	{	IH_TYPE_SCRIPT,     "script",	  "Script",		},
 	{	IH_TYPE_STANDALONE, "standalone", "Standalone Program", },
 	{	IH_TYPE_UBLIMAGE,   "ublimage",   "Davinci UBL image",},
+	{	IH_TYPE_MXSIMAGE,   "mxsimage",   "Freescale MXS Boot Image",},
 	{	-1,		    "",		  "",			},
 };
  
@@ -220,6 +220,15 @@
 endif
  
 # TODO(sjg@chromium.org): Is this correct on Mac OS?
+
+# MXSImage needs LibSSL
+ifneq ($(CONFIG_MX23)$(CONFIG_MX28),)
+HOSTLIBS	+= -lssl -lcrypto
+# Add CONFIG_MXS into host CFLAGS, so we can check whether or not register
+# the mxsimage support within tools/mxsimage.c .
+HOSTCFLAGS	+= -DCONFIG_MXS
+endif
+
 ifdef CONFIG_FIT_SIGNATURE
 HOSTLIBS	+= -lssl -lcrypto
  
@@ -15,9 +15,6 @@
 For more details refer Chapter 2 - System Boot and section 2.14
 (flash header description) of the processor's manual.
  
-This implementation does not use at the moment the secure boot feature
-of the processor. The image is generated disabling all security fields.
-
 Command syntax:
 --------------
 ./tools/mkimage -l <mx u-boot_file>
@@ -85,6 +82,33 @@
  
 				Example:
 				BOOT_FROM spi
+
+	CSF			value
+
+				Total size of CSF (Command Sequence File)
+				used for Secure Boot/ High Assurance Boot
+				(HAB).
+
+				Using this command will populate the IVT
+				(Initial Vector Table) CSF pointer and adjust
+				the length fields only. The CSF itself needs
+				to be generated with Freescale tools and
+				'manually' appended to the u-boot.imx file.
+
+				The CSF is then simply concatenated
+				to the u-boot image, making a signed bootloader,
+				that the processor can verify
+				if the fuses for the keys are burned.
+
+				Further infos how to configure the SOC to verify
+				the bootloader can be found in the "High
+				Assurance Boot Version Application Programming
+				Interface Reference Manual" as part of the
+				Freescale Code Signing Tool, available on the
+				manufacturer's website.
+
+				Example:
+				CSF 0x2000
  
 	DATA			type address value
  
+High Assurance Boot (HAB) for i.MX6 CPUs
+
+To authenticate U-Boot only by the CPU there is no code required in
+U-Boot itself. However, the U-Boot image to be programmed into the
+boot media needs to be properly constructed, i.e. it must contain a
+proper Command Sequence File (CSF).
+
+The Initial Vector Table contains a pointer to the CSF. Please see
+doc/README.imximage for how to prepare u-boot.imx.
+
+The CSF itself is being generated by Freescale HAB tools.
+
+mkimage will output additional information about "HAB Blocks"
+which can be used in the Freescale tooling to authenticate U-Boot
+(entries in the CSF file).
+
+Image Type:   Freescale IMX Boot Image
+Image Ver:    2 (i.MX53/6 compatible)
+Data Size:    327680 Bytes = 320.00 kB = 0.31 MB
+Load Address: 177ff420
+Entry Point:  17800000
+HAB Blocks:   177ff400 00000000 0004dc00
+              ^^^^^^^^ ^^^^^^^^ ^^^^^^^^
+		|	|	   |
+		|	|	   -------- (1)
+		|	|
+		|	------------------- (2)
+		|
+		--------------------------- (3)
+
+(1)	Size of area in file u-boot.imx to sign
+	This area should include the IVT, the Boot Data the DCD
+	and U-Boot itself.
+(2)	Start of area in u-boot.imx to sign
+(3)	Start of area in RAM to authenticate
+
+CONFIG_SECURE_BOOT currently enables only an additional command
+'hab_status' in U-Boot to retrieve the HAB status and events. This
+can be useful while developing and testing HAB.
+
+Commands to generate a signed U-Boot using Freescale HAB tools:
+cst --o U-Boot_CSF.bin < U-Boot.CSF
+objcopy -I binary -O binary --pad-to 0x2000 --gap-fill=0x00 \
+	U-Boot_CSF.bin U-Boot_CSF_pad.bin
+cat u-boot.imx U-Boot_CSF_pad.bin > u-boot-signed.imx
+
+NOTE: U-Boot_CSF.bin needs to be padded to the value specified in
+the imximage.cfg file.
+Freescale i.MX233/i.MX28 SB image generator via mkimage
+=======================================================
+
+This tool allows user to produce SB BootStream encrypted with a zero key.
+Such a BootStream is then bootable on i.MX23/i.MX28.
+
+Usage -- producing image:
+=========================
+The mxsimage tool is targeted to be a simple replacement for the elftosb2 .
+To generate an image, write an image configuration file and run:
+
+ mkimage -A arm -O u-boot -T mxsimage -n <path to configuration file> \
+         <output bootstream file>
+
+The output bootstream file is usually using the .sb file extension. Note
+that the example configuration files for producing bootable BootStream with
+the U-Boot bootloader can be found under arch/arm/boot/cpu/arm926ejs/mxs/
+directory. See the following files:
+
+ mxsimage.mx23.cfg -- This is an example configuration for i.MX23
+ mxsimage.mx28.cfg -- This is an example configuration for i.MX28
+
+Each configuration file uses very simple instruction semantics and a few
+additional rules have to be followed so that a useful image can be produced.
+These semantics and rules will be outlined now.
+
+- Each line of the configuration file contains exactly one instruction.
+- Every numeric value must be encoded in hexadecimal and in format 0xabcdef12 .
+- The configuration file is a concatenation of blocks called "sections" and
+  optionally "DCD blocks" (see below).
+  - Each "section" is started by the "SECTION" instruction.
+  - The "SECTION" instruction has the following semantics:
+
+      SECTION u32_section_number [BOOTABLE]
+      - u32_section_number :: User-selected ID of the section
+      - BOOTABLE           :: Sets the section as bootable
+
+  - A bootable section is one from which the BootROM starts executing
+    subsequent instructions or code. Exactly one section must be selected
+    as bootable, usually the one containing the instructions and data to
+    load the bootloader.
+
+  - A "SECTION" must be immediatelly followed by a "TAG" instruction.
+  - The "TAG" instruction has the following semantics:
+
+      TAG [LAST]
+      - LAST               :: Flag denoting the last section in the file
+
+  - After a "TAG" unstruction, any of the following instructions may follow
+    in any order and any quantity:
+
+      NOOP
+      - This instruction does nothing
+
+      LOAD     u32_address string_filename
+      - Instructs the BootROM to load file pointed by "string_filename" onto
+        address "u32_address".
+
+      LOAD IVT u32_address u32_IVT_entry_point
+      - Crafts and loads IVT onto address "u32_address" with the entry point
+        of u32_IVT_entry_point.
+      - i.MX28-specific instruction!
+
+      LOAD DCD u32_address u32_DCD_block_ID
+      - Loads the DCD block with ID "u32_DCD_block_ID" onto address
+        "u32_address" and executes the contents of this DCD block
+      - i.MX28-specific instruction!
+
+      FILL u32_address u32_pattern u32_length
+      - Starts to write memory from addres "u32_address" with a pattern
+        specified by "u32_pattern". Writes exactly "u32_length" bytes of the
+	pattern.
+
+      JUMP [HAB] u32_address [u32_r0_arg]
+      - Jumps onto memory address specified by "u32_address" by setting this
+        address in PT. The BootROM will pass the "u32_r0_arg" value in ARM
+	register "r0" to the executed code if this option is specified.
+	Otherwise, ARM register "r0" will default to value 0x00000000. The
+	optional "HAB" flag is i.MX28-specific flag turning on the HAB boot.
+
+      CALL [HAB] u32_address [u32_r0_arg]
+      - See JUMP instruction above, as the operation is exactly the same with
+        one difference. The CALL instruction does allow returning into the
+	BootROM from the executed code. U-Boot makes use of this in it's SPL
+	code.
+
+      MODE string_mode
+      - Restart the CPU and start booting from device specified by the
+	"string_mode" argument. The "string_mode" differs for each CPU
+	and can be:
+         i.MX23, string_mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
+                               JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
+         i.MX28, string_mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
+                               JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
+
+  - An optional "DCD" blocks can be added at the begining of the configuration
+    file. Note that the DCD is only supported on i.MX28.
+    - The DCD blocks must be inserted before the first "section" in the
+      configuration file.
+    - The DCD block has the following semantics:
+
+        DCD u32_DCD_block_ID
+	- u32_DCD_block_ID	:: The ID number of the DCD block, must match
+				   the ID number used by "LOAD DCD" instruction.
+
+    - The DCD block must be followed by one of the following instructions. All
+      of the instructions operate either on 1, 2 or 4 bytes. This is selected by
+      the 'n' suffix of the instruction:
+
+	WRITE.n u32_address u32_value
+	- Write the "u32_value" to the "u32_address" address.
+
+	ORR.n u32_address u32_value
+	- Read the "u32_address", perform a bitwise-OR with the "u32_value" and
+	  write the result back to "u32_address".
+
+	ANDC.n u32_address u32_value
+	- Read the "u32_address", perform a bitwise-AND with the complement of
+	  "u32_value" and write the result back to "u32_address".
+
+	EQZ.n u32_address u32_count
+	- Read the "u32_address" at most "u32_count" times and test if the value
+	  read is zero. If it is, break the loop earlier.
+
+	NEZ.n u32_address u32_count
+	- Read the "u32_address" at most "u32_count" times and test if the value
+	  read is non-zero. If it is, break the loop earlier.
+
+	EQ.n u32_address u32_mask
+	- Read the "u32_address" in a loop and test if the result masked with
+	  "u32_mask" equals the "u32_mask". If the values are equal, break the
+	  reading loop.
+
+	NEQ.n u32_address u32_mask
+	- Read the "u32_address" in a loop and test if the result masked with
+	  "u32_mask" does not equal the "u32_mask". If the values are not equal,
+	  break the reading loop.
+
+	NOOP
+	- This instruction does nothing.
+
+- If the verbose output from the BootROM is enabled, the BootROM will produce a
+  letter on the Debug UART for each instruction it started processing. Here is a
+  mapping between the above instructions and the BootROM verbose output:
+
+   H -- SB Image header loaded
+   T -- TAG instruction
+   N -- NOOP instruction
+   L -- LOAD instruction
+   F -- FILL instruction
+   J -- JUMP instruction
+   C -- CALL instruction
+   M -- MODE instruction
+
+Usage -- verifying image:
+=========================
+
+The mxsimage can also verify and dump contents of an image. Use the following
+syntax to verify and dump contents of an image:
+
+ mkimage -l <input bootstream file>
+
+This will output all the information from the SB image header and all the
+instructions contained in the SB image. It will also check if the various
+checksums in the SB image are correct.
@@ -410,7 +410,8 @@
 	mmc->voltages = MMC_VDD_32_33 | MMC_VDD_33_34;
  
 	mmc->host_caps = MMC_MODE_4BIT | MMC_MODE_8BIT |
-			 MMC_MODE_HS_52MHz | MMC_MODE_HS;
+			 MMC_MODE_HS_52MHz | MMC_MODE_HS |
+			 MMC_MODE_HC;
  
 	/*
 	 * SSPCLK = 480 * 18 / 29 / 1 = 297.731 MHz
@@ -980,6 +980,8 @@
 	if (fec_get_hwaddr(edev, dev_id, ethaddr) == 0) {
 		debug("got MAC%d address from fuse: %pM\n", dev_id, ethaddr);
 		memcpy(edev->enetaddr, ethaddr, 6);
+		if (!getenv("ethaddr"))
+			eth_setenv_enetaddr("ethaddr", ethaddr);
 	}
 	return ret;
 err3:
@@ -38,6 +38,7 @@
 COBJS-$(CONFIG_ZYNQ_SERIAL) += serial_zynq.o
 COBJS-$(CONFIG_BFIN_SERIAL) += serial_bfin.o
 COBJS-$(CONFIG_FSL_LPUART) += serial_lpuart.o
+COBJS-$(CONFIG_MXS_AUART) += mxs_auart.o
  
 ifndef CONFIG_SPL_BUILD
 COBJS-$(CONFIG_USB_TTY) += usbtty.o
+/*
+ * Freescale i.MX23/i.MX28 AUART driver
+ *
+ * Copyright (C) 2013 Andreas Wass <andreas.wass@dalelven.com>
+ *
+ * Based on the MXC serial driver:
+ *
+ * (c) 2007 Sascha Hauer <s.hauer@pengutronix.de>
+ *
+ * Further based on the Linux mxs-auart.c driver:
+ *
+ * Freescale STMP37XX/STMP38X Application UART drkiver
+ * Copyright 2008-2010 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+#include <common.h>
+#include <asm/io.h>
+#include <serial.h>
+#include <linux/compiler.h>
+#include <asm/arch/regs-base.h>
+#include <asm/arch/regs-uartapp.h>
+#include <asm/arch/sys_proto.h>
+
+DECLARE_GLOBAL_DATA_PTR;
+
+#ifndef CONFIG_MXS_AUART_BASE
+#error "CONFIG_MXS_AUART_BASE must be set to the base UART to use"
+#endif
+
+/* AUART clock always supplied by XTAL and always 24MHz */
+#define MXS_AUART_CLK 24000000
+
+static struct mxs_uartapp_regs *get_uartapp_registers(void)
+{
+	return (struct mxs_uartapp_regs *)CONFIG_MXS_AUART_BASE;
+}
+
+/**
+ * Sets the baud rate and settings.
+ * The settings are: 8 data bits, no parit and 1 stop bit.
+ */
+void mxs_auart_setbrg(void)
+{
+	u32 div;
+	u32 linectrl = 0;
+	struct mxs_uartapp_regs *regs = get_uartapp_registers();
+
+	if (!gd->baudrate)
+		gd->baudrate = CONFIG_BAUDRATE;
+
+	/*
+	 * From i.MX28 datasheet:
+	 * div is calculated by calculating UARTCLK*32/baudrate, rounded to int
+	 * div must be between 0xEC and 0x003FFFC0 inclusive
+	 * Lowest 6 bits of div goes in BAUD_DIVFRAC part of LINECTRL register
+	 * Next 16 bits goes in BAUD_DIVINT part of LINECTRL register
+	 */
+	div = (MXS_AUART_CLK * 32) / gd->baudrate;
+	if (div < 0xEC || div > 0x003FFFC0)
+		return;
+
+	linectrl |= ((div & UARTAPP_LINECTRL_EXTRACT_BAUD_DIVFRAC_MASK) <<
+		UARTAPP_LINECTRL_BAUD_DIVFRAC_OFFSET) &
+		UARTAPP_LINECTRL_BAUD_DIVFRAC_MASK;
+	linectrl |= ((div >> UARTAPP_LINECTRL_EXTRACT_BAUD_DIVINT_OFFSET) <<
+		UARTAPP_LINECTRL_BAUD_DIVINT_OFFSET) &
+		UARTAPP_LINECTRL_BAUD_DIVINT_MASK;
+
+	/* Word length: 8 bits */
+	linectrl |= UARTAPP_LINECTRL_WLEN_8BITS;
+
+	/* Enable FIFOs. */
+	linectrl |= UARTAPP_LINECTRL_FEN_MASK;
+
+	/* Write above settings, no parity, 1 stop bit */
+	writel(linectrl, &regs->hw_uartapp_linectrl);
+}
+
+int mxs_auart_init(void)
+{
+	struct mxs_uartapp_regs *regs = get_uartapp_registers();
+	/* Reset everything */
+	mxs_reset_block(&regs->hw_uartapp_ctrl0_reg);
+	/* Disable interrupts */
+	writel(0, &regs->hw_uartapp_intr);
+	/* Set baud rate and settings */
+	serial_setbrg();
+	/* Disable RTS and CTS, ignore LINECTRL2 register */
+	writel(UARTAPP_CTRL2_RTSEN_MASK |
+			UARTAPP_CTRL2_CTSEN_MASK |
+			UARTAPP_CTRL2_USE_LCR2_MASK,
+			&regs->hw_uartapp_ctrl2_clr);
+	/* Enable receiver, transmitter and UART */
+	writel(UARTAPP_CTRL2_RXE_MASK |
+			UARTAPP_CTRL2_TXE_MASK |
+			UARTAPP_CTRL2_UARTEN_MASK,
+			&regs->hw_uartapp_ctrl2_set);
+	return 0;
+}
+
+void mxs_auart_putc(const char c)
+{
+	struct mxs_uartapp_regs *regs = get_uartapp_registers();
+	/* Wait in loop while the transmit FIFO is full */
+	while (readl(&regs->hw_uartapp_stat) & UARTAPP_STAT_TXFF_MASK)
+		;
+
+	writel(c, &regs->hw_uartapp_data);
+
+	if (c == '\n')
+		mxs_auart_putc('\r');
+}
+
+int mxs_auart_tstc(void)
+{
+	struct mxs_uartapp_regs *regs = get_uartapp_registers();
+	/* Checks if receive FIFO is empty */
+	return !(readl(&regs->hw_uartapp_stat) & UARTAPP_STAT_RXFE_MASK);
+}
+
+int mxs_auart_getc(void)
+{
+	struct mxs_uartapp_regs *regs = get_uartapp_registers();
+	/* Wait until a character is available to read */
+	while (!mxs_auart_tstc())
+		;
+	/* Read the character from the data register */
+	return readl(&regs->hw_uartapp_data) & 0xFF;
+}
+
+static struct serial_device mxs_auart_drv = {
+	.name = "mxs_auart_serial",
+	.start = mxs_auart_init,
+	.stop = NULL,
+	.setbrg = mxs_auart_setbrg,
+	.putc = mxs_auart_putc,
+	.puts = default_serial_puts,
+	.getc = mxs_auart_getc,
+	.tstc = mxs_auart_tstc,
+};
+
+void mxs_auart_initialize(void)
+{
+	serial_register(&mxs_auart_drv);
+}
+
+__weak struct serial_device *default_serial_console(void)
+{
+	return &mxs_auart_drv;
+}
@@ -160,6 +160,7 @@
 serial_initfunc(sa1100_serial_initialize);
 serial_initfunc(sh_serial_initialize);
 serial_initfunc(arm_dcc_initialize);
+serial_initfunc(mxs_auart_initialize);
  
 /**
  * serial_register() - Register serial driver with serial driver core
@@ -253,6 +254,7 @@
 	sa1100_serial_initialize();
 	sh_serial_initialize();
 	arm_dcc_initialize();
+	mxs_auart_initialize();
  
 	serial_assign(default_serial_console()->name);
 }
@@ -119,11 +119,16 @@
 /* GPIO */
 #define CONFIG_MXS_GPIO
  
-/* DUART Serial Driver */
+/*
+ * DUART Serial Driver.
+ * Conflicts with AUART driver which can be set by board.
+ */
+#ifndef CONFIG_MXS_AUART
 #define CONFIG_PL011_SERIAL
 #define CONFIG_PL011_CLOCK		24000000
 #define CONFIG_PL01x_PORTS		{ (void *)MXS_UARTDBG_BASE }
 #define CONFIG_CONS_INDEX		0
+#endif
 /* Default baudrate can be overriden by board! */
 #ifndef CONFIG_BAUDRATE
 #define CONFIG_BAUDRATE			115200
@@ -212,6 +212,7 @@
 #define IH_TYPE_AISIMAGE	13	/* TI Davinci AIS Image		*/
 #define IH_TYPE_KERNEL_NOLOAD	14	/* OS Kernel Image, can run from any load address */
 #define IH_TYPE_PBLIMAGE	15	/* Freescale PBL Boot Image	*/
+#define IH_TYPE_MXSIMAGE	16	/* Freescale MXSBoot Image	*/
  
 /*
  * Compression Types
@@ -70,31 +70,32 @@
 EXT_OBJ_FILES-y += lib/sha1.o
  
 # Source files located in the tools directory
-OBJ_FILES-$(CONFIG_LCD_LOGO) += bmp_logo.o
-OBJ_FILES-$(CONFIG_VIDEO_LOGO) += bmp_logo.o
+NOPED_OBJ_FILES-y += aisimage.o
 NOPED_OBJ_FILES-y += default_image.o
-NOPED_OBJ_FILES-y += proftool.o
-OBJ_FILES-$(CONFIG_BUILD_ENVCRC) += envcrc.o
 NOPED_OBJ_FILES-y += fit_image.o
-OBJ_FILES-$(CONFIG_CMD_NET) += gen_eth_addr.o
-OBJ_FILES-$(CONFIG_CMD_LOADS) += img2srec.o
-OBJ_FILES-$(CONFIG_XWAY_SWAP_BYTES) += xway-swap-bytes.o
-NOPED_OBJ_FILES-y += aisimage.o
-NOPED_OBJ_FILES-y += kwbimage.o
-NOPED_OBJ_FILES-y += pblimage.o
-NOPED_OBJ_FILES-y += imximage.o
 NOPED_OBJ_FILES-y += image-host.o
-NOPED_OBJ_FILES-y += omapimage.o
+NOPED_OBJ_FILES-y += imximage.o
+NOPED_OBJ_FILES-y += kwbimage.o
 NOPED_OBJ_FILES-y += mkenvimage.o
 NOPED_OBJ_FILES-y += mkimage.o
-OBJ_FILES-$(CONFIG_SMDK5250) += mkexynosspl.o
+NOPED_OBJ_FILES-y += mxsimage.o
+NOPED_OBJ_FILES-y += omapimage.o
+NOPED_OBJ_FILES-y += os_support.o
+NOPED_OBJ_FILES-y += pblimage.o
+NOPED_OBJ_FILES-y += proftool.o
+NOPED_OBJ_FILES-y += ublimage.o
+OBJ_FILES-$(CONFIG_BUILD_ENVCRC) += envcrc.o
+OBJ_FILES-$(CONFIG_CMD_LOADS) += img2srec.o
+OBJ_FILES-$(CONFIG_CMD_NET) += gen_eth_addr.o
+OBJ_FILES-$(CONFIG_KIRKWOOD) += kwboot.o
+OBJ_FILES-$(CONFIG_LCD_LOGO) += bmp_logo.o
 OBJ_FILES-$(CONFIG_MX23) += mxsboot.o
 OBJ_FILES-$(CONFIG_MX28) += mxsboot.o
 OBJ_FILES-$(CONFIG_NETCONSOLE) += ncb.o
-NOPED_OBJ_FILES-y += os_support.o
 OBJ_FILES-$(CONFIG_SHA1_CHECK_UB_IMG) += ubsha1.o
-NOPED_OBJ_FILES-y += ublimage.o
-OBJ_FILES-$(CONFIG_KIRKWOOD) += kwboot.o
+OBJ_FILES-$(CONFIG_SMDK5250) += mkexynosspl.o
+OBJ_FILES-$(CONFIG_VIDEO_LOGO) += bmp_logo.o
+OBJ_FILES-$(CONFIG_XWAY_SWAP_BYTES) += xway-swap-bytes.o
  
 # Don't build by default
 #ifeq ($(ARCH),ppc)
  
  
  
  
  
@@ -203,20 +204,21 @@
 	$(HOSTSTRIP) $@
  
 $(obj)mkimage$(SFX):	$(obj)aisimage.o \
+			$(FIT_SIG_OBJS) \
 			$(obj)crc32.o \
 			$(obj)default_image.o \
 			$(obj)fit_image.o \
 			$(obj)image-fit.o \
-			$(obj)image.o \
 			$(obj)image-host.o \
-			$(FIT_SIG_OBJS) \
+			$(obj)image.o \
 			$(obj)imximage.o \
 			$(obj)kwbimage.o \
-			$(obj)pblimage.o \
 			$(obj)md5.o \
 			$(obj)mkimage.o \
-			$(obj)os_support.o \
+			$(obj)mxsimage.o \
 			$(obj)omapimage.o \
+			$(obj)os_support.o \
+			$(obj)pblimage.o \
 			$(obj)sha1.o \
 			$(obj)ublimage.o \
 			$(LIBFDT_OBJS) \
@@ -13,6 +13,8 @@
 #include <image.h>
 #include "imximage.h"
  
+#define UNDEFINED 0xFFFFFFFF
+
 /*
  * Supported commands for configuration file
  */
@@ -20,6 +22,7 @@
 	{CMD_BOOT_FROM,         "BOOT_FROM",            "boot command",	  },
 	{CMD_BOOT_OFFSET,       "BOOT_OFFSET",          "Boot offset",	  },
 	{CMD_DATA,              "DATA",                 "Reg Write Data", },
+	{CMD_CSF,               "CSF",           "Command Sequence File", },
 	{CMD_IMAGE_VERSION,     "IMAGE_VERSION",        "image version",  },
 	{-1,                    "",                     "",	          },
 };
@@ -28,7 +31,7 @@
  * Supported Boot options for configuration file
  * this is needed to set the correct flash offset
  */
-static table_entry_t imximage_bootops[] = {
+static table_entry_t imximage_boot_offset[] = {
 	{FLASH_OFFSET_ONENAND,	"onenand",	"OneNAND Flash",},
 	{FLASH_OFFSET_NAND,	"nand",		"NAND Flash",	},
 	{FLASH_OFFSET_NOR,	"nor",		"NOR Flash",	},
@@ -39,6 +42,20 @@
 };
  
 /*
+ * Supported Boot options for configuration file
+ * this is needed to determine the initial load size
+ */
+static table_entry_t imximage_boot_loadsize[] = {
+	{FLASH_LOADSIZE_ONENAND,	"onenand",	"OneNAND Flash",},
+	{FLASH_LOADSIZE_NAND,		"nand",		"NAND Flash",	},
+	{FLASH_LOADSIZE_NOR,		"nor",		"NOR Flash",	},
+	{FLASH_LOADSIZE_SATA,		"sata",		"SATA Disk",	},
+	{FLASH_LOADSIZE_SD,		"sd",		"SD Card",	},
+	{FLASH_LOADSIZE_SPI,		"spi",		"SPI Flash",	},
+	{-1,				"",		"Invalid",	},
+};
+
+/*
  * IMXIMAGE version definition for i.MX chips
  */
 static table_entry_t imximage_versions[] = {
  
@@ -49,12 +66,22 @@
  
 static struct imx_header imximage_header;
 static uint32_t imximage_version;
+/*
+ * Image Vector Table Offset
+ * Initialized to a wrong not 4-bytes aligned address to
+ * check if it is was set by the cfg file.
+ */
+static uint32_t imximage_ivt_offset = UNDEFINED;
+static uint32_t imximage_csf_size = UNDEFINED;
+/* Initial Load Region Size */
+static uint32_t imximage_init_loadsize;
  
 static set_dcd_val_t set_dcd_val;
 static set_dcd_rst_t set_dcd_rst;
 static set_imx_hdr_t set_imx_hdr;
 static uint32_t max_dcd_entries;
 static uint32_t *header_size_ptr;
+static uint32_t *csf_ptr;
  
 static uint32_t get_cfg_value(char *token, char *name,  int linenr)
 {
@@ -190,7 +217,8 @@
 	/* Set magic number */
 	fhdr_v1->app_code_barker = APP_CODE_BARKER;
  
-	hdr_base = entry_point - sizeof(struct imx_header);
+	/* TODO: check i.MX image V1 handling, for now use 'old' style */
+	hdr_base = entry_point - 4096;
 	fhdr_v1->app_dest_ptr = hdr_base - flash_offset;
 	fhdr_v1->app_code_jump_vector = entry_point;
  
  
  
  
  
@@ -217,16 +245,18 @@
  
 	fhdr_v2->entry = entry_point;
 	fhdr_v2->reserved1 = fhdr_v2->reserved2 = 0;
-	fhdr_v2->self = hdr_base = entry_point - sizeof(struct imx_header);
-
+	hdr_base = entry_point - imximage_init_loadsize +
+		flash_offset;
+	fhdr_v2->self = hdr_base;
 	fhdr_v2->dcd_ptr = hdr_base + offsetof(imx_header_v2_t, dcd_table);
 	fhdr_v2->boot_data_ptr = hdr_base
 			+ offsetof(imx_header_v2_t, boot_data);
-	hdr_v2->boot_data.start = hdr_base - flash_offset;
+	hdr_v2->boot_data.start = entry_point - imximage_init_loadsize;
  
-	/* Security feature are not supported */
 	fhdr_v2->csf = 0;
+
 	header_size_ptr = &hdr_v2->boot_data.size;
+	csf_ptr = &fhdr_v2->csf;
 }
  
 static void set_hdr_func(struct imx_header *imxhdr)
@@ -303,6 +333,13 @@
 	genimg_print_size(hdr_v2->boot_data.size);
 	printf("Load Address: %08x\n", (uint32_t)fhdr_v2->boot_data_ptr);
 	printf("Entry Point:  %08x\n", (uint32_t)fhdr_v2->entry);
+	if (fhdr_v2->csf && (imximage_ivt_offset != UNDEFINED) &&
+	    (imximage_csf_size != UNDEFINED)) {
+		printf("HAB Blocks:   %08x %08x %08x\n",
+		       (uint32_t)fhdr_v2->self, 0,
+		       hdr_v2->boot_data.size - imximage_ivt_offset -
+		       imximage_csf_size);
+	}
 }
  
 static void parse_cfg_cmd(struct imx_header *imxhdr, int32_t cmd, char *token,
  
  
  
@@ -324,18 +361,36 @@
 		set_hdr_func(imxhdr);
 		break;
 	case CMD_BOOT_FROM:
-		imxhdr->flash_offset = get_table_entry_id(imximage_bootops,
+		imximage_ivt_offset = get_table_entry_id(imximage_boot_offset,
 					"imximage boot option", token);
-		if (imxhdr->flash_offset == -1) {
+		if (imximage_ivt_offset == -1) {
 			fprintf(stderr, "Error: %s[%d] -Invalid boot device"
 				"(%s)\n", name, lineno, token);
 			exit(EXIT_FAILURE);
 		}
+
+		imximage_init_loadsize =
+			get_table_entry_id(imximage_boot_loadsize,
+					   "imximage boot option", token);
+
+		if (imximage_init_loadsize == -1) {
+			fprintf(stderr,
+				"Error: %s[%d] -Invalid boot device(%s)\n",
+				name, lineno, token);
+			exit(EXIT_FAILURE);
+		}
+
+		/*
+		 * The SOC loads from the storage starting at address 0
+		 * then ensures that the load size contains the offset
+		 */
+		if (imximage_init_loadsize < imximage_ivt_offset)
+			imximage_init_loadsize = imximage_ivt_offset;
 		if (unlikely(cmd_ver_first != 1))
 			cmd_ver_first = 0;
 		break;
 	case CMD_BOOT_OFFSET:
-		imxhdr->flash_offset = get_cfg_value(token, name, lineno);
+		imximage_ivt_offset = get_cfg_value(token, name, lineno);
 		if (unlikely(cmd_ver_first != 1))
 			cmd_ver_first = 0;
 		break;
@@ -345,6 +400,17 @@
 		if (unlikely(cmd_ver_first != 1))
 			cmd_ver_first = 0;
 		break;
+	case CMD_CSF:
+		if (imximage_version != 2) {
+			fprintf(stderr,
+				"Error: %s[%d] - CSF only supported for VERSION 2(%s)\n",
+				name, lineno, token);
+			exit(EXIT_FAILURE);
+		}
+		imximage_csf_size = get_cfg_value(token, name, lineno);
+		if (unlikely(cmd_ver_first != 1))
+			cmd_ver_first = 0;
+		break;
 	}
 }
  
@@ -405,7 +471,8 @@
 		exit(EXIT_FAILURE);
 	}
  
-	/* Very simple parsing, line starting with # are comments
+	/*
+	 * Very simple parsing, line starting with # are comments
 	 * and are dropped
 	 */
 	while ((getline(&line, &len, fd)) > 0) {
@@ -436,7 +503,7 @@
 	fclose(fd);
  
 	/* Exit if there is no BOOT_FROM field specifying the flash_offset */
-	if (imxhdr->flash_offset == FLASH_OFFSET_UNDEFINED) {
+	if (imximage_ivt_offset == FLASH_OFFSET_UNDEFINED) {
 		fprintf(stderr, "Error: No BOOT_FROM tag in %s\n", name);
 		exit(EXIT_FAILURE);
 	}
  
@@ -494,14 +561,15 @@
 	 */
 	imximage_version = IMXIMAGE_V1;
 	/* Be able to detect if the cfg file has no BOOT_FROM tag */
-	imxhdr->flash_offset = FLASH_OFFSET_UNDEFINED;
+	imximage_ivt_offset = FLASH_OFFSET_UNDEFINED;
+	imximage_csf_size = 0;
 	set_hdr_func(imxhdr);
  
 	/* Parse dcd configuration file */
 	dcd_len = parse_cfg_file(imxhdr, params->imagename);
  
 	/* Set the imx header */
-	(*set_imx_hdr)(imxhdr, dcd_len, params->ep, imxhdr->flash_offset);
+	(*set_imx_hdr)(imxhdr, dcd_len, params->ep, imximage_ivt_offset);
  
 	/*
 	 * ROM bug alert
@@ -512,7 +580,13 @@
 	 *
 	 * The remaining fraction of a block bytes would not be loaded!
 	 */
-	*header_size_ptr = ROUND(sbuf->st_size + imxhdr->flash_offset, 4096);
+	*header_size_ptr = ROUND(sbuf->st_size, 4096);
+
+	if (csf_ptr && imximage_csf_size) {
+		*csf_ptr = params->ep - imximage_init_loadsize +
+			*header_size_ptr;
+		*header_size_ptr += imximage_csf_size;
+	}
 }
  
 int imximage_check_params(struct mkimage_params *params)
  
  
@@ -537,18 +611,92 @@
 		(params->xflag) || !(strlen(params->imagename));
 }
  
+static int imximage_generate(struct mkimage_params *params,
+	struct image_type_params *tparams)
+{
+	struct imx_header *imxhdr;
+	size_t alloc_len;
+	struct stat sbuf;
+	char *datafile = params->datafile;
+	uint32_t pad_len;
+
+	memset(&imximage_header, 0, sizeof(imximage_header));
+
+	/*
+	 * In order to not change the old imx cfg file
+	 * by adding VERSION command into it, here need
+	 * set up function ptr group to V1 by default.
+	 */
+	imximage_version = IMXIMAGE_V1;
+	/* Be able to detect if the cfg file has no BOOT_FROM tag */
+	imximage_ivt_offset = FLASH_OFFSET_UNDEFINED;
+	imximage_csf_size = 0;
+	set_hdr_func(imxhdr);
+
+	/* Parse dcd configuration file */
+	parse_cfg_file(&imximage_header, params->imagename);
+
+	/* TODO: check i.MX image V1 handling, for now use 'old' style */
+	if (imximage_version == IMXIMAGE_V1) {
+		alloc_len = 4096;
+	} else {
+		if (imximage_init_loadsize < imximage_ivt_offset +
+			sizeof(imx_header_v2_t))
+				imximage_init_loadsize = imximage_ivt_offset +
+					sizeof(imx_header_v2_t);
+		alloc_len = imximage_init_loadsize - imximage_ivt_offset;
+	}
+
+	if (alloc_len < sizeof(struct imx_header)) {
+		fprintf(stderr, "%s: header error\n",
+			params->cmdname);
+		exit(EXIT_FAILURE);
+	}
+
+	imxhdr = malloc(alloc_len);
+
+	if (!imxhdr) {
+		fprintf(stderr, "%s: malloc return failure: %s\n",
+			params->cmdname, strerror(errno));
+		exit(EXIT_FAILURE);
+	}
+
+	memset(imxhdr, 0, alloc_len);
+
+	tparams->header_size = alloc_len;
+	tparams->hdr         = imxhdr;
+
+	/* determine data image file length */
+
+	if (stat(datafile, &sbuf) < 0) {
+		fprintf(stderr, "%s: Can't stat %s: %s\n",
+			params->cmdname, datafile, strerror(errno));
+		exit(EXIT_FAILURE);
+	}
+
+	pad_len = ROUND(sbuf.st_size, 4096) - sbuf.st_size;
+
+	/* TODO: check i.MX image V1 handling, for now use 'old' style */
+	if (imximage_version == IMXIMAGE_V1)
+		return 0;
+	else
+		return pad_len;
+}
+
+
 /*
  * imximage parameters
  */
 static struct image_type_params imximage_params = {
 	.name		= "Freescale i.MX Boot Image support",
-	.header_size	= sizeof(struct imx_header),
-	.hdr		= (void *)&imximage_header,
+	.header_size	= 0,
+	.hdr		= NULL,
 	.check_image_type = imximage_check_image_types,
 	.verify_header	= imximage_verify_header,
 	.print_header	= imximage_print_header,
 	.set_header	= imximage_set_header,
 	.check_params	= imximage_check_params,
+	.vrec_header	= imximage_generate,
 };
  
 void init_imx_image_type(void)
@@ -13,14 +13,14 @@
 #define APP_CODE_BARKER	0xB1
 #define DCD_BARKER	0xB17219E9
  
-#define HEADER_OFFSET	0x400
-
 /*
  * NOTE: This file must be kept in sync with arch/arm/include/asm/\
  *       imx-common/imximage.cfg because tools/imximage.c can not
  *       cross-include headers from arch/arm/ and vice-versa.
  */
 #define CMD_DATA_STR	"DATA"
+
+/* Initial Vector Table Offset */
 #define FLASH_OFFSET_UNDEFINED	0xFFFFFFFF
 #define FLASH_OFFSET_STANDARD	0x400
 #define FLASH_OFFSET_NAND	FLASH_OFFSET_STANDARD
@@ -30,6 +30,16 @@
 #define FLASH_OFFSET_NOR	0x1000
 #define FLASH_OFFSET_SATA	FLASH_OFFSET_STANDARD
  
+/* Initial Load Region Size */
+#define FLASH_LOADSIZE_UNDEFINED	0xFFFFFFFF
+#define FLASH_LOADSIZE_STANDARD		0x1000
+#define FLASH_LOADSIZE_NAND		FLASH_LOADSIZE_STANDARD
+#define FLASH_LOADSIZE_SD		FLASH_LOADSIZE_STANDARD
+#define FLASH_LOADSIZE_SPI		FLASH_LOADSIZE_STANDARD
+#define FLASH_LOADSIZE_ONENAND		0x400
+#define FLASH_LOADSIZE_NOR		0x0 /* entire image */
+#define FLASH_LOADSIZE_SATA		FLASH_LOADSIZE_STANDARD
+
 #define IVT_HEADER_TAG 0xD1
 #define IVT_VERSION 0x40
 #define DCD_HEADER_TAG 0xD2
@@ -42,7 +52,8 @@
 	CMD_IMAGE_VERSION,
 	CMD_BOOT_FROM,
 	CMD_BOOT_OFFSET,
-	CMD_DATA
+	CMD_DATA,
+	CMD_CSF,
 };
  
 enum imximage_fld_types {
@@ -147,8 +158,7 @@
 		imx_header_v1_t hdr_v1;
 		imx_header_v2_t hdr_v2;
 	} header;
-	uint32_t flash_offset;
-} __attribute__((aligned(4096)));
+};
  
 typedef void (*set_dcd_val_t)(struct imx_header *imxhdr,
 					char *name, int lineno,
@@ -137,6 +137,7 @@
 	char *ptr;
 	int retval = 0;
 	struct image_type_params *tparams = NULL;
+	int pad_len = 0;
  
 	/* Init Freescale PBL Boot image generation/list support */
 	init_pbl_image_type();
@@ -144,6 +145,8 @@
 	init_kwb_image_type ();
 	/* Init Freescale imx Boot image generation/list support */
 	init_imx_image_type ();
+	/* Init Freescale mxs Boot image generation/list support */
+	init_mxs_image_type();
 	/* Init FIT image generation/list support */
 	init_fit_image_type ();
 	/* Init TI OMAP Boot image generation/list support */
@@ -391,7 +394,7 @@
 	 * allocate memory for the header itself.
 	 */
 	if (tparams->vrec_header)
-		tparams->vrec_header(&params, tparams);
+		pad_len = tparams->vrec_header(&params, tparams);
 	else
 		memset(tparams->hdr, 0, tparams->header_size);
  
@@ -463,7 +466,7 @@
 			/* PBL has special Image format, implements its' own */
 			pbl_load_uboot(ifd, &params);
 		} else {
-			copy_file (ifd, params.datafile, 0);
+			copy_file(ifd, params.datafile, pad_len);
 		}
 	}
  
  
@@ -537,10 +540,19 @@
 	unsigned char *ptr;
 	int tail;
 	int zero = 0;
+	uint8_t zeros[4096];
 	int offset = 0;
 	int size;
 	struct image_type_params *tparams = mkimage_get_type (params.type);
  
+	if (pad >= sizeof(zeros)) {
+		fprintf(stderr, "%s: Can't pad to %d\n",
+			params.cmdname, pad);
+		exit(EXIT_FAILURE);
+	}
+
+	memset(zeros, 0, sizeof(zeros));
+
 	if (params.vflag) {
 		fprintf (stderr, "Adding Image %s\n", datafile);
 	}
  
@@ -598,13 +610,21 @@
 		exit (EXIT_FAILURE);
 	}
  
-	if (pad && ((tail = size % 4) != 0)) {
+	tail = size % 4;
+	if ((pad == 1) && (tail != 0)) {
  
 		if (write(ifd, (char *)&zero, 4-tail) != 4-tail) {
 			fprintf (stderr, "%s: Write error on %s: %s\n",
 				params.cmdname, params.imagefile,
 				strerror(errno));
 			exit (EXIT_FAILURE);
+		}
+	} else if (pad > 1) {
+		if (write(ifd, (char *)&zeros, pad) != pad) {
+			fprintf(stderr, "%s: Write error on %s: %s\n",
+				params.cmdname, params.imagefile,
+				strerror(errno));
+			exit(EXIT_FAILURE);
 		}
 	}
  
@@ -132,7 +132,10 @@
 	/*
 	 * This callback function will be executed for variable size record
 	 * It is expected to build this header in memory and return its length
-	 * and a pointer to it
+	 * and a pointer to it by using image_type_params.header_size and
+	 * image_type_params.hdr. The return value shall indicate if an
+	 * additional padding should be used when copying the data image
+	 * by returning the padding length.
 	 */
 	int (*vrec_header) (struct mkimage_params *,
 		struct image_type_params *);
@@ -158,6 +161,7 @@
 void init_ais_image_type(void);
 void init_kwb_image_type (void);
 void init_imx_image_type (void);
+void init_mxs_image_type(void);
 void init_default_image_type (void);
 void init_fit_image_type (void);
 void init_ubl_image_type(void);
@@ -28,14 +28,14 @@
  *
  * TWEAK this if you have different kind of NAND chip.
  */
-uint32_t nand_writesize = 2048;
-uint32_t nand_oobsize = 64;
-uint32_t nand_erasesize = 128 * 1024;
+static uint32_t nand_writesize = 2048;
+static uint32_t nand_oobsize = 64;
+static uint32_t nand_erasesize = 128 * 1024;
  
 /*
  * Sector on which the SigmaTel boot partition (0x53) starts.
  */
-uint32_t sd_sector = 2048;
+static uint32_t sd_sector = 2048;
  
 /*
  * Each of the U-Boot bootstreams is at maximum 1MB big.
@@ -434,7 +434,7 @@
 	return 0;
 }
  
-void usage(void)
+static void usage(void)
 {
 	printf(
 		"Usage: mxsboot [ops] <type> <infile> <outfile>\n"
@@ -575,7 +575,7 @@
 	return ret;
 }
  
-int parse_ops(int argc, char **argv)
+static int parse_ops(int argc, char **argv)
 {
 	int i;
 	int tmp;
+/*
+ * Freescale i.MX23/i.MX28 SB image generator
+ *
+ * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifdef CONFIG_MXS
+
+#include <errno.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <limits.h>
+
+#include <openssl/evp.h>
+
+#include "mkimage.h"
+#include "mxsimage.h"
+#include <image.h>
+
+
+/*
+ * DCD block
+ * |-Write to address command block
+ * |  0xf00 == 0xf33d
+ * |  0xba2 == 0xb33f
+ * |-ORR address with mask command block
+ * |  0xf00 |= 0x1337
+ * |-Write to address command block
+ * |  0xba2 == 0xd00d
+ * :
+ */
+#define SB_HAB_DCD_WRITE	0xccUL
+#define SB_HAB_DCD_CHECK	0xcfUL
+#define SB_HAB_DCD_NOOP		0xc0UL
+#define SB_HAB_DCD_MASK_BIT	(1 << 3)
+#define SB_HAB_DCD_SET_BIT	(1 << 4)
+
+/* Addr.n = Value.n */
+#define	SB_DCD_WRITE	\
+	(SB_HAB_DCD_WRITE << 24)
+/* Addr.n &= ~Value.n */
+#define	SB_DCD_ANDC	\
+	((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT)
+/* Addr.n |= Value.n */
+#define	SB_DCD_ORR	\
+	((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
+/* (Addr.n & Value.n) == 0 */
+#define	SB_DCD_CHK_EQZ	\
+	(SB_HAB_DCD_CHECK << 24)
+/* (Addr.n & Value.n) == Value.n */
+#define	SB_DCD_CHK_EQ	\
+	((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT)
+/* (Addr.n & Value.n) != Value.n */
+#define	SB_DCD_CHK_NEQ	\
+	((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT)
+/* (Addr.n & Value.n) != 0 */
+#define	SB_DCD_CHK_NEZ	\
+	((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
+/* NOP */
+#define	SB_DCD_NOOP	\
+	(SB_HAB_DCD_NOOP << 24)
+
+struct sb_dcd_ctx {
+	struct sb_dcd_ctx		*dcd;
+
+	uint32_t			id;
+
+	/* The DCD block. */
+	uint32_t			*payload;
+	/* Size of the whole DCD block. */
+	uint32_t			size;
+
+	/* Pointer to previous DCD command block. */
+	uint32_t			*prev_dcd_head;
+};
+
+/*
+ * IMAGE
+ *   |-SECTION
+ *   |    |-CMD
+ *   |    |-CMD
+ *   |    `-CMD
+ *   |-SECTION
+ *   |    |-CMD
+ *   :    :
+ */
+struct sb_cmd_list {
+	char				*cmd;
+	size_t				len;
+	unsigned int			lineno;
+};
+
+struct sb_cmd_ctx {
+	uint32_t			size;
+
+	struct sb_cmd_ctx		*cmd;
+
+	uint8_t				*data;
+	uint32_t			length;
+
+	struct sb_command		payload;
+	struct sb_command		c_payload;
+};
+
+struct sb_section_ctx {
+	uint32_t			size;
+
+	/* Section flags */
+	unsigned int			boot:1;
+
+	struct sb_section_ctx		*sect;
+
+	struct sb_cmd_ctx		*cmd_head;
+	struct sb_cmd_ctx		*cmd_tail;
+
+	struct sb_sections_header	payload;
+};
+
+struct sb_image_ctx {
+	unsigned int			in_section:1;
+	unsigned int			in_dcd:1;
+	/* Image configuration */
+	unsigned int			verbose_boot:1;
+	unsigned int			silent_dump:1;
+	char				*input_filename;
+	char				*output_filename;
+	char				*cfg_filename;
+	uint8_t				image_key[16];
+
+	/* Number of section in the image */
+	unsigned int			sect_count;
+	/* Bootable section */
+	unsigned int			sect_boot;
+	unsigned int			sect_boot_found:1;
+
+	struct sb_section_ctx		*sect_head;
+	struct sb_section_ctx		*sect_tail;
+
+	struct sb_dcd_ctx		*dcd_head;
+	struct sb_dcd_ctx		*dcd_tail;
+
+	EVP_CIPHER_CTX			cipher_ctx;
+	EVP_MD_CTX			md_ctx;
+	uint8_t				digest[32];
+	struct sb_key_dictionary_key	sb_dict_key;
+
+	struct sb_boot_image_header	payload;
+};
+
+/*
+ * Instruction semantics:
+ * NOOP
+ * TAG [LAST]
+ * LOAD       address file
+ * LOAD  IVT  address IVT_entry_point
+ * FILL address pattern length
+ * JUMP [HAB] address [r0_arg]
+ * CALL [HAB] address [r0_arg]
+ * MODE mode
+ *      For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
+ *                         JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
+ *      For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
+ *                         JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
+ */
+
+/*
+ * AES libcrypto
+ */
+static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
+{
+	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
+	int ret;
+
+	/* If there is no init vector, init vector is all zeroes. */
+	if (!iv)
+		iv = ictx->image_key;
+
+	EVP_CIPHER_CTX_init(ctx);
+	ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc);
+	if (ret == 1)
+		EVP_CIPHER_CTX_set_padding(ctx, 0);
+	return ret;
+}
+
+static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
+			uint8_t *out_data, int in_len)
+{
+	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
+	int ret, outlen;
+	uint8_t *outbuf;
+
+	outbuf = malloc(in_len);
+	if (!outbuf)
+		return -ENOMEM;
+	memset(outbuf, 0, sizeof(in_len));
+
+	ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
+	if (!ret) {
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (out_data)
+		memcpy(out_data, outbuf, outlen);
+
+err:
+	free(outbuf);
+	return ret;
+}
+
+static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
+{
+	return EVP_CIPHER_CTX_cleanup(ctx);
+}
+
+static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
+{
+	int ret;
+	EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
+	struct sb_boot_image_header *sb_header = &ictx->payload;
+	uint8_t *iv = sb_header->iv;
+
+	ret = sb_aes_deinit(ctx);
+	if (!ret)
+		return ret;
+	return sb_aes_init(ictx, iv, enc);
+}
+
+/*
+ * CRC32
+ */
+static uint32_t crc32(uint8_t *data, uint32_t len)
+{
+	const uint32_t poly = 0x04c11db7;
+	uint32_t crc32 = 0xffffffff;
+	unsigned int byte, bit;
+
+	for (byte = 0; byte < len; byte++) {
+		crc32 ^= data[byte] << 24;
+
+		for (bit = 8; bit > 0; bit--) {
+			if (crc32 & (1UL << 31))
+				crc32 = (crc32 << 1) ^ poly;
+			else
+				crc32 = (crc32 << 1);
+		}
+	}
+
+	return crc32;
+}
+
+/*
+ * Debug
+ */
+static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...)
+{
+	va_list ap;
+
+	if (ictx->silent_dump)
+		return;
+
+	va_start(ap, fmt);
+	vfprintf(stdout, fmt, ap);
+	va_end(ap);
+}
+
+/*
+ * Code
+ */
+static time_t sb_get_timestamp(void)
+{
+	struct tm time_2000 = {
+		.tm_yday	= 1,	/* Jan. 1st */
+		.tm_year	= 100,	/* 2000 */
+	};
+	time_t seconds_to_2000 = mktime(&time_2000);
+	time_t seconds_to_now = time(NULL);
+
+	return seconds_to_now - seconds_to_2000;
+}
+
+static int sb_get_time(time_t time, struct tm *tm)
+{
+	struct tm time_2000 = {
+		.tm_yday	= 1,	/* Jan. 1st */
+		.tm_year	= 0,	/* 1900 */
+	};
+	const time_t seconds_to_2000 = mktime(&time_2000);
+	const time_t seconds_to_now = seconds_to_2000 + time;
+	struct tm *ret;
+	ret = gmtime_r(&seconds_to_now, tm);
+	return ret ? 0 : -EINVAL;
+}
+
+static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
+{
+	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
+	struct sb_boot_image_header *sb_header = &ictx->payload;
+	uint8_t *sb_header_ptr = (uint8_t *)sb_header;
+
+	/* Encrypt the header, compute the digest. */
+	sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
+	EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
+}
+
+static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx)
+{
+	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
+	struct sb_section_ctx *sctx = ictx->sect_head;
+	struct sb_sections_header *shdr;
+	uint8_t *sb_sections_header_ptr;
+	const int size = sizeof(*shdr);
+
+	while (sctx) {
+		shdr = &sctx->payload;
+		sb_sections_header_ptr = (uint8_t *)shdr;
+
+		sb_aes_crypt(ictx, sb_sections_header_ptr,
+			     ictx->sb_dict_key.cbc_mac, size);
+		EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
+
+		sctx = sctx->sect;
+	};
+}
+
+static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx)
+{
+	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
+
+	sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key,
+		     sizeof(ictx->sb_dict_key.key));
+	EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
+}
+
+static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx)
+{
+	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
+
+	EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
+	sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key,
+		     sizeof(ictx->sb_dict_key.key));
+}
+
+static void sb_encrypt_tag(struct sb_image_ctx *ictx,
+		struct sb_cmd_ctx *cctx)
+{
+	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
+	struct sb_command *cmd = &cctx->payload;
+
+	sb_aes_crypt(ictx, (uint8_t *)cmd,
+		     (uint8_t *)&cctx->c_payload, sizeof(*cmd));
+	EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
+}
+
+static int sb_encrypt_image(struct sb_image_ctx *ictx)
+{
+	/* Start image-wide crypto. */
+	EVP_MD_CTX_init(&ictx->md_ctx);
+	EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
+
+	/*
+	 * SB image header.
+	 */
+	sb_aes_init(ictx, NULL, 1);
+	sb_encrypt_sb_header(ictx);
+
+	/*
+	 * SB sections header.
+	 */
+	sb_encrypt_sb_sections_header(ictx);
+
+	/*
+	 * Key dictionary.
+	 */
+	sb_aes_reinit(ictx, 1);
+	sb_encrypt_key_dictionary_key(ictx);
+
+	/*
+	 * Section tags.
+	 */
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	struct sb_section_ctx *sctx = ictx->sect_head;
+
+	while (sctx) {
+		cctx = sctx->cmd_head;
+
+		sb_aes_reinit(ictx, 1);
+
+		while (cctx) {
+			ccmd = &cctx->payload;
+
+			sb_encrypt_tag(ictx, cctx);
+
+			if (ccmd->header.tag == ROM_TAG_CMD) {
+				sb_aes_reinit(ictx, 1);
+			} else if (ccmd->header.tag == ROM_LOAD_CMD) {
+				sb_aes_crypt(ictx, cctx->data, cctx->data,
+					     cctx->length);
+				EVP_DigestUpdate(&ictx->md_ctx, cctx->data,
+						 cctx->length);
+			}
+
+			cctx = cctx->cmd;
+		}
+
+		sctx = sctx->sect;
+	};
+
+	/*
+	 * Dump the SHA1 of the whole image.
+	 */
+	sb_aes_reinit(ictx, 1);
+
+	EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
+	sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
+
+	/* Stop the encryption session. */
+	sb_aes_deinit(&ictx->cipher_ctx);
+
+	return 0;
+}
+
+static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
+{
+	long real_size, roundup_size;
+	uint8_t *data;
+	long ret;
+	unsigned long size;
+	FILE *fp;
+
+	if (!filename) {
+		fprintf(stderr, "ERR: Missing filename!\n");
+		return -EINVAL;
+	}
+
+	fp = fopen(filename, "r");
+	if (!fp)
+		goto err_open;
+
+	ret = fseek(fp, 0, SEEK_END);
+	if (ret < 0)
+		goto err_file;
+
+	real_size = ftell(fp);
+	if (real_size < 0)
+		goto err_file;
+
+	ret = fseek(fp, 0, SEEK_SET);
+	if (ret < 0)
+		goto err_file;
+
+	roundup_size = roundup(real_size, SB_BLOCK_SIZE);
+	data = calloc(1, roundup_size);
+	if (!data)
+		goto err_file;
+
+	size = fread(data, 1, real_size, fp);
+	if (size != (unsigned long)real_size)
+		goto err_alloc;
+
+	cctx->data = data;
+	cctx->length = roundup_size;
+
+	fclose(fp);
+	return 0;
+
+err_alloc:
+	free(data);
+err_file:
+	fclose(fp);
+err_open:
+	fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
+	return -EINVAL;
+}
+
+static uint8_t sb_command_checksum(struct sb_command *inst)
+{
+	uint8_t *inst_ptr = (uint8_t *)inst;
+	uint8_t csum = 0;
+	unsigned int i;
+
+	for (i = 0; i < sizeof(struct sb_command); i++)
+		csum += inst_ptr[i];
+
+	return csum;
+}
+
+static int sb_token_to_long(char *tok, uint32_t *rid)
+{
+	char *endptr;
+	unsigned long id;
+
+	if (tok[0] != '0' || tok[1] != 'x') {
+		fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
+		return -EINVAL;
+	}
+
+	tok += 2;
+
+	id = strtoul(tok, &endptr, 16);
+	if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
+		fprintf(stderr, "ERR: Value can't be decoded!\n");
+		return -EINVAL;
+	}
+
+	/* Check for 32-bit overflow. */
+	if (id > 0xffffffff) {
+		fprintf(stderr, "ERR: Value too big!\n");
+		return -EINVAL;
+	}
+
+	if (endptr == tok) {
+		fprintf(stderr, "ERR: Deformed value!\n");
+		return -EINVAL;
+	}
+
+	*rid = (uint32_t)id;
+	return 0;
+}
+
+static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size)
+{
+	uint32_t *tmp;
+
+	if (!inc_size)
+		return 0;
+
+	dctx->size += inc_size;
+	tmp = realloc(dctx->payload, dctx->size);
+	if (!tmp)
+		return -ENOMEM;
+
+	dctx->payload = tmp;
+
+	/* Assemble and update the HAB DCD header. */
+	dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) |
+				 (dctx->size << 8) |
+				 SB_HAB_VERSION);
+
+	return 0;
+}
+
+static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
+{
+	struct sb_dcd_ctx *dctx;
+
+	char *tok;
+	uint32_t id;
+	int ret;
+
+	dctx = calloc(1, sizeof(*dctx));
+	if (!dctx)
+		return -ENOMEM;
+
+	ret = sb_grow_dcd(dctx, 4);
+	if (ret)
+		goto err_dcd;
+
+	/* Read DCD block number. */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: DCD block without number!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err_dcd;
+	}
+
+	/* Parse the DCD block number. */
+	ret = sb_token_to_long(tok, &id);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Malformed DCD block number!\n",
+			cmd->lineno);
+		goto err_dcd;
+	}
+
+	dctx->id = id;
+
+	/*
+	 * The DCD block is now constructed. Append it to the list.
+	 * WARNING: The DCD size is still not computed and will be
+	 * updated while parsing it's commands.
+	 */
+	if (!ictx->dcd_head) {
+		ictx->dcd_head = dctx;
+		ictx->dcd_tail = dctx;
+	} else {
+		ictx->dcd_tail->dcd = dctx;
+		ictx->dcd_tail = dctx;
+	}
+
+	return 0;
+
+err_dcd:
+	free(dctx->payload);
+	free(dctx);
+	return ret;
+}
+
+static int sb_build_dcd_block(struct sb_image_ctx *ictx,
+			      struct sb_cmd_list *cmd,
+			      uint32_t type)
+{
+	char *tok;
+	uint32_t address, value, length;
+	int ret;
+
+	struct sb_dcd_ctx *dctx = ictx->dcd_tail;
+	uint32_t *dcd;
+
+	if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) &&
+	    ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) {
+		/* Same instruction as before, just append it. */
+		ret = sb_grow_dcd(dctx, 8);
+		if (ret)
+			return ret;
+	} else if (type == SB_DCD_NOOP) {
+		ret = sb_grow_dcd(dctx, 4);
+		if (ret)
+			return ret;
+
+		/* Update DCD command block pointer. */
+		dctx->prev_dcd_head = dctx->payload +
+				dctx->size / sizeof(*dctx->payload) - 1;
+
+		/* NOOP has only 4 bytes and no payload. */
+		goto noop;
+	} else {
+		/*
+		 * Either a different instruction block started now
+		 * or this is the first instruction block.
+		 */
+		ret = sb_grow_dcd(dctx, 12);
+		if (ret)
+			return ret;
+
+		/* Update DCD command block pointer. */
+		dctx->prev_dcd_head = dctx->payload +
+				dctx->size / sizeof(*dctx->payload) - 3;
+	}
+
+	dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2;
+
+	/*
+	 * Prepare the command.
+	 */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing DCD address!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Read DCD destination address. */
+	ret = sb_token_to_long(tok, &address);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect DCD address!\n",
+			cmd->lineno);
+		goto err;
+	}
+
+	tok = strtok(NULL, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing DCD value!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Read DCD operation value. */
+	ret = sb_token_to_long(tok, &value);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect DCD value!\n",
+			cmd->lineno);
+		goto err;
+	}
+
+	/* Fill in the new DCD entry. */
+	dcd[0] = htonl(address);
+	dcd[1] = htonl(value);
+
+noop:
+	/* Update the DCD command block. */
+	length = dctx->size -
+		 ((dctx->prev_dcd_head - dctx->payload) *
+		 sizeof(*dctx->payload));
+	dctx->prev_dcd_head[0] = htonl(type | (length << 8));
+
+err:
+	return ret;
+}
+
+static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
+{
+	struct sb_section_ctx *sctx;
+	struct sb_sections_header *shdr;
+	char *tok;
+	uint32_t bootable = 0;
+	uint32_t id;
+	int ret;
+
+	sctx = calloc(1, sizeof(*sctx));
+	if (!sctx)
+		return -ENOMEM;
+
+	/* Read section number. */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Section without number!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err_sect;
+	}
+
+	/* Parse the section number. */
+	ret = sb_token_to_long(tok, &id);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Malformed section number!\n",
+			cmd->lineno);
+		goto err_sect;
+	}
+
+	/* Read section's BOOTABLE flag. */
+	tok = strtok(NULL, " ");
+	if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
+		bootable = SB_SECTION_FLAG_BOOTABLE;
+
+	sctx->boot = bootable;
+
+	shdr = &sctx->payload;
+	shdr->section_number = id;
+	shdr->section_flags = bootable;
+
+	/*
+	 * The section is now constructed. Append it to the list.
+	 * WARNING: The section size is still not computed and will
+	 * be updated while parsing it's commands.
+	 */
+	ictx->sect_count++;
+
+	/* Mark that this section is bootable one. */
+	if (bootable) {
+		if (ictx->sect_boot_found) {
+			fprintf(stderr,
+				"#%i WARN: Multiple bootable section!\n",
+				cmd->lineno);
+		} else {
+			ictx->sect_boot = id;
+			ictx->sect_boot_found = 1;
+		}
+	}
+
+	if (!ictx->sect_head) {
+		ictx->sect_head = sctx;
+		ictx->sect_tail = sctx;
+	} else {
+		ictx->sect_tail->sect = sctx;
+		ictx->sect_tail = sctx;
+	}
+
+	return 0;
+
+err_sect:
+	free(sctx);
+	return ret;
+}
+
+static int sb_build_command_nop(struct sb_image_ctx *ictx)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= ROM_NOP_CMD;
+
+	cctx->size = sizeof(*ccmd);
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+}
+
+static int sb_build_command_tag(struct sb_image_ctx *ictx,
+				struct sb_cmd_list *cmd)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	char *tok;
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Prepare the command.
+	 */
+	/* Check for the LAST keyword. */
+	tok = strtok(cmd->cmd, " ");
+	if (tok && !strcmp(tok, "LAST"))
+		ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= ROM_TAG_CMD;
+
+	cctx->size = sizeof(*ccmd);
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+}
+
+static int sb_build_command_load(struct sb_image_ctx *ictx,
+				 struct sb_cmd_list *cmd)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	char *tok;
+	int ret, is_ivt = 0, is_dcd = 0;
+	uint32_t dest, dcd = 0;
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Prepare the command.
+	 */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Check for "IVT" flag. */
+	if (!strcmp(tok, "IVT"))
+		is_ivt = 1;
+	if (!strcmp(tok, "DCD"))
+		is_dcd = 1;
+	if (is_ivt || is_dcd) {
+		tok = strtok(NULL, " ");
+		if (!tok) {
+			fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
+				cmd->lineno);
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+
+	/* Read load destination address. */
+	ret = sb_token_to_long(tok, &dest);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
+			cmd->lineno);
+		goto err;
+	}
+
+	/* Read filename or IVT entrypoint or DCD block ID. */
+	tok = strtok(NULL, " ");
+	if (!tok) {
+		fprintf(stderr,
+			"#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	if (is_ivt) {
+		/* Handle IVT. */
+		struct sb_ivt_header *ivt;
+		uint32_t ivtep;
+		ret = sb_token_to_long(tok, &ivtep);
+
+		if (ret) {
+			fprintf(stderr,
+				"#%i ERR: Incorrect IVT entry point!\n",
+				cmd->lineno);
+			goto err;
+		}
+
+		ivt = calloc(1, sizeof(*ivt));
+		if (!ivt) {
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		ivt->header = sb_hab_ivt_header();
+		ivt->entry = ivtep;
+		ivt->self = dest;
+
+		cctx->data = (uint8_t *)ivt;
+		cctx->length = sizeof(*ivt);
+	} else if (is_dcd) {
+		struct sb_dcd_ctx *dctx = ictx->dcd_head;
+		uint32_t dcdid;
+		uint8_t *payload;
+		uint32_t asize;
+		ret = sb_token_to_long(tok, &dcdid);
+
+		if (ret) {
+			fprintf(stderr,
+				"#%i ERR: Incorrect DCD block ID!\n",
+				cmd->lineno);
+			goto err;
+		}
+
+		while (dctx) {
+			if (dctx->id == dcdid)
+				break;
+			dctx = dctx->dcd;
+		}
+
+		if (!dctx) {
+			fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
+				cmd->lineno, dcdid);
+			goto err;
+		}
+
+		asize = roundup(dctx->size, SB_BLOCK_SIZE);
+		payload = calloc(1, asize);
+		if (!payload) {
+			ret = -ENOMEM;
+			goto err;
+		}
+
+		memcpy(payload, dctx->payload, dctx->size);
+
+		cctx->data = payload;
+		cctx->length = asize;
+
+		/* Set the Load DCD flag. */
+		dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
+	} else {
+		/* Regular LOAD of a file. */
+		ret = sb_load_file(cctx, tok);
+		if (ret) {
+			fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
+				cmd->lineno, tok);
+			goto err;
+		}
+	}
+
+	if (cctx->length & (SB_BLOCK_SIZE - 1)) {
+		fprintf(stderr, "#%i ERR: Unaligned payload!\n",
+			cmd->lineno);
+	}
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= ROM_LOAD_CMD;
+	ccmd->header.flags	= dcd;
+
+	ccmd->load.address	= dest;
+	ccmd->load.count	= cctx->length;
+	ccmd->load.crc32	= crc32(cctx->data, cctx->length);
+
+	cctx->size = sizeof(*ccmd) + cctx->length;
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+
+err:
+	free(cctx);
+	return ret;
+}
+
+static int sb_build_command_fill(struct sb_image_ctx *ictx,
+				 struct sb_cmd_list *cmd)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	char *tok;
+	uint32_t address, pattern, length;
+	int ret;
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Prepare the command.
+	 */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing FILL address!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Read fill destination address. */
+	ret = sb_token_to_long(tok, &address);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect FILL address!\n",
+			cmd->lineno);
+		goto err;
+	}
+
+	tok = strtok(NULL, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing FILL pattern!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Read fill pattern address. */
+	ret = sb_token_to_long(tok, &pattern);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n",
+			cmd->lineno);
+		goto err;
+	}
+
+	tok = strtok(NULL, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing FILL length!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Read fill pattern address. */
+	ret = sb_token_to_long(tok, &length);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect FILL length!\n",
+			cmd->lineno);
+		goto err;
+	}
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= ROM_FILL_CMD;
+
+	ccmd->fill.address	= address;
+	ccmd->fill.count	= length;
+	ccmd->fill.pattern	= pattern;
+
+	cctx->size = sizeof(*ccmd);
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+
+err:
+	free(cctx);
+	return ret;
+}
+
+static int sb_build_command_jump_call(struct sb_image_ctx *ictx,
+				      struct sb_cmd_list *cmd,
+				      unsigned int is_call)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	char *tok;
+	uint32_t dest, arg = 0x0;
+	uint32_t hab = 0;
+	int ret;
+	const char *cmdname = is_call ? "CALL" : "JUMP";
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Prepare the command.
+	 */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr,
+			"#%i ERR: Missing %s address or 'HAB'!\n",
+			cmd->lineno, cmdname);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/* Check for "HAB" flag. */
+	if (!strcmp(tok, "HAB")) {
+		hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB;
+		tok = strtok(NULL, " ");
+		if (!tok) {
+			fprintf(stderr, "#%i ERR: Missing %s address!\n",
+				cmd->lineno, cmdname);
+			ret = -EINVAL;
+			goto err;
+		}
+	}
+	/* Read load destination address. */
+	ret = sb_token_to_long(tok, &dest);
+	if (ret) {
+		fprintf(stderr, "#%i ERR: Incorrect %s address!\n",
+			cmd->lineno, cmdname);
+		goto err;
+	}
+
+	tok = strtok(NULL, " ");
+	if (tok) {
+		ret = sb_token_to_long(tok, &arg);
+		if (ret) {
+			fprintf(stderr,
+				"#%i ERR: Incorrect %s argument!\n",
+				cmd->lineno, cmdname);
+			goto err;
+		}
+	}
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= is_call ? ROM_CALL_CMD : ROM_JUMP_CMD;
+	ccmd->header.flags	= hab;
+
+	ccmd->call.address	= dest;
+	ccmd->call.argument	= arg;
+
+	cctx->size = sizeof(*ccmd);
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+
+err:
+	free(cctx);
+	return ret;
+}
+
+static int sb_build_command_jump(struct sb_image_ctx *ictx,
+				 struct sb_cmd_list *cmd)
+{
+	return sb_build_command_jump_call(ictx, cmd, 0);
+}
+
+static int sb_build_command_call(struct sb_image_ctx *ictx,
+				 struct sb_cmd_list *cmd)
+{
+	return sb_build_command_jump_call(ictx, cmd, 1);
+}
+
+static int sb_build_command_mode(struct sb_image_ctx *ictx,
+				 struct sb_cmd_list *cmd)
+{
+	struct sb_section_ctx *sctx = ictx->sect_tail;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	char *tok;
+	int ret;
+	unsigned int i;
+	uint32_t mode = 0xffffffff;
+
+	cctx = calloc(1, sizeof(*cctx));
+	if (!cctx)
+		return -ENOMEM;
+
+	ccmd = &cctx->payload;
+
+	/*
+	 * Prepare the command.
+	 */
+	tok = strtok(cmd->cmd, " ");
+	if (!tok) {
+		fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(modetable); i++) {
+		if (!strcmp(tok, modetable[i].name)) {
+			mode = modetable[i].mode;
+			break;
+		}
+
+		if (!modetable[i].altname)
+			continue;
+
+		if (!strcmp(tok, modetable[i].altname)) {
+			mode = modetable[i].mode;
+			break;
+		}
+	}
+
+	if (mode == 0xffffffff) {
+		fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
+			cmd->lineno);
+		ret = -EINVAL;
+		goto err;
+	}
+
+	/*
+	 * Construct the command.
+	 */
+	ccmd->header.checksum	= 0x5a;
+	ccmd->header.tag	= ROM_MODE_CMD;
+
+	ccmd->mode.mode		= mode;
+
+	cctx->size = sizeof(*ccmd);
+
+	/*
+	 * Append the command to the last section.
+	 */
+	if (!sctx->cmd_head) {
+		sctx->cmd_head = cctx;
+		sctx->cmd_tail = cctx;
+	} else {
+		sctx->cmd_tail->cmd = cctx;
+		sctx->cmd_tail = cctx;
+	}
+
+	return 0;
+
+err:
+	free(cctx);
+	return ret;
+}
+
+static int sb_prefill_image_header(struct sb_image_ctx *ictx)
+{
+	struct sb_boot_image_header *hdr = &ictx->payload;
+
+	/* Fill signatures */
+	memcpy(hdr->signature1, "STMP", 4);
+	memcpy(hdr->signature2, "sgtl", 4);
+
+	/* SB Image version 1.1 */
+	hdr->major_version = SB_VERSION_MAJOR;
+	hdr->minor_version = SB_VERSION_MINOR;
+
+	/* Boot image major version */
+	hdr->product_version.major = htons(0x999);
+	hdr->product_version.minor = htons(0x999);
+	hdr->product_version.revision = htons(0x999);
+	/* Boot image major version */
+	hdr->component_version.major = htons(0x999);
+	hdr->component_version.minor = htons(0x999);
+	hdr->component_version.revision = htons(0x999);
+
+	/* Drive tag must be 0x0 for i.MX23 */
+	hdr->drive_tag = 0;
+
+	hdr->header_blocks =
+		sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
+	hdr->section_header_size =
+		sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
+	hdr->timestamp_us = sb_get_timestamp() * 1000000;
+
+	/* FIXME -- add proper config option */
+	hdr->flags = ictx->verbose_boot ? SB_IMAGE_FLAG_VERBOSE : 0,
+
+	/* FIXME -- We support only default key */
+	hdr->key_count = 1;
+
+	return 0;
+}
+
+static int sb_postfill_image_header(struct sb_image_ctx *ictx)
+{
+	struct sb_boot_image_header *hdr = &ictx->payload;
+	struct sb_section_ctx *sctx = ictx->sect_head;
+	uint32_t kd_size, sections_blocks;
+	EVP_MD_CTX md_ctx;
+
+	/* The main SB header size in blocks. */
+	hdr->image_blocks = hdr->header_blocks;
+
+	/* Size of the key dictionary, which has single zero entry. */
+	kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key);
+	hdr->image_blocks += kd_size / SB_BLOCK_SIZE;
+
+	/* Now count the payloads. */
+	hdr->section_count = ictx->sect_count;
+	while (sctx) {
+		hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
+		sctx = sctx->sect;
+	}
+
+	if (!ictx->sect_boot_found) {
+		fprintf(stderr, "ERR: No bootable section selected!\n");
+		return -EINVAL;
+	}
+	hdr->first_boot_section_id = ictx->sect_boot;
+
+	/* The n * SB section size in blocks. */
+	sections_blocks = hdr->section_count * hdr->section_header_size;
+	hdr->image_blocks += sections_blocks;
+
+	/* Key dictionary offset. */
+	hdr->key_dictionary_block = hdr->header_blocks + sections_blocks;
+
+	/* Digest of the whole image. */
+	hdr->image_blocks += 2;
+
+	/* Pointer past the dictionary. */
+	hdr->first_boot_tag_block =
+		hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE;
+
+	/* Compute header digest. */
+	EVP_MD_CTX_init(&md_ctx);
+
+	EVP_DigestInit(&md_ctx, EVP_sha1());
+	EVP_DigestUpdate(&md_ctx, hdr->signature1,
+			 sizeof(struct sb_boot_image_header) -
+			 sizeof(hdr->digest));
+	EVP_DigestFinal(&md_ctx, hdr->digest, NULL);
+
+	return 0;
+}
+
+static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx)
+{
+	/* Fixup the placement of sections. */
+	struct sb_boot_image_header *ihdr = &ictx->payload;
+	struct sb_section_ctx *sctx = ictx->sect_head;
+	struct sb_sections_header *shdr;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	uint32_t offset = ihdr->first_boot_tag_block;
+
+	while (sctx) {
+		shdr = &sctx->payload;
+
+		/* Fill in the section TAG offset. */
+		shdr->section_offset = offset + 1;
+		offset += shdr->section_size;
+
+		/* Section length is measured from the TAG block. */
+		shdr->section_size--;
+
+		/* Fixup the TAG command. */
+		cctx = sctx->cmd_head;
+		while (cctx) {
+			ccmd = &cctx->payload;
+			if (ccmd->header.tag == ROM_TAG_CMD) {
+				ccmd->tag.section_number = shdr->section_number;
+				ccmd->tag.section_length = shdr->section_size;
+				ccmd->tag.section_flags = shdr->section_flags;
+			}
+
+			/* Update the command checksum. */
+			ccmd->header.checksum = sb_command_checksum(ccmd);
+
+			cctx = cctx->cmd;
+		}
+
+		sctx = sctx->sect;
+	}
+
+	return 0;
+}
+
+static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
+{
+	char *tok;
+	char *line = cmd->cmd;
+	char *rptr;
+	int ret;
+
+	/* Analyze the identifier on this line first. */
+	tok = strtok_r(line, " ", &rptr);
+	if (!tok || (strlen(tok) == 0)) {
+		fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno);
+		return -EINVAL;
+	}
+
+	cmd->cmd = rptr;
+
+	/* DCD */
+	if (!strcmp(tok, "DCD")) {
+		ictx->in_section = 0;
+		ictx->in_dcd = 1;
+		sb_build_dcd(ictx, cmd);
+		return 0;
+	}
+
+	/* Section */
+	if (!strcmp(tok, "SECTION")) {
+		ictx->in_section = 1;
+		ictx->in_dcd = 0;
+		sb_build_section(ictx, cmd);
+		return 0;
+	}
+
+	if (!ictx->in_section && !ictx->in_dcd) {
+		fprintf(stderr, "#%i ERR: Data outside of a section!\n",
+			cmd->lineno);
+		return -EINVAL;
+	}
+
+	if (ictx->in_section) {
+		/* Section commands */
+		if (!strcmp(tok, "NOP")) {
+			ret = sb_build_command_nop(ictx);
+		} else if (!strcmp(tok, "TAG")) {
+			ret = sb_build_command_tag(ictx, cmd);
+		} else if (!strcmp(tok, "LOAD")) {
+			ret = sb_build_command_load(ictx, cmd);
+		} else if (!strcmp(tok, "FILL")) {
+			ret = sb_build_command_fill(ictx, cmd);
+		} else if (!strcmp(tok, "JUMP")) {
+			ret = sb_build_command_jump(ictx, cmd);
+		} else if (!strcmp(tok, "CALL")) {
+			ret = sb_build_command_call(ictx, cmd);
+		} else if (!strcmp(tok, "MODE")) {
+			ret = sb_build_command_mode(ictx, cmd);
+		} else {
+			fprintf(stderr,
+				"#%i ERR: Unsupported instruction '%s'!\n",
+				cmd->lineno, tok);
+			return -ENOTSUP;
+		}
+	} else if (ictx->in_dcd) {
+		char *lptr;
+		uint32_t ilen = '1';
+
+		tok = strtok_r(tok, ".", &lptr);
+		if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) {
+			fprintf(stderr, "#%i ERR: Invalid line!\n",
+				cmd->lineno);
+			return -EINVAL;
+		}
+
+		if (lptr &&
+		    (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) {
+			fprintf(stderr, "#%i ERR: Invalid instruction width!\n",
+				cmd->lineno);
+			return -EINVAL;
+		}
+
+		if (lptr)
+			ilen = lptr[0] - '1';
+
+		/* DCD commands */
+		if (!strcmp(tok, "WRITE")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_WRITE | ilen);
+		} else if (!strcmp(tok, "ANDC")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_ANDC | ilen);
+		} else if (!strcmp(tok, "ORR")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_ORR | ilen);
+		} else if (!strcmp(tok, "EQZ")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_CHK_EQZ | ilen);
+		} else if (!strcmp(tok, "EQ")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_CHK_EQ | ilen);
+		} else if (!strcmp(tok, "NEQ")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_CHK_NEQ | ilen);
+		} else if (!strcmp(tok, "NEZ")) {
+			ret = sb_build_dcd_block(ictx, cmd,
+						 SB_DCD_CHK_NEZ | ilen);
+		} else if (!strcmp(tok, "NOOP")) {
+			ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP);
+		} else {
+			fprintf(stderr,
+				"#%i ERR: Unsupported instruction '%s'!\n",
+				cmd->lineno, tok);
+			return -ENOTSUP;
+		}
+	} else {
+		fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
+			cmd->lineno, tok);
+		return -ENOTSUP;
+	}
+
+	/*
+	 * Here we have at least one section with one command, otherwise we
+	 * would have failed already higher above.
+	 *
+	 * FIXME -- should the updating happen here ?
+	 */
+	if (ictx->in_section && !ret) {
+		ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size;
+		ictx->sect_tail->payload.section_size =
+			ictx->sect_tail->size / SB_BLOCK_SIZE;
+	}
+
+	return ret;
+}
+
+static int sb_load_cmdfile(struct sb_image_ctx *ictx)
+{
+	struct sb_cmd_list cmd;
+	int lineno = 1;
+	FILE *fp;
+	char *line = NULL;
+	ssize_t rlen;
+	size_t len;
+
+	fp = fopen(ictx->cfg_filename, "r");
+	if (!fp)
+		goto err_file;
+
+	while ((rlen = getline(&line, &len, fp)) > 0) {
+		memset(&cmd, 0, sizeof(cmd));
+
+		/* Strip the trailing newline. */
+		line[rlen - 1] = '\0';
+
+		cmd.cmd = line;
+		cmd.len = rlen;
+		cmd.lineno = lineno++;
+
+		sb_parse_line(ictx, &cmd);
+	}
+
+	free(line);
+
+	fclose(fp);
+
+	return 0;
+
+err_file:
+	fclose(fp);
+	fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
+		ictx->cfg_filename);
+	return -EINVAL;
+}
+
+static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx)
+{
+	int ret;
+
+	ret = sb_load_cmdfile(ictx);
+	if (ret)
+		return ret;
+
+	ret = sb_prefill_image_header(ictx);
+	if (ret)
+		return ret;
+
+	ret = sb_postfill_image_header(ictx);
+	if (ret)
+		return ret;
+
+	ret = sb_fixup_sections_and_tags(ictx);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static int sb_verify_image_header(struct sb_image_ctx *ictx,
+				  FILE *fp, long fsize)
+{
+	/* Verify static fields in the image header. */
+	struct sb_boot_image_header *hdr = &ictx->payload;
+	const char *stat[2] = { "[PASS]", "[FAIL]" };
+	struct tm tm;
+	int sz, ret = 0;
+	unsigned char digest[20];
+	EVP_MD_CTX md_ctx;
+	unsigned long size;
+
+	/* Start image-wide crypto. */
+	EVP_MD_CTX_init(&ictx->md_ctx);
+	EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
+
+	soprintf(ictx, "---------- Verifying SB Image Header ----------\n");
+
+	size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp);
+	if (size != sizeof(ictx->payload)) {
+		fprintf(stderr, "ERR: SB image header too short!\n");
+		return -EINVAL;
+	}
+
+	/* Compute header digest. */
+	EVP_MD_CTX_init(&md_ctx);
+	EVP_DigestInit(&md_ctx, EVP_sha1());
+	EVP_DigestUpdate(&md_ctx, hdr->signature1,
+			 sizeof(struct sb_boot_image_header) -
+			 sizeof(hdr->digest));
+	EVP_DigestFinal(&md_ctx, digest, NULL);
+
+	sb_aes_init(ictx, NULL, 1);
+	sb_encrypt_sb_header(ictx);
+
+	if (memcmp(digest, hdr->digest, 20))
+		ret = -EINVAL;
+	soprintf(ictx, "%s Image header checksum:        %s\n", stat[!!ret],
+		 ret ? "BAD" : "OK");
+	if (ret)
+		return ret;
+
+	if (memcmp(hdr->signature1, "STMP", 4) ||
+	    memcmp(hdr->signature2, "sgtl", 4))
+		ret = -EINVAL;
+	soprintf(ictx, "%s Signatures:                   '%.4s' '%.4s'\n",
+		 stat[!!ret], hdr->signature1, hdr->signature2);
+	if (ret)
+		return ret;
+
+	if ((hdr->major_version != SB_VERSION_MAJOR) ||
+	    ((hdr->minor_version != 1) && (hdr->minor_version != 2)))
+		ret = -EINVAL;
+	soprintf(ictx, "%s Image version:                v%i.%i\n", stat[!!ret],
+		 hdr->major_version, hdr->minor_version);
+	if (ret)
+		return ret;
+
+	ret = sb_get_time(hdr->timestamp_us / 1000000, &tm);
+	soprintf(ictx,
+		 "%s Creation time:                %02i:%02i:%02i %02i/%02i/%04i\n",
+		 stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec,
+		 tm.tm_mday, tm.tm_mon, tm.tm_year + 2000);
+	if (ret)
+		return ret;
+
+	soprintf(ictx, "%s Product version:              %x.%x.%x\n", stat[0],
+		 ntohs(hdr->product_version.major),
+		 ntohs(hdr->product_version.minor),
+		 ntohs(hdr->product_version.revision));
+	soprintf(ictx, "%s Component version:            %x.%x.%x\n", stat[0],
+		 ntohs(hdr->component_version.major),
+		 ntohs(hdr->component_version.minor),
+		 ntohs(hdr->component_version.revision));
+
+	if (hdr->flags & ~SB_IMAGE_FLAG_VERBOSE)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Image flags:                  %s\n", stat[!!ret],
+		 hdr->flags & SB_IMAGE_FLAG_VERBOSE ? "Verbose_boot" : "");
+	if (ret)
+		return ret;
+
+	if (hdr->drive_tag != 0)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Drive tag:                    %i\n", stat[!!ret],
+		 hdr->drive_tag);
+	if (ret)
+		return ret;
+
+	sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
+	if (hdr->header_blocks != sz)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Image header size (blocks):   %i\n", stat[!!ret],
+		 hdr->header_blocks);
+	if (ret)
+		return ret;
+
+	sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
+	if (hdr->section_header_size != sz)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret],
+		 hdr->section_header_size);
+	if (ret)
+		return ret;
+
+	soprintf(ictx, "%s Sections count:               %i\n", stat[!!ret],
+		 hdr->section_count);
+	soprintf(ictx, "%s First bootable section        %i\n", stat[!!ret],
+		 hdr->first_boot_section_id);
+
+	if (hdr->image_blocks != fsize / SB_BLOCK_SIZE)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Image size (blocks):          %i\n", stat[!!ret],
+		 hdr->image_blocks);
+	if (ret)
+		return ret;
+
+	sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
+	if (hdr->key_dictionary_block != sz)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Key dict offset (blocks):     %i\n", stat[!!ret],
+		 hdr->key_dictionary_block);
+	if (ret)
+		return ret;
+
+	if (hdr->key_count != 1)
+		ret = -EINVAL;
+	soprintf(ictx, "%s Number of encryption keys:    %i\n", stat[!!ret],
+		 hdr->key_count);
+	if (ret)
+		return ret;
+
+	sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
+	sz += hdr->key_count *
+		sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE;
+	if (hdr->first_boot_tag_block != (unsigned)sz)
+		ret = -EINVAL;
+	soprintf(ictx, "%s First TAG block (blocks):     %i\n", stat[!!ret],
+		 hdr->first_boot_tag_block);
+	if (ret)
+		return ret;
+
+	return 0;
+}
+
+static void sb_decrypt_tag(struct sb_image_ctx *ictx,
+		struct sb_cmd_ctx *cctx)
+{
+	EVP_MD_CTX *md_ctx = &ictx->md_ctx;
+	struct sb_command *cmd = &cctx->payload;
+
+	sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload,
+		     (uint8_t *)&cctx->payload, sizeof(*cmd));
+	EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
+}
+
+static int sb_verify_command(struct sb_image_ctx *ictx,
+			     struct sb_cmd_ctx *cctx, FILE *fp,
+			     unsigned long *tsize)
+{
+	struct sb_command *ccmd = &cctx->payload;
+	unsigned long size, asize;
+	char *csum, *flag = "";
+	int ret;
+	unsigned int i;
+	uint8_t csn, csc = ccmd->header.checksum;
+	ccmd->header.checksum = 0x5a;
+	csn = sb_command_checksum(ccmd);
+	ccmd->header.checksum = csc;
+
+	if (csc == csn)
+		ret = 0;
+	else
+		ret = -EINVAL;
+	csum = ret ? "checksum BAD" : "checksum OK";
+
+	switch (ccmd->header.tag) {
+	case ROM_NOP_CMD:
+		soprintf(ictx, " NOOP # %s\n", csum);
+		return ret;
+	case ROM_TAG_CMD:
+		if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG)
+			flag = "LAST";
+		soprintf(ictx, " TAG %s # %s\n", flag, csum);
+		sb_aes_reinit(ictx, 0);
+		return ret;
+	case ROM_LOAD_CMD:
+		soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n",
+			 ccmd->load.address, ccmd->load.count, csum);
+
+		cctx->length = ccmd->load.count;
+		asize = roundup(cctx->length, SB_BLOCK_SIZE);
+		cctx->data = malloc(asize);
+		if (!cctx->data)
+			return -ENOMEM;
+
+		size = fread(cctx->data, 1, asize, fp);
+		if (size != asize) {
+			fprintf(stderr,
+				"ERR: SB LOAD command payload too short!\n");
+			return -EINVAL;
+		}
+
+		*tsize += size;
+
+		EVP_DigestUpdate(&ictx->md_ctx, cctx->data, asize);
+		sb_aes_crypt(ictx, cctx->data, cctx->data, asize);
+
+		if (ccmd->load.crc32 != crc32(cctx->data, asize)) {
+			fprintf(stderr,
+				"ERR: SB LOAD command payload CRC32 invalid!\n");
+			return -EINVAL;
+		}
+		return 0;
+	case ROM_FILL_CMD:
+		soprintf(ictx,
+			 " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n",
+			 ccmd->fill.address, ccmd->fill.count,
+			 ccmd->fill.pattern, csum);
+		return 0;
+	case ROM_JUMP_CMD:
+		if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB)
+			flag = " HAB";
+		soprintf(ictx,
+			 " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n",
+			 flag, ccmd->fill.address, ccmd->jump.argument, csum);
+		return 0;
+	case ROM_CALL_CMD:
+		if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB)
+			flag = " HAB";
+		soprintf(ictx,
+			 " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n",
+			 flag, ccmd->fill.address, ccmd->jump.argument, csum);
+		return 0;
+	case ROM_MODE_CMD:
+		for (i = 0; i < ARRAY_SIZE(modetable); i++) {
+			if (ccmd->mode.mode == modetable[i].mode) {
+				soprintf(ictx, " MODE %s # %s\n",
+					 modetable[i].name, csum);
+				break;
+			}
+		}
+		fprintf(stderr, " MODE !INVALID! # %s\n", csum);
+		return 0;
+	}
+
+	return ret;
+}
+
+static int sb_verify_commands(struct sb_image_ctx *ictx,
+			      struct sb_section_ctx *sctx, FILE *fp)
+{
+	unsigned long size, tsize = 0;
+	struct sb_cmd_ctx *cctx;
+	int ret;
+
+	sb_aes_reinit(ictx, 0);
+
+	while (tsize < sctx->size) {
+		cctx = calloc(1, sizeof(*cctx));
+		if (!cctx)
+			return -ENOMEM;
+		if (!sctx->cmd_head) {
+			sctx->cmd_head = cctx;
+			sctx->cmd_tail = cctx;
+		} else {
+			sctx->cmd_tail->cmd = cctx;
+			sctx->cmd_tail = cctx;
+		}
+
+		size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp);
+		if (size != sizeof(cctx->c_payload)) {
+			fprintf(stderr, "ERR: SB command header too short!\n");
+			return -EINVAL;
+		}
+
+		tsize += size;
+
+		sb_decrypt_tag(ictx, cctx);
+
+		ret = sb_verify_command(ictx, cctx, fp, &tsize);
+		if (ret)
+			return -EINVAL;
+	}
+
+	return 0;
+}
+
+static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp)
+{
+	struct sb_boot_image_header *hdr = &ictx->payload;
+	struct sb_sections_header *shdr;
+	unsigned int i;
+	int ret;
+	struct sb_section_ctx *sctx;
+	unsigned long size;
+	char *bootable = "";
+
+	soprintf(ictx, "----- Verifying  SB Sections and Commands -----\n");
+
+	for (i = 0; i < hdr->section_count; i++) {
+		sctx = calloc(1, sizeof(*sctx));
+		if (!sctx)
+			return -ENOMEM;
+		if (!ictx->sect_head) {
+			ictx->sect_head = sctx;
+			ictx->sect_tail = sctx;
+		} else {
+			ictx->sect_tail->sect = sctx;
+			ictx->sect_tail = sctx;
+		}
+
+		size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp);
+		if (size != sizeof(sctx->payload)) {
+			fprintf(stderr, "ERR: SB section header too short!\n");
+			return -EINVAL;
+		}
+	}
+
+	size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp);
+	if (size != sizeof(ictx->sb_dict_key)) {
+		fprintf(stderr, "ERR: SB key dictionary too short!\n");
+		return -EINVAL;
+	}
+
+	sb_encrypt_sb_sections_header(ictx);
+	sb_aes_reinit(ictx, 0);
+	sb_decrypt_key_dictionary_key(ictx);
+
+	sb_aes_reinit(ictx, 0);
+
+	sctx = ictx->sect_head;
+	while (sctx) {
+		shdr = &sctx->payload;
+
+		if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) {
+			sctx->boot = 1;
+			bootable = " BOOTABLE";
+		}
+
+		sctx->size = (shdr->section_size * SB_BLOCK_SIZE) +
+			     sizeof(struct sb_command);
+		soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n",
+			 shdr->section_number, bootable, sctx->size);
+
+		if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE)
+			fprintf(stderr, " WARN: Unknown section flag(s) %08x\n",
+				shdr->section_flags);
+
+		if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) &&
+		    (hdr->first_boot_section_id != shdr->section_number)) {
+			fprintf(stderr,
+				" WARN: Bootable section does ID not match image header ID!\n");
+		}
+
+		ret = sb_verify_commands(ictx, sctx, fp);
+		if (ret)
+			return ret;
+
+		sctx = sctx->sect;
+	}
+
+	/*
+	 * FIXME IDEA:
+	 * check if the first TAG command is at sctx->section_offset
+	 */
+	return 0;
+}
+
+static int sb_verify_image_end(struct sb_image_ctx *ictx,
+			       FILE *fp, off_t filesz)
+{
+	uint8_t digest[32];
+	unsigned long size;
+	off_t pos;
+	int ret;
+
+	soprintf(ictx, "------------- Verifying image end -------------\n");
+
+	size = fread(digest, 1, sizeof(digest), fp);
+	if (size != sizeof(digest)) {
+		fprintf(stderr, "ERR: SB key dictionary too short!\n");
+		return -EINVAL;
+	}
+
+	pos = ftell(fp);
+	if (pos != filesz) {
+		fprintf(stderr, "ERR: Trailing data past the image!\n");
+		return -EINVAL;
+	}
+
+	/* Check the image digest. */
+	EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
+
+	/* Decrypt the image digest from the input image. */
+	sb_aes_reinit(ictx, 0);
+	sb_aes_crypt(ictx, digest, digest, sizeof(digest));
+
+	/* Check all of 20 bytes of the SHA1 hash. */
+	ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0;
+
+	if (ret)
+		soprintf(ictx, "[FAIL] Full-image checksum:          BAD\n");
+	else
+		soprintf(ictx, "[PASS] Full-image checksum:          OK\n");
+
+	return ret;
+}
+
+
+static int sb_build_tree_from_img(struct sb_image_ctx *ictx)
+{
+	long filesize;
+	int ret;
+	FILE *fp;
+
+	if (!ictx->input_filename) {
+		fprintf(stderr, "ERR: Missing filename!\n");
+		return -EINVAL;
+	}
+
+	fp = fopen(ictx->input_filename, "r");
+	if (!fp)
+		goto err_open;
+
+	ret = fseek(fp, 0, SEEK_END);
+	if (ret < 0)
+		goto err_file;
+
+	filesize = ftell(fp);
+	if (filesize < 0)
+		goto err_file;
+
+	ret = fseek(fp, 0, SEEK_SET);
+	if (ret < 0)
+		goto err_file;
+
+	if (filesize < (signed)sizeof(ictx->payload)) {
+		fprintf(stderr, "ERR: File too short!\n");
+		goto err_file;
+	}
+
+	if (filesize & (SB_BLOCK_SIZE - 1)) {
+		fprintf(stderr, "ERR: The file is not aligned!\n");
+		goto err_file;
+	}
+
+	/* Load and verify image header */
+	ret = sb_verify_image_header(ictx, fp, filesize);
+	if (ret)
+		goto err_verify;
+
+	/* Load and verify sections and commands */
+	ret = sb_verify_sections_cmds(ictx, fp);
+	if (ret)
+		goto err_verify;
+
+	ret = sb_verify_image_end(ictx, fp, filesize);
+	if (ret)
+		goto err_verify;
+
+	ret = 0;
+
+err_verify:
+	soprintf(ictx, "-------------------- Result -------------------\n");
+	soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED");
+
+	/* Stop the encryption session. */
+	sb_aes_deinit(&ictx->cipher_ctx);
+
+	fclose(fp);
+	return ret;
+
+err_file:
+	fclose(fp);
+err_open:
+	fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
+		ictx->input_filename);
+	return -EINVAL;
+}
+
+static void sb_free_image(struct sb_image_ctx *ictx)
+{
+	struct sb_section_ctx *sctx = ictx->sect_head, *s_head;
+	struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head;
+	struct sb_cmd_ctx *cctx, *c_head;
+
+	while (sctx) {
+		s_head = sctx;
+		c_head = sctx->cmd_head;
+
+		while (c_head) {
+			cctx = c_head;
+			c_head = c_head->cmd;
+			if (cctx->data)
+				free(cctx->data);
+			free(cctx);
+		}
+
+		sctx = sctx->sect;
+		free(s_head);
+	}
+
+	while (dctx) {
+		d_head = dctx;
+		dctx = dctx->dcd;
+		free(d_head->payload);
+		free(d_head);
+	}
+}
+
+/*
+ * MXSSB-MKIMAGE glue code.
+ */
+static int mxsimage_check_image_types(uint8_t type)
+{
+	if (type == IH_TYPE_MXSIMAGE)
+		return EXIT_SUCCESS;
+	else
+		return EXIT_FAILURE;
+}
+
+static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd,
+				struct mkimage_params *params)
+{
+}
+
+int mxsimage_check_params(struct mkimage_params *params)
+{
+	if (!params)
+		return -1;
+	if (!strlen(params->imagename)) {
+		fprintf(stderr,
+			"Error: %s - Configuration file not specified, it is needed for mxsimage generation\n",
+			params->cmdname);
+		return -1;
+	}
+
+	/*
+	 * Check parameters:
+	 * XIP is not allowed and verify that incompatible
+	 * parameters are not sent at the same time
+	 * For example, if list is required a data image must not be provided
+	 */
+	return	(params->dflag && (params->fflag || params->lflag)) ||
+		(params->fflag && (params->dflag || params->lflag)) ||
+		(params->lflag && (params->dflag || params->fflag)) ||
+		(params->xflag) || !(strlen(params->imagename));
+}
+
+static int mxsimage_verify_print_header(char *file, int silent)
+{
+	int ret;
+	struct sb_image_ctx ctx;
+
+	memset(&ctx, 0, sizeof(ctx));
+
+	ctx.input_filename = file;
+	ctx.silent_dump = silent;
+
+	ret = sb_build_tree_from_img(&ctx);
+	sb_free_image(&ctx);
+
+	return ret;
+}
+
+char *imagefile;
+static int mxsimage_verify_header(unsigned char *ptr, int image_size,
+			struct mkimage_params *params)
+{
+	struct sb_boot_image_header *hdr;
+
+	if (!ptr)
+		return -EINVAL;
+
+	hdr = (struct sb_boot_image_header *)ptr;
+
+	/*
+	 * Check if the header contains the MXS image signatures,
+	 * if so, do a full-image verification.
+	 */
+	if (memcmp(hdr->signature1, "STMP", 4) ||
+	    memcmp(hdr->signature2, "sgtl", 4))
+		return -EINVAL;
+
+	imagefile = params->imagefile;
+
+	return mxsimage_verify_print_header(params->imagefile, 1);
+}
+
+static void mxsimage_print_header(const void *hdr)
+{
+	if (imagefile)
+		mxsimage_verify_print_header(imagefile, 0);
+}
+
+static int sb_build_image(struct sb_image_ctx *ictx,
+			  struct image_type_params *tparams)
+{
+	struct sb_boot_image_header *sb_header = &ictx->payload;
+	struct sb_section_ctx *sctx;
+	struct sb_cmd_ctx *cctx;
+	struct sb_command *ccmd;
+	struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
+
+	uint8_t *image, *iptr;
+
+	/* Calculate image size. */
+	uint32_t size = sizeof(*sb_header) +
+		ictx->sect_count * sizeof(struct sb_sections_header) +
+		sizeof(*sb_dict_key) + sizeof(ictx->digest);
+
+	sctx = ictx->sect_head;
+	while (sctx) {
+		size += sctx->size;
+		sctx = sctx->sect;
+	};
+
+	image = malloc(size);
+	if (!image)
+		return -ENOMEM;
+	iptr = image;
+
+	memcpy(iptr, sb_header, sizeof(*sb_header));
+	iptr += sizeof(*sb_header);
+
+	sctx = ictx->sect_head;
+	while (sctx) {
+		memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header));
+		iptr += sizeof(struct sb_sections_header);
+		sctx = sctx->sect;
+	};
+
+	memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key));
+	iptr += sizeof(*sb_dict_key);
+
+	sctx = ictx->sect_head;
+	while (sctx) {
+		cctx = sctx->cmd_head;
+		while (cctx) {
+			ccmd = &cctx->payload;
+
+			memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload));
+			iptr += sizeof(cctx->payload);
+
+			if (ccmd->header.tag == ROM_LOAD_CMD) {
+				memcpy(iptr, cctx->data, cctx->length);
+				iptr += cctx->length;
+			}
+
+			cctx = cctx->cmd;
+		}
+
+		sctx = sctx->sect;
+	};
+
+	memcpy(iptr, ictx->digest, sizeof(ictx->digest));
+	iptr += sizeof(ictx->digest);
+
+	/* Configure the mkimage */
+	tparams->hdr = image;
+	tparams->header_size = size;
+
+	return 0;
+}
+
+static int mxsimage_generate(struct mkimage_params *params,
+	struct image_type_params *tparams)
+{
+	int ret;
+	struct sb_image_ctx ctx;
+
+	/* Do not copy the U-Boot image! */
+	params->skipcpy = 1;
+
+	memset(&ctx, 0, sizeof(ctx));
+
+	ctx.cfg_filename = params->imagename;
+	ctx.output_filename = params->imagefile;
+	ctx.verbose_boot = 1;
+
+	ret = sb_build_tree_from_cfg(&ctx);
+	if (ret)
+		goto fail;
+
+	ret = sb_encrypt_image(&ctx);
+	if (!ret)
+		ret = sb_build_image(&ctx, tparams);
+
+fail:
+	sb_free_image(&ctx);
+
+	return ret;
+}
+
+/*
+ * mxsimage parameters
+ */
+static struct image_type_params mxsimage_params = {
+	.name		= "Freescale MXS Boot Image support",
+	.header_size	= 0,
+	.hdr		= NULL,
+	.check_image_type = mxsimage_check_image_types,
+	.verify_header	= mxsimage_verify_header,
+	.print_header	= mxsimage_print_header,
+	.set_header	= mxsimage_set_header,
+	.check_params	= mxsimage_check_params,
+	.vrec_header	= mxsimage_generate,
+};
+
+void init_mxs_image_type(void)
+{
+	mkimage_register(&mxsimage_params);
+}
+
+#else
+void init_mxs_image_type(void)
+{
+}
+#endif
+/*
+ * Freescale i.MX28 SB image generator
+ *
+ * Copyright (C) 2012 Marek Vasut <marex@denx.de>
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#ifndef __MXSSB_H__
+#define __MXSSB_H__
+
+#include <stdint.h>
+#include <arpa/inet.h>
+
+#define SB_BLOCK_SIZE		16
+
+#define roundup(x, y)		((((x) + ((y) - 1)) / (y)) * (y))
+#define ARRAY_SIZE(x)		(sizeof(x) / sizeof((x)[0]))
+
+struct sb_boot_image_version {
+	uint16_t	major;
+	uint16_t	pad0;
+	uint16_t	minor;
+	uint16_t	pad1;
+	uint16_t	revision;
+	uint16_t	pad2;
+};
+
+struct sb_boot_image_header {
+	union {
+		/* SHA1 of the header. */
+		uint8_t	digest[20];
+		struct {
+			/* CBC-MAC initialization vector. */
+			uint8_t iv[16];
+			uint8_t extra[4];
+		};
+	};
+	/* 'STMP' */
+	uint8_t		signature1[4];
+	/* Major version of the image format. */
+	uint8_t		major_version;
+	/* Minor version of the image format. */
+	uint8_t		minor_version;
+	/* Flags associated with the image. */
+	uint16_t	flags;
+	/* Size of the image in 16b blocks. */
+	uint32_t	image_blocks;
+	/* Offset of the first tag in 16b blocks. */
+	uint32_t	first_boot_tag_block;
+	/* ID of the section to boot from. */
+	uint32_t	first_boot_section_id;
+	/* Amount of crypto keys. */
+	uint16_t	key_count;
+	/* Offset to the key dictionary in 16b blocks. */
+	uint16_t	key_dictionary_block;
+	/* Size of this header in 16b blocks. */
+	uint16_t	header_blocks;
+	/* Amount of section headers. */
+	uint16_t	section_count;
+	/* Section header size in 16b blocks. */
+	uint16_t	section_header_size;
+	/* Padding to align timestamp to uint64_t. */
+	uint8_t		padding0[2];
+	/* 'sgtl' (since v1.1) */
+	uint8_t		signature2[4];
+	/* Image generation date, in microseconds since 1.1.2000 . */
+	uint64_t	timestamp_us;
+	/* Product version. */
+	struct sb_boot_image_version
+			product_version;
+	/* Component version. */
+	struct sb_boot_image_version
+			component_version;
+	/* Drive tag for the system drive. (since v1.1) */
+	uint16_t	drive_tag;
+	/* Padding. */
+	uint8_t		padding1[6];
+};
+
+#define	SB_VERSION_MAJOR	1
+#define	SB_VERSION_MINOR	1
+
+/* Enable to HTLLC verbose boot report. */
+#define SB_IMAGE_FLAG_VERBOSE	(1 << 0)
+
+struct sb_key_dictionary_key {
+	/* The CBC-MAC of image and sections header. */
+	uint8_t		cbc_mac[SB_BLOCK_SIZE];
+	/* The AES key encrypted by image key (zero). */
+	uint8_t		key[SB_BLOCK_SIZE];
+};
+
+struct sb_ivt_header {
+	uint32_t	header;
+	uint32_t	entry;
+	uint32_t	reserved1;
+	uint32_t	dcd;
+	uint32_t	boot_data;
+	uint32_t	self;
+	uint32_t	csf;
+	uint32_t	reserved2;
+};
+
+#define	SB_HAB_IVT_TAG			0xd1UL
+#define	SB_HAB_DCD_TAG			0xd2UL
+
+#define	SB_HAB_VERSION			0x40UL
+
+/*
+ * The "size" field in the IVT header is not naturally aligned,
+ * use this macro to fill first 4 bytes of the IVT header without
+ * causing issues on some systems (esp. M68k, PPC, MIPS-BE, ARM-BE).
+ */
+static inline uint32_t sb_hab_ivt_header(void)
+{
+	uint32_t ret = 0;
+	ret |= SB_HAB_IVT_TAG << 24;
+	ret |= sizeof(struct sb_ivt_header) << 16;
+	ret |= SB_HAB_VERSION;
+	return htonl(ret);
+}
+
+struct sb_sections_header {
+	/* Section number. */
+	uint32_t	section_number;
+	/* Offset of this sections first instruction after "TAG". */
+	uint32_t	section_offset;
+	/* Size of the section in 16b blocks. */
+	uint32_t	section_size;
+	/* Section flags. */
+	uint32_t	section_flags;
+};
+
+#define	SB_SECTION_FLAG_BOOTABLE	(1 << 0)
+
+struct sb_command {
+	struct {
+		uint8_t		checksum;
+		uint8_t		tag;
+		uint16_t	flags;
+#define ROM_TAG_CMD_FLAG_ROM_LAST_TAG	0x1
+#define ROM_LOAD_CMD_FLAG_DCD_LOAD	0x1	/* MX28 only */
+#define ROM_JUMP_CMD_FLAG_HAB		0x1	/* MX28 only */
+#define ROM_CALL_CMD_FLAG_HAB		0x1	/* MX28 only */
+	} header;
+
+	union {
+	struct {
+		uint32_t	reserved[3];
+	} nop;
+	struct {
+		uint32_t	section_number;
+		uint32_t	section_length;
+		uint32_t	section_flags;
+	} tag;
+	struct {
+		uint32_t	address;
+		uint32_t	count;
+		uint32_t	crc32;
+	} load;
+	struct {
+		uint32_t	address;
+		uint32_t	count;
+		uint32_t	pattern;
+	} fill;
+	struct {
+		uint32_t	address;
+		uint32_t	reserved;
+		/* Passed in register r0 before JUMP */
+		uint32_t	argument;
+	} jump;
+	struct {
+		uint32_t	address;
+		uint32_t	reserved;
+		/* Passed in register r0 before CALL */
+		uint32_t	argument;
+	} call;
+	struct {
+		uint32_t	reserved1;
+		uint32_t	reserved2;
+		uint32_t	mode;
+	} mode;
+
+	};
+};
+
+/*
+ * Most of the mode names are same or at least similar
+ * on i.MX23 and i.MX28, but some of the mode names
+ * differ. The "name" field represents the mode name
+ * on i.MX28 as seen in Table 12-2 of the datasheet.
+ * The "altname" field represents the differently named
+ * fields on i.MX23 as seen in Table 35-3 of the
+ * datasheet.
+ */
+static const struct {
+	const char	*name;
+	const char	*altname;
+	const uint8_t	mode;
+} modetable[] = {
+	{ "USB",		NULL,		0x00 },
+	{ "I2C",		NULL,		0x01 },
+	{ "SPI2_FLASH",		"SPI1_FLASH",	0x02 },
+	{ "SPI3_FLASH",		"SPI2_FLASH",	0x03 },
+	{ "NAND_BCH",		NULL,		0x04 },
+	{ "JTAG",		NULL,		0x06 },
+	{ "SPI3_EEPROM",	"SPI2_EEPROM",	0x08 },
+	{ "SD_SSP0",		NULL,		0x09 },
+	{ "SD_SSP1",		NULL,		0x0A }
+};
+
+enum sb_tag {
+	ROM_NOP_CMD	= 0x00,
+	ROM_TAG_CMD	= 0x01,
+	ROM_LOAD_CMD	= 0x02,
+	ROM_FILL_CMD	= 0x03,
+	ROM_JUMP_CMD	= 0x04,
+	ROM_CALL_CMD	= 0x05,
+	ROM_MODE_CMD	= 0x06
+};
+
+struct sb_source_entry {
+	uint8_t		tag;
+	uint32_t	address;
+	uint32_t	flags;
+	char		*filename;
+};
+
+#endif	/* __MXSSB_H__ */
	1	+SECTION 0x0 BOOTABLE
	2	+ TAG LAST
	3	+ LOAD 0x0 spl/u-boot-spl.bin
	4	+ CALL 0x14 0x0
	5	+ LOAD 0x40000100 u-boot.bin
	6	+ CALL 0x40000100 0x0
...	...	@@ -36,7 +36,7 @@
36	36	CLKCTRL_CLKSEQ_BYPASS_CPU);
37	37	}
38	38
39		-static void mxs_power_clear_auto_restart(void)
	39	+static void mxs_power_set_auto_restart(void)
40	40	{
41	41	struct mxs_rtc_regs *rtc_regs =
42	42	(struct mxs_rtc_regs *)MXS_RTC_BASE;
...	...	@@ -49,10 +49,7 @@
49	49	while (readl(&rtc_regs->hw_rtc_ctrl) & RTC_CTRL_CLKGATE)
50	50	;
51	51
52		- /*
53		- * Due to the hardware design bug of mx28 EVK-A
54		- * we need to set the AUTO_RESTART bit.
55		- */
	52	+ /* Do nothing if flag already set */
56	53	if (readl(&rtc_regs->hw_rtc_persistent0) & RTC_PERSISTENT0_AUTO_RESTART)
57	54	return;
58	55
...	...	@@ -911,7 +908,7 @@
911	908	mxs_ungate_power();
912	909
913	910	mxs_power_clock2xtal();
914		- mxs_power_clear_auto_restart();
	911	+ mxs_power_set_auto_restart();
915	912	mxs_power_set_linreg();
916	913	mxs_power_setup_5v_detect();
917	914
...	...	@@ -11,10 +11,11 @@
11	11
12	12	LIB = $(obj)lib$(SOC).o
13	13
14		-COBJS = soc.o clock.o
	14	+COBJS-y = soc.o clock.o
	15	+COBJS-$(CONFIG_SECURE_BOOT) += hab.o
15	16
16		-SRCS := $(SOBJS:.o=.S) $(COBJS:.o=.c)
17		-OBJS := $(addprefix $(obj),$(SOBJS) $(COBJS))
	17	+SRCS := $(SOBJS:.o=.S) $(COBJS-y:.o=.c)
	18	+OBJS := $(addprefix $(obj),$(SOBJS) $(COBJS-y))
18	19
19	20	all: $(obj).depend $(LIB)
20	21
	1	+/*
	2	+ * Copyright (C) 2010-2013 Freescale Semiconductor, Inc.
	3	+ *
	4	+ * SPDX-License-Identifier: GPL-2.0+
	5	+ */
	6	+
	7	+#include <common.h>
	8	+#include <asm/io.h>
	9	+#include <asm/arch/hab.h>
	10	+
	11	+/* -------- start of HAB API updates ------------*/
	12	+#define hab_rvt_report_event ((hab_rvt_report_event_t *)HAB_RVT_REPORT_EVENT)
	13	+#define hab_rvt_report_status ((hab_rvt_report_status_t *)HAB_RVT_REPORT_STATUS)
	14	+#define hab_rvt_authenticate_image \
	15	+ ((hab_rvt_authenticate_image_t *)HAB_RVT_AUTHENTICATE_IMAGE)
	16	+#define hab_rvt_entry ((hab_rvt_entry_t *)HAB_RVT_ENTRY)
	17	+#define hab_rvt_exit ((hab_rvt_exit_t *)HAB_RVT_EXIT)
	18	+#define hab_rvt_clock_init HAB_RVT_CLOCK_INIT
	19	+
	20	+bool is_hab_enabled(void)
	21	+{
	22	+ struct ocotp_regs ocotp = (struct ocotp_regs )OCOTP_BASE_ADDR;
	23	+ struct fuse_bank *bank = &ocotp->bank[0];
	24	+ struct fuse_bank0_regs *fuse =
	25	+ (struct fuse_bank0_regs *)bank->fuse_regs;
	26	+ uint32_t reg = readl(&fuse->cfg5);
	27	+
	28	+ return (reg & 0x2) == 0x2;
	29	+}
	30	+
	31	+void display_event(uint8_t *event_data, size_t bytes)
	32	+{
	33	+ uint32_t i;
	34	+
	35	+ if (!(event_data && bytes > 0))
	36	+ return;
	37	+
	38	+ for (i = 0; i < bytes; i++) {
	39	+ if (i == 0)
	40	+ printf("\t0x%02x", event_data[i]);
	41	+ else if ((i % 8) == 0)
	42	+ printf("\n\t0x%02x", event_data[i]);
	43	+ else
	44	+ printf(" 0x%02x", event_data[i]);
	45	+ }
	46	+}
	47	+
	48	+int get_hab_status(void)
	49	+{
	50	+ uint32_t index = 0; /* Loop index */
	51	+ uint8_t event_data[128]; /* Event data buffer */
	52	+ size_t bytes = sizeof(event_data); /* Event size in bytes */
	53	+ enum hab_config config = 0;
	54	+ enum hab_state state = 0;
	55	+
	56	+ if (is_hab_enabled())
	57	+ puts("\nSecure boot enabled\n");
	58	+ else
	59	+ puts("\nSecure boot disabled\n");
	60	+
	61	+ /* Check HAB status */
	62	+ if (hab_rvt_report_status(&config, &state) != HAB_SUCCESS) {
	63	+ printf("\nHAB Configuration: 0x%02x, HAB State: 0x%02x\n",
	64	+ config, state);
	65	+
	66	+ /* Display HAB Error events */
	67	+ while (hab_rvt_report_event(HAB_FAILURE, index, event_data,
	68	+ &bytes) == HAB_SUCCESS) {
	69	+ puts("\n");
	70	+ printf("--------- HAB Event %d -----------------\n",
	71	+ index + 1);
	72	+ puts("event data:\n");
	73	+ display_event(event_data, bytes);
	74	+ puts("\n");
	75	+ bytes = sizeof(event_data);
	76	+ index++;
	77	+ }
	78	+ }
	79	+ /* Display message if no HAB events are found */
	80	+ else {
	81	+ printf("\nHAB Configuration: 0x%02x, HAB State: 0x%02x\n",
	82	+ config, state);
	83	+ puts("No HAB Events Found!\n\n");
	84	+ }
	85	+ return 0;
	86	+}
	87	+
	88	+int do_hab_status(cmd_tbl_t cmdtp, int flag, int argc, char const argv[])
	89	+{
	90	+ if ((argc != 1)) {
	91	+ cmd_usage(cmdtp);
	92	+ return 1;
	93	+ }
	94	+
	95	+ get_hab_status();
	96	+
	97	+ return 0;
	98	+}
	99	+
	100	+U_BOOT_CMD(
	101	+ hab_status, CONFIG_SYS_MAXARGS, 1, do_hab_status,
	102	+ "display HAB status",
	103	+ ""
	104	+ );
...	...	@@ -213,6 +213,34 @@
213	213
214	214	void s_init(void)
215	215	{
	216	+ struct anatop_regs anatop = (struct anatop_regs )ANATOP_BASE_ADDR;
	217	+ int is_6q = is_cpu_type(MXC_CPU_MX6Q);
	218	+ u32 mask480;
	219	+ u32 mask528;
	220	+
	221	+ /* Due to hardware limitation, on MX6Q we need to gate/ungate all PFDs
	222	+ * to make sure PFD is working right, otherwise, PFDs may
	223	+ * not output clock after reset, MX6DL and MX6SL have added 396M pfd
	224	+ * workaround in ROM code, as bus clock need it
	225	+ */
	226	+
	227	+ mask480 = ANATOP_PFD_CLKGATE_MASK(0) \|
	228	+ ANATOP_PFD_CLKGATE_MASK(1) \|
	229	+ ANATOP_PFD_CLKGATE_MASK(2) \|
	230	+ ANATOP_PFD_CLKGATE_MASK(3);
	231	+ mask528 = ANATOP_PFD_CLKGATE_MASK(0) \|
	232	+ ANATOP_PFD_CLKGATE_MASK(1) \|
	233	+ ANATOP_PFD_CLKGATE_MASK(3);
	234	+
	235	+ /*
	236	+ * Don't reset PFD2 on DL/S
	237	+ */
	238	+ if (is_6q)
	239	+ mask528 \|= ANATOP_PFD_CLKGATE_MASK(2);
	240	+ writel(mask480, &anatop->pfd_480_set);
	241	+ writel(mask528, &anatop->pfd_528_set);
	242	+ writel(mask480, &anatop->pfd_480_clr);
	243	+ writel(mask528, &anatop->pfd_528_clr);
216	244	}
217	245
218	246	#ifdef CONFIG_IMX_HDMI
	1	+/*
	2	+ * Copyright (C) 2012 Freescale Semiconductor, Inc. All Rights Reserved.
	3	+ *
	4	+ * SPDX-License-Identifier: GPL-2.0+
	5	+ *
	6	+*/
	7	+
	8	+#ifndef __SECURE_MX6Q_H__
	9	+#define __SECURE_MX6Q_H__
	10	+
	11	+#include <linux/types.h>
	12	+
	13	+/* -------- start of HAB API updates ------------*/
	14	+/* The following are taken from HAB4 SIS */
	15	+
	16	+/* Status definitions */
	17	+enum hab_status {
	18	+ HAB_STS_ANY = 0x00,
	19	+ HAB_FAILURE = 0x33,
	20	+ HAB_WARNING = 0x69,
	21	+ HAB_SUCCESS = 0xf0
	22	+};
	23	+
	24	+/* Security Configuration definitions */
	25	+enum hab_config {
	26	+ HAB_CFG_RETURN = 0x33, /*< Field Return IC /
	27	+ HAB_CFG_OPEN = 0xf0, /*< Non-secure IC /
	28	+ HAB_CFG_CLOSED = 0xcc /*< Secure IC /
	29	+};
	30	+
	31	+/* State definitions */
	32	+enum hab_state {
	33	+ HAB_STATE_INITIAL = 0x33, /*< Initialising state (transitory) /
	34	+ HAB_STATE_CHECK = 0x55, /*< Check state (non-secure) /
	35	+ HAB_STATE_NONSECURE = 0x66, /*< Non-secure state /
	36	+ HAB_STATE_TRUSTED = 0x99, /*< Trusted state /
	37	+ HAB_STATE_SECURE = 0xaa, /*< Secure state /
	38	+ HAB_STATE_FAIL_SOFT = 0xcc, /*< Soft fail state /
	39	+ HAB_STATE_FAIL_HARD = 0xff, /*< Hard fail state (terminal) /
	40	+ HAB_STATE_NONE = 0xf0, /*< No security state machine /
	41	+ HAB_STATE_MAX
	42	+};
	43	+
	44	+/Function prototype description/
	45	+typedef enum hab_status hab_rvt_report_event_t(enum hab_status, uint32_t,
	46	+ uint8_t* , size_t*);
	47	+typedef enum hab_status hab_rvt_report_status_t(enum hab_config *,
	48	+ enum hab_state *);
	49	+typedef enum hab_status hab_loader_callback_f_t(void*, size_t, const void*);
	50	+typedef enum hab_status hab_rvt_entry_t(void);
	51	+typedef enum hab_status hab_rvt_exit_t(void);
	52	+typedef void *hab_rvt_authenticate_image_t(uint8_t, ptrdiff_t,
	53	+ void *, size_t , hab_loader_callback_f_t);
	54	+typedef void hapi_clock_init_t(void);
	55	+
	56	+#define HAB_RVT_REPORT_EVENT ((uint32_t )0x000000B4)
	57	+#define HAB_RVT_REPORT_STATUS ((uint32_t )0x000000B8)
	58	+#define HAB_RVT_AUTHENTICATE_IMAGE ((uint32_t )0x000000A4)
	59	+#define HAB_RVT_ENTRY ((uint32_t )0x00000098)
	60	+#define HAB_RVT_EXIT ((uint32_t )0x0000009C)
	61	+#define HAB_RVT_CLOCK_INIT ((hapi_clock_init_t *)0x0000024D)
	62	+
	63	+#define HAB_CID_ROM 0 /*< ROM Caller ID /
	64	+#define HAB_CID_UBOOT 1 /*< UBOOT Caller ID/
	65	+/* ----------- end of HAB API updates ------------*/
	66	+
	67	+#endif
...	...	@@ -456,7 +456,13 @@
456	456	u32 uid_low;
457	457	u32 rsvd1[3];
458	458	u32 uid_high;
459		- u32 rsvd2[0x17];
	459	+ u32 rsvd2[3];
	460	+ u32 rsvd3[4];
	461	+ u32 rsvd4[4];
	462	+ u32 rsvd5[4];
	463	+ u32 cfg5;
	464	+ u32 rsvd6[3];
	465	+ u32 rsvd7[4];
460	466	};
461	467
462	468	struct fuse_bank4_regs {
...	...	@@ -629,29 +635,12 @@
629	635	u32 digprog_sololite; /* 0x280 */
630	636	};
631	637
632		-#define ANATOP_PFD_480_PFD0_FRAC_SHIFT 0
633		-#define ANATOP_PFD_480_PFD0_FRAC_MASK (0x3f<<ANATOP_PFD_480_PFD0_FRAC_SHIFT)
634		-#define ANATOP_PFD_480_PFD0_STABLE_SHIFT 6
635		-#define ANATOP_PFD_480_PFD0_STABLE_MASK (1<<ANATOP_PFD_480_PFD0_STABLE_SHIFT)
636		-#define ANATOP_PFD_480_PFD0_CLKGATE_SHIFT 7
637		-#define ANATOP_PFD_480_PFD0_CLKGATE_MASK (1<<ANATOP_PFD_480_PFD0_CLKGATE_SHIFT)
638		-#define ANATOP_PFD_480_PFD1_FRAC_SHIFT 8
639		-#define ANATOP_PFD_480_PFD1_FRAC_MASK (0x3f<<ANATOP_PFD_480_PFD1_FRAC_SHIFT)
640		-#define ANATOP_PFD_480_PFD1_STABLE_SHIFT 14
641		-#define ANATOP_PFD_480_PFD1_STABLE_MASK (1<<ANATOP_PFD_480_PFD1_STABLE_SHIFT)
642		-#define ANATOP_PFD_480_PFD1_CLKGATE_SHIFT 15
643		-#define ANATOP_PFD_480_PFD1_CLKGATE_MASK (0x3f<<ANATOP_PFD_480_PFD1_CLKGATE_SHIFT)
644		-#define ANATOP_PFD_480_PFD2_FRAC_SHIFT 16
645		-#define ANATOP_PFD_480_PFD2_FRAC_MASK (1<<ANATOP_PFD_480_PFD2_FRAC_SHIFT)
646		-#define ANATOP_PFD_480_PFD2_STABLE_SHIFT 22
647		-#define ANATOP_PFD_480_PFD2_STABLE_MASK (1<<ANATOP_PFD_480_PFD2_STABLE_SHIFT)
648		-#define ANATOP_PFD_480_PFD2_CLKGATE_SHIFT 23
649		-#define ANATOP_PFD_480_PFD2_CLKGATE_MASK (0x3f<<ANATOP_PFD_480_PFD2_CLKGATE_SHIFT)
650		-#define ANATOP_PFD_480_PFD3_FRAC_SHIFT 24
651		-#define ANATOP_PFD_480_PFD3_FRAC_MASK (1<<ANATOP_PFD_480_PFD3_FRAC_SHIFT)
652		-#define ANATOP_PFD_480_PFD3_STABLE_SHIFT 30
653		-#define ANATOP_PFD_480_PFD3_STABLE_MASK (1<<ANATOP_PFD_480_PFD3_STABLE_SHIFT)
654		-#define ANATOP_PFD_480_PFD3_CLKGATE_SHIFT 31
	638	+#define ANATOP_PFD_FRAC_SHIFT(n) ((n)*8)
	639	+#define ANATOP_PFD_FRAC_MASK(n) (0x3f<<ANATOP_PFD_FRAC_SHIFT(n))
	640	+#define ANATOP_PFD_STABLE_SHIFT(n) (6+((n)*8))
	641	+#define ANATOP_PFD_STABLE_MASK(n) (1<<ANATOP_PFD_STABLE_SHIFT(n))
	642	+#define ANATOP_PFD_CLKGATE_SHIFT(n) (7+((n)*8))
	643	+#define ANATOP_PFD_CLKGATE_MASK(n) (1<<ANATOP_PFD_CLKGATE_SHIFT(n))
655	644
656	645	struct iomuxc_base_regs {
657	646	u32 gpr[14]; /* 0x000 */
...	...	@@ -19,6 +19,13 @@
19	19
20	20	#define is_soc_rev(rev) ((get_cpu_rev() & 0xFF) - rev)
21	21	u32 get_cpu_rev(void);
	22	+
	23	+/* returns MXC_CPU_ value */
	24	+#define cpu_type(rev) (((rev) >> 12)&0xff)
	25	+
	26	+/* use with MXC_CPU_ constants */
	27	+#define is_cpu_type(cpu) (cpu_type(get_cpu_rev()) == cpu)
	28	+
22	29	const char *get_imx_type(u32 imxtype);
23	30	unsigned imx_ddr_size(void);
24	31
	1	+/*
	2	+ * Freescale MXS UARTAPP Register Definitions
	3	+ *
	4	+ * Copyright (C) 2013 Andreas Wass <andreas.wass@dalelven.com>
	5	+ *
	6	+ * Based on code from LTIB:
	7	+ * Copyright 2008-2010 Freescale Semiconductor, Inc. All Rights Reserved.
	8	+ *
	9	+ * SPDX-License-Identifier: GPL-2.0+
	10	+ */
	11	+
	12	+#ifndef __ARCH_ARM___MXS_UARTAPP_H
	13	+#define __ARCH_ARM___MXS_UARTAPP_H
	14	+
	15	+#include <asm/imx-common/regs-common.h>
	16	+
	17	+#ifndef __ASSEMBLY__
	18	+struct mxs_uartapp_regs {
	19	+ mxs_reg_32(hw_uartapp_ctrl0)
	20	+ mxs_reg_32(hw_uartapp_ctrl1)
	21	+ mxs_reg_32(hw_uartapp_ctrl2)
	22	+ mxs_reg_32(hw_uartapp_linectrl)
	23	+ mxs_reg_32(hw_uartapp_linectrl2)
	24	+ mxs_reg_32(hw_uartapp_intr)
	25	+ mxs_reg_32(hw_uartapp_data)
	26	+ mxs_reg_32(hw_uartapp_stat)
	27	+ mxs_reg_32(hw_uartapp_debug)
	28	+ mxs_reg_32(hw_uartapp_version)
	29	+ mxs_reg_32(hw_uartapp_autobaud)
	30	+};
	31	+#endif
	32	+
	33	+#define UARTAPP_CTRL0_SFTRST_MASK (1 << 31)
	34	+#define UARTAPP_CTRL0_CLKGATE_MASK (1 << 30)
	35	+#define UARTAPP_CTRL0_RUN_MASK (1 << 29)
	36	+#define UARTAPP_CTRL0_RX_SOURCE_MASK (1 << 28)
	37	+#define UARTAPP_CTRL0_RXTO_ENABLE_MASK (1 << 27)
	38	+#define UARTAPP_CTRL0_RXTIMEOUT_OFFSET 16
	39	+#define UARTAPP_CTRL0_RXTIMEOUT_MASK (0x7FF << 16)
	40	+#define UARTAPP_CTRL0_XFER_COUNT_OFFSET 0
	41	+#define UARTAPP_CTRL0_XFER_COUNT_MASK 0xFFFF
	42	+
	43	+#define UARTAPP_CTRL1_RUN_MASK (1 << 28)
	44	+
	45	+#define UARTAPP_CTRL1_XFER_COUNT_OFFSET 0
	46	+#define UARTAPP_CTRL1_XFER_COUNT_MASK 0xFFFF
	47	+
	48	+#define UARTAPP_CTRL2_INVERT_RTS_MASK (1 << 31)
	49	+#define UARTAPP_CTRL2_INVERT_CTS_MASK (1 << 30)
	50	+#define UARTAPP_CTRL2_INVERT_TX_MASK (1 << 29)
	51	+#define UARTAPP_CTRL2_INVERT_RX_MASK (1 << 28)
	52	+#define UARTAPP_CTRL2_RTS_SEMAPHORE_MASK (1 << 27)
	53	+#define UARTAPP_CTRL2_DMAONERR_MASK (1 << 26)
	54	+#define UARTAPP_CTRL2_TXDMAE_MASK (1 << 25)
	55	+#define UARTAPP_CTRL2_RXDMAE_MASK (1 << 24)
	56	+#define UARTAPP_CTRL2_RXIFLSEL_OFFSET 20
	57	+#define UARTAPP_CTRL2_RXIFLSEL_MASK (0x7 << 20)
	58	+
	59	+#define UARTAPP_CTRL2_RXIFLSEL_NOT_EMPTY (0x0 << 20)
	60	+#define UARTAPP_CTRL2_RXIFLSEL_ONE_QUARTER (0x1 << 20)
	61	+#define UARTAPP_CTRL2_RXIFLSEL_ONE_HALF (0x2 << 20)
	62	+#define UARTAPP_CTRL2_RXIFLSEL_THREE_QUARTERS (0x3 << 20)
	63	+#define UARTAPP_CTRL2_RXIFLSEL_SEVEN_EIGHTHS (0x4 << 20)
	64	+#define UARTAPP_CTRL2_RXIFLSEL_INVALID5 (0x5 << 20)
	65	+#define UARTAPP_CTRL2_RXIFLSEL_INVALID6 (0x6 << 20)
	66	+#define UARTAPP_CTRL2_RXIFLSEL_INVALID7 (0x7 << 20)
	67	+#define UARTAPP_CTRL2_TXIFLSEL_OFFSET 16
	68	+#define UARTAPP_CTRL2_TXIFLSEL_MASK (0x7 << 16)
	69	+#define UARTAPP_CTRL2_TXIFLSEL_EMPTY (0x0 << 16)
	70	+#define UARTAPP_CTRL2_TXIFLSEL_ONE_QUARTER (0x1 << 16)
	71	+#define UARTAPP_CTRL2_TXIFLSEL_ONE_HALF (0x2 << 16)
	72	+#define UARTAPP_CTRL2_TXIFLSEL_THREE_QUARTERS (0x3 << 16)
	73	+#define UARTAPP_CTRL2_TXIFLSEL_SEVEN_EIGHTHS (0x4 << 16)
	74	+#define UARTAPP_CTRL2_TXIFLSEL_INVALID5 (0x5 << 16)
	75	+#define UARTAPP_CTRL2_TXIFLSEL_INVALID6 (0x6 << 16)
	76	+#define UARTAPP_CTRL2_TXIFLSEL_INVALID7 (0x7 << 16)
	77	+#define UARTAPP_CTRL2_CTSEN_MASK (1 << 15)
	78	+#define UARTAPP_CTRL2_RTSEN_MASK (1 << 14)
	79	+#define UARTAPP_CTRL2_OUT2_MASK (1 << 13)
	80	+#define UARTAPP_CTRL2_OUT1_MASK (1 << 12)
	81	+#define UARTAPP_CTRL2_RTS_MASK (1 << 11)
	82	+#define UARTAPP_CTRL2_DTR_MASK (1 << 10)
	83	+#define UARTAPP_CTRL2_RXE_MASK (1 << 9)
	84	+#define UARTAPP_CTRL2_TXE_MASK (1 << 8)
	85	+#define UARTAPP_CTRL2_LBE_MASK (1 << 7)
	86	+#define UARTAPP_CTRL2_USE_LCR2_MASK (1 << 6)
	87	+
	88	+#define UARTAPP_CTRL2_SIRLP_MASK (1 << 2)
	89	+#define UARTAPP_CTRL2_SIREN_MASK (1 << 1)
	90	+#define UARTAPP_CTRL2_UARTEN_MASK 0x01
	91	+
	92	+#define UARTAPP_LINECTRL_BAUD_DIVINT_OFFSET 16
	93	+#define UARTAPP_LINECTRL_BAUD_DIVINT_MASK (0xFFFF << 16)
	94	+#define UARTAPP_LINECTRL_EXTRACT_BAUD_DIVINT_OFFSET 6
	95	+
	96	+#define UARTAPP_LINECTRL_BAUD_DIVFRAC_OFFSET 8
	97	+#define UARTAPP_LINECTRL_BAUD_DIVFRAC_MASK (0x3F << 8)
	98	+#define UARTAPP_LINECTRL_EXTRACT_BAUD_DIVFRAC_MASK 0x3F
	99	+
	100	+#define UARTAPP_LINECTRL_SPS_MASK (1 << 7)
	101	+#define UARTAPP_LINECTRL_WLEN_OFFSET 5
	102	+#define UARTAPP_LINECTRL_WLEN_MASK (0x03 << 5)
	103	+#define UARTAPP_LINECTRL_WLEN_5BITS (0x00 << 5)
	104	+#define UARTAPP_LINECTRL_WLEN_6BITS (0x01 << 5)
	105	+#define UARTAPP_LINECTRL_WLEN_7BITS (0x02 << 5)
	106	+#define UARTAPP_LINECTRL_WLEN_8BITS (0x03 << 5)
	107	+
	108	+#define UARTAPP_LINECTRL_FEN_MASK (1 << 4)
	109	+#define UARTAPP_LINECTRL_STP2_MASK (1 << 3)
	110	+#define UARTAPP_LINECTRL_EPS_MASK (1 << 2)
	111	+#define UARTAPP_LINECTRL_PEN_MASK (1 << 1)
	112	+#define UARTAPP_LINECTRL_BRK_MASK 1
	113	+
	114	+#define UARTAPP_LINECTRL2_BAUD_DIVINT_OFFSET 16
	115	+#define UARTAPP_LINECTRL2_BAUD_DIVINT_MASK (0xFFFF << 16)
	116	+#define UARTAPP_LINECTRL2_EXTRACT_BAUD_DIVINT_OFFSET 6
	117	+
	118	+#define UARTAPP_LINECTRL2_BAUD_DIVFRAC_OFFSET 8
	119	+#define UARTAPP_LINECTRL2_BAUD_DIVFRAC_MASK (0x3F << 8)
	120	+#define UARTAPP_LINECTRL2_EXTRACT_BAUD_DIVFRAC_MASK 0x3F
	121	+
	122	+#define UARTAPP_LINECTRL2_SPS_MASK (1 << 7)
	123	+#define UARTAPP_LINECTRL2_WLEN_OFFSET 5
	124	+#define UARTAPP_LINECTRL2_WLEN_MASK (0x03 << 5)
	125	+#define UARTAPP_LINECTRL2_WLEN_5BITS (0x00 << 5)
	126	+#define UARTAPP_LINECTRL2_WLEN_6BITS (0x01 << 5)
	127	+#define UARTAPP_LINECTRL2_WLEN_7BITS (0x02 << 5)
	128	+#define UARTAPP_LINECTRL2_WLEN_8BITS (0x03 << 5)
	129	+
	130	+#define UARTAPP_LINECTRL2_FEN_MASK (1 << 4)
	131	+#define UARTAPP_LINECTRL2_STP2_MASK (1 << 3)
	132	+#define UARTAPP_LINECTRL2_EPS_MASK (1 << 2)
	133	+#define UARTAPP_LINECTRL2_PEN_MASK (1 << 1)
	134	+
	135	+#define UARTAPP_INTR_ABDIEN_MASK (1 << 27)
	136	+#define UARTAPP_INTR_OEIEN_MASK (1 << 26)
	137	+#define UARTAPP_INTR_BEIEN_MASK (1 << 25)
	138	+#define UARTAPP_INTR_PEIEN_MASK (1 << 24)
	139	+#define UARTAPP_INTR_FEIEN_MASK (1 << 23)
	140	+#define UARTAPP_INTR_RTIEN_MASK (1 << 22)
	141	+#define UARTAPP_INTR_TXIEN_MASK (1 << 21)
	142	+#define UARTAPP_INTR_RXIEN_MASK (1 << 20)
	143	+#define UARTAPP_INTR_DSRMIEN_MASK (1 << 19)
	144	+#define UARTAPP_INTR_DCDMIEN_MASK (1 << 18)
	145	+#define UARTAPP_INTR_CTSMIEN_MASK (1 << 17)
	146	+#define UARTAPP_INTR_RIMIEN_MASK (1 << 16)
	147	+
	148	+#define UARTAPP_INTR_ABDIS_MASK (1 << 11)
	149	+#define UARTAPP_INTR_OEIS_MASK (1 << 10)
	150	+#define UARTAPP_INTR_BEIS_MASK (1 << 9)
	151	+#define UARTAPP_INTR_PEIS_MASK (1 << 8)
	152	+#define UARTAPP_INTR_FEIS_MASK (1 << 7)
	153	+#define UARTAPP_INTR_RTIS_MASK (1 << 6)
	154	+#define UARTAPP_INTR_TXIS_MASK (1 << 5)
	155	+#define UARTAPP_INTR_RXIS_MASK (1 << 4)
	156	+#define UARTAPP_INTR_DSRMIS_MASK (1 << 3)
	157	+#define UARTAPP_INTR_DCDMIS_MASK (1 << 2)
	158	+#define UARTAPP_INTR_CTSMIS_MASK (1 << 1)
	159	+#define UARTAPP_INTR_RIMIS_MASK 0x1
	160	+
	161	+#define UARTAPP_DATA_DATA_OFFSET 0
	162	+#define UARTAPP_DATA_DATA_MASK 0xFFFFFFFF
	163	+#define UARTAPP_STAT_PRESENT_MASK (1 << 31)
	164	+#define UARTAPP_STAT_PRESENT_UNAVAILABLE (0x0 << 31)
	165	+#define UARTAPP_STAT_PRESENT_AVAILABLE (0x1 << 31)
	166	+
	167	+#define UARTAPP_STAT_HISPEED_MASK (1 << 30)
	168	+#define UARTAPP_STAT_HISPEED_UNAVAILABLE (0x0 << 30)
	169	+#define UARTAPP_STAT_HISPEED_AVAILABLE (0x1 << 30)
	170	+
	171	+#define UARTAPP_STAT_BUSY_MASK (1 << 29)
	172	+#define UARTAPP_STAT_CTS_MASK (1 << 28)
	173	+#define UARTAPP_STAT_TXFE_MASK (1 << 27)
	174	+#define UARTAPP_STAT_RXFF_MASK (1 << 26)
	175	+#define UARTAPP_STAT_TXFF_MASK (1 << 25)
	176	+#define UARTAPP_STAT_RXFE_MASK (1 << 24)
	177	+#define UARTAPP_STAT_RXBYTE_INVALID_OFFSET 20
	178	+#define UARTAPP_STAT_RXBYTE_INVALID_MASK (0xF << 20)
	179	+
	180	+#define UARTAPP_STAT_OERR_MASK (1 << 19)
	181	+#define UARTAPP_STAT_BERR_MASK (1 << 18)
	182	+#define UARTAPP_STAT_PERR_MASK (1 << 17)
	183	+#define UARTAPP_STAT_FERR_MASK (1 << 16)
	184	+#define UARTAPP_STAT_RXCOUNT_OFFSET 0
	185	+#define UARTAPP_STAT_RXCOUNT_MASK 0xFFFF
	186	+
	187	+#define UARTAPP_DEBUG_RXIBAUD_DIV_OFFSET 16
	188	+#define UARTAPP_DEBUG_RXIBAUD_DIV_MASK (0xFFFF << 16)
	189	+
	190	+#define UARTAPP_DEBUG_RXFBAUD_DIV_OFFSET 10
	191	+#define UARTAPP_DEBUG_RXFBAUD_DIV_MASK (0x3F << 10)
	192	+
	193	+#define UARTAPP_DEBUG_TXDMARUN_MASK (1 << 5)
	194	+#define UARTAPP_DEBUG_RXDMARUN_MASK (1 << 4)
	195	+#define UARTAPP_DEBUG_TXCMDEND_MASK (1 << 3)
	196	+#define UARTAPP_DEBUG_RXCMDEND_MASK (1 << 2)
	197	+#define UARTAPP_DEBUG_TXDMARQ_MASK (1 << 1)
	198	+#define UARTAPP_DEBUG_RXDMARQ_MASK 0x01
	199	+
	200	+#define UARTAPP_VERSION_MAJOR_OFFSET 24
	201	+#define UARTAPP_VERSION_MAJOR_MASK (0xFF << 24)
	202	+
	203	+#define UARTAPP_VERSION_MINOR_OFFSET 16
	204	+#define UARTAPP_VERSION_MINOR_MASK (0xFF << 16)
	205	+
	206	+#define UARTAPP_VERSION_STEP_OFFSET 0
	207	+#define UARTAPP_VERSION_STEP_MASK 0xFFFF
	208	+
	209	+#define UARTAPP_AUTOBAUD_REFCHAR1_OFFSET 24
	210	+#define UARTAPP_AUTOBAUD_REFCHAR1_MASK (0xFF << 24)
	211	+
	212	+#define UARTAPP_AUTOBAUD_REFCHAR0_OFFSET 16
	213	+#define UARTAPP_AUTOBAUD_REFCHAR0_MASK (0xFF << 16)
	214	+
	215	+#define UARTAPP_AUTOBAUD_UPDATE_TX_MASK (1 << 4)
	216	+#define UARTAPP_AUTOBAUD_TWO_REF_CHARS_MASK (1 << 3)
	217	+#define UARTAPP_AUTOBAUD_START_WITH_RUNBIT_MASK (1 << 2)
	218	+#define UARTAPP_AUTOBAUD_START_BAUD_DETECT_MASK (1 << 1)
	219	+#define UARTAPP_AUTOBAUD_BAUD_DETECT_ENABLE_MASK 0x01
	220	+#endif /* __ARCH_ARM___UARTAPP_H */
...	...	@@ -622,7 +622,6 @@
622	622	static void setup_display(void)
623	623	{
624	624	struct mxc_ccm_reg mxc_ccm = (struct mxc_ccm_reg )CCM_BASE_ADDR;
625		- struct anatop_regs anatop = (struct anatop_regs )ANATOP_BASE_ADDR;
626	625	struct iomuxc iomux = (struct iomuxc )IOMUXC_BASE_ADDR;
627	626	int reg;
628	627
...	...	@@ -633,10 +632,6 @@
633	632	reg \|= MXC_CCM_CCGR3_LDB_DI0_MASK;
634	633	writel(reg, &mxc_ccm->CCGR3);
635	634
636		- /* set PFD1_FRAC to 0x13 == 455 MHz (48018)/0x13 /
637		- writel(ANATOP_PFD_480_PFD1_FRAC_MASK, &anatop->pfd_480_clr);
638		- writel(0x13<<ANATOP_PFD_480_PFD1_FRAC_SHIFT, &anatop->pfd_480_set);
639		-
640	635	/* set LDB0, LDB1 clk select to 011/011 */
641	636	reg = readl(&mxc_ccm->cs2cdr);
642	637	reg &= ~(MXC_CCM_CS2CDR_LDB_DI0_CLK_SEL_MASK
...	...	@@ -666,7 +661,8 @@
666	661	writel(reg, &iomux->gpr[2]);
667	662
668	663	reg = readl(&iomux->gpr[3]);
669		- reg = (reg & ~IOMUXC_GPR3_LVDS0_MUX_CTL_MASK)
	664	+ reg = (reg & ~(IOMUXC_GPR3_LVDS0_MUX_CTL_MASK
	665	+ \|IOMUXC_GPR3_HDMI_MUX_CTL_MASK))
670	666	\| (IOMUXC_GPR3_MUX_SRC_IPU1_DI0
671	667	<<IOMUXC_GPR3_LVDS0_MUX_CTL_OFFSET);
672	668	writel(reg, &iomux->gpr[3]);