Merge branch 'master' of git://www.denx.de/git/u-boot-imx

10 files changed, 408 insertions, 503 deletions

diff --git a/arch/arm/cpu/armv7/mx6/Kconfig b/arch/arm/cpu/armv7/mx6/Kconfig
index 0b02e9e778..273e209cbb 100644
--- a/arch/arm/cpu/armv7/mx6/Kconfig
+++ b/arch/arm/cpu/armv7/mx6/Kconfig
@@ -23,10 +23,12 @@ config MX6SL
 	bool
 
 config MX6SX
+	select ROM_UNIFIED_SECTIONS
 	bool
 
 config MX6UL
 	select SYS_L2CACHE_OFF
+	select ROM_UNIFIED_SECTIONS
 	bool
 
 choice
diff --git a/arch/arm/cpu/armv7/mx6/Makefile b/arch/arm/cpu/armv7/mx6/Makefile
index bf6effc939..8af191d660 100644
--- a/arch/arm/cpu/armv7/mx6/Makefile
+++ b/arch/arm/cpu/armv7/mx6/Makefile
@@ -9,5 +9,4 @@
 
 obj-y	:= soc.o clock.o
 obj-$(CONFIG_SPL_BUILD)	     += ddr.o
-obj-$(CONFIG_SECURE_BOOT)    += hab.o
 obj-$(CONFIG_MP)             += mp.o
diff --git a/arch/arm/cpu/armv7/mx6/clock.c b/arch/arm/cpu/armv7/mx6/clock.c
index 11efd12c9a..67e0f3252f 100644
--- a/arch/arm/cpu/armv7/mx6/clock.c
+++ b/arch/arm/cpu/armv7/mx6/clock.c
@@ -473,6 +473,251 @@ static u32 get_mmdc_ch0_clk(void)
 	}
 }
 
