MA-9375 [Android IMX] uboot: enable BCB and bootctrl

* Add API to read\write MISC partition.
* get the boot mode from BCB command when boot up.
* get the boot up tactics from bootctrl.

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

7 files changed, 1053 insertions, 124 deletions

diff --git a/drivers/usb/gadget/Makefile b/drivers/usb/gadget/Makefile
index 0fbbb7c82c..6431d8aa64 100644
--- a/drivers/usb/gadget/Makefile
+++ b/drivers/usb/gadget/Makefile
@@ -28,6 +28,8 @@ 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_FSL_BOOTCTL) += bootctrl.o
+obj-$(CONFIG_BCB_SUPPORT) += command.o bcb.o
 endif
 endif
 ifdef CONFIG_USB_ETHER
diff --git a/drivers/usb/gadget/bcb.c b/drivers/usb/gadget/bcb.c
new file mode 100644
index 0000000000..159f9293f1
--- /dev/null
+++ b/drivers/usb/gadget/bcb.c
@@ -0,0 +1,170 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ * Copyright 2017 NXP
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <fsl_fastboot.h>
+#include <linux/stat.h>
+#include <linux/types.h>
+#include <common.h>
+#include <g_dnl.h>
+#include <mmc.h>
+#include "bcb.h"
+#include "bootctrl.h"
+#define ALIGN_BYTES 64 /*armv7 cache line need 64 bytes aligned */
+
+static ulong get_block_size(char *ifname, int dev)
+{
+	struct blk_desc *dev_desc = NULL;
+
+	dev_desc = blk_get_dev(ifname, dev);
+	if (dev_desc == NULL) {
+		printf("Block device %s %d not supported\n", ifname, dev);
+		return 0;
+	}
+
+	return dev_desc->blksz;
+}
+
+static int do_write(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	char *ep;
+	struct blk_desc *dev_desc = NULL;
+	int dev;
+	int part = 0;
+	disk_partition_t part_info;
+	ulong offset = 0u;
+	ulong limit = 0u;
+	void *addr;
+	uint blk;
+	uint cnt;
+
+	if (argc != 6) {
+		cmd_usage(cmdtp);
+		return 1;
+	}
+
+	dev = (int)simple_strtoul(argv[2], &ep, 16);
+	if (*ep) {
+		if (*ep != ':') {
+			printf("Invalid block device %s\n", argv[2]);
+			return 1;
+		}
+		part = (int)simple_strtoul(++ep, NULL, 16);
+	}
+
+	dev_desc = blk_get_dev(argv[1], dev);
+	if (dev_desc == NULL) {
+		printf("Block device %s %d not supported\n", argv[1], dev);
+		return 1;
+	}
+
+	addr = (void *)simple_strtoul(argv[3], NULL, 16);
+	blk = simple_strtoul(argv[4], NULL, 16);
+	cnt = simple_strtoul(argv[5], NULL, 16);
+
+	if (part != 0) {
+		if (part_get_info(dev_desc, part, &part_info)) {
+			printf("Cannot find partition %d\n", part);
+			return 1;
+		}
+		offset = part_info.start;
+		limit = part_info.size;
+	} else {
+		/* Largest address not available in block_dev_desc_t. */
+		limit = ~0;
+	}
+
+	if (cnt + blk > limit) {
+		printf("Write out of range\n");
+		return 1;
+	}
+
+	if (dev_desc->block_write(dev_desc, offset + blk, cnt, addr) != cnt) {
+		printf("Error writing blocks\n");
+		return 1;
+	}
+
+	return 0;
+}
+
+U_BOOT_CMD(
+	write,	6,	0,	do_write,
+	"write binary data to a partition",
+	"<interface> <dev[:part]> addr blk# cnt"
+);
+
+int bcb_rw_block(bool bread, char **ppblock,
+		uint *pblksize, char *pblock_write, uint offset, uint size)
+{
+	int ret;
+	char *argv[6];
+	char addr_str[20];
+	char cnt_str[8];
+	char devpart_str[8];
+	char block_begin_str[8];
+	ulong blk_size = 0;
+	uint blk_begin = 0;
+	uint blk_end = 0;
+	uint block_cnt = 0;
+	char *p_block = NULL;
+	unsigned int mmc_id;
+
+	if (bread && ((ppblock == NULL) || (pblksize == NULL)))
+		return -1;
+
+	if (!bread && (pblock_write == NULL))
+		return -1;
+
+	mmc_id = mmc_get_env_dev();
+	blk_size = get_block_size("mmc", mmc_id);
+	if (blk_size == 0) {
+		printf("bcb_rw_block, get_block_size return 0\n");
+		return -1;
+	}
+
+	blk_begin = offset/blk_size;
+	blk_end = (offset + size)/blk_size;
+	block_cnt = 1 + (blk_end - blk_begin);
+
+	sprintf(devpart_str, "0x%x:0x%x", mmc_id,
+			fastboot_flash_find_index(FASTBOOT_PARTITION_MISC));
+	sprintf(block_begin_str, "0x%x", blk_begin);
+	sprintf(cnt_str, "0x%x", block_cnt);
+
+	argv[0] = "rw"; /* not care */
+	argv[1] = "mmc";
+	argv[2] = devpart_str;
+	argv[3] = addr_str;
+	argv[4] = block_begin_str;
+	argv[5] = cnt_str;
+
+	if (bread) {
+		p_block = (char *)memalign(ALIGN_BYTES, blk_size * block_cnt);
+		if (NULL == p_block) {
+			printf("bcb_rw_block, memalign %d bytes failed\n",
+			(int)(blk_size * block_cnt));
+			return -1;
+		}
+		sprintf(addr_str, "0x%x", (unsigned int)p_block);
+		ret = do_raw_read(NULL, 0, 6, argv);
+		if (ret) {
+			free(p_block);
+			printf("do_raw_read failed, ret %d\n", ret);
+			return -1;
+		}
+
+		*ppblock = p_block;
+		*pblksize = (uint)blk_size;
+	} else {
+		sprintf(addr_str, "0x%x", (unsigned int)pblock_write);
+		ret = do_write(NULL, 0, 6, argv);
+		if (ret) {
+			printf("do_write failed, ret %d\n", ret);
+			return -1;
+		}
+	}
+	return 0;
+}
diff --git a/drivers/usb/gadget/bcb.h b/drivers/usb/gadget/bcb.h
new file mode 100644
index 0000000000..1e02508f2d
--- /dev/null
+++ b/drivers/usb/gadget/bcb.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#ifndef BCB_H
+#define BCB_H
+#include <linux/types.h>
+#include <linux/stat.h>
+
+#define FASTBOOT_BCB_CMD "bootonce-bootloader"
+#ifdef CONFIG_ANDROID_RECOVERY
+#define RECOVERY_BCB_CMD "boot-recovery"
+#endif
+/* keep same as bootable/recovery/bootloader.h */
+struct bootloader_message {
+	char command[32];
+	char status[32];
+	char recovery[768];
+
+	/* The 'recovery' field used to be 1024 bytes. It has only ever
+	 been used to store the recovery command line, so 768 bytes
+	 should be plenty.  We carve off the last 256 bytes to store the
+	 stage string (for multistage packages) and possible future
+	 expansion. */
+	char stage[32];
+
+	/* The 'reserved' field used to be 224 bytes when it was initially
+	 carved off from the 1024-byte recovery field. Bump it up to
+	 1184-byte so that the entire bootloader_message struct rounds up
+	 to 2048-byte.
+	 */
+	char reserved[1184];
+};
+
+struct bootloader_message_ab {
+	struct bootloader_message message;
+	char slot_suffix[32];
+
+	/* Round up the entire struct to 4096-byte. */
+	char reserved[2016];
+};
+
+/* start from bootloader_message_ab.slot_suffix[BOOTCTRL_IDX] */
+#define BOOTCTRL_IDX                            0
+#define MISC_COMMAND_IDX                        0
+#define BOOTCTRL_OFFSET         \
+	(u32)(&(((struct bootloader_message_ab *)0)->slot_suffix[BOOTCTRL_IDX]))
+#define MISC_COMMAND \
+	(u32)(&(((struct bootloader_message *)0)->command[MISC_COMMAND_IDX]))
+int bcb_rw_block(bool bread, char **ppblock,
+		uint *pblksize, char *pblock_write, uint offset, uint size);
+
+int bcb_write_command(char *bcb_command);
+int bcb_read_command(char *command);
+
+#endif
diff --git a/drivers/usb/gadget/bootctrl.c b/drivers/usb/gadget/bootctrl.c
new file mode 100644
index 0000000000..8e0b999ebe
--- /dev/null
+++ b/drivers/usb/gadget/bootctrl.c
@@ -0,0 +1,405 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <common.h>
+#include <g_dnl.h>
+#include <fsl_fastboot.h>
+#include "bootctrl.h"
+#include <linux/types.h>
+#include <linux/stat.h>
+static unsigned int g_slot_selected;
+static const char *g_slot_suffix[SLOT_NUM] = {"_a", "_b"};
+static int init_slotmeta(struct boot_ctl *ptbootctl);
+
+static int strcmp_l1(const char *s1, const char *s2)
+{
+	if (!s1 || !s2)
+		return -1;
+	return strncmp(s1, s2, strlen(s1));
+}
+
+
+static void dump_slotmeta(struct boot_ctl *ptbootctl)
+{
+	int i;
+
+	if (ptbootctl == NULL)
+		return;
+
+	printf("RecoveryTryRemain %d, crc %u\n",
+		   ptbootctl->recovery_tryremain, ptbootctl->crc);
+
+	for (i = 0; i < SLOT_NUM; i++) {
+		printf("slot %d: pri %d, try %d, suc %d\n", i,
+			   ptbootctl->a_slot_meta[i].priority,
+				ptbootctl->a_slot_meta[i].tryremain,
+				ptbootctl->a_slot_meta[i].bootsuc);
+	}
+
+	return;
+}
+
+const char *get_slot_suffix(void)
+{
+	return g_slot_suffix[g_slot_selected];
+}
+
+static unsigned int slot_find(struct boot_ctl *ptbootctl)
+{
+	unsigned int max_pri = 0;
+	unsigned int slot = -1;
+	int i;
+
+	for (i	= 0; i < SLOT_NUM; i++) {
+		struct slot_meta *pslot_meta = &(ptbootctl->a_slot_meta[i]);
+		if ((pslot_meta->priority > max_pri) &&
+			((pslot_meta->bootsuc > 0) ||
+			(pslot_meta->tryremain > 0))) {
+			max_pri = pslot_meta->priority;
+			slot = i;
+			printf("select_slot slot %d\n", slot);
+		}
+	}
+
+	return slot;
+}
+
+
+
+static int read_bootctl(struct boot_ctl *ptbootctl)
+{
+	int ret = 0;
+	unsigned int crc = 0;
+	char *p_block = NULL;
+	uint offset_in_block = 0;
+	uint blk_size = 0;
+	char *pmagic = NULL;
+
+	if (ptbootctl == NULL)
+		return -1;
+	ret = bcb_rw_block(true, &p_block, &blk_size, NULL,
+			BOOTCTRL_OFFSET, sizeof(struct boot_ctl));
+	if (ret) {
+		printf("read_bootctl, bcb_rw_block read failed\n");
+		return -1;
+	}
+
+	offset_in_block = BOOTCTRL_OFFSET%blk_size;
+	memcpy(ptbootctl, p_block + offset_in_block, sizeof(struct boot_ctl));
+
+	pmagic = ptbootctl->magic;
+	if (!((pmagic[0] == '\0') && (pmagic[1] == 'F') &&
+	      (pmagic[2] == 'S') && (pmagic[3] == 'L'))) {
+		printf("magic error, 0x%x 0x%x 0x%x 0x%x\n",
+		       pmagic[0], pmagic[1], pmagic[2], pmagic[3]);
+
+		ret = init_slotmeta(ptbootctl);
+		if (ret) {
+			printf("init_slotmeta failed, ret %d\n", ret);
+			free(p_block);
+			return -1;
+		}
+	}
+
+	/* check crc */
+	crc = crc32(0, (unsigned char *)ptbootctl + CRC_DATA_OFFSET,
+		sizeof(struct boot_ctl) - CRC_DATA_OFFSET);
+	if (crc != ptbootctl->crc) {
+		printf("crc check failed, caculated %d, read %d\n",
+		       crc, ptbootctl->crc);
+
+		free(p_block);
+		return -1;
+	}
+
+	free(p_block);
+	return 0;
+}
+
+static int write_bootctl(struct boot_ctl *ptbootctl)
+{
+	int ret = 0;
+	char *p_block = NULL;
+	uint offset_in_block = 0;
+	uint blk_size = 0;
+
+	if (ptbootctl == NULL)
+		return -1;
+
+	ptbootctl->crc = crc32(0, (unsigned char *)ptbootctl + CRC_DATA_OFFSET,
+		sizeof(struct boot_ctl) - CRC_DATA_OFFSET);
+
+	ret = bcb_rw_block(true, &p_block, &blk_size, NULL,
+				BOOTCTRL_OFFSET, sizeof(struct boot_ctl));
+	if (ret) {
+		printf("write_bootctl, bcb_rw_block read failed\n");
+		return -1;
+	}
+	offset_in_block = BOOTCTRL_OFFSET%blk_size;
+	memcpy(p_block + offset_in_block, ptbootctl, sizeof(struct boot_ctl));
+
+	ret = bcb_rw_block(false, NULL, NULL, p_block,
+			BOOTCTRL_OFFSET, sizeof(struct boot_ctl));
+	if (ret) {
+		free(p_block);
+		printf("write_bootctl, bcb_rw_block write failed\n");
+		return -1;
+	}
+	free(p_block);
+	return 0;
+}
+
+static int init_slotmeta(struct boot_ctl *ptbootctl)
+{
+	int ret = 0;
+	if (ptbootctl == NULL)
+		return -1;
+	memset(ptbootctl, 0, sizeof(struct boot_ctl));
+	ptbootctl->recovery_tryremain = 7;
+	ptbootctl->a_slot_meta[0].priority = 8;
+	ptbootctl->a_slot_meta[0].tryremain = 7;
+	ptbootctl->a_slot_meta[0].bootsuc = 0;
+	ptbootctl->a_slot_meta[1].priority = 6;
+	ptbootctl->a_slot_meta[1].tryremain = 7;
+	ptbootctl->a_slot_meta[1].bootsuc = 0;
+
+	ptbootctl->magic[0] = '\0';
+	ptbootctl->magic[1] = 'F';
+	ptbootctl->magic[2] = 'S';
+	ptbootctl->magic[3] = 'L';
+
+	ptbootctl->crc = crc32(0, (uint8_t *)ptbootctl + CRC_DATA_OFFSET,
+			sizeof(struct boot_ctl) - CRC_DATA_OFFSET);
+	ret = write_bootctl(ptbootctl);
+	return ret;
+}
+
+char *select_slot(void)
+{
+	int i = 0;
+	int ret = 0;
+	unsigned int slot;
+	struct boot_ctl t_bootctl;
+	bool b_need_write = false;
+
+	ret = read_bootctl(&t_bootctl);
+	if (ret) {
+		printf("read_bootctl failed, ret %d\n", ret);
+		return NULL;
+	}
+
+	dump_slotmeta(&t_bootctl);
+
+	slot = slot_find(&t_bootctl);
+	if (slot >= SLOT_NUM) {
+		printf("!!! select_slot, no valid slot\n");
+		return NULL;
+	}
+
+	/* invalid slot, set priority to 0 */
+	for (i	= 0; i < SLOT_NUM; i++) {
+		struct slot_meta *pslot_meta = &(t_bootctl.a_slot_meta[i]);
+		if ((pslot_meta->bootsuc == 0) &&
+			(pslot_meta->tryremain == 0) &&
+			(pslot_meta->priority > 0)) {
+			pslot_meta->priority = 0;
+			b_need_write = true;
+		}
+	}
+
+	if (t_bootctl.recovery_tryremain != 7) {
+		b_need_write = true;
+		t_bootctl.recovery_tryremain = 7;
+	}
+
+	if ((t_bootctl.a_slot_meta[slot].bootsuc == 0) &&
+		(t_bootctl.a_slot_meta[slot].tryremain > 0)) {
+		b_need_write = true;
+		t_bootctl.a_slot_meta[slot].tryremain--;
+	}
+
+	if (b_need_write) {
+		ret = write_bootctl(&t_bootctl);
+		if (ret)
+			printf("!!! write_bootctl failed, ret %d\n", ret);
+	}
+
+	g_slot_selected = slot;
+
+	if (slot == 0)
+		return FASTBOOT_PARTITION_BOOT_A;
+	else
+		return FASTBOOT_PARTITION_BOOT_B;
+}
+
+
+int invalid_curslot(void)
+{
+	int ret = 0;
+	struct boot_ctl t_bootctl;
+	unsigned int slot = g_slot_selected;
+
+	printf("invalid_curslot %d\n", slot);
+
+	if (slot >= SLOT_NUM)
+		return -1;
+
+	ret = read_bootctl(&t_bootctl);
+	if (ret) {
+		printf("invalid_slot failed, ret %d\n", ret);
+		return -1;
+	}
+
+	t_bootctl.a_slot_meta[slot].priority = 0;
+	ret = write_bootctl(&t_bootctl);
+	if (ret) {
+		printf("!!! write_bootctl failed, ret %d\n", ret);
+		return -1;
+	}
+
+	return 0;
+}
+
+
+static unsigned int slotidx_from_suffix(char *suffix)
+{
+	unsigned int slot = -1;
+
+	if (!strcmp(suffix, "_a") ||
+			!strcmp(suffix, "a"))
+		slot = 0;
+	else if (!strcmp(suffix, "_b") ||
+			!strcmp(suffix, "b"))
+		slot = 1;
+
+	return slot;
+}
+
+
+int get_slotvar(char *cmd, char *response, size_t chars_left)
+{
+	int ret;
+	struct boot_ctl t_bootctl;
+	memset(&t_bootctl, 0, sizeof(t_bootctl));
+
+	/* these two var no need to read_bootctl */
+	if (!strcmp_l1("has-slot:", cmd)) {
+		char *ptnname = NULL;
+		ptnname = strchr(cmd, ':') + 1;
+		if (!strcmp(ptnname, "system") || !strcmp(ptnname, "boot"))
+			strlcpy(response, "yes", chars_left);
+		else
+			strlcpy(response, "no", chars_left);
+		return 0;
+	} else if (!strcmp_l1("slot-suffixes", cmd)) {
+		strlcpy(response, "_a,_b", chars_left);
+		return 0;
+	}
+
+	ret = read_bootctl(&t_bootctl);
+	if (ret) {
+		error("get_slotvar, read_bootctl failed\n");
+		strcpy(response, "get_slotvar read_bootctl failed");
+		return -1;
+	}
+	if (!strcmp_l1("current-slot", cmd)) {
+		unsigned int slot = slot_find(&t_bootctl);
+		if (slot < SLOT_NUM)
+			strlcpy(response, g_slot_suffix[slot], chars_left);
+		else {
+			strlcpy(response, "no valid slot", chars_left);
+			return -1;
+		}
+	} else if (!strcmp_l1("slot-successful:", cmd)) {
+		char *suffix = strchr(cmd, ':') + 1;
+		unsigned int slot = slotidx_from_suffix(suffix);
+		if (slot >= SLOT_NUM) {
+			strlcpy(response, "no such slot", chars_left);
+			return -1;
+		} else {
+			bool suc = t_bootctl.a_slot_meta[slot].bootsuc;
+			strlcpy(response, suc ? "yes" : "no", chars_left);
+		}
+	} else if (!strcmp_l1("slot-unbootable:", cmd)) {
+		char *suffix = strchr(cmd, ':') + 1;
+		unsigned int slot = slotidx_from_suffix(suffix);
+		if (slot >= SLOT_NUM) {
+			strlcpy(response, "no such slot", chars_left);
+			return -1;
+		} else {
+			unsigned int pri = t_bootctl.a_slot_meta[slot].priority;
+			strlcpy(response, pri ? "no" : "yes", chars_left);
+		}
+	} else if (!strcmp_l1("slot-retry-count:", cmd)) {
+		char *suffix = strchr(cmd, ':') + 1;
+		unsigned int slot = slotidx_from_suffix(suffix);
+		if (slot >= SLOT_NUM) {
+			strlcpy(response, "no such slot", chars_left);
+			return -1;
+		} else {
+			char str_num[7];
+			sprintf(str_num, "%d",
+				t_bootctl.a_slot_meta[slot].tryremain);
+			strlcpy(response, str_num, chars_left);
+		}
+	} else {
+		strlcpy(response, "no such slot command", chars_left);
+		return -1;
+	}
+
+	return 0;
+}
+
+
+void cb_set_active(struct usb_ep *ep, struct usb_request *req)
+{
+	int ret;
+	int i;
+	unsigned int slot = 0;
+	char *cmd = req->buf;
+	struct boot_ctl t_bootctl;
+
+	memset(&t_bootctl, 0, sizeof(t_bootctl));
+
+	strsep(&cmd, ":");
+	if (!cmd) {
+		error("missing slot suffix\n");
+		fastboot_tx_write_str("FAILmissing slot suffix");
+		return;
+	}
+
+	slot = slotidx_from_suffix(cmd);
+	if (slot >= SLOT_NUM) {
+		fastboot_tx_write_str("FAILerr slot suffix");
+		return;
+	}
+
+	ret = read_bootctl(&t_bootctl);
+	if (ret)
+		fastboot_tx_write_str("FAILReadBootCtl failed");
+
+	t_bootctl.a_slot_meta[slot].bootsuc = 0;
+	t_bootctl.a_slot_meta[slot].priority = 15;
+	t_bootctl.a_slot_meta[slot].tryremain = 7;
+
+	/* low other slot priority */
+	for (i = 0; i < SLOT_NUM; i++) {
+		if (i == slot)
+			continue;
+
+		if (t_bootctl.a_slot_meta[i].priority >= 15)
+			t_bootctl.a_slot_meta[i].priority = 14;
+	}
+
+	ret = write_bootctl(&t_bootctl);
+	if (ret)
+		fastboot_tx_write_str("write_bootctl failed");
+	else
+		fastboot_tx_write_str("OKAY");
+
+	return;
+}
+
diff --git a/drivers/usb/gadget/bootctrl.h b/drivers/usb/gadget/bootctrl.h
new file mode 100644
index 0000000000..a181cd73db
--- /dev/null
+++ b/drivers/usb/gadget/bootctrl.h
@@ -0,0 +1,37 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#ifndef BOOTCTRL_H
+#define BOOTCTRL_H
+
+#include <common.h>
+#include <g_dnl.h>
+#include <linux/types.h>
+#include <linux/stat.h>
+#include "bcb.h"
+
+#define SLOT_NUM (unsigned int)2
+#define CRC_DATA_OFFSET		\
+	(uint32_t)(&(((struct boot_ctl *)0)->a_slot_meta[0]))
+
+struct slot_meta {
+	u8 bootsuc:1;
+	u8 tryremain:3;
+	u8 priority:4;
+};
+
+struct boot_ctl {
+	char magic[4]; /* "\0FSL" */
+	u32 crc;
+	struct slot_meta a_slot_meta[SLOT_NUM];
+	u8 recovery_tryremain;
+};
+char *select_slot(void);
+int invalid_curslot(void);
+int get_slotvar(char *cmd, char *response, size_t chars_left);
+void cb_set_active(struct usb_ep *ep, struct usb_request *req);
+const char *get_slot_suffix(void);
+#endif
diff --git a/drivers/usb/gadget/command.c b/drivers/usb/gadget/command.c
new file mode 100644
index 0000000000..e66e033904
--- /dev/null
+++ b/drivers/usb/gadget/command.c
@@ -0,0 +1,120 @@
+/*
+ * Copyright (C) 2015-2016 Freescale Semiconductor, Inc.
+ *
+ * SPDX-License-Identifier:     GPL-2.0+
+ */
+
+#include <common.h>
+#include <g_dnl.h>
+#ifdef CONFIG_FASTBOOT_STORAGE_NAND
+#include <nand.h>
+#endif
+#include "bcb.h"
+
+#ifndef CONFIG_FASTBOOT_STORAGE_NAND
+int bcb_read_command(char *command)
+{
+	int ret = 0;
+	char *p_block = NULL;
+	uint offset_in_block = 0;
+	uint blk_size = 0;
+
+	if (command == NULL)
+		return -1;
+
+	ret = bcb_rw_block(true, &p_block, &blk_size, NULL, MISC_COMMAND, 32);
+	if (ret) {
+		printf("read_bootctl, bcb_rw_block read failed\n");
+		return -1;
+	}
+
+	offset_in_block = MISC_COMMAND%blk_size;
+	memcpy(command, p_block + offset_in_block, 32);
+	free(p_block);
+
+	return 0;
+}
+int bcb_write_command(char *bcb_command)
+{
+	int ret = 0;
+	char *p_block = NULL;
+	uint offset_in_block = 0;
+	uint blk_size = 0;
+
+	if (bcb_command == NULL)
+		return -1;
+
+
+	ret = bcb_rw_block(true, &p_block, &blk_size, NULL,  MISC_COMMAND, 32);
+	if (ret) {
+		printf("write_bootctl, bcb_rw_block read failed\n");
+		return -1;
+	}
+
+	offset_in_block = MISC_COMMAND%blk_size;
+	memcpy(p_block + offset_in_block, bcb_command, 32);
+
+	ret = bcb_rw_block(false, NULL, NULL, p_block, MISC_COMMAND, 32);
+	if (ret) {
+		free(p_block);
+		printf("write_bootctl, bcb_rw_block write failed\n");
+		return -1;
+	}
+
+	free(p_block);
+	return 0;
+}
+#else
+#define ALIGN_BYTES 64
+#define MISC_PAGES 3
+int bcb_read_command(char *command)
+{
+	char read_cmd[128];
+	char *addr_str;
+	char *nand_str;
+	ulong misc_info_size;
+	nand_info_t *nand = &nand_info[0];
+	if (command == NULL)
+		return -1;
+	memset(read_cmd, 0, 128);
+	misc_info_size = MISC_PAGES * nand->writesize;
+	nand_str = (char *)memalign(ALIGN_BYTES, misc_info_size);
+	sprintf(read_cmd, "nand read 0x%x ${misc_nand_offset} \
+			0x%x", nand_str, misc_info_size);
+	run_command(read_cmd, 0);
+	/* The offset of bootloader_message is 1 PAGE.
+	 * The offset of bootloader_message and the size of misc info
+	 * need align with user space and recovery.
+	 */
+	addr_str = nand_str + nand->writesize;
+	memcpy(command, (char *)addr_str, 32);
+	free(nand_str);
+	return 0;
+}
+int bcb_write_command(char *command)
+{
+	char cmd[128];
+	char *addr_str;
+	char *nand_str;
+	ulong misc_info_size;
+	nand_info_t *nand = &nand_info[0];
+	if (command == NULL)
+		return -1;
+	memset(cmd, 0, 128);
+	misc_info_size = MISC_PAGES * nand->writesize;
+	nand_str = (char *)memalign(ALIGN_BYTES, misc_info_size);
+	sprintf(cmd, "nand read 0x%x ${misc_nand_offset} \
+			0x%x", nand_str, misc_info_size);
+	run_command(cmd, 0);
+	/* the offset of bootloader_message is 1 PAGE*/
+	addr_str = nand_str +  nand->writesize;
+	memcpy((char *)addr_str, command, 32);
+	/* erase 3 pages which hold BCB struct.*/
+	sprintf(cmd, "nand erase ${misc_nand_offset} 0x%x",nand->erasesize);
+	run_command(cmd, 0);
+	sprintf(cmd, "nand write 0x%x ${misc_nand_offset} 0x%x",nand_str, misc_info_size);
+	run_command(cmd, 0);
+	free(nand_str);
+	return 0;
+}
+#endif
diff --git a/drivers/usb/gadget/f_fastboot.c b/drivers/usb/gadget/f_fastboot.c
index 9f07940496..f8c9414f29 100644
--- a/drivers/usb/gadget/f_fastboot.c
+++ b/drivers/usb/gadget/f_fastboot.c
@@ -40,17 +40,24 @@
 #include <part.h>
 #include <sparse_format.h>
 #include <image-sparse.h>
