HSD #22010730370: rsu: changes for Configuration Pointer Block (CPB) enhancements

Changes are including:
1). integrity check
        a). check CPB header
        b). check CPB data fields
        c). check both CPBs if both are same
2) new commands and RSU core extension to support
        a). rsu create_empty_cpb - create the empty CPB from the scratch
        b). rsu restore_cpb <address> - restore a saved CPB from address
        c). rsu save_cpb <address> - save CPB to address

If one of CPBs is corrupted, update the corrupted CPB with the good one.

If both CPBs are corrupted, all CPB related operations are blocked until
user successfully run "rsu create_empty_cpb <address>" or
"rsu restore_cpb <address>".

Signed-off-by: Richard Gong <richard.gong@intel.com>
---
v5: add HSD to commit title, update commit messages, replace
    cpb_backup_offset with cpb_reserved
v4: update global variables (cpb and cpb_slots) after
    restore_cpb_from_address() and empty_cpb(), add error check at
    rsu_save_cpb()
v3: return with the standard error codes, use memcmp
v2: add check for magic number to prevent user from accidentally restoring
    SPB

5 files changed, 462 insertions, 22 deletions

diff --git a/arch/arm/mach-socfpga/include/mach/rsu.h b/arch/arm/mach-socfpga/include/mach/rsu.h
index 942dd3e7d4..bff5dee2ad 100644
--- a/arch/arm/mach-socfpga/include/mach/rsu.h
+++ b/arch/arm/mach-socfpga/include/mach/rsu.h
@@ -25,6 +25,7 @@
 #define ELOWLEVEL	12
 #define EWRPROT		13
 #define EARGS		14
+#define ECORRUPTED_CPB	15
 
 /* RSU Version Bitmasks */
 #define RSU_VERSION_CRT_IDX_MASK	GENMASK(31, 28)
@@ -354,4 +355,32 @@ int rsu_max_retry(u8 *value);
  */
 int rsu_dcmf_status(u16 *status);
 
+/**
+ * rsu_create_empty_cpb() - create a cpb with header field only
+ * This function is used to create a empty configuration pointer block
+ * (CPB) with the header field only.
+ *
+ * Returns: 0 on success, or error code
+ */
+int rsu_create_empty_cpb(void);
+
+/**
+ * rsu_restore_cpb() - restore the cpb from an address
+ * @address: the address which cpb is restored from.
+ *
+ * This function is used to restore a saved CPB from an address.
+ *
+ * Returns: 0 on success, or error code
+ */
+int rsu_restore_cpb(u64 address);
+
+/**
+ * rsu_save_cpb() - save cpb to the address
+ * @address: the address which cpb is saved to.
+ *
+ * This function is used to save CPB to an address.
+ *
+ * Returns: 0 on success, or error code
+ */
+int rsu_save_cpb(u64 address);
 #endif
diff --git a/arch/arm/mach-socfpga/include/mach/rsu_ll.h b/arch/arm/mach-socfpga/include/mach/rsu_ll.h
index 517eb6953d..365a3e229f 100644
--- a/arch/arm/mach-socfpga/include/mach/rsu_ll.h
+++ b/arch/arm/mach-socfpga/include/mach/rsu_ll.h
@@ -65,6 +65,13 @@ struct rsu_ll_intf {
 		int (*dcmf_status)(u16 *status);
 		int (*max_retry)(u8 *value);
 	} fw_ops;