+#if defined(CONFIG_VIDEO_MXS)
+static int enable_pll_video(u32 pll_div, u32 pll_num, u32 pll_denom,
+			    u32 post_div)
+{
+	u32 reg = 0;
+	ulong start;
+
+	debug("pll5 div = %d, num = %d, denom = %d\n",
+	      pll_div, pll_num, pll_denom);
+
+	/* Power up PLL5 video */
+	writel(BM_ANADIG_PLL_VIDEO_POWERDOWN |
+	       BM_ANADIG_PLL_VIDEO_BYPASS |
+	       BM_ANADIG_PLL_VIDEO_DIV_SELECT |
+	       BM_ANADIG_PLL_VIDEO_POST_DIV_SELECT,
+	       &imx_ccm->analog_pll_video_clr);
+
+	/* Set div, num and denom */
+	switch (post_div) {
+	case 1:
+		writel(BF_ANADIG_PLL_VIDEO_DIV_SELECT(pll_div) |
+		       BF_ANADIG_PLL_VIDEO_POST_DIV_SELECT(0x2),
+		       &imx_ccm->analog_pll_video_set);
+		break;
+	case 2:
+		writel(BF_ANADIG_PLL_VIDEO_DIV_SELECT(pll_div) |
+		       BF_ANADIG_PLL_VIDEO_POST_DIV_SELECT(0x1),
+		       &imx_ccm->analog_pll_video_set);
+		break;
+	case 4:
+		writel(BF_ANADIG_PLL_VIDEO_DIV_SELECT(pll_div) |
+		       BF_ANADIG_PLL_VIDEO_POST_DIV_SELECT(0x0),
+		       &imx_ccm->analog_pll_video_set);
+		break;
+	default:
+		puts("Wrong test_div!\n");
+		return -EINVAL;
+	}
+
+	writel(BF_ANADIG_PLL_VIDEO_NUM_A(pll_num),
+	       &imx_ccm->analog_pll_video_num);
+	writel(BF_ANADIG_PLL_VIDEO_DENOM_B(pll_denom),
+	       &imx_ccm->analog_pll_video_denom);
+
+	/* Wait PLL5 lock */
+	start = get_timer(0);	/* Get current timestamp */
+
+	do {
+		reg = readl(&imx_ccm->analog_pll_video);
+		if (reg & BM_ANADIG_PLL_VIDEO_LOCK) {
+			/* Enable PLL out */
+			writel(BM_ANADIG_PLL_VIDEO_ENABLE,
+			       &imx_ccm->analog_pll_video_set);
+			return 0;
+		}
+	} while (get_timer(0) < (start + 10)); /* Wait 10ms */
+
+	puts("Lock PLL5 timeout\n");
+
+	return -ETIME;
+}
+
+/*
+ * 24M--> PLL_VIDEO -> LCDIFx_PRED -> LCDIFx_PODF -> LCD
+ *
+ * 'freq' using KHz as unit, see driver/video/mxsfb.c.
+ */
+void mxs_set_lcdclk(u32 base_addr, u32 freq)
+{
+	u32 reg = 0;
+	u32 hck = MXC_HCLK / 1000;
+	/* DIV_SELECT ranges from 27 to 54 */
+	u32 min = hck * 27;
+	u32 max = hck * 54;
+	u32 temp, best = 0;
+	u32 i, j, max_pred = 8, max_postd = 8, pred = 1, postd = 1;
+	u32 pll_div, pll_num, pll_denom, post_div = 1;
+
+	debug("mxs_set_lcdclk, freq = %dKHz\n", freq);
+
+	if ((!is_cpu_type(MXC_CPU_MX6SX)) && !is_cpu_type(MXC_CPU_MX6UL)) {
+		debug("This chip not support lcd!\n");
+		return;
+	}
+
+	if (base_addr == LCDIF1_BASE_ADDR) {
+		reg = readl(&imx_ccm->cscdr2);
+		/* Can't change clocks when clock not from pre-mux */
+		if ((reg & MXC_CCM_CSCDR2_LCDIF1_CLK_SEL_MASK) != 0)
+			return;
+	}
+
+	if (is_cpu_type(MXC_CPU_MX6SX)) {
+		reg = readl(&imx_ccm->cscdr2);
+		/* Can't change clocks when clock not from pre-mux */
+		if ((reg & MXC_CCM_CSCDR2_LCDIF2_CLK_SEL_MASK) != 0)
+			return;
+	}
+
+	temp = freq * max_pred * max_postd;
+	if (temp > max) {
+		puts("Please decrease freq, too large!\n");
+		return;
+	}
+	if (temp < min) {
+		/*
+		 * Register: PLL_VIDEO
+		 * Bit Field: POST_DIV_SELECT
+		 * 00 — Divide by 4.
+		 * 01 — Divide by 2.
+		 * 10 — Divide by 1.
+		 * 11 — Reserved
+		 * No need to check post_div(1)
+		 */
+		for (post_div = 2; post_div <= 4; post_div <<= 1) {
+			if ((temp * post_div) > min) {
+				freq *= post_div;
+				break;
+			}
+		}
+
+		if (post_div > 4) {
+			printf("Fail to set rate to %dkhz", freq);
+			return;
+		}
+	}
+
+	/* Choose the best pred and postd to match freq for lcd */
+	for (i = 1; i <= max_pred; i++) {
+		for (j = 1; j <= max_postd; j++) {
+			temp = freq * i * j;
+			if (temp > max || temp < min)
+				continue;
+			if (best == 0 || temp < best) {
+				best = temp;
+				pred = i;
+				postd = j;
+			}
+		}
+	}
+
+	if (best == 0) {
+		printf("Fail to set rate to %dKHz", freq);
+		return;
+	}
+
+	debug("best %d, pred = %d, postd = %d\n", best, pred, postd);
+
+	pll_div = best / hck;
+	pll_denom = 1000000;
+	pll_num = (best - hck * pll_div) * pll_denom / hck;
+
+	/*
+	 *                                  pll_num
+	 *             (24MHz * (pll_div + --------- ))
+	 *                                 pll_denom
+	 *freq KHz =  --------------------------------
+	 *             post_div * pred * postd * 1000
+	 */
+
+	if (base_addr == LCDIF1_BASE_ADDR) {
+		if (enable_pll_video(pll_div, pll_num, pll_denom, post_div))
+			return;
+
+		/* Select pre-lcd clock to PLL5 and set pre divider */
+		clrsetbits_le32(&imx_ccm->cscdr2,
+				MXC_CCM_CSCDR2_LCDIF1_PRED_SEL_MASK |
+				MXC_CCM_CSCDR2_LCDIF1_PRE_DIV_MASK,
+				(0x2 << MXC_CCM_CSCDR2_LCDIF1_PRED_SEL_OFFSET) |
+				((pred - 1) <<
+				 MXC_CCM_CSCDR2_LCDIF1_PRE_DIV_OFFSET));
+
+		/* Set the post divider */
+		clrsetbits_le32(&imx_ccm->cbcmr,
+				MXC_CCM_CBCMR_LCDIF1_PODF_MASK,
+				((postd - 1) <<
+				 MXC_CCM_CBCMR_LCDIF1_PODF_OFFSET));
+	} else if (is_cpu_type(MXC_CPU_MX6SX)) {
+		/* Setting LCDIF2 for i.MX6SX */
+		if (enable_pll_video(pll_div, pll_num, pll_denom, post_div))
+			return;
+
+		/* Select pre-lcd clock to PLL5 and set pre divider */
+		clrsetbits_le32(&imx_ccm->cscdr2,
+				MXC_CCM_CSCDR2_LCDIF2_PRED_SEL_MASK |
+				MXC_CCM_CSCDR2_LCDIF2_PRE_DIV_MASK,
+				(0x2 << MXC_CCM_CSCDR2_LCDIF2_PRED_SEL_OFFSET) |
+				((pred - 1) <<
+				 MXC_CCM_CSCDR2_LCDIF2_PRE_DIV_OFFSET));
+
+		/* Set the post divider */
+		clrsetbits_le32(&imx_ccm->cscmr1,
+				MXC_CCM_CSCMR1_LCDIF2_PODF_MASK,
+				((postd - 1) <<
+				 MXC_CCM_CSCMR1_LCDIF2_PODF_OFFSET));
+	}
+}
+
+int enable_lcdif_clock(u32 base_addr)
+{
+	u32 reg = 0;
+	u32 lcdif_clk_sel_mask, lcdif_ccgr3_mask;
+
+	if (is_cpu_type(MXC_CPU_MX6SX)) {
+		if ((base_addr == LCDIF1_BASE_ADDR) ||
+		    (base_addr == LCDIF2_BASE_ADDR)) {
+			puts("Wrong LCD interface!\n");
+			return -EINVAL;
+		}
+		/* Set to pre-mux clock at default */
+		lcdif_clk_sel_mask = (base_addr == LCDIF2_BASE_ADDR) ?
+			MXC_CCM_CSCDR2_LCDIF2_CLK_SEL_MASK :
+			MXC_CCM_CSCDR2_LCDIF1_CLK_SEL_MASK;
+		lcdif_ccgr3_mask = (base_addr == LCDIF2_BASE_ADDR) ?
+			(MXC_CCM_CCGR3_LCDIF2_PIX_MASK |
+			 MXC_CCM_CCGR3_DISP_AXI_MASK) :
+			(MXC_CCM_CCGR3_LCDIF1_PIX_MASK |
+			 MXC_CCM_CCGR3_DISP_AXI_MASK);
+	} else if (is_cpu_type(MXC_CPU_MX6UL)) {
+		if (base_addr != LCDIF1_BASE_ADDR) {
+			puts("Wrong LCD interface!\n");
+			return -EINVAL;
+		}
+		/* Set to pre-mux clock at default */
+		lcdif_clk_sel_mask = MXC_CCM_CSCDR2_LCDIF1_CLK_SEL_MASK;
+		lcdif_ccgr3_mask =  MXC_CCM_CCGR3_LCDIF1_PIX_MASK;
+	} else {
+		return 0;
+	}
+
+	reg = readl(&imx_ccm->cscdr2);
+	reg &= ~lcdif_clk_sel_mask;
+	writel(reg, &imx_ccm->cscdr2);
+
+	/* Enable the LCDIF pix clock */
+	reg = readl(&imx_ccm->CCGR3);
+	reg |= lcdif_ccgr3_mask;
+	writel(reg, &imx_ccm->CCGR3);
+
+	reg = readl(&imx_ccm->CCGR2);
+	reg |= MXC_CCM_CCGR2_LCD_MASK;
+	writel(reg, &imx_ccm->CCGR2);
+}
+#endif
+
 #ifdef CONFIG_FSL_QSPI
 /* qspi_num can be from 0 - 1 */
 void enable_qspi_clk(int qspi_num)
