[iot] Check Trusty OS available before load it

When SECURE_BOOT enabled, use HAB verify
Trusty OS image or check its IVT available.

If not available, bootloader consider the TOS
lost unexpected and wipe all data on disk.
Then enter limited fastboot mode. In this situation
only bootloader and tos is able to flash, gpt won't
be available for external.

Change-Id: I04f037f5bd5a51f53174b5b99b2c3053182a8fcf
Signed-off-by: Haoran.Wang <elven.wang@nxp.com>

cmd/bootm.c

@@ -174,12 +174,14 @@ int do_bootm(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 #endif
 #ifdef CONFIG_ANDROID_BOOT_IMAGE
 	case IMAGE_FORMAT_ANDROID:
-		/* Do this authentication in boota command */
+	default:
+		/* Android use AVB verify. Also here we cannot get IMAGE_FORMAT_ANDROID */
 		break;
-#endif
+#else
 	default:
 		printf("Not valid image format for Authentication, Please check\n");
 		return 1;
+#endif /* CONFIG_ANDROID_BOOT_IMAGE */
 	}
 #endif
 #endif

drivers/usb/gadget/f_fastboot.c

@@ -37,6 +37,7 @@
 extern int armv7_init_nonsec(void);
 extern void trusty_os_init(void);
 #include <trusty/libtipc.h>
+extern bool tos_flashed;
 #endif
 
 #ifdef CONFIG_FASTBOOT_FLASH_NAND_DEV
@@ -1695,6 +1696,13 @@ void tee_setup(void)
 	}
 	mmc_switch_part(mmc, FASTBOOT_MMC_USER_PARTITION_ID);
 
+	tos_flashed = false;
+	if(!valid_tos()) {
+		printf("TOS not flashed! Will enter TOS recovery mode. Everything will be wiped!\n");
+		fastboot_wipe_all();
+		run_command("fastboot 0", 0);
+		goto fail;
+	}
 #ifdef NON_SECURE_FASTBOOT
 	armv7_init_nonsec();
 	trusty_os_init();