+
+	struct {
+		int (*empty)(void);
+		int (*restore)(u64 address);
+		int (*save)(u64 address);
+		int (*corrupted)(void);
+	} cpb_ops;
 };
 
 /**
diff --git a/arch/arm/mach-socfpga/rsu.c b/arch/arm/mach-socfpga/rsu.c
index b1f1998aa4..6315c154db 100644
--- a/arch/arm/mach-socfpga/rsu.c
+++ b/arch/arm/mach-socfpga/rsu.c
@@ -55,6 +55,16 @@ void rsu_exit(void)
 }
 
 /**
+ * rsu_cpb_corrupted_info() - output warning info to user
+ */
+void rsu_cpb_corrupted_info(void)
+{
+	rsu_log(RSU_ERR, "corrupted CPB --");
+	rsu_log(RSU_ERR, "run rsu create_empty_cpb");
+	rsu_log(RSU_ERR, "or rsu restore_cpb <address> first\n");
+};
+
+/**
  * rsu_slot_count() - get the number of slots defined
  *
  * Returns: the number of defined slots
@@ -126,6 +136,11 @@ int rsu_slot_get_info(int slot, struct rsu_slot_info *info)
 	if (!info)
 		return -EARGS;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -187,6 +202,11 @@ int rsu_slot_priority(int slot)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -215,6 +235,11 @@ int rsu_slot_erase(int slot)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -261,6 +286,11 @@ int rsu_slot_program_buf(int slot, void *buf, int size)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (rsu_cb_buf_init(buf, size)) {
 		rsu_log(RSU_ERR, "Bad buf/size arguments\n");
 		return -EARGS;
@@ -349,6 +379,11 @@ int rsu_slot_verify_buf(int slot, void *buf, int size)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -424,6 +459,11 @@ int rsu_slot_enable(int slot)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -458,6 +498,11 @@ int rsu_slot_disable(int slot)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -490,6 +535,16 @@ int rsu_slot_load(int slot)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->spt_ops.corrupted()) {
+		rsu_spt_corrupted_info();
+		return -ECORRUPTED_SPT;
+	}
+
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "invalid slot number\n");
 		return -ESLOTNUM;
@@ -592,6 +647,11 @@ int rsu_slot_delete(int slot)
 	if (!ll_intf)
 		return -EINTF;
 
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
 	if (slot < 0 || slot >= rsu_slot_count()) {
 		rsu_log(RSU_ERR, "Invalid slot number\n");
 		return -ESLOTNUM;
@@ -881,3 +941,47 @@ int rsu_dcmf_status(u16 *status)
 
 	return copy_dcmf_status_to_smc(status);
 }
+
+/**
+ * rsu_create_empty_cpb() - create a CPB with header field only
+ *
+ * This function is used to create a empty configuration pointer block
+ * (CPB) with the header field only.
+ *
+ * Returns: 0 on success, or error code
+ */
+int rsu_create_empty_cpb(void)
+{
+	return ll_intf->cpb_ops.empty();
+}
+
+/**
+ * rsu_restore_cpb() - restore the cpb from an address
+ * @address: the address which cpb will be restored from.
+ *
+ * This function is used to restore a saved CPB from an address.
+ *
+ * Returns: 0 on success, or error code
+ */
+int rsu_restore_cpb(u64 address)
+{
+	return ll_intf->cpb_ops.restore(address);
+}
+
+/**
+ * rsu_save_cpb() - save cpb to the address
+ * @address: the address which cpb will be saved to.
+ *
+ * This function is used to save CPB to an address.
+ *
+ * Returns: 0 on success, or error code
+ */
+int rsu_save_cpb(u64 address)
+{
+	if (ll_intf->cpb_ops.corrupted()) {
+		rsu_cpb_corrupted_info();
+		return -ECORRUPTED_CPB;
+	}
+
+	return ll_intf->cpb_ops.save(address);
+}
diff --git a/arch/arm/mach-socfpga/rsu_ll_qspi.c b/arch/arm/mach-socfpga/rsu_ll_qspi.c
index 6ee68b63aa..6934722a15 100644
--- a/arch/arm/mach-socfpga/rsu_ll_qspi.c
+++ b/arch/arm/mach-socfpga/rsu_ll_qspi.c
@@ -11,6 +11,8 @@
 #include <asm/arch/rsu_misc.h>
 #include <spi.h>
 #include <spi_flash.h>
+#include <linux/sizes.h>
+#include <u-boot/zlib.h>
 
 #define SPT_MAGIC_NUMBER	0x57713427
 #define SPT_FLAG_RESERVED	1
@@ -44,6 +46,10 @@
 #define SPT_MAX_PARTITIONS 	127
 #define MIN_QSPI_ERASE_SIZE	4096
 
