MA-9376 [Android IMX] uboot: enable lock&unlock

Fix compile error for api change.

Porting below patches from v2015.o4:
    MA-7875 Enable CAAM for i.MX6
    MA-7875-1 Support fastboot lock&unlock in u-boot
    MA-7875-2 Support fastboot lock/unlock in i.MX6 platform
    MA-7875-3 Support fastboot lock/unlock in i.MX6UL
    MA-8425 fastboot: return OKAY in fastboot erase
    MA-8418 fix return value check for get_device_and_partition
    MA-8622 - [brillo] fix uboot compile warnings and code style warnings

Change-Id: I2370c3e5851cc1f92aaa93c200e6c079f7929af2
Signed-off-by: zhang sanshan <sanshan.zhang@nxp.com>

8 files changed, 1591 insertions, 47 deletions

diff --git a/drivers/crypto/Makefile b/drivers/crypto/Makefile
index fb8c10b..233d48e 100644
--- a/drivers/crypto/Makefile
+++ b/drivers/crypto/Makefile
@@ -6,5 +6,6 @@
 #
 
 obj-$(CONFIG_EXYNOS_ACE_SHA)	+= ace_sha.o
+obj-$(CONFIG_FSL_CAAM_KB)      += fsl_caam.o
 obj-y += rsa_mod_exp/
 obj-y += fsl/