diff --git a/arch/arm/cpu/armv7/mx6/hab.c b/arch/arm/cpu/armv7/mx6/hab.c
deleted file mode 100644
index 27cabe477d..0000000000
--- a/arch/arm/cpu/armv7/mx6/hab.c
+++ /dev/null
@@ -1,502 +0,0 @@
-/*
- * Copyright (C) 2010-2015 Freescale Semiconductor, Inc.
- *
- * SPDX-License-Identifier:    GPL-2.0+
- */
-
-#include <common.h>
-#include <asm/io.h>
-#include <asm/system.h>
-#include <asm/arch/hab.h>
-#include <asm/arch/clock.h>
-#include <asm/arch/sys_proto.h>
-
-/* -------- start of HAB API updates ------------*/
-
-#define hab_rvt_report_event_p					\
-(								\
-	((is_cpu_type(MXC_CPU_MX6Q) ||				\
-	  is_cpu_type(MXC_CPU_MX6D)) &&				\
-	  (soc_rev() >= CHIP_REV_1_5)) ?			\
-	((hab_rvt_report_event_t *)HAB_RVT_REPORT_EVENT_NEW) :	\
-	(is_cpu_type(MXC_CPU_MX6DL) &&				\
-	 (soc_rev() >= CHIP_REV_1_2)) ?				\
-	((hab_rvt_report_event_t *)HAB_RVT_REPORT_EVENT_NEW) :	\
-	((hab_rvt_report_event_t *)HAB_RVT_REPORT_EVENT)	\
-)
-
-#define hab_rvt_report_status_p					\
-(								\
-	((is_cpu_type(MXC_CPU_MX6Q) ||				\
-	  is_cpu_type(MXC_CPU_MX6D)) &&				\
-	  (soc_rev() >= CHIP_REV_1_5)) ?			\
-	((hab_rvt_report_status_t *)HAB_RVT_REPORT_STATUS_NEW) :\
-	(is_cpu_type(MXC_CPU_MX6DL) &&				\
-	 (soc_rev() >= CHIP_REV_1_2)) ?				\
-	((hab_rvt_report_status_t *)HAB_RVT_REPORT_STATUS_NEW) :\
-	((hab_rvt_report_status_t *)HAB_RVT_REPORT_STATUS)	\
-)
-
-#define hab_rvt_authenticate_image_p				\
-(								\
-	((is_cpu_type(MXC_CPU_MX6Q) ||				\
-	  is_cpu_type(MXC_CPU_MX6D)) &&				\
-	  (soc_rev() >= CHIP_REV_1_5)) ?			\
-	((hab_rvt_authenticate_image_t *)HAB_RVT_AUTHENTICATE_IMAGE_NEW) : \
-	(is_cpu_type(MXC_CPU_MX6DL) &&				\
-	 (soc_rev() >= CHIP_REV_1_2)) ?				\
-	((hab_rvt_authenticate_image_t *)HAB_RVT_AUTHENTICATE_IMAGE_NEW) : \
-	((hab_rvt_authenticate_image_t *)HAB_RVT_AUTHENTICATE_IMAGE)	\
-)
-
-#define hab_rvt_entry_p						\
-(								\
-	((is_cpu_type(MXC_CPU_MX6Q) ||				\
-	  is_cpu_type(MXC_CPU_MX6D)) &&				\
-	  (soc_rev() >= CHIP_REV_1_5)) ?			\
-	((hab_rvt_entry_t *)HAB_RVT_ENTRY_NEW) :		\
-	(is_cpu_type(MXC_CPU_MX6DL) &&				\
-	 (soc_rev() >= CHIP_REV_1_2)) ?				\
-	((hab_rvt_entry_t *)HAB_RVT_ENTRY_NEW) :		\
-	((hab_rvt_entry_t *)HAB_RVT_ENTRY)			\
-)
-
-#define hab_rvt_exit_p						\
-(								\
-	((is_cpu_type(MXC_CPU_MX6Q) ||				\
-	  is_cpu_type(MXC_CPU_MX6D)) &&				\
-	  (soc_rev() >= CHIP_REV_1_5)) ?			\
-	((hab_rvt_exit_t *)HAB_RVT_EXIT_NEW) :			\
-	(is_cpu_type(MXC_CPU_MX6DL) &&				\
-	 (soc_rev() >= CHIP_REV_1_2)) ?				\
-	((hab_rvt_exit_t *)HAB_RVT_EXIT_NEW) :			\
-	((hab_rvt_exit_t *)HAB_RVT_EXIT)			\
-)
-
-#define IVT_SIZE		0x20
-#define ALIGN_SIZE		0x1000
-#define CSF_PAD_SIZE		0x2000
-#define MX6DQ_PU_IROM_MMU_EN_VAR	0x009024a8
-#define MX6DLS_PU_IROM_MMU_EN_VAR	0x00901dd0
-#define MX6SL_PU_IROM_MMU_EN_VAR	0x00900a18
-
-/*
- * +------------+  0x0 (DDR_UIMAGE_START) -
- * |   Header   |                          |
- * +------------+  0x40                    |
- * |            |                          |
- * |            |                          |
- * |            |                          |
- * |            |                          |
- * | Image Data |                          |
- * .            |                          |
- * .            |                           > Stuff to be authenticated ----+
- * .            |                          |                                |
- * |            |                          |                                |
- * |            |                          |                                |
- * +------------+                          |                                |
- * |            |                          |                                |
- * | Fill Data  |                          |                                |
- * |            |                          |                                |
- * +------------+ Align to ALIGN_SIZE      |                                |
- * |    IVT     |                          |                                |
- * +------------+ + IVT_SIZE              -                                 |
- * |            |                                                           |
- * |  CSF DATA  | <---------------------------------------------------------+
- * |            |
- * +------------+
- * |            |
- * | Fill Data  |
- * |            |
- * +------------+ + CSF_PAD_SIZE
- */
-
-#define MAX_RECORD_BYTES     (8*1024) /* 4 kbytes */
-
-struct record {
-	uint8_t  tag;						/* Tag */
-	uint8_t  len[2];					/* Length */
-	uint8_t  par;						/* Version */
-	uint8_t  contents[MAX_RECORD_BYTES];/* Record Data */
-	bool	 any_rec_flag;
-};
-
-char *rsn_str[] = {"RSN = HAB_RSN_ANY (0x00)\n",
-				   "RSN = HAB_ENG_FAIL (0x30)\n",
-				   "RSN = HAB_INV_ADDRESS (0x22)\n",
-				   "RSN = HAB_INV_ASSERTION (0x0C)\n",
-				   "RSN = HAB_INV_CALL (0x28)\n",
-				   "RSN = HAB_INV_CERTIFICATE (0x21)\n",
-				   "RSN = HAB_INV_COMMAND (0x06)\n",
-				   "RSN = HAB_INV_CSF (0x11)\n",
-				   "RSN = HAB_INV_DCD (0x27)\n",
-				   "RSN = HAB_INV_INDEX (0x0F)\n",
-				   "RSN = HAB_INV_IVT (0x05)\n",
-				   "RSN = HAB_INV_KEY (0x1D)\n",
-				   "RSN = HAB_INV_RETURN (0x1E)\n",
-				   "RSN = HAB_INV_SIGNATURE (0x18)\n",
-				   "RSN = HAB_INV_SIZE (0x17)\n",
-				   "RSN = HAB_MEM_FAIL (0x2E)\n",
-				   "RSN = HAB_OVR_COUNT (0x2B)\n",
-				   "RSN = HAB_OVR_STORAGE (0x2D)\n",
-				   "RSN = HAB_UNS_ALGORITHM (0x12)\n",
-				   "RSN = HAB_UNS_COMMAND (0x03)\n",
-				   "RSN = HAB_UNS_ENGINE (0x0A)\n",
-				   "RSN = HAB_UNS_ITEM (0x24)\n",
-				   "RSN = HAB_UNS_KEY (0x1B)\n",
-				   "RSN = HAB_UNS_PROTOCOL (0x14)\n",
-				   "RSN = HAB_UNS_STATE (0x09)\n",
-				   "RSN = INVALID\n",
-				   NULL};
-
-char *sts_str[] = {"STS = HAB_SUCCESS (0xF0)\n",
-				   "STS = HAB_FAILURE (0x33)\n",
-				   "STS = HAB_WARNING (0x69)\n",
-				   "STS = INVALID\n",
-				   NULL};
-
-char *eng_str[] = {"ENG = HAB_ENG_ANY (0x00)\n",
-				   "ENG = HAB_ENG_SCC (0x03)\n",
-				   "ENG = HAB_ENG_RTIC (0x05)\n",
-				   "ENG = HAB_ENG_SAHARA (0x06)\n",
-				   "ENG = HAB_ENG_CSU (0x0A)\n",
-				   "ENG = HAB_ENG_SRTC (0x0C)\n",
-				   "ENG = HAB_ENG_DCP (0x1B)\n",
-				   "ENG = HAB_ENG_CAAM (0x1D)\n",
-				   "ENG = HAB_ENG_SNVS (0x1E)\n",
-				   "ENG = HAB_ENG_OCOTP (0x21)\n",
-				   "ENG = HAB_ENG_DTCP (0x22)\n",
-				   "ENG = HAB_ENG_ROM (0x36)\n",
-				   "ENG = HAB_ENG_HDCP (0x24)\n",
-				   "ENG = HAB_ENG_RTL (0x77)\n",
-				   "ENG = HAB_ENG_SW (0xFF)\n",
-				   "ENG = INVALID\n",
-				   NULL};
-
-char *ctx_str[] = {"CTX = HAB_CTX_ANY(0x00)\n",
-				   "CTX = HAB_CTX_FAB (0xFF)\n",
-				   "CTX = HAB_CTX_ENTRY (0xE1)\n",
-				   "CTX = HAB_CTX_TARGET (0x33)\n",
-				   "CTX = HAB_CTX_AUTHENTICATE (0x0A)\n",
-				   "CTX = HAB_CTX_DCD (0xDD)\n",
-				   "CTX = HAB_CTX_CSF (0xCF)\n",
-				   "CTX = HAB_CTX_COMMAND (0xC0)\n",
-				   "CTX = HAB_CTX_AUT_DAT (0xDB)\n",
-				   "CTX = HAB_CTX_ASSERT (0xA0)\n",
-				   "CTX = HAB_CTX_EXIT (0xEE)\n",
-				   "CTX = INVALID\n",
-				   NULL};
-
-uint8_t hab_statuses[5] = {
-	HAB_STS_ANY,
-	HAB_FAILURE,
-	HAB_WARNING,
-	HAB_SUCCESS,
-	-1
-};
-
-uint8_t hab_reasons[26] = {
-	HAB_RSN_ANY,
-	HAB_ENG_FAIL,
-	HAB_INV_ADDRESS,
-	HAB_INV_ASSERTION,
-	HAB_INV_CALL,
-	HAB_INV_CERTIFICATE,
-	HAB_INV_COMMAND,
-	HAB_INV_CSF,
-	HAB_INV_DCD,
-	HAB_INV_INDEX,
-	HAB_INV_IVT,
-	HAB_INV_KEY,
-	HAB_INV_RETURN,
-	HAB_INV_SIGNATURE,
-	HAB_INV_SIZE,
-	HAB_MEM_FAIL,
-	HAB_OVR_COUNT,
-	HAB_OVR_STORAGE,
-	HAB_UNS_ALGORITHM,
-	HAB_UNS_COMMAND,
-	HAB_UNS_ENGINE,
-	HAB_UNS_ITEM,
-	HAB_UNS_KEY,
-	HAB_UNS_PROTOCOL,
-	HAB_UNS_STATE,
-	-1
-};
-
-uint8_t hab_contexts[12] = {
-	HAB_CTX_ANY,
-	HAB_CTX_FAB,
-	HAB_CTX_ENTRY,
-	HAB_CTX_TARGET,
-	HAB_CTX_AUTHENTICATE,
-	HAB_CTX_DCD,
-	HAB_CTX_CSF,
-	HAB_CTX_COMMAND,
-	HAB_CTX_AUT_DAT,
-	HAB_CTX_ASSERT,
-	HAB_CTX_EXIT,
-	-1
-};
-
-uint8_t hab_engines[16] = {
-	HAB_ENG_ANY,
-	HAB_ENG_SCC,
-	HAB_ENG_RTIC,
-	HAB_ENG_SAHARA,
-	HAB_ENG_CSU,
-	HAB_ENG_SRTC,
-	HAB_ENG_DCP,
-	HAB_ENG_CAAM,
-	HAB_ENG_SNVS,
-	HAB_ENG_OCOTP,
-	HAB_ENG_DTCP,
-	HAB_ENG_ROM,
-	HAB_ENG_HDCP,
-	HAB_ENG_RTL,
-	HAB_ENG_SW,
-	-1
-};
-
-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;
-}
-
-static inline uint8_t get_idx(uint8_t *list, uint8_t tgt)
-{
-	uint8_t idx = 0;
-	uint8_t element = list[idx];
-	while (element != -1) {
-		if (element == tgt)
-			return idx;
-		element = list[++idx];
-	}
-	return -1;
-}
-
-void process_event_record(uint8_t *event_data, size_t bytes)
-{
-	struct record *rec = (struct record *)event_data;
-
-	printf("\n\n%s", sts_str[get_idx(hab_statuses, rec->contents[0])]);
-	printf("%s", rsn_str[get_idx(hab_reasons, rec->contents[1])]);
-	printf("%s", ctx_str[get_idx(hab_contexts, rec->contents[2])]);
-	printf("%s", eng_str[get_idx(hab_engines, rec->contents[3])]);
-}
-
-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]);
-	}
-
-	process_event_record(event_data, bytes);
-}
-
-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;
-	hab_rvt_report_event_t *hab_rvt_report_event;
-	hab_rvt_report_status_t *hab_rvt_report_status;
-
-	hab_rvt_report_event = hab_rvt_report_event_p;
-	hab_rvt_report_status = hab_rvt_report_status_p;
-
-	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;
-}
-
-uint32_t authenticate_image(uint32_t ddr_start, uint32_t image_size)
-{
-	uint32_t load_addr = 0;
-	size_t bytes;
-	ptrdiff_t ivt_offset = 0;
-	int result = 0;
-	ulong start;
-	hab_rvt_authenticate_image_t *hab_rvt_authenticate_image;
-	hab_rvt_entry_t *hab_rvt_entry;
-	hab_rvt_exit_t *hab_rvt_exit;
-
-	hab_rvt_authenticate_image = hab_rvt_authenticate_image_p;
-	hab_rvt_entry = hab_rvt_entry_p;
-	hab_rvt_exit = hab_rvt_exit_p;
-
-	if (is_hab_enabled()) {
-		printf("\nAuthenticate image from DDR location 0x%x...\n",
-		       ddr_start);
-
-		hab_caam_clock_enable(1);
-
-		if (hab_rvt_entry() == HAB_SUCCESS) {
-			/* If not already aligned, Align to ALIGN_SIZE */
-			ivt_offset = (image_size + ALIGN_SIZE - 1) &
-					~(ALIGN_SIZE - 1);
-
-			start = ddr_start;
-			bytes = ivt_offset + IVT_SIZE + CSF_PAD_SIZE;
-#ifdef DEBUG
-			printf("\nivt_offset = 0x%x, ivt addr = 0x%x\n",
-			       ivt_offset, ddr_start + ivt_offset);
-			puts("Dumping IVT\n");
-			print_buffer(ddr_start + ivt_offset,
-				     (void *)(ddr_start + ivt_offset),
-				     4, 0x8, 0);
-
-			puts("Dumping CSF Header\n");
-			print_buffer(ddr_start + ivt_offset+IVT_SIZE,
-				     (void *)(ddr_start + ivt_offset+IVT_SIZE),
-				     4, 0x10, 0);
-
-			get_hab_status();
-
-			puts("\nCalling authenticate_image in ROM\n");
-			printf("\tivt_offset = 0x%x\n", ivt_offset);
-			printf("\tstart = 0x%08lx\n", start);
-			printf("\tbytes = 0x%x\n", bytes);
-#endif
-			/*
-			 * If the MMU is enabled, we have to notify the ROM
-			 * code, or it won't flush the caches when needed.
-			 * This is done, by setting the "pu_irom_mmu_enabled"
-			 * word to 1. You can find its address by looking in
-			 * the ROM map. This is critical for
-			 * authenticate_image(). If MMU is enabled, without
-			 * setting this bit, authentication will fail and may
-			 * crash.
-			 */
-			/* Check MMU enabled */
-			if (get_cr() & CR_M) {
-				if (is_cpu_type(MXC_CPU_MX6Q) ||
-				    is_cpu_type(MXC_CPU_MX6D)) {
-					/*
-					 * This won't work on Rev 1.0.0 of
-					 * i.MX6Q/D, since their ROM doesn't
-					 * do cache flushes. don't think any
-					 * exist, so we ignore them.
-					 */
-					if (!is_mx6dqp())
-						writel(1, MX6DQ_PU_IROM_MMU_EN_VAR);
-				} else if (is_cpu_type(MXC_CPU_MX6DL) ||
-					   is_cpu_type(MXC_CPU_MX6SOLO)) {
-					writel(1, MX6DLS_PU_IROM_MMU_EN_VAR);
-				} else if (is_cpu_type(MXC_CPU_MX6SL)) {
-					writel(1, MX6SL_PU_IROM_MMU_EN_VAR);
-				}
-			}
-
-			load_addr = (uint32_t)hab_rvt_authenticate_image(
-					HAB_CID_UBOOT,
-					ivt_offset, (void **)&start,
-					(size_t *)&bytes, NULL);
-			if (hab_rvt_exit() != HAB_SUCCESS) {
-				puts("hab exit function fail\n");
-				load_addr = 0;
-			}
-		} else {
-			puts("hab entry function fail\n");
-		}
-
-		hab_caam_clock_enable(0);
-
-		get_hab_status();
-	} else {
-		puts("hab fuse not enabled\n");
-	}
-
-	if ((!is_hab_enabled()) || (load_addr != 0))
-		result = 1;
-
-	return result;
-}
-
-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;
-}
-
-static int do_authenticate_image(cmd_tbl_t *cmdtp, int flag, int argc,
-				char * const argv[])
-{
-	ulong	addr, ivt_offset;
-	int	rcode = 0;
-
-	if (argc < 3)
-		return CMD_RET_USAGE;
-
-	addr = simple_strtoul(argv[1], NULL, 16);
-	ivt_offset = simple_strtoul(argv[2], NULL, 16);
-
-	rcode = authenticate_image(addr, ivt_offset);
-
-	return rcode;
-}
-
-U_BOOT_CMD(
-		hab_status, CONFIG_SYS_MAXARGS, 1, do_hab_status,
-		"display HAB status",
-		""
-	  );
-
-U_BOOT_CMD(
-		hab_auth_img, 3, 0, do_authenticate_image,
-		"authenticate image via HAB",
-		"addr ivt_offset\n"
-		"addr - image hex address\n"
-		"ivt_offset - hex offset of IVT in the image"
-	  );
diff --git a/arch/arm/cpu/armv7/mx6/soc.c b/arch/arm/cpu/armv7/mx6/soc.c
index 282302b1fc..bf5ae8cdff 100644
--- a/arch/arm/cpu/armv7/mx6/soc.c
+++ b/arch/arm/cpu/armv7/mx6/soc.c
@@ -15,6 +15,7 @@
 #include <asm/arch/sys_proto.h>
 #include <asm/imx-common/boot_mode.h>
 #include <asm/imx-common/dma.h>
