MA-9478-2 [iot] Support keyslot package function

RPMB keyblob stored in the last block of boot1
partition of MMC1. It will be load into
CAAM SEC_RAM and format into "keyslot" package.

This is enabled on pico7d and iopb6ul.

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

5 files changed, 196 insertions, 0 deletions

diff --git a/common/board_r.c b/common/board_r.c
index 6e0397132f..8001f3669c 100644
--- a/common/board_r.c
+++ b/common/board_r.c
@@ -737,6 +737,13 @@ static int initr_check_fastboot(void)
 #endif
 
 #ifdef CONFIG_IMX_TRUSTY_OS
+#ifdef TRUSTY_KEYSLOT_PACKAGE
+static int initr_avbkey(void)
+{
+	return init_avbkey();
+}
+#endif
+
 static int initr_tee_setup(void)
 {
 	tee_setup();
@@ -974,6 +981,9 @@ static init_fnc_t init_sequence_r[] = {
 	prom_init,
 #endif
 #ifdef CONFIG_IMX_TRUSTY_OS
+#ifdef TRUSTY_KEYSLOT_PACKAGE
+	initr_avbkey,
+#endif
 	initr_tee_setup,
 #endif
 #ifdef CONFIG_FSL_FASTBOOT
diff --git a/include/configs/mx6ul_nxpu_iopb_android_things.h b/include/configs/mx6ul_nxpu_iopb_android_things.h
index 494a96a759..5664d26c69 100644
--- a/include/configs/mx6ul_nxpu_iopb_android_things.h
+++ b/include/configs/mx6ul_nxpu_iopb_android_things.h
@@ -15,6 +15,7 @@
 
 #ifdef CONFIG_IMX_TRUSTY_OS
 #define NON_SECURE_FASTBOOT
+#define TRUSTY_KEYSLOT_PACKAGE
 #endif
 
 /* For NAND we don't support lock/unlock */
diff --git a/include/configs/pico-imx7dandroidthings.h b/include/configs/pico-imx7dandroidthings.h
index f64748b0bc..1f43697723 100644
--- a/include/configs/pico-imx7dandroidthings.h
+++ b/include/configs/pico-imx7dandroidthings.h
@@ -14,6 +14,7 @@
 
 #ifdef CONFIG_IMX_TRUSTY_OS
 #define NON_SECURE_FASTBOOT
+#define TRUSTY_KEYSLOT_PACKAGE
 #endif
 #include "mx_android_common.h"
 
diff --git a/lib/avb/fsl/fsl_avbkey.c b/lib/avb/fsl/fsl_avbkey.c
index 9f2f11a82c..de701dd211 100644
--- a/lib/avb/fsl/fsl_avbkey.c
+++ b/lib/avb/fsl/fsl_avbkey.c
@@ -24,6 +24,7 @@
 #define RPMBKEY_LENGTH 32
 #define RPMBKEY_FUSE_LEN ((RPMBKEY_LENGTH) + (CAAM_PAD))
 #define RPMBKEY_FUSE_LENW (RPMBKEY_FUSE_LEN / 4)
+#define RPMBKEY_BLOB_LEN ((RPMBKEY_LENGTH) + (CAAM_PAD))
 
 static int mmc_dev_no = -1;
 
@@ -109,6 +110,177 @@ static int fsl_fuse_write(const uint32_t *buffer, uint32_t length, uint32_t offs
 		);
 }
 
+#ifdef TRUSTY_KEYSLOT_PACKAGE
+static void fill_secure_keyslot_package(struct keyslot_package *kp) {
+
+	memcpy((void*)CAAM_ARB_BASE_ADDR, kp, sizeof(struct keyslot_package));
+
+	/* invalidate the cache to make sure no critical information left in it */
+	memset(kp, 0, sizeof(struct keyslot_package));
+	invalidate_dcache_range(((uint32_t)kp) & 0xffffffc0,
+		(((((uint32_t)kp) + sizeof(struct keyslot_package)) & 0xffffff00) + 0x100));
+}
+
+
+static int read_keyslot_package(struct keyslot_package* kp) {
+	char original_part;
+	int blksz;
+
+	int ret = 0;
+	/* load tee from boot1 of eMMC. */
+	int mmcc = mmc_get_env_dev();
+	struct blk_desc *dev_desc = NULL;
+
+	struct mmc *mmc;
+	mmc = find_mmc_device(mmcc);
+	if (!mmc) {
+		printf("boota: cannot find '%d' mmc device\n", mmcc);
+		return -1;
+	}
+	original_part = mmc->block_dev.hwpart;
+
+	dev_desc = blk_get_dev("mmc", mmcc);
+	if (NULL == dev_desc) {
+		printf("** Block device MMC %d not supported\n", mmcc);
+		goto fail;
+	}
+
+	blksz = dev_desc->blksz;
+	unsigned char* fill = (unsigned char *)memalign(ALIGN_BYTES, blksz);
+
+	/* below was i.MX mmc operation code */
+	if (mmc_init(mmc)) {
+		printf("mmc%d init failed\n", mmcc);
+		goto fail;
+	}
+
+	mmc_switch_part(mmc, TEE_HWPARTITION_ID);
+	if (mmc->block_dev.block_read(dev_desc, TRUSTY_OS_MMC_BLKS,
+		    1, fill) != 1) {
+		printf("Failed to read rpmbkeyblob.");
+	}
+
+	memcpy(kp, fill, sizeof(struct keyslot_package));
+
+	ret = 0;
+
+fail:
+	/* Return to original partition */
+	if (mmc->block_dev.hwpart != original_part) {
+		if (mmc_switch_part(mmc, original_part) != 0)
+			return -1;
+		mmc->block_dev.hwpart = original_part;
+	}
+	return ret;
+}
+
+static int gen_rpmb_key(struct keyslot_package *kp) {
+	char original_part;
+	uint8_t plain_key[RPMBKEY_LENGTH];
+	int blksz;
+
+	kp->rpmb_keyblob_len = RPMBKEY_LEN;
+	strcpy(kp->magic, KEYPACK_MAGIC);
+
+	int ret = 0;
+	/* load tee from boot1 of eMMC. */
+	int mmcc = mmc_get_env_dev();
+	struct blk_desc *dev_desc = NULL;
+
+	struct mmc *mmc;
+	mmc = find_mmc_device(mmcc);
+	if (!mmc) {
+		printf("boota: cannot find '%d' mmc device\n", mmcc);
+		return -1;
+	}
+	original_part = mmc->block_dev.hwpart;
+
+	dev_desc = blk_get_dev("mmc", mmcc);
+	if (NULL == dev_desc) {
+		printf("** Block device MMC %d not supported\n", mmcc);
+		goto fail;
+	}
+
+	blksz = dev_desc->blksz;
+	unsigned char* fill = (unsigned char *)memalign(ALIGN_BYTES, blksz);
+
+	/* below was i.MX mmc operation code */
+	if (mmc_init(mmc)) {
+		printf("mmc%d init failed\n", mmcc);
+		goto fail;
+	}
+
+	/* Switch to the RPMB partition */
+
+	/* use caam hwrng to generate */
+	caam_open();
+
+#ifdef TRUSTY_RPMB_RANDOM_KEY
+	/* 
+	 * Since boot1 is a bit easy to be erase during development
+	 * so that before production stage use full 0 rpmb key
+	 */
+	if (caam_hwrng(plain_key, RPMBKEY_LENGTH)) {
+		ERR("ERROR - caam rng\n");
+		ret = -1;
+		goto fail;
+	}
+#else
+	memset(plain_key, 0, RPMBKEY_LENGTH);
+#endif
+
+	/* generate keyblob and program to fuse */
+	if (caam_gen_blob((uint32_t)plain_key, (uint32_t)(kp->rpmb_keyblob), RPMBKEY_LENGTH)) {
+		ERR("gen rpmb key blb error\n");
+		ret = -1;
+		goto fail;
+	}
+	memcpy(fill, kp, sizeof(struct keyslot_package));
+
+	mmc_switch_part(mmc, TEE_HWPARTITION_ID);
+	if (mmc->block_dev.block_write(dev_desc, TRUSTY_OS_MMC_BLKS,
+		    1, (void *)fill) != 1) {
+		printf("Failed to write rpmbkeyblob.");
+	}
+
+	/* program key to mmc */
+	if (mmc->block_dev.hwpart != MMC_PART_RPMB) {
+		if (mmc_switch_part(mmc, MMC_PART_RPMB) != 0)
+			goto fail;
+		mmc->block_dev.hwpart = MMC_PART_RPMB;
+	}
+	if (mmc_rpmb_set_key(mmc, plain_key)) {
+		ERR("Key already programmed ?\n");
+		ret = -1;
+		goto fail;
+	}
+
+	ret = 0;
+
+fail:
+	/* Return to original partition */
+	if (mmc->block_dev.hwpart != original_part) {
+		if (mmc_switch_part(mmc, original_part) != 0)
+			return -1;
+		mmc->block_dev.hwpart = original_part;
+	}
+	return ret;
+
+}
+
+int init_avbkey(void) {
+	struct keyslot_package kp;
+	read_keyslot_package(&kp);
+	if (strcmp(kp.magic, KEYPACK_MAGIC)) {
+		printf("keyslot package magic error. Will generate new one\n");
+		gen_rpmb_key(&kp);
+	}
+	fill_secure_keyslot_package(&kp);
+	return 0;
+}
+
+#endif
+
 static int rpmb_key(struct mmc *mmc) {
 
 	char original_part;
diff --git a/lib/avb/fsl/fsl_avbkey.h b/lib/avb/fsl/fsl_avbkey.h
index b671f03335..54b37753e5 100644
--- a/lib/avb/fsl/fsl_avbkey.h
+++ b/lib/avb/fsl/fsl_avbkey.h
@@ -41,4 +41,16 @@ struct kblb_hdr {
 };
 typedef struct kblb_hdr kblb_hdr_t;
 
+#ifdef TRUSTY_KEYSLOT_PACKAGE
+#define RPMBKEY_LEN (32 + CAAM_PAD)
+#define KEYPACK_MAGIC "!KS"
+
+struct keyslot_package
+{
+    char magic[4];
+    unsigned int rpmb_keyblob_len;
+    unsigned char rpmb_keyblob[RPMBKEY_LEN];
+};
+#endif
+
 #endif