diff --git a/drivers/crypto/fsl_caam.c b/drivers/crypto/fsl_caam.c
new file mode 100755
index 0000000..ee556bd
--- /dev/null
+++ b/drivers/crypto/fsl_caam.c
@@ -0,0 +1,432 @@
+/*
+ * Copyright (c) 2012-2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/crm_regs.h>
+#include "fsl_caam_internal.h"
+#include <fsl_caam.h>
+
+/*---------- Global variables ----------*/
+/* Input job ring - single entry input ring */
+uint32_t g_input_ring[JOB_RING_ENTRIES] = {0};
+
+
+/* Output job ring - single entry output ring (consists of two words) */
+uint32_t g_output_ring[2*JOB_RING_ENTRIES] = {0, 0};
+
+uint32_t decap_dsc[] =
+{
+	DECAP_BLOB_DESC1,
+	DECAP_BLOB_DESC2,
+	DECAP_BLOB_DESC3,
+	DECAP_BLOB_DESC4,
+	DECAP_BLOB_DESC5,
+	DECAP_BLOB_DESC6,
+	DECAP_BLOB_DESC7,
+	DECAP_BLOB_DESC8,
+	DECAP_BLOB_DESC9
+};
+
+uint32_t encap_dsc[] =
+{
+	ENCAP_BLOB_DESC1,
+	ENCAP_BLOB_DESC2,
+	ENCAP_BLOB_DESC3,
+	ENCAP_BLOB_DESC4,
+	ENCAP_BLOB_DESC5,
+	ENCAP_BLOB_DESC6,
+	ENCAP_BLOB_DESC7,
+	ENCAP_BLOB_DESC8,
+	ENCAP_BLOB_DESC9
+};
+
+uint32_t hwrng_dsc[6] = {0};
+uint32_t rng_inst_dsc[] =
+{
+	RNG_INST_DESC1,
+	RNG_INST_DESC2,
+	RNG_INST_DESC3,
+	RNG_INST_DESC4,
+	RNG_INST_DESC5,
+	RNG_INST_DESC6,
+	RNG_INST_DESC7,
+	RNG_INST_DESC8,
+	RNG_INST_DESC9
+};
+
+static uint8_t skeymod[] = {
+	0x0f, 0x0e, 0x0d, 0x0c, 0x0b, 0x0a, 0x09, 0x08,
+	0x07, 0x06, 0x05, 0x04, 0x03, 0x02, 0x01, 0x00
+};
+
+
+/* arm v7 need 64 align */
+#define ALIGN_MASK 0xffffffc0
+
+/*!
+ * Secure memory run command.
+ *
+ * @param   sec_mem_cmd  Secure memory command register
+ * @return  cmd_status  Secure memory command status register
+ */
+uint32_t secmem_set_cmd_1(uint32_t sec_mem_cmd)
+{
+	uint32_t temp_reg;
+	__raw_writel(sec_mem_cmd, CAAM_SMCJR0);
+	do {
+	temp_reg = __raw_readl(CAAM_SMCSJR0);
+	} while(temp_reg & CMD_COMPLETE);
+
+	return temp_reg;
+}
+
+
+/*!
+ * Use CAAM to decapsulate a blob to secure memory.
+ * Such blob of secret key cannot be read once decrypted,
+ * but can still be used for enc/dec operation of user's data.
+ *
+ * @param   blob_addr  Location address of the blob.
+ *
+ * @return  SUCCESS or ERROR_XXX
+ */
+uint32_t caam_decap_blob(uint32_t plain_text, uint32_t blob_addr, uint32_t size)
+{
+	uint32_t ret = SUCCESS;
+
+	/* Buffer that holds blob */
+
+
+	/* TODO: Fix Hardcoded Descriptor */
+	decap_dsc[0] = (uint32_t)0xB0800008;
+	decap_dsc[1] = (uint32_t)0x14400010;
+	decap_dsc[2] = (uint32_t)skeymod;
+	decap_dsc[3] = (uint32_t)0xF0000000 | (0x0000ffff & (size+48) );
+	decap_dsc[4] = blob_addr;
+	decap_dsc[5] = (uint32_t)0xF8000000 | (0x0000ffff & (size));
+	decap_dsc[6] = (uint32_t)(uint8_t*)plain_text;
+	decap_dsc[7] = (uint32_t)0x860D0000;
+
+/* uncomment when using descriptor from "fsl_caam_internal.h"
+   does not use key modifier. */
+
+    /* Run descriptor with result written to blob buffer */
+    /* Add job to input ring */
+	g_input_ring[0] = (uint32_t)decap_dsc;
+
+	flush_dcache_range((uint32_t)blob_addr & ALIGN_MASK,
+			   (((uint32_t)blob_addr + 2 * size + 64) & ALIGN_MASK));
+	flush_dcache_range((uint32_t)plain_text & ALIGN_MASK,
+			   (((uint32_t)plain_text + 2 * size + 64) & ALIGN_MASK));
+	flush_dcache_range((uint32_t)decap_dsc & ALIGN_MASK,
+			   ((uint32_t)decap_dsc & ALIGN_MASK) + 128);
+	flush_dcache_range((uint32_t)g_input_ring & ALIGN_MASK,
+			   ((uint32_t)g_input_ring & ALIGN_MASK) + 128);
+
+    /* Increment jobs added */
+	__raw_writel(1, CAAM_IRJAR0);
+
+    /* Wait for job ring to complete the job: 1 completed job expected */
+	while(__raw_readl(CAAM_ORSFR0) != 1);
+
+	// TODO: check if Secure memory is cacheable.
+	flush_dcache_range((uint32_t)g_output_ring & ALIGN_MASK,
+				((uint32_t)g_output_ring & ALIGN_MASK) + 128);
+	/* check that descriptor address is the one expected in the output ring */
+	if(g_output_ring[0] == (uint32_t)decap_dsc)
+	{
+		/* check if any error is reported in the output ring */
+		if ((g_output_ring[1] & JOB_RING_STS) != 0)
+		{
+			printf("Error: blob decap job completed with errors 0x%X\n",
+						g_output_ring[1]);
+		}
+	}
+	else
+	{
+		printf("Error: blob decap job output ring descriptor address does" \
+	                " not match\n");
+	}
+
+
+	/* Remove job from Job Ring Output Queue */
+	__raw_writel(1, CAAM_ORJRR0);
+
+	return ret;
+}
+
+/*!
+ * Use CAAM to generate a blob.
+ *
+ * @param   plain_data_addr  Location address of the plain data.
+ * @param   blob_addr  Location address of the blob.
+ *
+ * @return  SUCCESS or ERROR_XXX
+ */
+uint32_t caam_gen_blob(uint32_t plain_data_addr, uint32_t blob_addr, uint32_t size)
+{
+	uint32_t ret = SUCCESS;
+
+	/* Buffer to hold the resulting blob */
+	uint8_t *blob = (uint8_t *)blob_addr;
+
+	/* initialize the blob array */
+	memset(blob,0,size);
+
+
+    /* TODO: Fix Hardcoded Descriptor */
+	encap_dsc[0] = (uint32_t)0xB0800008;
+	encap_dsc[1] = (uint32_t)0x14400010;
+	encap_dsc[2] = (uint32_t)skeymod;
+	encap_dsc[3] = (uint32_t)0xF0000000 | (0x0000ffff & (size));
+	encap_dsc[4] = (uint32_t)plain_data_addr;
+	encap_dsc[5] = (uint32_t)0xF8000000 | (0x0000ffff & (size+48));
+	encap_dsc[6] = (uint32_t)blob;
+	encap_dsc[7] = (uint32_t)0x870D0000;
+
+    /* Run descriptor with result written to blob buffer */
+    /* Add job to input ring */
+	g_input_ring[0] = (uint32_t)encap_dsc;
+
+	flush_dcache_range((uint32_t)plain_data_addr & ALIGN_MASK,
+			   (((uint32_t)plain_data_addr + 2 * size + 64) & ALIGN_MASK));
+	flush_dcache_range((uint32_t)encap_dsc & ALIGN_MASK,
+			   ((uint32_t)encap_dsc & ALIGN_MASK) + 128);
+	flush_dcache_range((uint32_t)blob & ALIGN_MASK,
+			   (((uint32_t)blob + 2 * size + 64) & ALIGN_MASK));
+	flush_dcache_range((uint32_t)g_input_ring & ALIGN_MASK,
+			   ((uint32_t)g_input_ring & ALIGN_MASK) + 128);
+	/* Increment jobs added */
+	__raw_writel(1, CAAM_IRJAR0);
+
+    /* Wait for job ring to complete the job: 1 completed job expected */
+	while(__raw_readl(CAAM_ORSFR0) != 1);
+
+    // flush cache
+	flush_dcache_range((uint32_t)g_output_ring & ALIGN_MASK,
+				((uint32_t)g_output_ring & ALIGN_MASK) + 128);
+	/* check that descriptor address is the one expected in the output ring */
+	if(g_output_ring[0] == (uint32_t)encap_dsc)
+	{
+	/* check if any error is reported in the output ring */
+		if ((g_output_ring[1] & JOB_RING_STS) != 0)
+		{
+			printf("Error: blob encap job completed with errors 0x%X\n",
+			      g_output_ring[1]);
+		}
+	}
+	else
+	{
+	printf("Error: blob encap job output ring descriptor address does" \
+		" not match\n");
+	}
+
+	/* Remove job from Job Ring Output Queue */
+	__raw_writel(1, CAAM_ORJRR0);
+
+	return ret;
+}
+
+uint32_t caam_hwrng(uint8_t *output_ptr, uint32_t output_len) {
+	uint32_t ret = SUCCESS;
+
+	/* Buffer to hold the resulting output*/
+	uint8_t *output = (uint8_t *)output_ptr;
+
+	/* initialize the output array */
+	memset(output,0,output_len);
+
+	int n = 0;
+	hwrng_dsc[n++] = (uint32_t)0xB0800004;
+	hwrng_dsc[n++] = (uint32_t)0x82500000;
+	hwrng_dsc[n++] = (uint32_t)0x60340000| (0x0000ffff & output_len);
+	hwrng_dsc[n++] = (uint32_t)output;
+
+	/* Run descriptor with result written to blob buffer */
+	/* Add job to input ring */
+	// flush cache
+	g_input_ring[0] = (uint32_t)hwrng_dsc;
+
+	flush_dcache_range((uint32_t)hwrng_dsc & ALIGN_MASK,
+			   ((uint32_t)hwrng_dsc & ALIGN_MASK) + 128);
+	flush_dcache_range((uint32_t)g_input_ring & ALIGN_MASK,
+			   ((uint32_t)g_input_ring & ALIGN_MASK) + 128);
+	invalidate_dcache_range((uint32_t)hwrng_dsc & ALIGN_MASK,
+			   ((uint32_t)hwrng_dsc & ALIGN_MASK) + 128);
+	invalidate_dcache_range((uint32_t)g_input_ring & ALIGN_MASK,
+			   ((uint32_t)g_input_ring & ALIGN_MASK) + 128);
+	invalidate_dcache_range((uint32_t)output & ALIGN_MASK,
+			   (((uint32_t)output + 2 * output_len + 64) & ALIGN_MASK));
+	/* Increment jobs added */
+	__raw_writel(1, CAAM_IRJAR0);
+
+	/* Wait for job ring to complete the job: 1 completed job expected */
+	size_t timeout = 100000;
+	while(__raw_readl(CAAM_ORSFR0) != 1 && timeout--);
+	flush_dcache_range((uint32_t)g_output_ring & ALIGN_MASK,
+				((uint32_t)g_output_ring & ALIGN_MASK) + 128);
+
+	/* check that descriptor address is the one expected in the output ring */
+	if(g_output_ring[0] == (uint32_t)hwrng_dsc) {
+	/* check if any error is reported in the output ring */
+		if ((g_output_ring[1] & JOB_RING_STS) != 0) {
+			printf("Error: RNG job completed with errors 0x%X\n",
+			g_output_ring[1]);
+			ret = -1;
+		}
+	} else {
+		printf("Error: RNG output ring descriptor address does" \
+			" not match\n");
+		ret = -1;
+
+	}
+
+	/* Remove job from Job Ring Output Queue */
+	__raw_writel(1, CAAM_ORJRR0);
+
+	return ret;
+}
+
+/*!
+ * Initialize the CAAM.
+ *
+ */
+void caam_open(void)
+{
+	uint32_t temp_reg;
+	//uint32_t addr;
+
+    /* switch on the clock */
+#if defined(CONFIG_MX6)
+	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;
+	temp_reg = __raw_readl(&mxc_ccm->CCGR0);
+	temp_reg |= MXC_CCM_CCGR0_CAAM_SECURE_MEM_MASK |
+		MXC_CCM_CCGR0_CAAM_WRAPPER_ACLK_MASK |
+		MXC_CCM_CCGR0_CAAM_WRAPPER_IPG_MASK;
+	__raw_writel(temp_reg, &mxc_ccm->CCGR0);
+#elif defined(CONFIG_MX7)
+	HW_CCM_CCGR_SET(36,	MXC_CCM_CCGR36_CAAM_DOMAIN0_MASK);
+#endif
+
+    /* MID for CAAM - already done by HAB in ROM during preconfigure,
+     * That is JROWN for JR0/1 = 1 (TZ, Secure World, ARM)
+     * JRNSMID and JRSMID for JR0/1 = 2 (TZ, Secure World, CAAM)
+     *
+     * However, still need to initialize Job Rings as these are torn
+     * down by HAB for each command
+     */
+
+    /* Initialize job ring addresses */
+	__raw_writel((uint32_t)g_input_ring, CAAM_IRBAR0);   // input ring address
+	__raw_writel((uint32_t)g_output_ring, CAAM_ORBAR0);  // output ring address
+
+	/* Initialize job ring sizes to 1 */
+	__raw_writel(JOB_RING_ENTRIES, CAAM_IRSR0);
+	__raw_writel(JOB_RING_ENTRIES, CAAM_ORSR0);
+
+    /* HAB disables interrupts for JR0 so do the same here */
+	temp_reg = __raw_readl(CAAM_JRCFGR0_LS) | JRCFG_LS_IMSK;
+	__raw_writel(temp_reg, CAAM_JRCFGR0_LS);
+
+    /********* Initialize and instantiate the RNG *******************/
+    /* if RNG already instantiated then skip it */
+	if ((__raw_readl(CAAM_RDSTA) & RDSTA_IF0) != RDSTA_IF0)
+	{
+	/* Enter TRNG Program mode */
+	__raw_writel(RTMCTL_PGM, CAAM_RTMCTL);
+
+	/* Set OSC_DIV field to TRNG */
+	temp_reg = __raw_readl(CAAM_RTMCTL) | (RNG_TRIM_OSC_DIV << 2);
+	__raw_writel(temp_reg, CAAM_RTMCTL);
+
+	/* Set delay */
+	__raw_writel(((RNG_TRIM_ENT_DLY << 16) | 0x09C4), CAAM_RTSDCTL);
+	__raw_writel((RNG_TRIM_ENT_DLY >> 1), CAAM_RTFRQMIN);
+	__raw_writel((RNG_TRIM_ENT_DLY << 4), CAAM_RTFRQMAX);
+
+	/* Resume TRNG Run mode */
+	temp_reg = __raw_readl(CAAM_RTMCTL) ^ RTMCTL_PGM;
+	__raw_writel(temp_reg, CAAM_RTMCTL);
+
+	/* Clear the ERR bit in RTMCTL if set. The TRNG error can occur when the
+	 * RNG clock is not within 1/2x to 8x the system clock.
+	 * This error is possible if ROM code does not initialize the system PLLs
+	 * immediately after PoR.
+	 */
+	temp_reg = __raw_readl(CAAM_RTMCTL) | RTMCTL_ERR;
+	__raw_writel(temp_reg, CAAM_RTMCTL);
+
+	/* Run descriptor to instantiate the RNG */
+	/* Add job to input ring */
+	g_input_ring[0] = (uint32_t)rng_inst_dsc;
+
+	flush_dcache_range((uint32_t)g_input_ring & 0xffffffe0,
+			   ((uint32_t)g_input_ring & 0xffffffe0) + 128);
+	/* Increment jobs added */
+	__raw_writel(1, CAAM_IRJAR0);
+
+	/* Wait for job ring to complete the job: 1 completed job expected */
+	while(__raw_readl(CAAM_ORSFR0) != 1);
+
+
+	invalidate_dcache_range((uint32_t)g_output_ring & 0xffffffe0,
+				((uint32_t)g_output_ring & 0xffffffe0) + 128);
+
+	/* check that descriptor address is the one expected in the out ring */
+	if(g_output_ring[0] == (uint32_t)rng_inst_dsc)
+	{
+		/* check if any error is reported in the output ring */
+		if ((g_output_ring[1] & JOB_RING_STS) != 0)
+		{
+		printf("Error: RNG instantiation errors g_output_ring[1]: 0x%X\n"
+			, g_output_ring[1]);
+		printf("RTMCTL 0x%X\n", __raw_readl(CAAM_RTMCTL));
+		printf("RTSTATUS 0x%X\n", __raw_readl(CAAM_RTSTATUS));
+		printf("RTSTA 0x%X\n", __raw_readl(CAAM_RDSTA));
+            }
+	}
+	else
+	{
+		printf("Error: RNG job output ring descriptor address does " \
+				"not match: 0x%X != 0x%X \n", g_output_ring[0], rng_inst_dsc[0]);
+	}
+
+		/* ensure that the RNG was correctly instantiated */
+		temp_reg = __raw_readl(CAAM_RDSTA);
+		if (temp_reg != (RDSTA_IF0 | RDSTA_SKVN))
+		{
+			printf("Error: RNG instantiation failed 0x%X\n", temp_reg);
+		}
+		/* Remove job from Job Ring Output Queue */
+		__raw_writel(1, CAAM_ORJRR0);
+	}
+	return;
+}
diff --git a/drivers/crypto/fsl_caam_internal.h b/drivers/crypto/fsl_caam_internal.h
new file mode 100644
index 0000000..7cc8d7e
--- /dev/null
+++ b/drivers/crypto/fsl_caam_internal.h
@@ -0,0 +1,401 @@
+/*
+ * Copyright (c) 2012-2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef __CAAM_INTERNAL_H__
+#define	__CAAM_INTERNAL_H__
+
+/* 4kbyte pages */
+#define CAAM_SEC_RAM_START_ADDR CAAM_ARB_BASE_ADDR
+
+#define SEC_MEM_PAGE0       CAAM_SEC_RAM_START_ADDR
+#define SEC_MEM_PAGE1       (CAAM_SEC_RAM_START_ADDR + 0x1000)
+#define SEC_MEM_PAGE2       (CAAM_SEC_RAM_START_ADDR + 0x2000)
+#define SEC_MEM_PAGE3       (CAAM_SEC_RAM_START_ADDR + 0x3000)
+
+/* Configuration and special key registers */
+#define CAAM_MCFGR          CONFIG_SYS_FSL_SEC_ADDR + 0x0004
+#define CAAM_SCFGR          CONFIG_SYS_FSL_SEC_ADDR + 0x000c
+#define CAAM_JR0MIDR        CONFIG_SYS_FSL_SEC_ADDR + 0x0010
+#define CAAM_JR1MIDR        CONFIG_SYS_FSL_SEC_ADDR + 0x0018
+#define CAAM_DECORR         CONFIG_SYS_FSL_SEC_ADDR + 0x009c
+#define CAAM_DECO0MID       CONFIG_SYS_FSL_SEC_ADDR + 0x00a0
+#define CAAM_DAR            CONFIG_SYS_FSL_SEC_ADDR + 0x0120
+#define CAAM_DRR            CONFIG_SYS_FSL_SEC_ADDR + 0x0124
+#define CAAM_JDKEKR         CONFIG_SYS_FSL_SEC_ADDR + 0x0400
+#define CAAM_TDKEKR         CONFIG_SYS_FSL_SEC_ADDR + 0x0420
+#define CAAM_TDSKR          CONFIG_SYS_FSL_SEC_ADDR + 0x0440
+#define CAAM_SKNR           CONFIG_SYS_FSL_SEC_ADDR + 0x04e0
+#define CAAM_SMSTA          CONFIG_SYS_FSL_SEC_ADDR + 0x0FB4
+#define CAAM_STA            CONFIG_SYS_FSL_SEC_ADDR + 0x0FD4
+#define CAAM_SMPO_0         CONFIG_SYS_FSL_SEC_ADDR + 0x1FBC
+
+/* RNG registers */
+#define CAAM_RTMCTL         CONFIG_SYS_FSL_SEC_ADDR + 0x0600
+#define CAAM_RTSDCTL        CONFIG_SYS_FSL_SEC_ADDR + 0x0610
+#define CAAM_RTFRQMIN       CONFIG_SYS_FSL_SEC_ADDR + 0x0618
+#define CAAM_RTFRQMAX       CONFIG_SYS_FSL_SEC_ADDR + 0x061C
+#define CAAM_RTSTATUS       CONFIG_SYS_FSL_SEC_ADDR + 0x063C
+#define CAAM_RDSTA          CONFIG_SYS_FSL_SEC_ADDR + 0x06C0
+
+/* Job Ring 0 registers */
+#define CAAM_IRBAR0         CONFIG_SYS_FSL_SEC_ADDR + 0x1004
+#define CAAM_IRSR0          CONFIG_SYS_FSL_SEC_ADDR + 0x100c
+#define CAAM_IRSAR0         CONFIG_SYS_FSL_SEC_ADDR + 0x1014
+#define CAAM_IRJAR0         CONFIG_SYS_FSL_SEC_ADDR + 0x101c
+#define CAAM_ORBAR0         CONFIG_SYS_FSL_SEC_ADDR + 0x1024
+#define CAAM_ORSR0          CONFIG_SYS_FSL_SEC_ADDR + 0x102c
+#define CAAM_ORJRR0         CONFIG_SYS_FSL_SEC_ADDR + 0x1034
+#define CAAM_ORSFR0         CONFIG_SYS_FSL_SEC_ADDR + 0x103c
+#define CAAM_JRSTAR0        CONFIG_SYS_FSL_SEC_ADDR + 0x1044
+#define CAAM_JRINTR0        CONFIG_SYS_FSL_SEC_ADDR + 0x104c
+#define CAAM_JRCFGR0_MS     CONFIG_SYS_FSL_SEC_ADDR + 0x1050
+#define CAAM_JRCFGR0_LS     CONFIG_SYS_FSL_SEC_ADDR + 0x1054
+#define CAAM_IRRIR0         CONFIG_SYS_FSL_SEC_ADDR + 0x105c
+#define CAAM_ORWIR0         CONFIG_SYS_FSL_SEC_ADDR + 0x1064
+#define CAAM_JRCR0          CONFIG_SYS_FSL_SEC_ADDR + 0x106c
+#define CAAM_SMCJR0         CONFIG_SYS_FSL_SEC_ADDR + 0x10f4
+#define CAAM_SMCSJR0        CONFIG_SYS_FSL_SEC_ADDR + 0x10fc
+#define CAAM_SMAPJR0(y)     (CONFIG_SYS_FSL_SEC_ADDR + 0x1104 + y*16)
+#define CAAM_SMAG2JR0(y)    (CONFIG_SYS_FSL_SEC_ADDR + 0x1108 + y*16)
+#define CAAM_SMAG1JR0(y)    (CONFIG_SYS_FSL_SEC_ADDR + 0x110C + y*16)
+#define CAAM_SMAPJR0_PRTN1  CONFIG_SYS_FSL_SEC_ADDR + 0x1114
+#define CAAM_SMAG2JR0_PRTN1 CONFIG_SYS_FSL_SEC_ADDR + 0x1118
+#define CAAM_SMAG1JR0_PRTN1 CONFIG_SYS_FSL_SEC_ADDR + 0x111c
+#define CAAM_SMPO           CONFIG_SYS_FSL_SEC_ADDR + 0x1fbc
+
+#define JRCFG_LS_IMSK       0x00000001      /* Interrupt Mask */
+#define JR_MID              2               /* Matches ROM configuration */
+#define KS_G1               (1 << JR_MID)   /* CAAM only */
+#define PERM                0x0000B008      /* Clear on release,
+                                             * lock SMAP
+                                             * lock SMAG
+                                             * group 1 Blob
+                                             */
+
+#define CMD_PAGE_ALLOC      0x1
+#define CMD_PAGE_DEALLOC    0x2
+#define CMD_PART_DEALLOC    0x3
+#define CMD_INQUIRY         0x5
+#define PAGE(x)             (x << 16)
+#define PARTITION(x)        (x << 8)
+
+#define SMCSJR_AERR         (3 << 12)
+#define SMCSJR_CERR         (3 << 14)
+#define CMD_COMPLETE        (3 << 14)
+
+#define SMCSJR_PO           (3 << 6)
+#define PAGE_AVAILABLE      0
+#define PAGE_OWNED          (3 << 6)
+
+#define PARTITION_OWNER(x)  (0x3 << (x*2))
+
+#define CAAM_BUSY_MASK      0x00000001      /* BUSY from status reg */
+#define CAAM_IDLE_MASK      0x00000002      /* IDLE from status reg */
+
+#define JOB_RING_ENTRIES    1
+#define JOB_RING_STS        (0xF << 28)
+
+/** OSC_DIV in RNG trim fuses */
+#define RNG_TRIM_OSC_DIV    0
+/** ENT_DLY multiplier in RNG trim fuses */
+//#define RNG_TRIM_ENT_DLY    200*4
+#define RNG_TRIM_ENT_DLY    3200
+
+#define RTMCTL_PGM  (1 << 16)
+#define RTMCTL_ERR  (1 << 12)
+#define RDSTA_IF0   1
+#define RDSTA_SKVN  (1 << 30)
+
+typedef enum {
+    PAGE_0,
+    PAGE_1,
+    PAGE_2,
+    PAGE_3,
+} page_num_e;
+
+typedef enum {
+    PARTITION_0,
+    PARTITION_1,
+    PARTITION_2,
+    PARTITION_3,
+    PARTITION_4,
+    PARTITION_5,
+    PARTITION_6,
+    PARTITION_7,
+} partition_num_e;
+
+/*****************************************
+ *----- Blob decapsulate descriptor -----*
+ *****************************************/
+/* 1. Header
+ *
+ *  1011 0000 1000 0000 0000 0000 0000 1001
+ *  |||| |                             ||||
+ *  ++++-+-- Header                    ++++-- 9 words in descriptor
+ */
+#define DECAP_BLOB_DESC1 0xB0800009
+
+/* 2. Load command KEY 2 immediate
+ *
+ *  0001 0100 1100 0000 0000 1100 0000 1000
+ *  |||| |||  |||| |||| |||| |||| |||| ||||
+ *  |||| |||  |||| |||| |||| |||| ++++-++++-- Length
+ *  |||| |||  |||| |||| ++++-++++-- Offset
+ *  |||| |||  |+++-++++-- DST (Destination Register) Key2
+ *  |||| |||  +-- IMM (Immediate flag)
+ *  |||| |++-- class 2
+ *  ++++-+-- Load command
+ */
+#define DECAP_BLOB_DESC2 0x14C00C08
+
+/* 3. 8 bytes for load command above - aad data
+ *
+ *  0000 0000 0001 0000 0101 0101 0110 0110
+ *  |||| |||| |||| |||| |||| |||| |||| ||||
+ *  |||| |||| |||| |||| |||| |||| ++++-++++-- CCM Mode
+ *  |||| |||| |||| |||| ++++-++++-- AES
+ *  |||| |||| ++++-++++-- Length
+ *  ++++-++++-- Flag
+ */
+#define DECAP_BLOB_DESC3 0x00105566
+#define DECAP_BLOB_DESC4 0x00000000
+
+/* 5. SEQ In Ptr
+ *
+ *  1111 0000 0000 0000 0000 0000 0100 0000
+ *  |||| |              |||| |||| |||| ||||
+ *  |||| |              ++++-++++-++++-++++-- Length in bytes (64)
+ *  ++++-+-- Seq In Ptr
+ */
+#define DECAP_BLOB_DESC5 0xF0000400
+//#define DECAP_BLOB_DESC5 0xF0000040
+
+/* 6. Pointer for above SEQ In ptr command */
+/* Address is provided during run time */
+#define DECAP_BLOB_DESC6 0x00000000
+
+/* 7. SEQ Out Ptr
+ *
+ *  1111 1000 0000 0000 0000 0000 0001 0000
+ *  |||| |              |||| |||| |||| ||||
+ *  |||| |              ++++-++++-++++-++++-- Length in bytes (16)
+ *  ++++-+-- Seq In Ptr
+ */
+#define DECAP_BLOB_DESC7 0xF80003d0
+//#define DECAP_BLOB_DESC7 0xF8000010
+
+/* 8. Pointer for above SEQ Out ptr command */
+/* Address could be changed during run time */
+#define DECAP_BLOB_DESC8 SEC_MEM_PAGE1
+
+/* 9. Protocol
+ *
+ *  1000 0110 0000 1101 0000 0000 0000 1000
+ *  |||| |||| |||| |||| |||| |||| |||| ||||
+ *  |||| |||| |||| |||| ++++-++++-++++-++++-- Proto Info = sec mem blob
+ *  |||| |||| ++++-++++-- Protocol ID = Blob
+ *  |||| |+++-- Optype - decapsulation protocol
+ *  ++++-+-- Seq In Ptr
+ */
+#define DECAP_BLOB_DESC9 0x860D0008
+
+/*****************************************
+ *----- Blob encapsulate descriptor -----*
+ *****************************************/
+/* Blob Header
+ *
+ *  1011 0000 1000 0000 0000 0000 0000 1001
+ *  |||| |                             |
+ *  ++++-+-- Header                    +-- 9 words in descriptor
+ */
+#define ENCAP_BLOB_DESC1  0xB0800009
+
+/* 2. Load command KEY 2 immediate
+ *
+ *  0001 0100 1100 0000 0000 1100 0000 1000
+ *  |||| |||  |||| |||| |||| |||| |||| ||||
+ *  |||| |||  |||| |||| |||| |||| ++++-++++-- Length
+ *  |||| |||  |||| |||| ++++-++++-- Offset
+ *  |||| |||  |+++-++++-- DST (Destination Register) Key2
+ *  |||| |||  +-- IMM (Immediate flag)
+ *  |||| |++-- class 2
+ *  ++++-+-- Load command
+ */
+#define ENCAP_BLOB_DESC2 0x14C00C08
+
+/* 3. 8 bytes for load command above - aad data
+ *
+ *  0000 0000 0001 0000 0101 0101 0110 0110
+ *  |||| |||| |||| |||| |||| |||| |||| ||||
+ *  |||| |||| |||| |||| |||| |||| ++++-++++-- CCM Mode
+ *  |||| |||| |||| |||| ++++-++++-- AES
+ *  |||| |||| ++++-++++-- Length
+ *  ++++-++++-- Flag
+ */
+#define ENCAP_BLOB_DESC3 0x00105566
+#define ENCAP_BLOB_DESC4 0x00000000
+
+/* 5. SEQ In Ptr
+ *
+ *  1111 0000 0000 0000 0000 0000 0001 0000
+ *  |||| |              |||| |||| |||| ||||
+ *  |||| |              ++++-++++-++++-++++-- Length in bytes (16)
+ *  ++++-+-- Seq In Ptr
+ */
+#define ENCAP_BLOB_DESC5 0xF00003d0
+//#define ENCAP_BLOB_DESC5 0xF0000010
+
+/* 6. Pointer for above SEQ In ptr command */
+/* Address could be changed during run time */
+#define ENCAP_BLOB_DESC6 SEC_MEM_PAGE1
+
+/* 7. SEQ Out Ptr
+ *
+ *  1111 1000 0000 0000 0000 0000 0100 0000
+ *  |||| |              |||| |||| |||| ||||
+ *  |||| |              ++++-++++-++++-++++-- Length in bytes (64)
+ *  ++++-+-- Seq Out Ptr
+ */
+#define ENCAP_BLOB_DESC7  0xF8000400
+//#define ENCAP_BLOB_DESC7  0xF8000040
+
+/* 8. Pointer for above SEQ Out ptr command */
+/* Address is provided during run time */
+#define ENCAP_BLOB_DESC8  0x00000000
+
+/* 9. Protocol
+ *
+ *  1000 0111 0000 1101 0000 0000 0000 1000
+ *  |||| |||| |||| |||| |||| |||| |||| ||||
+ *  |||| |||| |||| |||| ++++-++++-++++-++++-- Proto Info = sec mem blob
+ *  |||| |||| ++++-++++-- Protocol ID = Blob
+ *  |||| |+++-- Optype - encapsulation protocol
+ *  ++++-+-- Seq In Ptr
+ */
+#define ENCAP_BLOB_DESC9  0x870D0008
+
+/****************************************
+ *----- Data encryption descriptor -----*
+ ****************************************/
+/* 1. Header
+ *
+ * 1011 0000 1000 0000 0000 0000 0000 1000
+ * |||| | | ||||
+ * ++++-+-- Header +-++++-- 8 words in descriptor
+ */
+#define ENCRYPT_DESC1 0xB0800008
+
+/* 2. Load AES-128 key from secure memory
+ *
+ * 0010 0010 0000 0000 0000 0000 0001 0000
+ * |||| | | |||| |||| |||| ||||
+ * |||| | | ++++-++++-++++-++++-- 16 bytes
+ * |||| | +-- Load FIFO with data for Class 1 CHA
+ * ++++-+-- FIFO Load
+ */
+#define ENCRYPT_DESC2 0x02200010
+
+/* 3. Pointer to key data in secure memory */
+/* Address is provided during run time */
+#define ENCRYPT_DESC3 0x00000000
+
+/* 4. Algorith Operation - Decrypt with ECB mode
+ *
+ * 1000 0010 0001 0000 0000 0010 0000 1101
+ * |||| |||| |||| |||| |||| |||| |||| ||||
+ * |||| |||| |||| |||| |||| |||| |||| |||+-- Encrypt
+ * |||| |||| |||| |||| |||| |||| |||| ++-- Initialize/Finalize
+ * |||| |||| |||| |||| ---+-++++-++++-- ECB mode
+ * |||| |||| ++++-++++-- AES
+ * |||| |+++-- Optype: Class 1 algorithm
+ * ++++-+-- ALGORITHM OP.
+ */
+#define ENCRYPT_DESC4 0x8210020D
+
+/* 5. Load 16 bytes of message data
+ *
+ * 0010 0010 0001 0010 0000 0000 0001 0000
+ * |||| |||| |||| |||| |||| |||| |||| ||||
+ * |||| |||| |||| |||| |||| |||| |||| ||||
+ * |||| |||| |||| |||| |||| |||| |||| ||||
+ * |||| |||| |||| |||| ++++-++++-++++-++++-- Msg Length = 16Bytes
+ * |||| |||| ||++-++++-- Input data type: Msg data LC1=1
+ * |||| |||| |+-- EXT: No extended length
+ * |||| |||| +-- IMM: data begins at the location pointed to by the next word
+ * |||| |||++-SGT/VLF: FIFO Load-Pointer points to actual data
+ * |||| |++-- Load FIFO with data for Class 1 CHA
+ * ++++-+-- FIFO Load
+ */
+#define ENCRYPT_DESC5 0x22120010
+
+/* 6. Pointer to plain text test vector message */
+/* Address is provided during run time */
+#define ENCRYPT_DESC6 0x00000000
+
+/* 7. FIFO STORE - encrypted result.
+ * 0110 0000 0011 0000 0000 0000 0001 0000
+ * |||| |||| |||| |||| |||| |||| |||| ||||
+ * |||| |||| |||| |||| ++++-++++-++++-++++-- Length = 16Bytes
+ * |||| |||| ||++-++++-- Output data type: Msg Data
+ * |||| |||| |+-- EXT: No extended length
+ * |||| |||| +-- CONT: No continue
+ * |||| |||+-- SGT/VLF: Pointer points to actual data
+ * |||| |++-- AUX: None
+ * ++++-+-- FIFO Store
+ */
+#define ENCRYPT_DESC7 0x60300010
+
+/* 8. Pointer to ciphered text buffer */
+/* Address is provided during run time */
+#define ENCRYPT_DESC8 0x00000000
+
+/*********************************************************************
+ *----- Descriptor to instantiate RNG in non-deterministic mode -----*
+ *********************************************************************/
+// Header
+#define RNG_INST_DESC1 0xB0800009
+// Class 1 context load for personalization string, 8bytes
+#define RNG_INST_DESC2 0x12A00008
+// 8 bytes of personalization string (8-byte UID + zeros)
+#define RNG_INST_DESC3 0x01020304
+#define RNG_INST_DESC4 0x05060708
+// Instantiate State Handle 0 using entropy from TRNG
+// without prediction resistance
+#define RNG_INST_DESC5 0x82500404
+// Wait for Class 1 done
+#define RNG_INST_DESC6 0xA2000001
+// Immediate 4 byte load to clear written register
+#define RNG_INST_DESC7 0x10880004
+// Clear primary mode bit
+#define RNG_INST_DESC8 0x00000001
+// Generate secure keys without prediction resistance
+#define RNG_INST_DESC9 0x82501000
+
+#endif /* __CAAM_INTERNAL_H__ */
diff --git a/drivers/serial/serial_mxc.c b/drivers/serial/serial_mxc.c
index 1cfcbf2..6c2cf4c 100644
--- a/drivers/serial/serial_mxc.c
+++ b/drivers/serial/serial_mxc.c
@@ -13,6 +13,7 @@
 #include <dm/platform_data/serial_mxc.h>
 #include <serial.h>
 #include <linux/compiler.h>