+#include <asm/imx-common/boot_mode.h>
 #ifdef CONFIG_ANDROID_RECOVERY
 #include <recovery.h>
 #endif
 #endif
 
+#ifdef CONFIG_FSL_BOOTCTL
+#include "bootctrl.h"
+#endif
+#ifdef CONFIG_BCB_SUPPORT
+#include "bcb.h"
+#endif
+
+
 #define FASTBOOT_VERSION		"0.4"
 
-#ifdef CONFIG_EFI_PARTITION
 #define ANDROID_GPT_OFFSET         0
 #define ANDROID_GPT_SIZE           0x100000
-#endif
 #define FASTBOOT_INTERFACE_CLASS	0xff
 #define FASTBOOT_INTERFACE_SUB_CLASS	0x42
 #define FASTBOOT_INTERFACE_PROTOCOL	0x03
@@ -170,20 +177,6 @@ static struct usb_gadget_strings *fastboot_strings[] = {
 
 #ifdef CONFIG_FSL_FASTBOOT
 
-#ifdef CONFIG_EFI_PARTITION
-#define ANDROID_BOOT_PARTITION_MMC 1
-#define ANDROID_RECOVERY_PARTITION_MMC 2
-#define ANDROID_SYSTEM_PARTITION_MMC 3
-#define ANDROID_CACHE_PARTITION_MMC 4
-#define ANDROID_MISC_PARTITION_MMC 6
-#define ANDROID_DATA_PARTITION_MMC 10
-#else
-#define ANDROID_BOOT_PARTITION_MMC 1
-#define ANDROID_SYSTEM_PARTITION_MMC 5
-#define ANDROID_RECOVERY_PARTITION_MMC 2
-#define ANDROID_CACHE_PARTITION_MMC 6
-#define ANDROID_DATA_PARTITION_MMC 4
-#endif
 
 #define ANDROID_MBR_OFFSET	    0
 #define ANDROID_MBR_SIZE	    0x200
@@ -199,39 +192,13 @@ static struct usb_gadget_strings *fastboot_strings[] = {
 #define MMC_SATA_BLOCK_SIZE 512
 #define FASTBOOT_FBPARTS_ENV_MAX_LEN 1024
 /* To support the Android-style naming of flash */
-#define MAX_PTN		    16
+#define MAX_PTN		    32
 
 
-/*pentry index internally*/
-#ifdef CONFIG_EFI_PARTITION
 enum {
 	PTN_GPT_INDEX = 0,
-	PTN_BOOTLOADER_INDEX,
-	PTN_BOOT_INDEX,
-	PTN_RECOVERY_INDEX,
-	PTN_SYSTEM_INDEX,
-	PTN_CACHE_INDEX,
-	PTN_DEVICE_INDEX,
-	PTN_MISC_INDEX,
-	PTN_DATAFOOTER_INDEX,
-	PTN_VBMETA_INDEX,
-	PTN_PRESISTDATA_INDEX,
-	PTN_DATA_INDEX,
-	PTN_FBMISC_INDEX,
-	PTN_NUM
+	PTN_BOOTLOADER_INDEX
 };
-#else
-enum {
-    PTN_MBR_INDEX = 0,
-    PTN_BOOTLOADER_INDEX,
-    PTN_KERNEL_INDEX,
-    PTN_URAMDISK_INDEX,
-    PTN_SYSTEM_INDEX,
-    PTN_RECOVERY_INDEX,
-    PTN_DATA_INDEX,
-    PTN_NUM
-};
-#endif /*CONFIG_EFI_PARTITION*/
 static unsigned int download_bytes_unpadded;
 
 static struct cmd_fastboot_interface interface = {
@@ -862,16 +829,39 @@ static void process_flash_sata(const char *cmdbuf)
 #endif
 
 #if defined(CONFIG_FASTBOOT_STORAGE_MMC)
-static int is_sparse_partition(struct fastboot_ptentry *ptn)
-{
-	 if (ptn && (!strncmp(ptn->name,
-				 FASTBOOT_PARTITION_SYSTEM, strlen(FASTBOOT_PARTITION_SYSTEM))
-				 || !strncmp(ptn->name,
-				 FASTBOOT_PARTITION_DATA, strlen(FASTBOOT_PARTITION_DATA)))) {
+static int is_raw_partition(struct fastboot_ptentry *ptn)
+{
+#ifdef CONFIG_ANDROID_AB_SUPPORT
+	if (ptn && (!strncmp(ptn->name, FASTBOOT_PARTITION_BOOTLOADER,
+			strlen(FASTBOOT_PARTITION_BOOTLOADER)) ||
+			!strncmp(ptn->name, FASTBOOT_PARTITION_GPT,
+			strlen(FASTBOOT_PARTITION_GPT)) ||
+			!strncmp(ptn->name, FASTBOOT_PARTITION_BOOT_A,
+			strlen(FASTBOOT_PARTITION_BOOT_A)) ||
+			!strncmp(ptn->name, FASTBOOT_PARTITION_BOOT_B,
+			strlen(FASTBOOT_PARTITION_BOOT_B)) ||
+#ifdef CONFIG_FASTBOOT_LOCK
+			!strncmp(ptn->name, FASTBOOT_PARTITION_FBMISC,
+			strlen(FASTBOOT_PARTITION_FBMISC)) ||
+#endif
+			!strncmp(ptn->name, FASTBOOT_PARTITION_MISC,
+			strlen(FASTBOOT_PARTITION_MISC)))) {
+#else
+	 if (ptn && (!strncmp(ptn->name, FASTBOOT_PARTITION_BOOTLOADER,
+		strlen(FASTBOOT_PARTITION_BOOTLOADER)) ||
+		!strncmp(ptn->name, FASTBOOT_PARTITION_BOOT,
+		strlen(FASTBOOT_PARTITION_BOOT)) ||
+#ifdef CONFIG_FASTBOOT_LOCK
+		!strncmp(ptn->name, FASTBOOT_PARTITION_FBMISC,
+		strlen(FASTBOOT_PARTITION_FBMISC)) ||
+#endif
+		!strncmp(ptn->name, FASTBOOT_PARTITION_MISC,
+		strlen(FASTBOOT_PARTITION_MISC)))) {
+#endif
 		printf("support sparse flash partition for %s\n", ptn->name);
 		return 1;
 	 } else
-		 return 0;
+		return 0;
 }
 
 static lbaint_t mmc_sparse_write(struct sparse_storage *info,
@@ -964,7 +954,7 @@ static void process_flash_mmc(const char *cmdbuf)
 				sprintf(mmc_dev, "mmc dev %x",
 					fastboot_devinfo.dev_id /*slot no*/);
 
-			if (is_sparse_partition(ptn) &&
+			if (!is_raw_partition(ptn) &&
 				is_sparse_image(interface.transfer_buffer)) {
 				int mmc_no = 0;
 				struct mmc *mmc;
@@ -1154,25 +1144,6 @@ static struct fastboot_ptentry ptable[MAX_PTN];
 static unsigned int pcount;
 struct fastboot_device_info fastboot_devinfo;
 
-/*
-   Get mmc control number from passed string, eg, "mmc1" mean device 1. Only
-   support "mmc0" to "mmc9" currently. It will be treated as device 0 for
-   other string.
-*/
-static int _fastboot_get_mmc_no(char *env_str)
-{
-	int digit = 0;
-	unsigned char a;
-
-	if (env_str && (strlen(env_str) >= 4) &&
-	    !strncmp(env_str, "mmc", 3)) {
-		a = env_str[3];
-		if (a >= '0' && a <= '9')
-			digit = a - '0';
-	}
-
-	return digit;
-}
 static int _fastboot_setup_dev(void)
 {
 	char *fastboot_env;
@@ -1187,7 +1158,7 @@ static int _fastboot_setup_dev(void)
 			fastboot_devinfo.dev_id = 0;
 		} else if (!strncmp(fastboot_env, "mmc", 3)) {
 			fastboot_devinfo.type = DEV_MMC;
-			fastboot_devinfo.dev_id = _fastboot_get_mmc_no(fastboot_env);
+			fastboot_devinfo.dev_id = mmc_get_env_dev();
 		}
 	} else {
 		return 1;
@@ -1245,7 +1216,7 @@ static int _fastboot_parts_load_from_ptable(void)
 
 	struct mmc *mmc;
 	struct blk_desc *dev_desc;
-	struct fastboot_ptentry ptable[PTN_NUM + 1];
+	struct fastboot_ptentry ptable[MAX_PTN];
 
 	/* sata case in env */
 	if (fastboot_devinfo.type == DEV_SATA) {
@@ -1292,20 +1263,12 @@ static int _fastboot_parts_load_from_ptable(void)
 	}
 
 	memset((char *)ptable, 0,
-		    sizeof(struct fastboot_ptentry) * (PTN_NUM + 1));
-#ifdef CONFIG_EFI_PARTITION
+		    sizeof(struct fastboot_ptentry) * (MAX_PTN));
 	/* GPT */
-	strcpy(ptable[PTN_GPT_INDEX].name, "gpt");
+	strcpy(ptable[PTN_GPT_INDEX].name, FASTBOOT_PARTITION_GPT);
 	ptable[PTN_GPT_INDEX].start = ANDROID_GPT_OFFSET / dev_desc->blksz;
 	ptable[PTN_GPT_INDEX].length = ANDROID_GPT_SIZE  / dev_desc->blksz;
 	ptable[PTN_GPT_INDEX].partition_id = user_partition;
-#else
-	/* MBR */
-	strcpy(ptable[PTN_MBR_INDEX].name, "mbr");
-	ptable[PTN_MBR_INDEX].start = ANDROID_MBR_OFFSET / dev_desc->blksz;
-	ptable[PTN_MBR_INDEX].length = ANDROID_MBR_SIZE / dev_desc->blksz;
-	ptable[PTN_MBR_INDEX].partition_id = user_partition;
-#endif
 	/* Bootloader */
 	strcpy(ptable[PTN_BOOTLOADER_INDEX].name, FASTBOOT_PARTITION_BOOTLOADER);
 	ptable[PTN_BOOTLOADER_INDEX].start =
@@ -1314,28 +1277,10 @@ static int _fastboot_parts_load_from_ptable(void)
 				 ANDROID_BOOTLOADER_SIZE / dev_desc->blksz;
 	ptable[PTN_BOOTLOADER_INDEX].partition_id = boot_partition;
 
-#ifndef CONFIG_EFI_PARTITION
-	_fastboot_parts_add_ptable_entry(PTN_KERNEL_INDEX,
-				   ANDROID_BOOT_PARTITION_MMC,
-				   user_partition,
-				   FASTBOOT_PARTITION_BOOT , dev_desc, ptable);
-	_fastboot_parts_add_ptable_entry(PTN_RECOVERY_INDEX,
-				   ANDROID_RECOVERY_PARTITION_MMC,
-				   user_partition,
-				   FASTBOOT_PARTITION_RECOVERY, dev_desc, ptable);
-	_fastboot_parts_add_ptable_entry(PTN_SYSTEM_INDEX,
-				   ANDROID_SYSTEM_PARTITION_MMC,
-				   user_partition,
-				   FASTBOOT_PARTITION_SYSTEM, dev_desc, ptable);
-	_fastboot_parts_add_ptable_entry(PTN_DATA_INDEX,
-				   ANDROID_DATA_PARTITION_MMC,
-				   user_partition,
-				   FASTBOOT_PARTITION_DATA, dev_desc, ptable);
-#else
 	int tbl_idx;
 	int part_idx = 1;
 	int ret;
-	for (tbl_idx = 2; tbl_idx < PTN_NUM; tbl_idx++) {
+	for (tbl_idx = 2; tbl_idx < MAX_PTN; tbl_idx++) {
 		ret = _fastboot_parts_add_ptable_entry(tbl_idx,
 				part_idx++,
 				user_partition,
@@ -1344,8 +1289,7 @@ static int _fastboot_parts_load_from_ptable(void)
 		if (ret)
 			break;
 	}
-#endif /*CONFIG_EFI_PARTITION*/
-	for (i = 0; i <= PTN_NUM; i++)
+	for (i = 0; i <= part_idx; i++)
 		fastboot_flash_add_ptn(&ptable[i]);
 
 	return 0;
@@ -1563,6 +1507,16 @@ struct fastboot_ptentry *fastboot_flash_find_ptn(const char *name)
 	return 0;
 }
 
+int fastboot_flash_find_index(const char *name)
+{
+	struct fastboot_ptentry *ptentry = fastboot_flash_find_ptn(name);
+	if (ptentry == NULL) {
+		printf("cannot get the partion info for %s\n",name);
+		return -1;
+	}
+	return ptentry->partition_index;
+}
+
 struct fastboot_ptentry *fastboot_flash_get_ptn(unsigned int n)
 {
 	if (n < pcount)
@@ -1576,17 +1530,93 @@ unsigned int fastboot_flash_get_ptn_count(void)
 	return pcount;
 }
 
-/*
- * CPU and board-specific fastboot initializations.  Aliased function
- * signals caller to move on
- */
-static void __def_fastboot_setup(void)
+#ifdef CONFIG_FSL_FASTBOOT
+void board_fastboot_setup(void)
 {
-	/*do nothing here*/
+#if defined(CONFIG_FASTBOOT_STORAGE_MMC)
+	static char boot_dev_part[32];
+	u32 dev_no;
+#endif
+	switch (get_boot_device()) {
+#if defined(CONFIG_FASTBOOT_STORAGE_MMC)
+	case SD1_BOOT:
+	case SD2_BOOT:
+	case SD3_BOOT:
+	case SD4_BOOT:
+	case MMC1_BOOT:
+	case MMC2_BOOT:
+	case MMC3_BOOT:
+	case MMC4_BOOT:
+		dev_no = mmc_get_env_dev();
+		sprintf(boot_dev_part,"mmc%d",dev_no);
+		if (!getenv("fastboot_dev"))
+			setenv("fastboot_dev", boot_dev_part);
+		sprintf(boot_dev_part, "boota mmc%d", dev_no);
+		if (!getenv("bootcmd"))
+			setenv("bootcmd", boot_dev_part);
+		break;
+#endif /*CONFIG_FASTBOOT_STORAGE_MMC*/
+#if defined(CONFIG_FASTBOOT_STORAGE_NAND)
+	case NAND_BOOT:
+		if (!getenv("fastboot_dev"))
+			setenv("fastboot_dev", "nand");
+		if (!getenv("fbparts"))
+			setenv("fbparts", ANDROID_FASTBOOT_NAND_PARTS);
+		if (!getenv("bootcmd"))
+			setenv("bootcmd",
+				"nand read ${loadaddr} ${boot_nand_offset} "
+				"${boot_nand_size};boota ${loadaddr}");
+		break;
+#endif /*CONFIG_FASTBOOT_STORAGE_NAND*/
+	default:
+		printf("unsupported boot devices\n");
+		break;
+	}
 }
-void board_fastboot_setup(void) \
-	__attribute__((weak, alias("__def_fastboot_setup")));
 
+#ifdef CONFIG_ANDROID_RECOVERY
+void board_recovery_setup(void)
+{
+#if defined(CONFIG_FASTBOOT_STORAGE_MMC)
+	static char boot_dev_part[32];
+	u32 dev_no;
+#endif
+	int bootdev = get_boot_device();
+	switch (bootdev) {
+#if defined(CONFIG_FASTBOOT_STORAGE_MMC)
+	case SD1_BOOT:
+	case SD2_BOOT:
+	case SD3_BOOT:
+	case SD4_BOOT:
+	case MMC1_BOOT:
+	case MMC2_BOOT:
+	case MMC3_BOOT:
+	case MMC4_BOOT:
+		dev_no = mmc_get_env_dev();
+		sprintf(boot_dev_part,"boota mmc%d recovery",dev_no);
+		if (!getenv("bootcmd_android_recovery"))
+			setenv("bootcmd_android_recovery", boot_dev_part);
+		break;
+#endif /*CONFIG_FASTBOOT_STORAGE_MMC*/
+#if defined(CONFIG_FASTBOOT_STORAGE_NAND)
+	case NAND_BOOT:
+		if (!getenv("bootcmd_android_recovery"))
+			setenv("bootcmd_android_recovery",
+				"nand read ${loadaddr} ${recovery_nand_offset} "
+				"${recovery_nand_size};boota ${loadaddr}");
+		break;
+#endif /*CONFIG_FASTBOOT_STORAGE_NAND*/
+	default:
+		printf("Unsupported bootup device for recovery: dev: %d\n",
+			bootdev);
+		return;
+	}
+
+	printf("setup env for recovery..\n");
+	setenv("bootcmd", "run bootcmd_android_recovery");
+}
+#endif /*CONFIG_ANDROID_RECOVERY*/
+#endif /*CONFIG_FSL_FASTBOOT*/
 
 void fastboot_setup(void)
 {
@@ -1603,10 +1633,6 @@ void fastboot_setup(void)
 	/*get the fastboot dev*/
 	_fastboot_setup_dev();
 
-	/*check if we need to setup recovery*/
-#ifdef CONFIG_ANDROID_RECOVERY
-    check_recovery_mode();
-#endif
 
 	/*load partitions information for the fastboot dev*/
 	_fastboot_load_partitions();
@@ -1614,11 +1640,79 @@ void fastboot_setup(void)
 	parameters_setup();
 }
 
-/* export to lib_arm/board.c */
-void check_fastboot(void)
+/* Write the bcb with fastboot bootloader commands */
+static void enable_fastboot_command(void)
+{
+	char fastboot_command[32] = {0};
+	strncpy(fastboot_command, FASTBOOT_BCB_CMD, 31);
+	bcb_write_command(fastboot_command);
+}
+
+/* Get the Boot mode from BCB cmd or Key pressed */
+static FbBootMode fastboot_get_bootmode(void)
 {
-	if (fastboot_check_and_clean_flag())
-		run_command("fastboot", 0);
+	int ret = 0;
+	int boot_mode = BOOTMODE_NORMAL;
+	char command[32];
+#ifdef CONFIG_ANDROID_RECOVERY
+	if(is_recovery_key_pressing()) {
+		boot_mode = BOOTMODE_RECOVERY_KEY_PRESSED;
+		return boot_mode;
+	}
+#endif
+	ret = bcb_read_command(command);
+	if (ret < 0) {
+		printf("read command failed\n");
+		return boot_mode;
+	}
+	if (!strcmp(command, FASTBOOT_BCB_CMD)) {
+		memset(command, 0, 32);
+		bcb_write_command(command);
+		boot_mode = BOOTMODE_FASTBOOT_BCB_CMD;
+	}
+#ifdef CONFIG_ANDROID_RECOVERY
+	else if (!strcmp(command, RECOVERY_BCB_CMD)) {
+		memset(command, 0, 32);
+		bcb_write_command(command);
+		boot_mode = BOOTMODE_RECOVERY_BCB_CMD;
+	}
+#endif
+	return boot_mode;
+}
+
+/* export to lib_arm/board.c */
+void fastboot_run_bootmode(void)
+{
+	FbBootMode boot_mode = fastboot_get_bootmode();
+	switch(boot_mode){
+	case BOOTMODE_FASTBOOT_BCB_CMD:
+		/* Make the boot into fastboot mode*/
+		puts("Fastboot: Got bootloader commands!\n");
+#ifdef CONFIG_SYSTEM_RAMDISK_SUPPORT
+		is_recovery_mode = false;
+#endif
+		run_command("fastboot 0", 0);
+		break;
+#ifdef CONFIG_ANDROID_RECOVERY
+	case BOOTMODE_RECOVERY_BCB_CMD:
+	case BOOTMODE_RECOVERY_KEY_PRESSED:
+		/* Make the boot into recovery mode */
+		puts("Fastboot: Got Recovery key pressing or recovery commands!\n");
+#ifdef CONFIG_SYSTEM_RAMDISK_SUPPORT
+		is_recovery_mode = true;
+#else
+		board_recovery_setup();
+#endif
+		break;
+#endif
+	default:
+		/* skip special mode boot*/
+#ifdef CONFIG_SYSTEM_RAMDISK_SUPPORT
+		is_recovery_mode = false;
+#endif
+		puts("Fastboot: Normal\n");
+		break;
+	}
 }
 
 #ifdef CONFIG_CMD_BOOTA
@@ -1690,6 +1784,19 @@ int do_boota(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	if (argc > 2)
 		ptn = argv[2];
 
+#ifdef CONFIG_FSL_BOOTCTL
+	else  {
+slot_select:
+		ptn = select_slot();
+		if (ptn == NULL) {
+			printf("no valid slot found, enter to recovery\n");
+			ptn = "recovery";
+		}
+use_given_ptn:
+		printf("use slot %s\n", ptn);
+	}
+#endif
+
 	if (mmcc != -1) {
 #ifdef CONFIG_MMC
 		struct fastboot_ptentry *pte;
@@ -1917,7 +2024,23 @@ int do_boota(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 	return 1;
 
 fail:
-#ifdef CONFIG_FSL_FASTBOOT
+#if defined(CONFIG_FSL_BOOTCTL)
+	if (argc > 2)
+		return -1;
+	if (0 == strcmp(ptn, "recovery")) {
+		printf("boot recovery partition failed\n");
+		return -1;
+	}
+	printf("invalid slot %s\n", ptn);
+	int ret = 0;
+	ret = invalid_curslot();
+	if (ret == 0) {
+		goto slot_select;
+	} else {
+		ptn = "recovery";
+		goto use_given_ptn;
+	}
+#elif defined(CONFIG_FSL_FASTBOOT)
 	return run_command("fastboot", 0);
 #else /*! CONFIG_FSL_FASTBOOT*/
 	return -1;
@@ -2155,7 +2278,7 @@ static int fastboot_tx_write(const char *buffer, unsigned int buffer_size)
 	return 0;
 }
 
-static int fastboot_tx_write_str(const char *buffer)
+int fastboot_tx_write_str(const char *buffer)
 {
 	return fastboot_tx_write(buffer, strlen(buffer));
 }
@@ -2207,6 +2330,13 @@ static void cb_getvar(struct usb_ep *ep, struct usb_request *req)
 		return;
 	}
 
+#ifdef CONFIG_FSL_BOOTCTL
+	if (is_sotvar(cmd)) {
+		get_slotvar(cmd, response, chars_left);
+		fastboot_tx_write_str(response);
+		return;
+	}
+#endif
 	if (!strcmp_l1("version", cmd)) {
 		strncat(response, FASTBOOT_VERSION, chars_left);
 	} else if (!strcmp_l1("bootloader-version", cmd)) {
@@ -2225,6 +2355,8 @@ static void cb_getvar(struct usb_ep *ep, struct usb_request *req)
 			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 {
 		char envstr[32];
 
@@ -2234,7 +2366,6 @@ static void cb_getvar(struct usb_ep *ep, struct usb_request *req)
 			strncat(response, s, chars_left);
 		} else {
 			printf("WARNING: unknown variable: %s\n", cmd);
-			strcpy(response, "FAILVariable not implemented");
 		}
 	}
 	fastboot_tx_write_str(response);
@@ -2503,7 +2634,7 @@ static void cb_reboot_bootloader(struct usb_ep *ep, struct usb_request *req)
 	fastboot_tx_write_str("OKAY");
 
 	udelay(1000000);
-	fastboot_enable_flag();
+	enable_fastboot_command();
 	do_reset(NULL, 0, 0, NULL);
 }
 #endif
@@ -2549,6 +2680,12 @@ static const struct cmd_dispatch_info cmd_dispatch_info[] = {
 		.cmd = "oem",
 		.cb = cb_oem,
 	},
+#ifdef CONFIG_FSL_BOOTCTL
+	{
+		.cmd = "set_active",
+		.cb = cb_set_active,
+	},
+#endif
 };
 
 static void rx_handler_command(struct usb_ep *ep, struct usb_request *req)