@@ -3298,6 +3306,11 @@ static int partition_table_valid(void)
 {
 	int status, mmc_no;
 	struct blk_desc *dev_desc;
+#ifdef CONFIG_IMX_TRUSTY_OS
+	//Prevent other partition accessing when no TOS flashed.
+	if (!tos_flashed)
+		return 0;
+#endif
 	disk_partition_t info;
 	mmc_no = fastboot_devinfo.dev_id;
 	dev_desc = blk_get_dev("mmc", mmc_no);

drivers/usb/gadget/fastboot_lock_unlock.c

@@ -39,6 +39,7 @@
 #include <fsl_fastboot.h>
 #ifdef CONFIG_IMX_TRUSTY_OS
 #include <trusty/libtipc.h>
+#include <asm/imx-common/hab.h>
 #endif
 
 #ifdef FASTBOOT_ENCRYPT_LOCK
@@ -53,6 +54,68 @@
 
 int fastboot_flash_find_index(const char *name);
 
+#ifdef CONFIG_IMX_TRUSTY_OS
+#define HAB_TAG_IVT       0xD1
+#define IVT_HDR_LEN       0x20
+#define HAB_MAJ_VER       0x40
+#define HAB_MAJ_MASK      0xF0
+
+bool tos_flashed;
+
+static bool tos_ivt_check(ulong start_addr, int ivt_offset) {
+	const struct hab_ivt *ivt_initial = NULL;
+	const uint8_t *start = (const uint8_t *)start_addr;
+
+	if (start_addr & 0x3) {
+		puts("Error: tos's start address is not 4 byte aligned\n");
+		return false;
+	}
+
+	ivt_initial = (const struct hab_ivt *)(start + ivt_offset);
+
+	const struct hab_hdr *ivt_hdr = &ivt_initial->hdr;
+
+	if ((ivt_hdr->tag == HAB_TAG_IVT && \
+		((ivt_hdr->len[0] << 8) + ivt_hdr->len[1]) == IVT_HDR_LEN && \
+		(ivt_hdr->par & HAB_MAJ_MASK) == HAB_MAJ_VER) && \
+		(ivt_initial->entry != 0x0) && \
+		(ivt_initial->reserved1 == 0x0) && \
+		(ivt_initial->self == (uint32_t)ivt_initial) && \
+		(ivt_initial->csf != 0x0) && \
+		(ivt_initial->reserved2 == 0x0)) {
+		if (ivt_initial->dcd != 0x0)
+			return false;
+		else
+			return true;
+	}
+
+	return false;
+}
+
+bool valid_tos() {
+	/*
+	 * If enabled SECURE_BOOT then use HAB to verify tos.
+	 * Or check the IVT only.
+	 */
+	bool valid = false;
+#ifdef CONFIG_SECURE_BOOT
+	if (is_hab_enabled()) {
+		valid = authenticate_image(TRUSTY_OS_ENTRY, TRUSTY_OS_PADDED_SZ);
+	} else
+#endif
+	valid = tos_ivt_check(TRUSTY_OS_ENTRY, TRUSTY_OS_PADDED_SZ);
+
+	if (valid) {
+		tos_flashed = true;
+		return true;
+	} else {
+		tos_flashed = false;
+		return false;
+	}
+}
+
+#endif
+
 #if !defined(FASTBOOT_ENCRYPT_LOCK) || defined(NON_SECURE_FASTBOOT)
 
 /*
@@ -222,6 +285,13 @@ static FbLockState g_lockstat = FASTBOOT_UNLOCK;
 FbLockState fastboot_get_lock_stat(void) {
 	uint8_t l_status;
 	int ret;
+	/*
+	 * If Trusty OS not flashed, then must return
+	 * unlock status to make device been able
+	 * to flash Trusty OS binary.
+	 */
+	if (!tos_flashed)
+		return FASTBOOT_UNLOCK;
 	ret = trusty_read_lock_state(&l_status);
 	if (ret < 0)
 		return g_lockstat;
@@ -232,6 +302,12 @@ FbLockState fastboot_get_lock_stat(void) {
 
 int fastboot_set_lock_stat(FbLockState lock) {
 	int ret;
+	/*
+	 * If Trusty OS not flashed, we must prevent set lock
+	 * status. Due the Trusty IPC won't work here.
+	 */
+	if (!tos_flashed)
+		return 0;
 	ret = trusty_write_lock_state(lock);
 	if (ret < 0) {
 		printf("cannot set lock status due Trusty return %d\n", ret);
@@ -493,3 +569,27 @@ int fastboot_wipe_data_partition(void)
 
 	return 0;
 }
+
+void fastboot_wipe_all(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_GPT);
+	if (mmc_id < 0) {
+		printf("%s: error in get mmc part\n", __FUNCTION__);
+		return;
+	}
+	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 user partition\n");
+		return;
+	}
+	status = blk_derase(fs_dev_desc, fs_partition.start , fs_partition.size );
+	if (status != fs_partition.size ) {
+		printf("erase not complete\n");
+		return;
+	}
+	printf("fastboot wiped all.\n");
+}

drivers/usb/gadget/fastboot_lock_unlock.h

@@ -63,9 +63,12 @@ FbLockState fastboot_get_lock_stat(void);
 int fastboot_set_lock_stat(FbLockState lock);
 
 int fastboot_wipe_data_partition(void);
+void fastboot_wipe_all(void);
 
 FbLockEnableResult fastboot_lock_enable(void);
 void set_fastboot_lock_disable(void);
 
 int display_lock(FbLockState lock, int verify);
+
+bool valid_tos(void);
 #endif

include/configs/pico-imx7dandroidthings.h

@@ -12,6 +12,7 @@
 #define TRUSTY_OS_RAM_SIZE 0x2000000
 #define TEE_HWPARTITION_ID 2
 #define TRUSTY_OS_MMC_BLKS 0xFFF
+#define TRUSTY_OS_PADDED_SZ 0x180000
 
 #ifdef CONFIG_AVB_ATX
 #define PERMANENT_ATTRIBUTE_HASH_OFFSET 0