+#include <asm/imx-common/sys_proto.h>
 
 /* UART Control Register Bit Fields.*/
 #define  URXD_CHARRDY    (1<<15)
diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 6431d8a..21ea876 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -28,6 +28,7 @@ obj-$(CONFIG_USB_FUNCTION_THOR) += f_thor.o
 obj-$(CONFIG_USB_FUNCTION_DFU) += f_dfu.o
 obj-$(CONFIG_USB_FUNCTION_MASS_STORAGE) += f_mass_storage.o
 obj-$(CONFIG_USB_FUNCTION_FASTBOOT) += f_fastboot.o
+obj-$(CONFIG_FASTBOOT_LOCK) += fastboot_lock_unlock.o
 obj-$(CONFIG_FSL_BOOTCTL) += bootctrl.o
 obj-$(CONFIG_BCB_SUPPORT) += command.o bcb.o
 endif
diff --git a/drivers/usb/gadget/f_fastboot.c b/drivers/usb/gadget/f_fastboot.c
index f8c9414..6c1ce07 100644
--- a/drivers/usb/gadget/f_fastboot.c
+++ b/drivers/usb/gadget/f_fastboot.c
@@ -16,6 +16,7 @@
 #include <config.h>
 #include <common.h>
 #include <errno.h>
+#include <stdlib.h>
 #include <fastboot.h>
 #include <malloc.h>
 #include <linux/usb/ch9.h>