+#include <asm/imx-common/hab.h>
 #include <stdbool.h>
 #include <asm/arch/mxc_hdmi.h>
 #include <asm/arch/crm_regs.h>
@@ -48,6 +49,13 @@ U_BOOT_DEVICE(imx6_thermal) = {
 };
 #endif
 
+#if defined(CONFIG_SECURE_BOOT)
+struct imx_sec_config_fuse_t const imx_sec_config_fuse = {
+	.bank = 0,
+	.word = 6,
+};
+#endif
+
 u32 get_nr_cpus(void)
 {
 	struct scu_regs *scu = (struct scu_regs *)SCU_BASE_ADDR;
@@ -392,6 +400,13 @@ const struct boot_mode soc_boot_modes[] = {
 	{NULL,		0},
 };
 
+void reset_misc(void)
+{
+#ifdef CONFIG_VIDEO_MXS
+	lcdif_power_down();
+#endif
+}
+
 void s_init(void)
 {
 	struct anatop_regs *anatop = (struct anatop_regs *)ANATOP_BASE_ADDR;
diff --git a/arch/arm/cpu/armv7/mx7/Kconfig b/arch/arm/cpu/armv7/mx7/Kconfig
index ea19e5c411..97d62389fd 100644
--- a/arch/arm/cpu/armv7/mx7/Kconfig
+++ b/arch/arm/cpu/armv7/mx7/Kconfig
@@ -2,9 +2,11 @@ if ARCH_MX7
 
 config MX7
 	bool
+	select ROM_UNIFIED_SECTIONS
 	default y
 
 config MX7D
+	select ROM_UNIFIED_SECTIONS
 	bool
 
 choice
diff --git a/arch/arm/cpu/armv7/mx7/Makefile b/arch/arm/cpu/armv7/mx7/Makefile
index e6ecef010c..d21f87f18c 100644
--- a/arch/arm/cpu/armv7/mx7/Makefile
+++ b/arch/arm/cpu/armv7/mx7/Makefile
@@ -6,3 +6,7 @@
 #
 
 obj-y	:= soc.o clock.o clock_slice.o
+
+ifdef CONFIG_ARMV7_PSCI
+obj-y  += psci-mx7.o psci.o
+endif
diff --git a/arch/arm/cpu/armv7/mx7/psci-mx7.c b/arch/arm/cpu/armv7/mx7/psci-mx7.c
new file mode 100644
index 0000000000..9a330476cf
--- /dev/null
+++ b/arch/arm/cpu/armv7/mx7/psci-mx7.c
@@ -0,0 +1,69 @@
+#include <asm/io.h>
+#include <asm/psci.h>
+#include <asm/arch/imx-regs.h>
+#include <common.h>
+
+#define __secure __attribute__((section("._secure.text")))
+
+#define GPC_CPU_PGC_SW_PDN_REQ	0xfc
+#define GPC_CPU_PGC_SW_PUP_REQ	0xf0
+#define GPC_PGC_C1		0x840
+
+#define BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7	0x2
+
+/* below is for i.MX7D */
+#define SRC_GPR1_MX7D		0x074
+#define SRC_A7RCR0		0x004
+#define SRC_A7RCR1		0x008
+
+#define BP_SRC_A7RCR0_A7_CORE_RESET0	0
+#define BP_SRC_A7RCR1_A7_CORE1_ENABLE	1
+
+static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset)
+{
+	writel(enable, GPC_IPS_BASE_ADDR + offset);
+}
+
+__secure void imx_gpcv2_set_core1_power(bool pdn)
+{
+	u32 reg = pdn ? GPC_CPU_PGC_SW_PUP_REQ : GPC_CPU_PGC_SW_PDN_REQ;
+	u32 val;
+
+	imx_gpcv2_set_m_core_pgc(true, GPC_PGC_C1);
+
+	val = readl(GPC_IPS_BASE_ADDR + reg);
+	val |= BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7;
+	writel(val, GPC_IPS_BASE_ADDR + reg);
+
+	while ((readl(GPC_IPS_BASE_ADDR + reg) &
+	       BM_CPU_PGC_SW_PDN_PUP_REQ_CORE1_A7) != 0)
+		;
+
+	imx_gpcv2_set_m_core_pgc(false, GPC_PGC_C1);
+}
+
+__secure void imx_enable_cpu_ca7(int cpu, bool enable)
+{
+	u32 mask, val;
+
+	mask = 1 << (BP_SRC_A7RCR1_A7_CORE1_ENABLE + cpu - 1);
+	val = readl(SRC_BASE_ADDR + SRC_A7RCR1);
+	val = enable ? val | mask : val & ~mask;
+	writel(val, SRC_BASE_ADDR + SRC_A7RCR1);
+}
+
+__secure int imx_cpu_on(int fn, int cpu, int pc)
+{
+	writel(pc, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D);
+	imx_gpcv2_set_core1_power(true);
+	imx_enable_cpu_ca7(cpu, true);
+	return 0;
+}
+
+__secure int imx_cpu_off(int cpu)
+{
+	imx_enable_cpu_ca7(cpu, false);
+	imx_gpcv2_set_core1_power(false);
+	writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4);
+	return 0;
+}
diff --git a/arch/arm/cpu/armv7/mx7/psci.S b/arch/arm/cpu/armv7/mx7/psci.S
new file mode 100644
index 0000000000..34c6ab33f0
--- /dev/null
+++ b/arch/arm/cpu/armv7/mx7/psci.S
@@ -0,0 +1,54 @@
+#include <config.h>
+#include <linux/linkage.h>
+
+#include <asm/armv7.h>
+#include <asm/arch-armv7/generictimer.h>
+#include <asm/psci.h>
+
+	.pushsection ._secure.text, "ax"
+
+	.arch_extension sec
+
+	@ r1 = target CPU
+	@ r2 = target PC
+
+.globl	psci_arch_init
+psci_arch_init:
+	mov	r6, lr
+
+	bl	psci_get_cpu_id
+	bl	psci_get_cpu_stack_top
+	mov	sp, r0
+
+	bx	r6
+
+	@ r1 = target CPU
+	@ r2 = target PC
+
+.globl psci_cpu_on
+psci_cpu_on:
+	push	{lr}
+
+	mov	r0, r1
+	bl	psci_get_cpu_stack_top
+	str	r2, [r0]
+	dsb
+
+	ldr	r2, =psci_cpu_entry
+	bl	imx_cpu_on
+
+	pop	{pc}
+
+.globl psci_cpu_off
+psci_cpu_off:
+
+	bl	psci_cpu_off_common
+	bl	psci_get_cpu_id
+	bl	imx_cpu_off
+
+1: 	wfi
+	b 1b
+
+	.globl psci_text_end
+psci_text_end:
+	.popsection
diff --git a/arch/arm/cpu/armv7/mx7/soc.c b/arch/arm/cpu/armv7/mx7/soc.c
index 2ed05ea4f9..c777922e9d 100644
--- a/arch/arm/cpu/armv7/mx7/soc.c
+++ b/arch/arm/cpu/armv7/mx7/soc.c
@@ -11,6 +11,7 @@
 #include <asm/arch/sys_proto.h>
 #include <asm/imx-common/boot_mode.h>
 #include <asm/imx-common/dma.h>
+#include <asm/imx-common/hab.h>
 #include <asm/arch/crm_regs.h>
 #include <dm.h>
 #include <imx_thermal.h>
@@ -28,6 +29,13 @@ U_BOOT_DEVICE(imx7_thermal) = {
 };
 #endif
 
+#if defined(CONFIG_SECURE_BOOT)
+struct imx_sec_config_fuse_t const imx_sec_config_fuse = {
+	.bank = 1,
+	.word = 3,
+};
+#endif
+
 /*
  * OCOTP_TESTER3[9:8] (see Fusemap Description Table offset 0x440)
  * defines a 2-bit SPEED_GRADING
@@ -114,10 +122,19 @@ u32 __weak get_board_rev(void)
 }
 #endif
 
+/* enable all periherial can be accessed in nosec mode */
+static void init_csu(void)
+{
+	int i = 0;
+	for (i = 0; i < CSU_NUM_REGS; i++)
+		writel(CSU_INIT_SEC_LEVEL0, CSU_IPS_BASE_ADDR + i * 4);
+}
+
 int arch_cpu_init(void)
 {
 	init_aips();
 
+	init_csu();
 	/* Disable PDE bit of WMCR register */
 	imx_set_wdog_powerdown(false);