+#define CPB_SIZE		SZ_4K
+#define CPB_IMAGE_PTR_OFFSET	24
+#define CPB_IMAGE_PTR_NSLOTS	508
+
 /**
  * struct sub_partition_table_partition - SPT partition structure
  * @name: sub-partition name
@@ -79,7 +85,7 @@ struct sub_partition_table {
  * @header.magic_number: CMF pointer block magic number
  * @header.header_size: size of CMF pointer block header
  * @header.cpb_size: size of CMF pointer block
- * @header.cpb_backup_offset: offset of CMF pointer block
+ * @header.cpb_reserved: reserved
  * @header.image_ptr_offset: offset of image pointers
  * @header.image_ptr_slots: number of image pointer slots
  * @data: image pointer slot array
@@ -89,7 +95,7 @@ union cmf_pointer_block {
 		u32 magic_number;
 		u32 header_size;
 		u32 cpb_size;
-		u32 cpb_backup_offset;
+		u32 cpb_reserved;
 		u32 image_ptr_offset;
 		u32 image_ptr_slots;
 	} header;
@@ -103,6 +109,10 @@ struct spi_flash *flash;
 static u32 spt0_offset;
 static u32 spt1_offset;
 
+static int cpb0_part = -1;
+static int cpb1_part = -1;
+static bool cpb_corrupted;
+
 /**
  * get_part_offset() - get a selected partition offset
  * @part_num: the selected partition number
@@ -446,7 +456,7 @@ static int load_spt(void)
 /**
  * check_cpb() - check if CPB is valid
  *
- * Return: 0 for the valid CPB, or -1 for not
+ * Return: 0 for the valid CPB, or -ve on error
  */
 static int check_cpb(void)
 {
@@ -459,22 +469,29 @@ static int check_cpb(void)
 	}
 
 	for (x = 0; x < cpb.header.image_ptr_slots; x++) {
-		if (cpb_slots[x] != ERASED_ENTRY &&
-		    cpb_slots[x] != SPENT_ENTRY) {
-			for (y = 0; y < spt.partitions; y++) {
-				if (cpb_slots[x] ==
-				    spt.partition[y].offset) {
-					rsu_log(RSU_DEBUG,
-						"cpb_slots[%i] = %s\n",
-						x, spt.partition[y].name);
-					break;
-				}
+		if (cpb_slots[x] == ERASED_ENTRY ||
+		    cpb_slots[x] == SPENT_ENTRY)
+			continue;
+
+		for (y = 0; y < spt.partitions; y++) {
+			if (cpb_slots[x] == spt.partition[y].offset) {
+				rsu_log(RSU_DEBUG, "cpb_slots[%i] = %s\n",
+					x, spt.partition[y].name);
+				break;
 			}
+		}
+
+		if (y >= spt.partitions) {
+			rsu_log(RSU_ERR, "CPB is not included in SPT\n");
+			rsu_log(RSU_DEBUG, "cpb_slots[%i] = %016llX ???",
+				x, cpb_slots[x]);
+			return -EINVAL;
+		}
 
-			if (y >= spt.partitions)
-				rsu_log(RSU_DEBUG,
-					"cpb_slots[%i] = %016llX ???",
-					x, cpb_slots[x]);
+		if (spt.partition[y].flags & SPT_FLAG_RESERVED) {
+			rsu_log(RSU_ERR, "CPB is included in SPT ");
+			rsu_log(RSU_ERR, "but it is reserved\n");
+			return -EINVAL;
 		}
 	}
 
@@ -482,6 +499,100 @@ static int check_cpb(void)
 }
 
 /**
+ * check_both_cpb() - check if both CPBs are same
+ *
+ * Return: 0 for the identical CPBs, or -ve on error
+ */
+static int check_both_cpb(void)
+{
+	int ret;
+	char *cpb0_data;
+	char *cpb1_data;
+
+	cpb0_data = (char *)malloc(CPB_SIZE);
+	if (!cpb0_data) {
+		rsu_log(RSU_ERR, "failed to allocate cpb0_data\n");
+		return -ENOMEM;
+	}
+
+	cpb1_data = (char *)malloc(CPB_SIZE);
+	if (!cpb1_data) {
+		rsu_log(RSU_ERR, "failed to allocate cpb1_data\n");
+		free(cpb0_data);
+		return -ENOMEM;
+	}
+
+	ret = read_part(cpb0_part, 0, cpb0_data, CPB_SIZE);
+	if (ret) {
+		rsu_log(RSU_ERR, "failed to read cpb0_data\n");
+		goto ops_error;
+	}
+
+	ret = read_part(cpb1_part, 0, cpb1_data, CPB_SIZE);
+	if (ret) {
+		rsu_log(RSU_ERR, "failed to read cpb1_data\n");
+		goto ops_error;
+	}
+
+	ret = memcmp(cpb0_data, cpb1_data, CPB_SIZE);
+
+ops_error:
+	free(cpb1_data);
+	free(cpb0_data);
+	return ret;
+}
+
+/**
+ * save_cpb_to_address() - save cpb to the address
+ * @address: the address which cpb is saved to
+ *
+ * Return: 0 for successful operation, or -ve on error
+ */
+static int save_cpb_to_address(u64 address)
+{
+	int ret;
+	char *cpb_data_dst = (char *)address;
+	char *cpb_data_src;
+	u32 calc_crc;
+
+	if (!cpb_data_dst) {
+		rsu_log(RSU_ERR, "failed due to invalid address");
+		return -EINVAL;
+	}
+
+	cpb_data_src = (char *)malloc(CPB_SIZE);
+	if (!cpb_data_src) {
+		rsu_log(RSU_ERR, "failed to allocate cpb_data_src\n");
+		return -ENOMEM;
+	}
+
+	ret = read_part(cpb0_part, 0, cpb_data_src, CPB_SIZE);
+	if (ret) {
+		rsu_log(RSU_ERR, "failed to read CPB data\n");
+		free(cpb_data_src);
+		return ret;
+	}
+
+	calc_crc = crc32(0, (void *)cpb_data_src, CPB_SIZE);
+	rsu_log(RSU_DEBUG, "%s - calc_crc is 0x%x\n", __func__, calc_crc);
+	memcpy(cpb_data_dst, cpb_data_src, CPB_SIZE);
+	memcpy(cpb_data_dst + CPB_SIZE, &calc_crc, sizeof(calc_crc));
+
+	free(cpb_data_src);
+	return ret;
+}
+
+/**
+ * corrupted_cpb() - check if cpb is corrupted
+ *
+ * Return: 1 for the corrupted cpb , or 0 for not
+ */
+static int corrupted_cpb(void)
+{
+	return cpb_corrupted;
+}
+
+/**
  * load_cpb() - retrieve CPB from flash
  *
  * Return: 0 on success, or -1 for error
@@ -489,9 +600,7 @@ static int check_cpb(void)
 static int load_cpb(void)
 {
 	int x;
-	int cpb0_part = -1;
 	int cpb0_good = 0;
-	int cpb1_part = -1;
 	int cpb1_good = 0;
 
 	for (x = 0; x < spt.partitions; x++) {
@@ -532,6 +641,11 @@ static int load_cpb(void)
 	}
 
 	if (cpb0_good && cpb1_good) {
+		if (check_both_cpb()) {
+			rsu_log(RSU_ERR, "unmatched CPB0/1 data");
+			cpb_corrupted = true;
+			return -EINVAL;
+		}
 		rsu_log(RSU_INFO, "CPBs are GOOD!!!\n");
 		cpb_slots = (u64 *)
 			     &cpb.data[cpb.header.image_ptr_offset];
@@ -592,6 +706,7 @@ static int load_cpb(void)
 		return 0;
 	}
 
+	cpb_corrupted = true;
 	rsu_log(RSU_ERR, "No valid CPB0 or CPB1 found\n");
 	return -1;
 }
@@ -679,6 +794,106 @@ static int writeback_cpb(void)
 }
 
 /**
+ * empty_cpb() - create a cpb with header field only
+ *
+ * Return: 0 for successful operation, or -ve on error
+ */
+static int empty_cpb(void)
+{
+	int ret;
+	struct cpb_header {
+		u32 magic_number;
+		u32 header_size;
+		u32 cpb_size;
+		u32 cpb_reserved;
+		u32 image_ptr_offset;
+		u32 image_ptr_slots;
+	} *c_header;
+
+	c_header = (struct cpb_header *)malloc(sizeof(struct cpb_header));
+	if (!c_header) {
+		rsu_log(RSU_ERR, "failed to allocate cpb_header\n");
+		return -ENOMEM;
+	}
+
+	c_header->magic_number = CPB_MAGIC_NUMBER;
+	c_header->header_size = CPB_HEADER_SIZE;
+	c_header->cpb_size = CPB_SIZE;
+	c_header->cpb_reserved = 0;
+	c_header->image_ptr_offset = CPB_IMAGE_PTR_OFFSET;
+	c_header->image_ptr_slots = CPB_IMAGE_PTR_NSLOTS;
+
+	memset(&cpb, -1, CPB_SIZE);
+	memcpy(&cpb, c_header, sizeof(c_header));
+
+	ret = writeback_cpb();
+	if (ret) {
+		rsu_log(RSU_ERR, "failed to write back cpb\n");
+		goto ops_error;
+	}
+
+	cpb_slots = (u64 *)&cpb.data[cpb.header.image_ptr_offset];
+	cpb_corrupted = false;
+
+ops_error:
+	free(c_header);
+	return ret;
+}
+
+/**
+ * restore_cpb_from_address() - restore the cpb from an address
+ * @address: the address which cpb is restored from
+ *
+ * Return: 0 for successful operation, or -ve on error
+ */
+static int restore_cpb_from_address(u64 address)
+{
+	int ret;
+	u32 calc_crc;
+	u32 crc_from_saved;
+	u32 magic_number;
+	char *cpb_data = (char *)address;
+
+	if (!cpb_data) {
+		rsu_log(RSU_ERR, "failed due to invalid address\n");
+		return -EINVAL;
+	}
+
+	calc_crc = crc32(0, (void *)cpb_data, CPB_SIZE);
+	rsu_log(RSU_DEBUG, "%s - calc_crc is 0x%x\n", __func__, calc_crc);
+	memcpy(&crc_from_saved, cpb_data + CPB_SIZE, sizeof(crc_from_saved));
+	rsu_log(RSU_DEBUG, "%s - crc_from_saved is 0x%x\n", __func__,
+		crc_from_saved);
+
+	if (calc_crc != crc_from_saved) {
+		rsu_log(RSU_ERR, "saved data is corrupted\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * check the magic number to prevent user from accidentally
+	 * restoring SPB
+	 */
+	memcpy(&magic_number, cpb_data, sizeof(magic_number));
+	if (magic_number != CPB_MAGIC_NUMBER) {
+		rsu_log(RSU_ERR, "failure due to mismatch magic number\n");
+		return -EINVAL;
+	}
+
+	memcpy(&cpb, cpb_data, CPB_SIZE);
+	ret = writeback_cpb();
+	if (ret) {
+		rsu_log(RSU_ERR, "failed to write back cpb\n");
+		return ret;
+	}
+
+	cpb_slots = (u64 *)&cpb.data[cpb.header.image_ptr_offset];
+
+	cpb_corrupted = false;
+	return 0;
+}
+
+/**
  * partition_count() - get the partition count
  *
  * Return: the number of partition at flash
@@ -1247,6 +1462,10 @@ static int max_retry(__u8 *value)
 
 static void ll_exit(void)
 {
+	cpb0_part = -1;
+	cpb1_part = -1;
+	cpb_corrupted = false;
+
 	if (flash) {
 		spi_flash_free(flash);
 		flash = NULL;
@@ -1279,7 +1498,12 @@ static struct rsu_ll_intf qspi_ll_intf = {
 	.fw_ops.notify = notify_fw,
 	.fw_ops.dcmf_version = dcmf_version,
 	.fw_ops.dcmf_status = dcmf_status,
-	.fw_ops.max_retry = max_retry
+	.fw_ops.max_retry = max_retry,
+
+	.cpb_ops.empty = empty_cpb,
+	.cpb_ops.restore = restore_cpb_from_address,
+	.cpb_ops.save = save_cpb_to_address,
+	.cpb_ops.corrupted = corrupted_cpb
 };
 
 int rsu_ll_qspi_init(struct rsu_ll_intf **intf)
@@ -1312,7 +1536,7 @@ int rsu_ll_qspi_init(struct rsu_ll_intf **intf)
 		return -1;
 	}
 
-	if (load_cpb()) {
+	if (load_cpb() && !cpb_corrupted) {
 		rsu_log(RSU_ERR, "Bad CPB\n");
 		return -1;
 	}
diff --git a/arch/arm/mach-socfpga/rsu_s10.c b/arch/arm/mach-socfpga/rsu_s10.c
index b5985098ea..db475b91d3 100644
--- a/arch/arm/mach-socfpga/rsu_s10.c
+++ b/arch/arm/mach-socfpga/rsu_s10.c
@@ -920,6 +920,76 @@ static int display_max_retry(int argc, char * const argv[])
 	return CMD_RET_SUCCESS;
 }
 
+static int create_empty_cpb(int argc, char * const argv[])
+{
+	int ret;
+
+	if (argc != 1)
+		return CMD_RET_USAGE;
+
+	if (!initialized) {
+		if (rsu_init(NULL))
+			return CMD_RET_FAILURE;
+
+		initialized = 1;
+	}
+
+	ret = rsu_create_empty_cpb();
+
+	if (ret < 0)
+		return CMD_RET_FAILURE;
+
+	return CMD_RET_SUCCESS;
+}
+
+static int restore_cpb(int argc, char * const argv[])
+{
+	int ret;
+	u64 addr;
+	char *endp;
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+
+	if (!initialized) {
+		if (rsu_init(NULL))
+			return CMD_RET_FAILURE;
+
+		initialized = 1;
+	}
+
+	addr = simple_strtoul(argv[1], &endp, 16);
+	ret = rsu_restore_cpb(addr);
+	if (ret < 0)
+		return CMD_RET_FAILURE;
+
+	return CMD_RET_SUCCESS;
+}
+
+static int save_cpb(int argc, char * const argv[])
+{
+	int ret;
+	u64 addr;
+	char *endp;
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+
+	if (!initialized) {
+		if (rsu_init(NULL))
+			return CMD_RET_FAILURE;
+
+		initialized = 1;
+	}
+
+	addr =  simple_strtoul(argv[1], &endp, 16);
+	ret = rsu_save_cpb(addr);
+	if (ret < 0)
+		return CMD_RET_FAILURE;
+
+	return CMD_RET_SUCCESS;
+}
+
 struct func_t {
 	const char *cmd_string;
 	int (*func_ptr)(int cmd_argc, char * const cmd_argv[]);
@@ -953,7 +1023,10 @@ static const struct func_t rsu_func_t[] = {
 	{"reset_retry_counter", reset_retry_counter},
 	{"display_dcmf_version", display_dcmf_version},
 	{"display_dcmf_status", display_dcmf_status},
-	{"display_max_retry", display_max_retry}
+	{"display_max_retry", display_max_retry},
+	{"create_empty_cpb", create_empty_cpb},
+	{"restore_cpb", restore_cpb},
+	{"save_cpb", save_cpb}
 };
 
 int do_rsu(struct cmd_tbl *cmdtp, int flag, int argc, char * const argv[])
@@ -1011,5 +1084,8 @@ U_BOOT_CMD(
 	"display_dcmf_version - display DCMF versions and store them for SMC handler usage\n"
 	"display_dcmf_status - display DCMF status and store it for SMC handler usage\n"
 	"display_max_retry - display max_retry parameter, and store it for SMC handler usage\n"
+	"create_empty_cpb - create a empty CPB\n"
+	"restore_cpb <address> - restore CPB from an address\n"
+	"save_cpb <address> - save CPB to an address\n"
 	""
 );