@@ -56,6 +57,12 @@
 
 #define FASTBOOT_VERSION		"0.4"
 
+#ifdef CONFIG_FASTBOOT_LOCK
+#include "fastboot_lock_unlock.h"
+#endif
+#define FASTBOOT_VAR_YES    "yes"
+#define FASTBOOT_VAR_NO     "no"
+
 #define ANDROID_GPT_OFFSET         0
 #define ANDROID_GPT_SIZE           0x100000
 #define FASTBOOT_INTERFACE_CLASS	0xff
@@ -1140,8 +1147,8 @@ static void parameters_setup(void)
 				CONFIG_FASTBOOT_BUF_SIZE;
 }
 
-static struct fastboot_ptentry ptable[MAX_PTN];
-static unsigned int pcount;
+static struct fastboot_ptentry g_ptable[MAX_PTN];
+static unsigned int g_pcount;
 struct fastboot_device_info fastboot_devinfo;
 
 static int _fastboot_setup_dev(void)
@@ -1435,7 +1442,7 @@ static int _fastboot_parts_load_from_env(void)
 			while (s < e) {
 				if (_fastboot_parts_add_env_entry(s, &s)) {
 					printf("Error:Fastboot: Abort adding partitions\n");
-					pcount = 0;
+					g_pcount = 0;
 					return 1;
 				}
 				/* Skip a bunch of delimiters */
@@ -1460,7 +1467,7 @@ static int _fastboot_parts_load_from_env(void)
 
 static void _fastboot_load_partitions(void)
 {
-	pcount = 0;
+	g_pcount = 0;
 #if defined(CONFIG_FASTBOOT_STORAGE_NAND)
 	_fastboot_parts_load_from_env();
 #elif defined(CONFIG_FASTBOOT_STORAGE_SATA) \
@@ -1473,19 +1480,19 @@ static void _fastboot_load_partitions(void)
  * Android style flash utilties */
 void fastboot_flash_add_ptn(struct fastboot_ptentry *ptn)
 {
-	if (pcount < MAX_PTN) {
-		memcpy(ptable + pcount, ptn, sizeof(struct fastboot_ptentry));
-		pcount++;
+	if (g_pcount < MAX_PTN) {
+		memcpy(g_ptable + g_pcount, ptn, sizeof(struct fastboot_ptentry));
+		g_pcount++;
 	}
 }
 
 void fastboot_flash_dump_ptn(void)
 {
 	unsigned int n;
-	for (n = 0; n < pcount; n++) {
-		struct fastboot_ptentry *ptn = ptable + n;
-		printf("ptn %d name='%s' start=%d len=%d\n",
-			n, ptn->name, ptn->start, ptn->length);
+	for (n = 0; n < g_pcount; n++) {
+		struct fastboot_ptentry *ptn = g_ptable + n;
+		printf("idx %d, ptn %d name='%s' start=%d len=%d\n",
+			n, ptn->partition_index, ptn->name, ptn->start, ptn->length);
 	}
 }
 
@@ -1494,11 +1501,11 @@ struct fastboot_ptentry *fastboot_flash_find_ptn(const char *name)
 {
 	unsigned int n;
 
-	for (n = 0; n < pcount; n++) {
+	for (n = 0; n < g_pcount; n++) {
 		/* Make sure a substring is not accepted */
-		if (strlen(name) == strlen(ptable[n].name)) {
-			if (0 == strcmp(ptable[n].name, name))
-				return ptable + n;
+		if (strlen(name) == strlen(g_ptable[n].name)) {
+			if (0 == strcmp(g_ptable[n].name, name))
+				return g_ptable + n;
 		}
 	}
 
@@ -1519,15 +1526,15 @@ int fastboot_flash_find_index(const char *name)
 
 struct fastboot_ptentry *fastboot_flash_get_ptn(unsigned int n)
 {
-	if (n < pcount)
-		return ptable + n;
+	if (n < g_pcount)
+		return g_ptable + n;
 	else
 		return 0;
 }
 
 unsigned int fastboot_flash_get_ptn_count(void)
 {
-	return pcount;
+	return g_pcount;
 }
 
 #ifdef CONFIG_FSL_FASTBOOT
@@ -1742,7 +1749,7 @@ bootimg_print_image_hdr(struct andr_img_hdr *hdr)
 	printf("   page_size:   0x%x\n", hdr->page_size);
 
 	printf("   name:      %s\n", hdr->name);
-	printf("   cmdline:   %s%x\n", hdr->cmdline);
+	printf("   cmdline:   %s\n", hdr->cmdline);
 
 	for (i = 0; i < 8; i++)
 		printf("   id[%d]:   0x%x\n", i, hdr->id[i]);
@@ -1876,6 +1883,9 @@ use_given_ptn:
 		flush_cache((ulong)load_addr, bootimg_sectors * 512); /* FIXME */
 
 		addr = load_addr;
+#ifdef CONFIG_FASTBOOT_LOCK
+		int verifyresult = -1;
+#endif
 
 #ifdef CONFIG_SECURE_BOOT
 		extern uint32_t authenticate_image(uint32_t ddr_start,
@@ -1883,11 +1893,28 @@ use_given_ptn:
 
 		if (authenticate_image(load_addr, image_size)) {
 			printf("Authenticate OK\n");
+#ifdef CONFIG_FASTBOOT_LOCK
+			verifyresult = 0;
+#endif
 		} else {
 			printf("Authenticate image Fail, Please check\n\n");
+			/* For Android if the verify not passed we continue the boot process */
+#ifdef CONFIG_FASTBOOT_LOCK
+#ifndef CONFIG_ANDROID_SUPPORT
 			return 1;
+#endif
+			verifyresult = 1;
+#endif
 		}
 #endif /*CONFIG_SECURE_BOOT*/
+#ifdef CONFIG_FASTBOOT_LOCK
+		int lock_status = fastboot_get_lock_stat();
+		if (lock_status == FASTBOOT_LOCK_ERROR) {
+			printf("In boota get fastboot lock status error. Set lock status\n");
+			fastboot_set_lock_stat(FASTBOOT_LOCK);
+		}
+		display_lock(fastboot_get_lock_stat(), verifyresult);
+#endif
 
 		sector = pte->start + (hdr->page_size / 512);
 		sector += ALIGN(hdr->kernel_size, hdr->page_size) / 512;
@@ -2313,6 +2340,20 @@ static int strcmp_l1(const char *s1, const char *s2)
 	return strncmp(s1, s2, strlen(s1));
 }
 
+bool is_slotvar(char *cmd)
+{
+	assert(cmd != NULL);
+	if (!strcmp_l1("has-slot:", cmd) ||
+		!strcmp_l1("slot-successful:", cmd) ||
+		!strcmp_l1("slot-count", cmd) ||
+		!strcmp_l1("slot-suffixes", cmd) ||
+		!strcmp_l1("current-slot", cmd) ||
+		!strcmp_l1("slot-unbootable:", cmd) ||
+		!strcmp_l1("slot-retry-count:", cmd))
+			return true;
+	return false;
+}
+
 static void cb_getvar(struct usb_ep *ep, struct usb_request *req)
 {
 	char *cmd = req->buf;
@@ -2330,13 +2371,15 @@ static void cb_getvar(struct usb_ep *ep, struct usb_request *req)
 		return;
 	}
 
+	else if (is_slotvar(cmd)) {
 #ifdef CONFIG_FSL_BOOTCTL
-	if (is_sotvar(cmd)) {
 		get_slotvar(cmd, response, chars_left);
 		fastboot_tx_write_str(response);
 		return;
-	}
+#else
+		strncat(response, FASTBOOT_VAR_NO, chars_left);
 #endif
+	}
 	if (!strcmp_l1("version", cmd)) {
 		strncat(response, FASTBOOT_VERSION, chars_left);
 	} else if (!strcmp_l1("bootloader-version", cmd)) {
@@ -2353,11 +2396,28 @@ static void cb_getvar(struct usb_ep *ep, struct usb_request *req)
 			strncat(response, s, chars_left);
 		else
 			strcpy(response, "FAILValue not set");
-	} else if (!strcmp_l1("partition-type", cmd)) {
-		strcpy(response, "FAILVariable not implemented");
 	} else if (!strcmp_l1("product", cmd)) {
 		strncat(response, "Freescale i.MX", chars_left);
-	} else {
+	}
+#ifdef CONFIG_FASTBOOT_LOCK
+	else if (!strcmp_l1("secure", cmd)) {
+		strncat(response, FASTBOOT_VAR_YES, chars_left);
+	} else if (!strcmp_l1("unlocked",cmd)){
+		int status = fastboot_get_lock_stat();
+		if (status == FASTBOOT_UNLOCK) {
+			strncat(response, FASTBOOT_VAR_YES, chars_left);
+		} else {
+			strncat(response, FASTBOOT_VAR_NO, chars_left);
+		}
+	}
+#else
+	else if (!strcmp_l1("secure", cmd)) {
+		strncat(response, FASTBOOT_VAR_NO, chars_left);
+	} else if (!strcmp_l1("unlocked",cmd)) {
+		strncat(response, FASTBOOT_VAR_NO, chars_left);
+	}
+#endif
+	else {
 		char envstr[32];
 
 		snprintf(envstr, sizeof(envstr) - 1, "fastboot.%s", cmd);
@@ -2540,6 +2600,113 @@ static void cb_continue(struct usb_ep *ep, struct usb_request *req)
 	fastboot_tx_write_str("OKAY");
 }
 
+
+#ifdef CONFIG_FASTBOOT_LOCK
+
+int do_lock_status(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[]) {
+	FbLockState status = fastboot_get_lock_stat();
+	if (status != FASTBOOT_LOCK_ERROR) {
+		if (status == FASTBOOT_LOCK)
+			printf("fastboot lock status: locked.\n");
+		else
+			printf("fastboot lock status: unlocked.\n");
+	} else
+		printf("fastboot lock status error!\n");
+
+	display_lock(status, -1);
+
+	return 0;
+
+}
+
+U_BOOT_CMD(
+	lock_status, 2, 1, do_lock_status,
+	"lock_status",
+	"lock_status");
+
+static int do_fastboot_unlock(bool force)
+{
+	int status;
+	if (force)
+		set_fastboot_lock_disable();
+	if ((fastboot_lock_enable() == FASTBOOT_UL_ENABLE) || force) {
+		printf("It is able to unlock device. %d\n",fastboot_lock_enable());
+		if (fastboot_get_lock_stat() == FASTBOOT_UNLOCK) {
+			printf("The device is already unlocked\n");
+			return 1;
+		}
+		status = fastboot_set_lock_stat(FASTBOOT_UNLOCK);
+		if (status < 0)
+			return status;
+
+		printf("Start /data wipe process....\n");
+		fastboot_wipe_data_partition();
+		printf("Wipe /data completed.\n");
+
+#ifdef CONFIG_AVB_SUPPORT
+		printf("Start stored_rollback_index wipe process....\n");
+		rbkidx_erase();
+		printf("Wipe stored_rollback_index completed.\n");
+#endif
+	} else {
+		printf("It is not able to unlock device.");
+		return -1;
+	}
+
+	return status;
+}
+
+static int do_fastboot_lock(void)
+{
+	int status;
+	if (fastboot_get_lock_stat() == FASTBOOT_LOCK) {
+		printf("The device is already locked\n");
+		return 1;
+	}
+	status = fastboot_set_lock_stat(FASTBOOT_LOCK);
+	if (status < 0)
+		return status;
+
+	printf("Start /data wipe process....\n");
+	fastboot_wipe_data_partition();
+	printf("Wipe /data completed.\n");
+
+	return status;
+}
+
+static void cb_flashing(struct usb_ep *ep, struct usb_request *req)
+{
+	char *cmd = req->buf;
+	char response[FASTBOOT_RESPONSE_LEN];
+	unsigned char len = strlen(cmd);
+	FbLockState status;
+	if (!strncmp(cmd + len - 15, "unlock_critical", 15)) {
+		strcpy(response, "OKAY");
+	} else if (!strncmp(cmd + len - 13, "lock_critical", 13)) {
+		strcpy(response, "OKAY");
+	} else if (!strncmp(cmd + len - 6, "unlock", 6)) {
+		printf("flashing unlock.\n");
+		status = do_fastboot_unlock(false);
+		if (status >= 0)
+			strcpy(response, "OKAY");
+		else
+			strcpy(response, "FAIL unlock device failed.");
+	} else if (!strncmp(cmd + len - 4, "lock", 4)) {
+		printf("flashing lock.\n");
+		status = do_fastboot_lock();
+		if (status >= 0)
+			strcpy(response, "OKAY");
+		else
+			strcpy(response, "FAIL lock device failed.");
+	} else {
+		printf("Unknown flashing command:%s\n", cmd);
+		strcpy(response, "FAIL command not defined");
+	}
+	fastboot_tx_write_str(response);
+}
+
+#endif
+
 #ifdef CONFIG_FASTBOOT_FLASH
 static void cb_flash(struct usb_ep *ep, struct usb_request *req)
 {
@@ -2556,6 +2723,24 @@ static void cb_flash(struct usb_ep *ep, struct usb_request *req)
 	/* initialize the response buffer */
 	fb_response_str = response;
 
+#ifdef CONFIG_FASTBOOT_LOCK
+	int status;
+	status = fastboot_get_lock_stat();
+
+	if (status == FASTBOOT_LOCK) {
+		error("device is LOCKed!\n");
+		strcpy(response, "FAIL device is locked.");
+		fastboot_tx_write_str(response);
+		return;
+
+	} else if (status == FASTBOOT_LOCK_ERROR) {
+		error("write lock status into device!\n");
+		fastboot_set_lock_stat(FASTBOOT_LOCK);
+		strcpy(response, "FAIL device is locked.");
+		fastboot_tx_write_str(response);
+		return;
+	}
+#endif
 	fastboot_fail("no flash device defined");
 
 #ifdef CONFIG_FSL_FASTBOOT
@@ -2575,28 +2760,6 @@ static void cb_flash(struct usb_ep *ep, struct usb_request *req)
 }
 #endif
 
-static void cb_oem(struct usb_ep *ep, struct usb_request *req)
-{
-	char *cmd = req->buf;
-#ifdef CONFIG_FASTBOOT_FLASH_MMC_DEV
-	if (strncmp("format", cmd + 4, 6) == 0) {
-		char cmdbuf[32];
-                sprintf(cmdbuf, "gpt write mmc %x $partitions",
-			CONFIG_FASTBOOT_FLASH_MMC_DEV);
-                if (run_command(cmdbuf, 0))
-			fastboot_tx_write_str("FAIL");
-                else
-			fastboot_tx_write_str("OKAY");
-	} else
-#endif
-	if (strncmp("unlock", cmd + 4, 8) == 0) {
-		fastboot_tx_write_str("FAILnot implemented");
-	}
-	else {
-		fastboot_tx_write_str("FAILunknown oem command");
-	}
-}
-
 #ifdef CONFIG_FASTBOOT_FLASH
 static void cb_erase(struct usb_ep *ep, struct usb_request *req)
 {
@@ -2667,6 +2830,12 @@ static const struct cmd_dispatch_info cmd_dispatch_info[] = {
 		.cmd = "continue",
 		.cb = cb_continue,
 	},
+#ifdef CONFIG_FASTBOOT_LOCK
+	{
+		.cmd = "flashing",
+		.cb = cb_flashing,
+	},
+#endif
 #ifdef CONFIG_FASTBOOT_FLASH
 	{
 		.cmd = "flash",
@@ -2676,10 +2845,12 @@ static const struct cmd_dispatch_info cmd_dispatch_info[] = {
 		.cb = cb_erase,
 	},
 #endif
+#ifdef CONFIG_FASTBOOT_LOCK
 	{
 		.cmd = "oem",
-		.cb = cb_oem,
+		.cb = cb_flashing,
 	},
+#endif
 #ifdef CONFIG_FSL_BOOTCTL
 	{
 		.cmd = "set_active",
diff --git a/drivers/usb/gadget/fastboot_lock_unlock.c b/drivers/usb/gadget/fastboot_lock_unlock.c
new file mode 100644
index 0000000..ee26f17
--- /dev/null
+++ b/drivers/usb/gadget/fastboot_lock_unlock.c
@@ -0,0 +1,468 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <fsl_fastboot.h>
+#include "fastboot_lock_unlock.h"
+
+#include <common.h>
+#include <mapmem.h>
+#include <linux/types.h>
+#include <part.h>
+#include <mmc.h>
+#include <ext_common.h>
+#include <stdio_dev.h>
+#include <stdlib.h>
+
+#ifdef FASTBOOT_ENCRYPT_LOCK
+
+#include <hash.h>
+#include <fsl_caam.h>
+
+//Encrypted data is 80bytes length.
+#define ENDATA_LEN 80
+
+#endif
+
+
+#ifndef FASTBOOT_ENCRYPT_LOCK
+
+/*
+ * This will return FASTBOOT_LOCK, FASTBOOT_UNLOCK or FASTBOOT_ERROR
+ */
+static inline unsigned char decrypt_lock_store(unsigned char* bdata) {
+	if (*bdata >= FASTBOOT_LOCK_NUM)
+		return FASTBOOT_LOCK_ERROR;
+	else
+		return *bdata;
+}
+
+static inline int encrypt_lock_store(FbLockState lock, unsigned char* bdata) {
+	*bdata  = lock;
+	return 0;
+}
+#else
+
+static int sha1sum(unsigned char* data, int len, unsigned char* output) {
+	struct hash_algo *algo;
+	void *buf;
+	if (hash_lookup_algo("sha1", &algo)) {
+		printf("error in lookup sha1 algo!\n");
+		return -1;
+	}
+	buf = map_sysmem((ulong)data, len);
+	algo->hash_func_ws(buf, len, output, algo->chunk_size);
+	unmap_sysmem(buf);
+
+	return algo->digest_size;
+
+}
+
+static int generate_salt(unsigned char* salt) {
+	unsigned long time = get_timer(0);
+	return sha1sum((unsigned char *)&time, sizeof(unsigned long), salt);
+
+}
+
+static unsigned char decrypt_lock_store(unsigned char *bdata) {
+	unsigned char plain_data[ENDATA_LEN];
+	int p = 0, ret;
+
+	caam_open();
+	ret = caam_decap_blob((uint32_t)plain_data,
+			      (uint32_t)bdata + ENDATA_LEN, ENDATA_LEN);
+	if (ret != 0) {
+		printf("Error during blob decap operation: 0x%x\n",ret);
+		return FASTBOOT_LOCK_ERROR;
+	}
+#ifdef FASTBOOT_LOCK_DEBUG
+	FB_DEBUG("Decrypt data block are:\n \t=======\t\n");
+	for (p = 0; p < ENDATA_LEN; p++) {
+		FB_DEBUG("0x%2x  ", *(bdata + p));
+		if (p % 16 == 0)
+			FB_DEBUG("\n");
+	}
+	FB_DEBUG("\n \t========\t\n");
+	for (p = ENDATA_LEN; p < (ENDATA_LEN + ENDATA_LEN + 48 ); p++) {
+		FB_DEBUG("0x%2x  ", *(bdata + p));
+		if (p % 16 == 0)
+			FB_DEBUG("\n");
+	}
+
+	FB_DEBUG("\n plain text are:\n");
+	for (p = 0; p < ENDATA_LEN; p++) {
+		FB_DEBUG("0x%2x  ", plain_data[p]);
+		if (p % 16 == 0)
+			FB_DEBUG("\n");
+	}
+	FB_DEBUG("\n");
+#endif
+
+	for (p = 0; p < ENDATA_LEN-1; p++) {
+		if (*(bdata+p) != plain_data[p]) {
+			FB_DEBUG("Verify salt in decrypt error on pointer %d\n", p);
+			return FASTBOOT_LOCK_ERROR;
+		}
+	}
+
+	if (plain_data[ENDATA_LEN - 1] >= FASTBOOT_LOCK_NUM)
+		return FASTBOOT_LOCK_ERROR;
+	else
+		return plain_data[ENDATA_LEN-1];
+}
+
+static int encrypt_lock_store(FbLockState lock, unsigned char* bdata) {
+	unsigned int p = 0;
+	int ret;
+	int salt_len = generate_salt(bdata);
+	if (salt_len < 0)
+		return -1;
+
+    //salt_len cannot be longer than endata block size.
+	if (salt_len >= ENDATA_LEN)
+		salt_len = ENDATA_LEN - 1;
+
+	p = ENDATA_LEN - 1;
+
+	//Set lock value
+	*(bdata + p) = lock;
+
+	caam_open();
+	ret = caam_gen_blob((uint32_t)bdata, (uint32_t)(bdata + ENDATA_LEN), ENDATA_LEN);
+	if (ret != 0) {
+		printf("error in caam_gen_blob:0x%x\n", ret);
+		return -1;
+	}
+
+
+#ifdef FASTBOOT_LOCK_DEBUG
+	int i = 0;
+	FB_DEBUG("encrypt plain_text:\n");
+	for (i = 0; i < ENDATA_LEN; i++) {
+		FB_DEBUG("0x%2x\t", *(bdata+i));
+		if (i % 16 == 0)
+			printf("\n");
+	}
+	printf("\nto:\n");
+	for (i=0; i < ENDATA_LEN + 48; i++) {
+		FB_DEBUG("0x%2x\t", *(bdata + ENDATA_LEN + i));
+		if (i % 16 == 0)
+			printf("\n");
+	}
+	printf("\n");
+
+#endif
+	//protect value
+	*(bdata + p) = 0xff;
+	return 0;
+}
+
+#endif
+
+static char mmc_dev_part[16];
+static char* get_mmc_part(int part) {
+	u32 dev_no = mmc_get_env_dev();
+	sprintf(mmc_dev_part,"%x:%x",dev_no, part);
+	return mmc_dev_part;
+}
+
+static inline void set_lock_disable_data(unsigned char* bdata) {
+	*(bdata + SECTOR_SIZE -1) = 0;
+}
+
+/*
+ * The enabling value is stored in the last byte of target partition.
+ */
+static inline unsigned char lock_enable_parse(unsigned char* bdata) {
+	FB_DEBUG("lock_enable_parse: 0x%x\n", *(bdata + SECTOR_SIZE -1));
+	if (*(bdata + SECTOR_SIZE -1) >= FASTBOOT_UL_NUM)
+		return FASTBOOT_UL_ERROR;
+	else
+		return *(bdata + SECTOR_SIZE -1);
+}
+
+static FbLockState g_lockstat = FASTBOOT_UNLOCK;
+/*
+ * Set status of the lock&unlock to FSL_FASTBOOT_FB_PART
+ * Currently use the very first Byte of FSL_FASTBOOT_FB_PART
+ * to store the fastboot lock&unlock status
+ */
+int fastboot_set_lock_stat(FbLockState lock) {
+	struct blk_desc *fs_dev_desc;
+	disk_partition_t fs_partition;
+	unsigned char *bdata;
+	int mmc_id;
+	int status, ret;
+
+	bdata = (unsigned char *)memalign(ALIGN_BYTES, SECTOR_SIZE);
+	if (bdata == NULL)
+		goto fail2;
+	memset(bdata, 0, SECTOR_SIZE);
+
+	mmc_id = fastboot_flash_find_index(FASTBOOT_PARTITION_FBMISC);
+	if (mmc_id < 0) {
+		printf("%s: error in get mmc part\n", __FUNCTION__);
+		ret = -1;
+		goto fail;
+	}
+	status = blk_get_device_part_str(FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id),
+		&fs_dev_desc, &fs_partition, 1);
+	if (status < 0) {
+		printf("%s:error in getdevice partition.\n", __FUNCTION__);
+		ret = -1;
+		goto fail;
+	}
+	FB_DEBUG("%s %s partition.start=%d, size=%d\n",FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id), fs_partition.start, fs_partition.size);
+
+	status = encrypt_lock_store(lock, bdata);
+	if (status < 0) {
+		ret = -1;
+		goto fail;
+	}
+	status = fs_dev_desc->block_write(fs_dev_desc, fs_partition.start, 1, bdata);
+	if (!status) {
+		printf("%s:error in block write.\n", __FUNCTION__);
+		ret = -1;
+		goto fail;
+	}
+	ret = 0;
+fail:
+	free(bdata);
+	return ret;
+fail2:
+	g_lockstat = lock;
+	return 0;
+}
+
+FbLockState fastboot_get_lock_stat(void) {
+	struct blk_desc *fs_dev_desc;
+	disk_partition_t fs_partition;
+	unsigned char *bdata;
+	int mmc_id;
+	FbLockState ret;
+
+	bdata = (unsigned char *)memalign(ALIGN_BYTES, SECTOR_SIZE);
+	if (bdata == NULL)
+		return g_lockstat;
+
+	int status;
+	mmc_id = fastboot_flash_find_index(FASTBOOT_PARTITION_FBMISC);
+	if (mmc_id < 0) {
+		printf("%s: error in get mmc part\n", __FUNCTION__);
+		ret = g_lockstat;
+		goto fail;
+	}
+	status = blk_get_device_part_str(FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id),
+		&fs_dev_desc, &fs_partition, 1);
+
+	if (status < 0) {
+		printf("%s:error in getdevice partition.\n", __FUNCTION__);
+		ret = g_lockstat;
+		goto fail;
+	}
+	FB_DEBUG("%s %s partition.start=%d, size=%d\n",FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id), fs_partition.start, fs_partition.size);
+
+	status = fs_dev_desc->block_read(fs_dev_desc, fs_partition.start, 1, bdata);
+	if (!status) {
+		printf("%s:error in block read.\n", __FUNCTION__);
+		ret = FASTBOOT_LOCK_ERROR;
+		goto fail;
+	}
+
+	ret = decrypt_lock_store(bdata);
+fail:
+	free(bdata);
+	return ret;
+}
+
+
+/* Return the last byte of of FSL_FASTBOOT_PR_DATA
+ * which is managed by PresistDataService
+ */
+
+#ifdef CONFIG_ENABLE_LOCKSTATUS_SUPPORT
+//Brillo has no presist data partition
+FbLockEnableResult fastboot_lock_enable(void) {
+	return FASTBOOT_UL_ENABLE;
+}
+void set_fastboot_lock_disable(void) {
+}
+#else
+void set_fastboot_lock_disable(void) {
+	struct blk_desc *fs_dev_desc;
+	disk_partition_t fs_partition;
+	unsigned char *bdata;
+	int mmc_id;
+
+	bdata = (unsigned char *)memalign(ALIGN_BYTES, SECTOR_SIZE);
+	if (bdata == NULL)
+		return;
+	set_lock_disable_data(bdata);
+	int status;
+	mmc_id = fastboot_flash_find_index(FASTBOOT_PARTITION_PRDATA);
+	if (mmc_id < 0) {
+		printf("%s: error in get mmc part\n", __FUNCTION__);
+		goto fail;
+	}
+	status = blk_get_device_part_str(FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id),
+		&fs_dev_desc, &fs_partition, 1);
+	if (status < 0) {
+		printf("%s:error in getdevice partition.\n", __FUNCTION__);
+		goto fail;
+	}
+
+	lbaint_t target_block = fs_partition.start + fs_partition.size - 1;
+	FB_DEBUG("target_block.start=%d, size=%d target_block=%d\n", fs_partition.start, fs_partition.size, target_block);
+	status = fs_dev_desc->block_write(fs_dev_desc, target_block, 1, bdata);
+	if (!status) {
+		printf("%s: error in block read\n", __FUNCTION__);
+		goto fail;
+	}
+
+fail:
+	free(bdata);
+	return;
+
+}
+FbLockEnableResult fastboot_lock_enable() {
+	struct blk_desc *fs_dev_desc;
+	disk_partition_t fs_partition;
+	unsigned char *bdata;
+	int mmc_id;
+	FbLockEnableResult ret;
+
+	bdata = (unsigned char *)memalign(ALIGN_BYTES, SECTOR_SIZE);
+	if (bdata == NULL)
+		return FASTBOOT_UL_ERROR;
+	int status;
+	mmc_id = fastboot_flash_find_index(FASTBOOT_PARTITION_PRDATA);
+	if (mmc_id < 0) {
+		printf("%s: error in get mmc part\n", __FUNCTION__);
+		ret = FASTBOOT_UL_ERROR;
+		goto fail;
+	}
+	status = blk_get_device_part_str(FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id),
+		&fs_dev_desc, &fs_partition, 1);
+	if (status < 0) {
+		printf("%s:error in getdevice partition.\n", __FUNCTION__);
+		ret = FASTBOOT_UL_ERROR;
+		goto fail;
+	}
+
+    //The data is stored in the last blcok of this partition.
+	lbaint_t target_block = fs_partition.start + fs_partition.size - 1;
+	FB_DEBUG("target_block.start=%d, size=%d target_block=%d\n", fs_partition.start, fs_partition.size, target_block);
+	status = fs_dev_desc->block_read(fs_dev_desc, target_block, 1, bdata);
+	if (!status) {
+		printf("%s: error in block read\n", __FUNCTION__);
+		ret = FASTBOOT_UL_ERROR;
+		goto fail;
+	}
+	int i = 0;
+	FB_DEBUG("\n PRIST last sector is:\n");
+	for (i = 0; i < SECTOR_SIZE; i++) {
+		FB_DEBUG("0x%x  ", *(bdata + i));
+		if (i % 32 == 0)
+			FB_DEBUG("\n");
+	}
+	FB_DEBUG("\n");
+	ret = lock_enable_parse(bdata);
+fail:
+	free(bdata);
+	return ret;
+
+}
+#endif
+
+int display_lock(FbLockState lock, int verify) {
+	struct stdio_dev *disp;
+	disp = stdio_get_by_name("vga");
+	if (disp != NULL) {
+		if (lock == FASTBOOT_UNLOCK) {
+			disp->puts(disp, "\n============= NOTICE ============\n");
+			disp->puts(disp,   "|                               |\n");
+			disp->puts(disp,   "|   Your device is NOT locked.  |\n");
+			disp->puts(disp,   "|                               |\n");
+			disp->puts(disp,   "=================================\n");
+		} else {
+			if (verify == -1) {
+				disp->puts(disp, "\n============= NOTICE ============\n");
+				disp->puts(disp,   "|                               |\n");
+				disp->puts(disp,   "| Your device is NOT protected. |\n");
+				disp->puts(disp,   "|                               |\n");
+				disp->puts(disp,   "=================================\n");
+			} else if (verify == 1) {
+				disp->puts(disp, "\n============= NOTICE ============\n");
+				disp->puts(disp,   "|                               |\n");
+				disp->puts(disp,   "|       Boot verify failed!     |\n");
+				disp->puts(disp,   "|                               |\n");
+				disp->puts(disp,   "=================================\n");
+			}
+		}
+		return 0;
+	} else
+		printf("not found VGA disp console.\n");
+
+	return -1;
+
+}
+
+int fastboot_wipe_data_partition(void)
+{
+	struct blk_desc *fs_dev_desc;
+	disk_partition_t fs_partition;
+	int status;
+	int mmc_id;
+	mmc_id = fastboot_flash_find_index(FASTBOOT_PARTITION_DATA);
+	if (mmc_id < 0) {
+		printf("%s: error in get mmc part\n", __FUNCTION__);
+		return -1;
+	}
+	status = blk_get_device_part_str(FSL_FASTBOOT_FB_DEV,
+		get_mmc_part(mmc_id), &fs_dev_desc, &fs_partition, 1);
+	if (status < 0) {
+		printf("error in get device partition for wipe /data\n");
+		return -1;
+	}
+	FB_DEBUG("fs->start=%x, size=%d\n", fs_partition.start, fs_partition.size);
+	status = fs_dev_desc->block_erase(fs_dev_desc, fs_partition.start , fs_partition.size );
+	if (status != fs_partition.size ) {
+		printf("erase not complete\n");
+		return -1;
+	}
+	mdelay(2000);
+
+	return 0;
+}
diff --git a/drivers/usb/gadget/fastboot_lock_unlock.h b/drivers/usb/gadget/fastboot_lock_unlock.h
new file mode 100644
index 0000000..380be1a
--- /dev/null
+++ b/drivers/usb/gadget/fastboot_lock_unlock.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2016, Freescale Semiconductor, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted provided that the following conditions are met:
+ *
+ * o Redistributions of source code must retain the above copyright notice, this list
+ *   of conditions and the following disclaimer.
+ *
+ * o Redistributions in binary form must reproduce the above copyright notice, this
+ *   list of conditions and the following disclaimer in the documentation and/or
+ *   other materials provided with the distribution.
+ *
+ * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
+ *   contributors may be used to endorse or promote products derived from this
+ *   software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
+ * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
+ * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef FASTBOOT_LOCK_UNLOCK_H
+#define FASTBOOT_LOCK_UNLOCK_H
+
+#define ALIGN_BYTES 64 /*armv7 cache line need 64 bytes aligned */
+
+//#define FASTBOOT_LOCK_DEBUG
+#define FASTBOOT_ENCRYPT_LOCK
+
+#ifdef FASTBOOT_LOCK_DEBUG
+#define FB_DEBUG(format, ...) printf(format, ##__VA_ARGS__)
+#else
+#define FB_DEBUG(format, ...)
+#endif
+
+typedef enum {
+	FASTBOOT_UNLOCK,
+	FASTBOOT_LOCK,
+	FASTBOOT_LOCK_ERROR,
+	FASTBOOT_LOCK_NUM
+}FbLockState;
+
+typedef enum {
+	FASTBOOT_UL_DISABLE,
+	FASTBOOT_UL_ENABLE,
+	FASTBOOT_UL_ERROR,
+	FASTBOOT_UL_NUM
+}FbLockEnableResult;
+
+FbLockState fastboot_get_lock_stat(void);
+
+int fastboot_set_lock_stat(FbLockState lock);
+
+int fastboot_wipe_data_partition(void);
+
+FbLockEnableResult fastboot_lock_enable(void);
+void set_fastboot_lock_disable(void);
+
+int display_lock(FbLockState lock, int verify);
+#endif