[U-Boot] [PATCH 0/2] eMMC: support for Read Protected Memory Block (RPMB)

[U-Boot] [PATCH 0/2] eMMC: support for Read Protected Memory Block (RPMB)

[Pierre Aubert]

This serie of patches adds some functions and a sub-command of 'mmc' for  programming the authentication key and for reading and writing the RPMB partition of an eMMC according to the JEDEC standard No. 64-A441

The sub-command rpmb is enabled by the flag CONFIG_SUPPORT_EMMC_RPMB defined
in the board configuration file.
It has been tested on a SabreSDP iMX6 board.

Pierre Aubert (2):
  eMMC: add support for operations in RPMB partition
  eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

 common/cmd_mmc.c     |  128 ++++++++++++++++++++-
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  317 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 5 files changed, 457 insertions(+), 2 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

-- 
1.7.6.5

[U-Boot] [PATCH 1/2] eMMC: add support for operations in RPMB partition

[Pierre Aubert]

This patch adds functions for read, write and authentication
key programming for the Replay Protected Memory Block partition
in the eMMC.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  317 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 4 files changed, 330 insertions(+), 1 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
index 931922b..4c6ab9e 100644
--- a/drivers/mmc/Makefile
+++ b/drivers/mmc/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_DWMMC) += dw_mmc.o
 obj-$(CONFIG_EXYNOS_DWMMC) += exynos_dw_mmc.o
 obj-$(CONFIG_ZYNQ_SDHCI) += zynq_sdhci.o
 obj-$(CONFIG_SOCFPGA_DWMMC) += socfpga_dw_mmc.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
 ifdef CONFIG_SPL_BUILD
 obj-$(CONFIG_SPL_MMC_BOOT) += fsl_esdhc_spl.o
 else
diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
new file mode 100644
index 0000000..7f9dffc
--- /dev/null
+++ b/drivers/mmc/rpmb.c
@@ -0,0 +1,317 @@
+/*
+ * Copyright 2014, Staubli Faverges
+ * Pierre Aubert
+ *
+ * eMMC- Replay Protected Memory Block
+ * According to JEDEC Standard No. 84-A441
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <common.h>
+#include <mmc.h>
+#include <sha256.h>
+#include "mmc_private.h"
+
+/* Request codes */
+#define RPMB_REQ_KEY		1
+#define RPMB_REQ_WCOUNTER	2
+#define RPMB_REQ_WRITE_DATA	3
+#define RPMB_REQ_READ_DATA	4
+#define RPMB_REQ_STATUS		5
+
+/* Response code */
+#define RPMB_RESP_KEY		0x0100
+#define RPMB_RESP_WCOUNTER	0x0200
+#define RPMB_RESP_WRITE_DATA	0x0300
+#define RPMB_RESP_READ_DATA	0x0400
+
+/* Error codes */
+#define RPMB_OK			0
+#define RPMB_ERR_GENERAL	1
+#define RPMB_ERR_AUTH	2
+#define RPMB_ERR_COUNTER	3
+#define RPMB_ERR_ADDRESS	4
+#define RPMB_ERR_WRITE		5
+#define RPMB_ERR_READ		6
+#define RPMB_ERR_KEY		7
+#define RPMB_ERR_CNT_EXPIRED	0x80
+#define RPMB_ERR_MSK		0x7
+
+/* Sizes of RPMB data frame */
+#define RPMB_SZ_STUFF		196
+#define RPMB_SZ_MAC		32
+#define RPMB_SZ_DATA		256
+#define RPMB_SZ_NONCE		16
+
+#define SHA256_BLOCK_SIZE	64
+
+/* Error messages */
+static const char * const rpmb_err_msg[] = {
+	"",
+	"General failure",
+	"Authentication failure",
+	"Counter failure",
+	"Address failure",
+	"Write failure",
+	"Read failure",
+	"Authentication key not yet programmed",
+};
+
+
+/* Structure of RPMB data frame. */
+static struct s_rpmb {
+	unsigned char stuff[RPMB_SZ_STUFF];
+	unsigned char mac[RPMB_SZ_MAC];
+	unsigned char data[RPMB_SZ_DATA];
+	unsigned char nonce[RPMB_SZ_NONCE];
+	unsigned long write_counter;
+	unsigned short address;
+	unsigned short block_count;
+	unsigned short result;
+	unsigned short request;
+} __aligned(32) rpmb_frame;
+
+static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
+			      bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+
+	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
+	cmd.cmdarg = blockcount & 0x0000FFFF;
+	if (is_rel_write)
+		cmd.cmdarg |= 1 << 31;
+	cmd.resp_type = MMC_RSP_R1;
+
+	return mmc_send_cmd(mmc, &cmd, NULL);
+}
+static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
+			    unsigned int count, bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, count, is_rel_write);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+
+	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1b;
+
+	data.src = (const char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_WRITE;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+	return 0;
+}
+static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
+			     unsigned short expected)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, 1, false);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1;
+
+	data.dest = (char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_READ;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	/* Check the response and the status */
+	if (be16_to_cpu(rpmb_frame.request) != expected) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:response= %x\n", __func__,
+		       be16_to_cpu(rpmb_frame.request));
+#endif
+		return -1;
+	}
+	ret = be16_to_cpu(rpmb_frame.result);
+	if (ret) {
+		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
+		       (ret & RPMB_ERR_CNT_EXPIRED) ?
+		       "Write counter has expired" : "");
+	}
+
+	/* Return the status of the command */
+	return ret;
+}
+static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
+{
+	memset(&rpmb_frame, 0, sizeof(rpmb_frame));
+	rpmb_frame.request = cpu_to_be16(RPMB_REQ_STATUS);
+	if (mmc_rpmb_request(mmc, &rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	return mmc_rpmb_response(mmc, &rpmb_frame, expected);
+}
+static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
+		      unsigned char *output)
+{
+	sha256_context ctx;
+	int i;
+	unsigned char k_ipad[SHA256_BLOCK_SIZE];
+	unsigned char k_opad[SHA256_BLOCK_SIZE];
+
+	sha256_starts(&ctx);
+
+	/* According to RFC 4634, the HMAC transform looks like:
+	   SHA(K XOR opad, SHA(K XOR ipad, text))
+
+	   where K is an n byte key.
+	   ipad is the byte 0x36 repeated blocksize times
+	   opad is the byte 0x5c repeated blocksize times
+	   and text is the data being protected.
+	*/
+
+	for (i = 0; i < RPMB_SZ_MAC; i++) {
+		k_ipad[i] = key[i] ^ 0x36;
+		k_opad[i] = key[i] ^ 0x5c;
+	}
+	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
+	for ( ; i < SHA256_BLOCK_SIZE; i++) {
+		k_ipad[i] = 0x36;
+		k_opad[i] = 0x5c;
+	}
+	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
+	sha256_update(&ctx, buff, len);
+	sha256_finish(&ctx, output);
+
+	/* Init context for second pass */
+	sha256_starts(&ctx);
+
+	/* start with outer pad */
+	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
+
+	/* then results of 1st hash */
+	sha256_update(&ctx, output, RPMB_SZ_MAC);
+
+	/* finish up 2nd pass */
+	sha256_finish(&ctx, output);
+}
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
+{
+	int ret;
+
+	/* Fill the request */
+	memset(&rpmb_frame, 0, sizeof(rpmb_frame));
+	rpmb_frame.request = cpu_to_be16(RPMB_REQ_WCOUNTER);
+	if (mmc_rpmb_request(mmc, &rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	ret = mmc_rpmb_response(mmc, &rpmb_frame, RPMB_RESP_WCOUNTER);
+	if (ret)
+		return ret;
+
+	*pcounter = be32_to_cpu(rpmb_frame.write_counter);
+	return 0;
+}
+int mmc_rpmb_set_key(struct mmc *mmc, void *key)
+{
+	/* Fill the request */
+	memset(&rpmb_frame, 0, sizeof(rpmb_frame));
+	rpmb_frame.request = cpu_to_be16(RPMB_REQ_KEY);
+	memcpy(&rpmb_frame.mac, key, sizeof(rpmb_frame.mac));
+
+	if (mmc_rpmb_request(mmc, &rpmb_frame, 1, true))
+		return -1;
+
+	/* read the operation status */
+	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
+}
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Fill the request */
+		memset(&rpmb_frame, 0, sizeof(rpmb_frame));
+		rpmb_frame.address = cpu_to_be16(blk + i);
+		rpmb_frame.request = cpu_to_be16(RPMB_REQ_READ_DATA);
+		if (mmc_rpmb_request(mmc, &rpmb_frame, 1, false))
+			break;
+
+		/* Read the result */
+		if (mmc_rpmb_response(mmc, &rpmb_frame, RPMB_RESP_READ_DATA))
+			break;
+
+		/* Check the HMAC if key is provided */
+		if (key) {
+			unsigned char ret_hmac[RPMB_SZ_MAC];
+
+			rpmb_hmac(key, rpmb_frame.data, 284, ret_hmac);
+			if (memcmp(ret_hmac, rpmb_frame.mac, RPMB_SZ_MAC)) {
+				printf("MAC error on block #%d\n", i);
+				break;
+			}
+		}
+		/* Copy data */
+		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame.data, RPMB_SZ_DATA);
+	}
+	return i;
+}
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	unsigned long wcount;
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		if (mmc_rpmb_get_counter(mmc, &wcount)) {
+			printf("Cannot read RPMB write counter\n");
+			break;
+		}
+
+		/* Fill the request */
+		memset(&rpmb_frame, 0, sizeof(rpmb_frame));
+		memcpy(rpmb_frame.data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
+		rpmb_frame.address = cpu_to_be16(blk + i);
+		rpmb_frame.block_count = cpu_to_be16(1);
+		rpmb_frame.write_counter = cpu_to_be32(wcount);
+		rpmb_frame.request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
+		/* Computes HMAC */
+		rpmb_hmac(key, rpmb_frame.data, 284, rpmb_frame.mac);
+
+		if (mmc_rpmb_request(mmc, &rpmb_frame, 1, true))
+			break;
+
+		/* Get status */
+		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
+			break;
+	}
+	return i;
+}
diff --git a/include/mmc.h b/include/mmc.h
index 42d0125..14d296c 100644
--- a/include/mmc.h
+++ b/include/mmc.h
@@ -69,6 +69,7 @@
 #define MMC_CMD_SET_BLOCKLEN		16
 #define MMC_CMD_READ_SINGLE_BLOCK	17
 #define MMC_CMD_READ_MULTIPLE_BLOCK	18
+#define MMC_CMD_SET_BLOCK_COUNT         23
 #define MMC_CMD_WRITE_SINGLE_BLOCK	24
 #define MMC_CMD_WRITE_MULTIPLE_BLOCK	25
 #define MMC_CMD_ERASE_GROUP_START	35
@@ -224,6 +225,7 @@
  * boot partitions (2), general purpose partitions (4) in MMC v4.4.
  */
 #define MMC_NUM_BOOT_PARTITION	2
+#define MMC_PART_RPMB           3       /* RPMB partition number */
 
 struct mmc_cid {
 	unsigned long psn;
@@ -335,7 +337,13 @@ int mmc_set_part_conf(struct mmc *mmc, u8 ack, u8 part_num, u8 access);
 int mmc_set_boot_bus_width(struct mmc *mmc, u8 width, u8 reset, u8 mode);
 /* Function to modify the RST_n_FUNCTION field of EXT_CSD */
 int mmc_set_rst_n_function(struct mmc *mmc, u8 enable);
-
+/* Functions to read / write the RPMB partition */
+int mmc_rpmb_set_key(struct mmc *mmc, void *key);
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *counter);
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key);
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		   unsigned short cnt, unsigned char *key);
 /**
  * Start device initialization and return immediately; it does not block on
  * polling OCR (operation condition register) status.  Then you should call
diff --git a/lib/Makefile b/lib/Makefile
index 27e4f78..2bf90f3 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -35,6 +35,9 @@ obj-y += net_utils.o
 obj-$(CONFIG_PHYSMEM) += physmem.o
 obj-y += qsort.o
 obj-$(CONFIG_SHA1) += sha1.o
+ifdef CONFIG_SUPPORT_EMMC_RPMB
+CONFIG_SHA256 := y
+endif
 obj-$(CONFIG_SHA256) += sha256.o
 obj-y	+= strmhz.o
 obj-$(CONFIG_TPM) += tpm.o
-- 
1.7.6.5

[U-Boot] [PATCH 1/2] eMMC: add support for operations in RPMB partition

[Roman Peniaev]

Hello, Pierre.

Could you please help me to understand why you used R1b with 
MMC_CMD_WRITE_MULTIPLE_BLOCK in mmc_rpmb_request:

+	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1b;

Because according to the spec CMD25 has R1 response, also
RPMB section does not explicitly point on the response type,
maybe these words are the key:

"The busy signaling in the Dat0 line after the CRC status by the e?MMC 
is indicating request busy."

But I am not sure.

--
Roman

[U-Boot] [PATCH 2/2] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Pierre Aubert]

This sub-command adds support for the RPMB partition of an eMMC:
* mmc rpmb key <address of the authentication key>
  Programs the authentication key in the eMMC This key can not
  be overwritten.
* mmc rpmb read <address> <block> <#count> [address of key]
  Reads <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. If the optionnal
  address of the authentication key is provided, the
  Message Authentication Code (MAC) is verified on each
  block.
* mmc rpmb write <address> <block> <#count> <address of key>
  Writes <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. The datas are signed
  with the key provided.
* mmc rpmb counter
  Returns the 'Write counter' of the RPMB partition.

The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 common/cmd_mmc.c |  128 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 files changed, 127 insertions(+), 1 deletions(-)

diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
index c1916c9..3cf11e7 100644
--- a/common/cmd_mmc.c
+++ b/common/cmd_mmc.c
@@ -130,7 +130,123 @@ U_BOOT_CMD(
 	"display MMC info",
 	"- display info of the current MMC device"
 );
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+static int confirm_key_prog(void)
+{
+	puts("Warning: Programming authentication key can be done only once !\n"
+	     "         Use this command only if you are sure of what you are doing,\n"
+	     "Really perform the key programming ? ");
+	if (getc() == 'y') {
+		int c;
+
+		putc('y');
+		c = getc();
+		putc('\n');
+		if (c == '\r')
+			return 1;
+	}
 
+	puts("Authentication key programming aborted\n");
+	return 0;
+}
+static int do_mmcrpmb(int argc, char * const argv[])
+{
+	enum rpmb_state {
+		RPMB_INVALID,
+		RPMB_READ,
+		RPMB_WRITE,
+		RPMB_KEY,
+		RPMB_COUNTER,
+	} state;
+
+	state = RPMB_INVALID;
+	if (argc == 4 && strcmp(argv[2], "key") == 0)
+		state = RPMB_KEY;
+	if ((argc == 6 || argc == 7) && strcmp(argv[2], "read") == 0)
+		state = RPMB_READ;
+	else if (argc == 7 && strcmp(argv[2], "write") == 0)
+		state = RPMB_WRITE;
+	else if (argc == 3 && strcmp(argv[2], "counter") == 0)
+		state = RPMB_COUNTER;
+
+	if (state != RPMB_INVALID) {
+		struct mmc *mmc = find_mmc_device(curr_device);
+		void *key_addr;
+		char original_part;
+		int ret;
+
+		if (!mmc) {
+			printf("no mmc device at slot %x\n", curr_device);
+			return CMD_RET_FAILURE;
+		}
+		mmc_init(mmc);
+		if (IS_SD(mmc)) {
+			printf("It is not a EMMC device\n");
+			return CMD_RET_FAILURE;
+		}
+		/* Switch to the RPMB partition */
+		original_part = mmc->part_num;
+		if (mmc->part_num != MMC_PART_RPMB) {
+			if (mmc_switch_part(curr_device, MMC_PART_RPMB) != 0)
+				return CMD_RET_FAILURE;
+			mmc->part_num = MMC_PART_RPMB;
+		}
+		ret = CMD_RET_SUCCESS;
+		if (state == RPMB_KEY) {
+			key_addr = (void *)simple_strtoul(argv[3], NULL, 16);
+			if (confirm_key_prog()) {
+				if (mmc_rpmb_set_key(mmc, key_addr)) {
+					printf("ERROR - Key already programmed ?\n");
+					ret = CMD_RET_FAILURE;
+				}
+			} else {
+				ret = CMD_RET_FAILURE;
+			}
+		} else if (state == RPMB_COUNTER) {
+			unsigned long counter;
+			if (mmc_rpmb_get_counter(mmc, &counter))
+				ret = CMD_RET_FAILURE;
+			else
+				printf("Write counter= %lx\n", counter);
+		} else {
+			u16 blk, cnt;
+			void *addr;
+			int n;
+
+			addr = (void *)simple_strtoul(argv[3], NULL, 16);
+			blk = simple_strtoul(argv[4], NULL, 16);
+			cnt = simple_strtoul(argv[5], NULL, 16);
+
+			if (state == RPMB_READ) {
+				key_addr = (argc == 7) ?
+					(void *)simple_strtoul(argv[6],
+							       NULL, 16) :
+					NULL;
+				n =  mmc_rpmb_read(mmc, addr, blk, cnt,
+						   key_addr);
+			} else {
+				key_addr = (void *)simple_strtoul(argv[6],
+								  NULL, 16);
+				n =  mmc_rpmb_write(mmc, addr, blk, cnt,
+						    key_addr);
+			}
+			printf("%d RPMB blocks %s: %s\n",
+			       n, argv[2], (n == cnt) ? "OK" : "ERROR");
+			if (n != cnt)
+				ret = CMD_RET_FAILURE;
+		}
+
+		/* Return to orginal partition */
+		if (mmc->part_num != original_part) {
+			if (mmc_switch_part(curr_device, original_part) != 0)
+				return CMD_RET_FAILURE;
+			mmc->part_num = original_part;
+		}
+		return ret;
+	} else
+		return CMD_RET_USAGE;
+}
+#endif
 static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	enum mmc_state state;
@@ -365,6 +481,10 @@ static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 
 		return mmc_set_rst_n_function(mmc, enable);
 #endif /* CONFIG_SUPPORT_EMMC_BOOT */
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	} else if (strcmp(argv[1], "rpmb") == 0) {
+		return do_mmcrpmb(argc, argv);
+#endif /*  CONFIG_SUPPORT_EMMC_RPMB */
 	}
 
 	else if (argc == 3 && strcmp(argv[1], "setdsr") == 0) {
@@ -454,7 +574,7 @@ static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 }
 
 U_BOOT_CMD(
-	mmc, 6, 1, do_mmcops,
+	mmc, 7, 1, do_mmcops,
 	"MMC sub system",
 	"read addr blk# cnt\n"
 	"mmc write addr blk# cnt\n"
@@ -474,6 +594,12 @@ U_BOOT_CMD(
 	" - Change the RST_n_FUNCTION field of the specified device\n"
 	"   WARNING: This is a write-once field and 0 / 1 / 2 are the only valid values.\n"
 #endif
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	"mmc rpmb read addr blk# cnt [address of auth-key] - block size is 256 bytes\n"
+	"mmc rpmb write addr blk# cnt <address of auth-key> - block size is 256 bytes\n"
+	"mmc rpmb key <address of auth-key> - program the RPMB authentication key.\n"
+	"mmc rpmb counter - read the value of the write counter\n"
+#endif
 	"mmc setdsr - set DSR register value\n"
 	);
 #endif /* !CONFIG_GENERIC_MMC */
-- 
1.7.6.5

[U-Boot] [PATCH V2 0/2] eMMC: support for Read Protected Memory Block (RPMB)

[Pierre Aubert]

This serie of patches adds some functions and a sub-command of 'mmc' for  programming the authentication key and for reading and writing the RPMB partition of an eMMC according to the JEDEC standard No. 64-A441

The sub-command rpmb is enabled by the flag CONFIG_SUPPORT_EMMC_RPMB defined
in the board configuration file.
It has been tested on a SabreSDP iMX6 board.

Changes in V2:
- use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the 
  RPMB frames.

Pierre Aubert (2):
  eMMC: add support for operations in RPMB partition
  eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

 common/cmd_mmc.c     |  128 ++++++++++++++++++++-
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 5 files changed, 463 insertions(+), 2 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

-- 
1.7.6.5

[U-Boot] [PATCH V2 1/2] eMMC: add support for operations in RPMB partition

[Pierre Aubert]

This patch adds functions for read, write and authentication
key programming for the Replay Protected Memory Block partition
in the eMMC.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 4 files changed, 336 insertions(+), 1 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
index 931922b..4c6ab9e 100644
--- a/drivers/mmc/Makefile
+++ b/drivers/mmc/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_DWMMC) += dw_mmc.o
 obj-$(CONFIG_EXYNOS_DWMMC) += exynos_dw_mmc.o
 obj-$(CONFIG_ZYNQ_SDHCI) += zynq_sdhci.o
 obj-$(CONFIG_SOCFPGA_DWMMC) += socfpga_dw_mmc.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
 ifdef CONFIG_SPL_BUILD
 obj-$(CONFIG_SPL_MMC_BOOT) += fsl_esdhc_spl.o
 else
diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
new file mode 100644
index 0000000..05936f5
--- /dev/null
+++ b/drivers/mmc/rpmb.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright 2014, Staubli Faverges
+ * Pierre Aubert
+ *
+ * eMMC- Replay Protected Memory Block
+ * According to JEDEC Standard No. 84-A441
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <common.h>
+#include <mmc.h>
+#include <sha256.h>
+#include "mmc_private.h"
+
+/* Request codes */
+#define RPMB_REQ_KEY		1
+#define RPMB_REQ_WCOUNTER	2
+#define RPMB_REQ_WRITE_DATA	3
+#define RPMB_REQ_READ_DATA	4
+#define RPMB_REQ_STATUS		5
+
+/* Response code */
+#define RPMB_RESP_KEY		0x0100
+#define RPMB_RESP_WCOUNTER	0x0200
+#define RPMB_RESP_WRITE_DATA	0x0300
+#define RPMB_RESP_READ_DATA	0x0400
+
+/* Error codes */
+#define RPMB_OK			0
+#define RPMB_ERR_GENERAL	1
+#define RPMB_ERR_AUTH	2
+#define RPMB_ERR_COUNTER	3
+#define RPMB_ERR_ADDRESS	4
+#define RPMB_ERR_WRITE		5
+#define RPMB_ERR_READ		6
+#define RPMB_ERR_KEY		7
+#define RPMB_ERR_CNT_EXPIRED	0x80
+#define RPMB_ERR_MSK		0x7
+
+/* Sizes of RPMB data frame */
+#define RPMB_SZ_STUFF		196
+#define RPMB_SZ_MAC		32
+#define RPMB_SZ_DATA		256
+#define RPMB_SZ_NONCE		16
+
+#define SHA256_BLOCK_SIZE	64
+
+/* Error messages */
+static const char * const rpmb_err_msg[] = {
+	"",
+	"General failure",
+	"Authentication failure",
+	"Counter failure",
+	"Address failure",
+	"Write failure",
+	"Read failure",
+	"Authentication key not yet programmed",
+};
+
+
+/* Structure of RPMB data frame. */
+struct s_rpmb {
+	unsigned char stuff[RPMB_SZ_STUFF];
+	unsigned char mac[RPMB_SZ_MAC];
+	unsigned char data[RPMB_SZ_DATA];
+	unsigned char nonce[RPMB_SZ_NONCE];
+	unsigned long write_counter;
+	unsigned short address;
+	unsigned short block_count;
+	unsigned short result;
+	unsigned short request;
+};
+
+static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
+			      bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+
+	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
+	cmd.cmdarg = blockcount & 0x0000FFFF;
+	if (is_rel_write)
+		cmd.cmdarg |= 1 << 31;
+	cmd.resp_type = MMC_RSP_R1;
+
+	return mmc_send_cmd(mmc, &cmd, NULL);
+}
+static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
+			    unsigned int count, bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, count, is_rel_write);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+
+	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1b;
+
+	data.src = (const char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_WRITE;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+	return 0;
+}
+static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
+			     unsigned short expected)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, 1, false);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1;
+
+	data.dest = (char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_READ;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	/* Check the response and the status */
+	if (be16_to_cpu(s->request) != expected) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:response= %x\n", __func__,
+		       be16_to_cpu(s->request));
+#endif
+		return -1;
+	}
+	ret = be16_to_cpu(s->result);
+	if (ret) {
+		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
+		       (ret & RPMB_ERR_CNT_EXPIRED) ?
+		       "Write counter has expired" : "");
+	}
+
+	/* Return the status of the command */
+	return ret;
+}
+static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	return mmc_rpmb_response(mmc, rpmb_frame, expected);
+}
+static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
+		      unsigned char *output)
+{
+	sha256_context ctx;
+	int i;
+	unsigned char k_ipad[SHA256_BLOCK_SIZE];
+	unsigned char k_opad[SHA256_BLOCK_SIZE];
+
+	sha256_starts(&ctx);
+
+	/* According to RFC 4634, the HMAC transform looks like:
+	   SHA(K XOR opad, SHA(K XOR ipad, text))
+
+	   where K is an n byte key.
+	   ipad is the byte 0x36 repeated blocksize times
+	   opad is the byte 0x5c repeated blocksize times
+	   and text is the data being protected.
+	*/
+
+	for (i = 0; i < RPMB_SZ_MAC; i++) {
+		k_ipad[i] = key[i] ^ 0x36;
+		k_opad[i] = key[i] ^ 0x5c;
+	}
+	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
+	for ( ; i < SHA256_BLOCK_SIZE; i++) {
+		k_ipad[i] = 0x36;
+		k_opad[i] = 0x5c;
+	}
+	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
+	sha256_update(&ctx, buff, len);
+	sha256_finish(&ctx, output);
+
+	/* Init context for second pass */
+	sha256_starts(&ctx);
+
+	/* start with outer pad */
+	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
+
+	/* then results of 1st hash */
+	sha256_update(&ctx, output, RPMB_SZ_MAC);
+
+	/* finish up 2nd pass */
+	sha256_finish(&ctx, output);
+}
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
+{
+	int ret;
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER);
+	if (ret)
+		return ret;
+
+	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
+	return 0;
+}
+int mmc_rpmb_set_key(struct mmc *mmc, void *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
+	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);
+
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+		return -1;
+
+	/* read the operation status */
+	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
+}
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+			break;
+
+		/* Read the result */
+		if (mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_READ_DATA))
+			break;
+
+		/* Check the HMAC if key is provided */
+		if (key) {
+			unsigned char ret_hmac[RPMB_SZ_MAC];
+
+			rpmb_hmac(key, rpmb_frame->data, 284, ret_hmac);
+			if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
+				printf("MAC error on block #%d\n", i);
+				break;
+			}
+		}
+		/* Copy data */
+		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
+	}
+	return i;
+}
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	unsigned long wcount;
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		if (mmc_rpmb_get_counter(mmc, &wcount)) {
+			printf("Cannot read RPMB write counter\n");
+			break;
+		}
+
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->block_count = cpu_to_be16(1);
+		rpmb_frame->write_counter = cpu_to_be32(wcount);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
+		/* Computes HMAC */
+		rpmb_hmac(key, rpmb_frame->data, 284, rpmb_frame->mac);
+
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+			break;
+
+		/* Get status */
+		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
+			break;
+	}
+	return i;
+}
diff --git a/include/mmc.h b/include/mmc.h
index 42d0125..14d296c 100644
--- a/include/mmc.h
+++ b/include/mmc.h
@@ -69,6 +69,7 @@
 #define MMC_CMD_SET_BLOCKLEN		16
 #define MMC_CMD_READ_SINGLE_BLOCK	17
 #define MMC_CMD_READ_MULTIPLE_BLOCK	18
+#define MMC_CMD_SET_BLOCK_COUNT         23
 #define MMC_CMD_WRITE_SINGLE_BLOCK	24
 #define MMC_CMD_WRITE_MULTIPLE_BLOCK	25
 #define MMC_CMD_ERASE_GROUP_START	35
@@ -224,6 +225,7 @@
  * boot partitions (2), general purpose partitions (4) in MMC v4.4.
  */
 #define MMC_NUM_BOOT_PARTITION	2
+#define MMC_PART_RPMB           3       /* RPMB partition number */
 
 struct mmc_cid {
 	unsigned long psn;
@@ -335,7 +337,13 @@ int mmc_set_part_conf(struct mmc *mmc, u8 ack, u8 part_num, u8 access);
 int mmc_set_boot_bus_width(struct mmc *mmc, u8 width, u8 reset, u8 mode);
 /* Function to modify the RST_n_FUNCTION field of EXT_CSD */
 int mmc_set_rst_n_function(struct mmc *mmc, u8 enable);
-
+/* Functions to read / write the RPMB partition */
+int mmc_rpmb_set_key(struct mmc *mmc, void *key);
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *counter);
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key);
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		   unsigned short cnt, unsigned char *key);
 /**
  * Start device initialization and return immediately; it does not block on
  * polling OCR (operation condition register) status.  Then you should call
diff --git a/lib/Makefile b/lib/Makefile
index 27e4f78..2bf90f3 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -35,6 +35,9 @@ obj-y += net_utils.o
 obj-$(CONFIG_PHYSMEM) += physmem.o
 obj-y += qsort.o
 obj-$(CONFIG_SHA1) += sha1.o
+ifdef CONFIG_SUPPORT_EMMC_RPMB
+CONFIG_SHA256 := y
+endif
 obj-$(CONFIG_SHA256) += sha256.o
 obj-y	+= strmhz.o
 obj-$(CONFIG_TPM) += tpm.o
-- 
1.7.6.5

[U-Boot] [PATCH V2 2/2] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Pierre Aubert]

This sub-command adds support for the RPMB partition of an eMMC:
* mmc rpmb key <address of the authentication key>
  Programs the authentication key in the eMMC This key can not
  be overwritten.
* mmc rpmb read <address> <block> <#count> [address of key]
  Reads <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. If the optionnal
  address of the authentication key is provided, the
  Message Authentication Code (MAC) is verified on each
  block.
* mmc rpmb write <address> <block> <#count> <address of key>
  Writes <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. The datas are signed
  with the key provided.
* mmc rpmb counter
  Returns the 'Write counter' of the RPMB partition.

The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 common/cmd_mmc.c |  128 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
 1 files changed, 127 insertions(+), 1 deletions(-)

diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
index c1916c9..3cf11e7 100644
--- a/common/cmd_mmc.c
+++ b/common/cmd_mmc.c
@@ -130,7 +130,123 @@ U_BOOT_CMD(
 	"display MMC info",
 	"- display info of the current MMC device"
 );
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+static int confirm_key_prog(void)
+{
+	puts("Warning: Programming authentication key can be done only once !\n"
+	     "         Use this command only if you are sure of what you are doing,\n"
+	     "Really perform the key programming ? ");
+	if (getc() == 'y') {
+		int c;
+
+		putc('y');
+		c = getc();
+		putc('\n');
+		if (c == '\r')
+			return 1;
+	}
 
+	puts("Authentication key programming aborted\n");
+	return 0;
+}
+static int do_mmcrpmb(int argc, char * const argv[])
+{
+	enum rpmb_state {
+		RPMB_INVALID,
+		RPMB_READ,
+		RPMB_WRITE,
+		RPMB_KEY,
+		RPMB_COUNTER,
+	} state;
+
+	state = RPMB_INVALID;
+	if (argc == 4 && strcmp(argv[2], "key") == 0)
+		state = RPMB_KEY;
+	if ((argc == 6 || argc == 7) && strcmp(argv[2], "read") == 0)
+		state = RPMB_READ;
+	else if (argc == 7 && strcmp(argv[2], "write") == 0)
+		state = RPMB_WRITE;
+	else if (argc == 3 && strcmp(argv[2], "counter") == 0)
+		state = RPMB_COUNTER;
+
+	if (state != RPMB_INVALID) {
+		struct mmc *mmc = find_mmc_device(curr_device);
+		void *key_addr;
+		char original_part;
+		int ret;
+
+		if (!mmc) {
+			printf("no mmc device at slot %x\n", curr_device);
+			return CMD_RET_FAILURE;
+		}
+		mmc_init(mmc);
+		if (IS_SD(mmc)) {
+			printf("It is not a EMMC device\n");
+			return CMD_RET_FAILURE;
+		}
+		/* Switch to the RPMB partition */
+		original_part = mmc->part_num;
+		if (mmc->part_num != MMC_PART_RPMB) {
+			if (mmc_switch_part(curr_device, MMC_PART_RPMB) != 0)
+				return CMD_RET_FAILURE;
+			mmc->part_num = MMC_PART_RPMB;
+		}
+		ret = CMD_RET_SUCCESS;
+		if (state == RPMB_KEY) {
+			key_addr = (void *)simple_strtoul(argv[3], NULL, 16);
+			if (confirm_key_prog()) {
+				if (mmc_rpmb_set_key(mmc, key_addr)) {
+					printf("ERROR - Key already programmed ?\n");
+					ret = CMD_RET_FAILURE;
+				}
+			} else {
+				ret = CMD_RET_FAILURE;
+			}
+		} else if (state == RPMB_COUNTER) {
+			unsigned long counter;
+			if (mmc_rpmb_get_counter(mmc, &counter))
+				ret = CMD_RET_FAILURE;
+			else
+				printf("Write counter= %lx\n", counter);
+		} else {
+			u16 blk, cnt;
+			void *addr;
+			int n;
+
+			addr = (void *)simple_strtoul(argv[3], NULL, 16);
+			blk = simple_strtoul(argv[4], NULL, 16);
+			cnt = simple_strtoul(argv[5], NULL, 16);
+
+			if (state == RPMB_READ) {
+				key_addr = (argc == 7) ?
+					(void *)simple_strtoul(argv[6],
+							       NULL, 16) :
+					NULL;
+				n =  mmc_rpmb_read(mmc, addr, blk, cnt,
+						   key_addr);
+			} else {
+				key_addr = (void *)simple_strtoul(argv[6],
+								  NULL, 16);
+				n =  mmc_rpmb_write(mmc, addr, blk, cnt,
+						    key_addr);
+			}
+			printf("%d RPMB blocks %s: %s\n",
+			       n, argv[2], (n == cnt) ? "OK" : "ERROR");
+			if (n != cnt)
+				ret = CMD_RET_FAILURE;
+		}
+
+		/* Return to orginal partition */
+		if (mmc->part_num != original_part) {
+			if (mmc_switch_part(curr_device, original_part) != 0)
+				return CMD_RET_FAILURE;
+			mmc->part_num = original_part;
+		}
+		return ret;
+	} else
+		return CMD_RET_USAGE;
+}
+#endif
 static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	enum mmc_state state;
@@ -365,6 +481,10 @@ static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 
 		return mmc_set_rst_n_function(mmc, enable);
 #endif /* CONFIG_SUPPORT_EMMC_BOOT */
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	} else if (strcmp(argv[1], "rpmb") == 0) {
+		return do_mmcrpmb(argc, argv);
+#endif /*  CONFIG_SUPPORT_EMMC_RPMB */
 	}
 
 	else if (argc == 3 && strcmp(argv[1], "setdsr") == 0) {
@@ -454,7 +574,7 @@ static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 }
 
 U_BOOT_CMD(
-	mmc, 6, 1, do_mmcops,
+	mmc, 7, 1, do_mmcops,
 	"MMC sub system",
 	"read addr blk# cnt\n"
 	"mmc write addr blk# cnt\n"
@@ -474,6 +594,12 @@ U_BOOT_CMD(
 	" - Change the RST_n_FUNCTION field of the specified device\n"
 	"   WARNING: This is a write-once field and 0 / 1 / 2 are the only valid values.\n"
 #endif
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	"mmc rpmb read addr blk# cnt [address of auth-key] - block size is 256 bytes\n"
+	"mmc rpmb write addr blk# cnt <address of auth-key> - block size is 256 bytes\n"
+	"mmc rpmb key <address of auth-key> - program the RPMB authentication key.\n"
+	"mmc rpmb counter - read the value of the write counter\n"
+#endif
 	"mmc setdsr - set DSR register value\n"
 	);
 #endif /* !CONFIG_GENERIC_MMC */
-- 
1.7.6.5

[U-Boot] [PATCH V2 2/2] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Wolfgang Denk]

Dear Pierre Aubert,

In message <1397747435-24042-3-git-send-email-p.aubert@staubli.com> you wrote:
> This sub-command adds support for the RPMB partition of an eMMC:
> * mmc rpmb key <address of the authentication key>
>   Programs the authentication key in the eMMC This key can not
>   be overwritten.
> * mmc rpmb read <address> <block> <#count> [address of key]
>   Reads <#count> blocks of 256 bytes in the RPMB partition
>   beginning at block number <block>. If the optionnal
>   address of the authentication key is provided, the
>   Message Authentication Code (MAC) is verified on each
>   block.
> * mmc rpmb write <address> <block> <#count> <address of key>
>   Writes <#count> blocks of 256 bytes in the RPMB partition
>   beginning at block number <block>. The datas are signed
>   with the key provided.
> * mmc rpmb counter
>   Returns the 'Write counter' of the RPMB partition.
> 
> The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB

Such new options must be documented in the README.

> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
> CC: Pantelis Antoniou <panto@antoniou-consulting.com>
> ---
>  common/cmd_mmc.c |  128 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
>  1 files changed, 127 insertions(+), 1 deletions(-)
> 
> diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
> index c1916c9..3cf11e7 100644
> --- a/common/cmd_mmc.c
> +++ b/common/cmd_mmc.c
> @@ -130,7 +130,123 @@ U_BOOT_CMD(
>  	"display MMC info",
>  	"- display info of the current MMC device"
>  );
> +#ifdef CONFIG_SUPPORT_EMMC_RPMB
> +static int confirm_key_prog(void)
> +{
> +	puts("Warning: Programming authentication key can be done only once !\n"
> +	     "         Use this command only if you are sure of what you are doing,\n"
> +	     "Really perform the key programming ? ");
> +	if (getc() == 'y') {

Would it not makes sense to flush the input before reading the char,
so that you don;t react on any type-ahead that might already be
buffered?

> +		int c;
> +
> +		putc('y');
> +		c = getc();
> +		putc('\n');
> +		if (c == '\r')
> +			return 1;
> +	}

Should we allow for 'Y"? And for "yes" / "Yes"?

We have getenv_yesno() - maybe we should provide a similar function
that can be used in other places where such interactive confirmation
is needed?

> +	if (state != RPMB_INVALID) {

Change this into

	if (state == RPMB_INVALID)
		return CMD_RET_USAGE;

and avoid one level of indentation; this will make the code much
easier to read.

> +		if (IS_SD(mmc)) {

Is IS_SD() a reliable test for eMMC devics, or would that also return
true in other cases?

> +			if (confirm_key_prog()) {
> +				if (mmc_rpmb_set_key(mmc, key_addr)) {
> +					printf("ERROR - Key already programmed ?\n");
> +					ret = CMD_RET_FAILURE;
> +				}
> +			} else {
> +				ret = CMD_RET_FAILURE;
> +			}

You should really avoid deep nesting and take early exits.
You can write this as:

			if (!confirm_key_prog())
				return CMD_RET_FAILURE;

			if (mmc_rpmb_set_key(mmc, key_addr)) {
				printf("ERROR - Key already programmed ?\n");
				ret = CMD_RET_FAILURE;
			}

Please fix globally.


> +		} else if (state == RPMB_COUNTER) {
> +			unsigned long counter;
> +			if (mmc_rpmb_get_counter(mmc, &counter))

Please insert a blank line between declarations and code.

> +			printf("%d RPMB blocks %s: %s\n",
> +			       n, argv[2], (n == cnt) ? "OK" : "ERROR");

As the input is in hex, it is usually also a good idea to (also) print
the count in hex.

>  #endif /* CONFIG_SUPPORT_EMMC_BOOT */
> +#ifdef CONFIG_SUPPORT_EMMC_RPMB
> +	} else if (strcmp(argv[1], "rpmb") == 0) {
> +		return do_mmcrpmb(argc, argv);
> +#endif /*  CONFIG_SUPPORT_EMMC_RPMB */

I think that now, with more subcommands being added, we should
convert the mmc code to proper subcommand handling. [It might even
make sense to do so for "mmc rpmb", too.]

Best regards,

Wolfgang Denk

-- 
DENX Software Engineering GmbH,     MD: Wolfgang Denk & Detlev Zundel
HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd at denx.de
"The computer programmer is a creator of universes for which he alone
is responsible. Universes of virtually unlimited  complexity  can  be
created  in  the  form  of  computer  programs." - Joseph Weizenbaum,
_Computer Power and Human Reason_

[U-Boot] [PATCH V2 2/2] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Pierre AUBERT]

Hello Wolfgang,

Le 17/04/2014 21:56, Wolfgang Denk a ?crit :
> Dear Pierre Aubert,
>
> In message <1397747435-24042-3-git-send-email-p.aubert@staubli.com> you wrote:
>> This sub-command adds support for the RPMB partition of an eMMC:
>> * mmc rpmb key <address of the authentication key>
>>    Programs the authentication key in the eMMC This key can not
>>    be overwritten.
>> * mmc rpmb read <address> <block> <#count> [address of key]
>>    Reads <#count> blocks of 256 bytes in the RPMB partition
>>    beginning at block number <block>. If the optionnal
>>    address of the authentication key is provided, the
>>    Message Authentication Code (MAC) is verified on each
>>    block.
>> * mmc rpmb write <address> <block> <#count> <address of key>
>>    Writes <#count> blocks of 256 bytes in the RPMB partition
>>    beginning at block number <block>. The datas are signed
>>    with the key provided.
>> * mmc rpmb counter
>>    Returns the 'Write counter' of the RPMB partition.
>>
>> The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB
> Such new options must be documented in the README.
I will add it in a V3
>
>> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
>> CC: Pantelis Antoniou <panto@antoniou-consulting.com>
>> ---
>>   common/cmd_mmc.c |  128 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
>>   1 files changed, 127 insertions(+), 1 deletions(-)
>>
>> diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
>> index c1916c9..3cf11e7 100644
>> --- a/common/cmd_mmc.c
>> +++ b/common/cmd_mmc.c
>> @@ -130,7 +130,123 @@ U_BOOT_CMD(
>>   	"display MMC info",
>>   	"- display info of the current MMC device"
>>   );
>> +#ifdef CONFIG_SUPPORT_EMMC_RPMB
>> +static int confirm_key_prog(void)
>> +{
>> +	puts("Warning: Programming authentication key can be done only once !\n"
>> +	     "         Use this command only if you are sure of what you are doing,\n"
>> +	     "Really perform the key programming ? ");
>> +	if (getc() == 'y') {
> Would it not makes sense to flush the input before reading the char,
> so that you don;t react on any type-ahead that might already be
> buffered?
>
>> +		int c;
>> +
>> +		putc('y');
>> +		c = getc();
>> +		putc('\n');
>> +		if (c == '\r')
>> +			return 1;
>> +	}
> Should we allow for 'Y"? And for "yes" / "Yes"?
>
> We have getenv_yesno() - maybe we should provide a similar function
> that can be used in other places where such interactive confirmation
> is needed?
I have found such a confirmation in cmd_fuse, cmd_otp and cmd_mmc. It 
makes sense to provide
a global function. I will try to submit a patch for that.
>> +	if (state != RPMB_INVALID) {
> Change this into
>
> 	if (state == RPMB_INVALID)
> 		return CMD_RET_USAGE;
>
> and avoid one level of indentation; this will make the code much
> easier to read.
>
>> +		if (IS_SD(mmc)) {
> Is IS_SD() a reliable test for eMMC devics, or would that also return
> true in other cases?
You're right, the test must be more restrictive. The RPMB partition is 
available only since the release 4.41 of the Jedec
standard. I will fix it in a V3.
>
>> +			if (confirm_key_prog()) {
>> +				if (mmc_rpmb_set_key(mmc, key_addr)) {
>> +					printf("ERROR - Key already programmed ?\n");
>> +					ret = CMD_RET_FAILURE;
>> +				}
>> +			} else {
>> +				ret = CMD_RET_FAILURE;
>> +			}
> You should really avoid deep nesting and take early exits.
> You can write this as:
>
> 			if (!confirm_key_prog())
> 				return CMD_RET_FAILURE;
>
> 			if (mmc_rpmb_set_key(mmc, key_addr)) {
> 				printf("ERROR - Key already programmed ?\n");
> 				ret = CMD_RET_FAILURE;
> 			}
>
> Please fix globally.
No problem, it will be fixed globally in V3
>
>> +		} else if (state == RPMB_COUNTER) {
>> +			unsigned long counter;
>> +			if (mmc_rpmb_get_counter(mmc, &counter))
> Please insert a blank line between declarations and code.
Ok
>
>> +			printf("%d RPMB blocks %s: %s\n",
>> +			       n, argv[2], (n == cnt) ? "OK" : "ERROR");
> As the input is in hex, it is usually also a good idea to (also) print
> the count in hex.
For coherency with the mmc read and mmc write, I kept the same output. 
But it can be changed, of course.
>
>>   #endif /* CONFIG_SUPPORT_EMMC_BOOT */
>> +#ifdef CONFIG_SUPPORT_EMMC_RPMB
>> +	} else if (strcmp(argv[1], "rpmb") == 0) {
>> +		return do_mmcrpmb(argc, argv);
>> +#endif /*  CONFIG_SUPPORT_EMMC_RPMB */
> I think that now, with more subcommands being added, we should
> convert the mmc code to proper subcommand handling. [It might even
> make sense to do so for "mmc rpmb", too.]
Do you think about the use of the macro U_BOOT_CMD_MKENT ?
>
> Best regards,
>
> Wolfgang Denk
>
Best regards
Pierre Aubert

[U-Boot] [PATCH V2 2/2] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Wolfgang Denk]

Dear Pierre,

In message <5350C8BC.9000607@staubli.com> you wrote:
> 
> > I think that now, with more subcommands being added, we should
> > convert the mmc code to proper subcommand handling. [It might even
> > make sense to do so for "mmc rpmb", too.]
> Do you think about the use of the macro U_BOOT_CMD_MKENT ?

Yes - see for example how the "env" sub-commands are implemented.

Best regards,

Wolfgang Denk

-- 
DENX Software Engineering GmbH,     MD: Wolfgang Denk & Detlev Zundel
HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd at denx.de
Never underestimate the power of human stupidity  when  it  comes  to
using technology they don't understand.

[U-Boot] [PATCH V3 0/3] eMMC: support for Read Protected Memory Block (RPMB)

[Pierre Aubert]

This serie of patches adds some functions and a sub-command of 'mmc' for  programming the authentication key and for reading and writing the RPMB partition of an eMMC according to the JEDEC standard No. 64-A441

The sub-command rpmb is enabled by the flag CONFIG_SUPPORT_EMMC_RPMB defined
in the board configuration file.
It has been tested on a SabreSDP iMX6 board.

Changes in V3:
- add entries in README for configuration options related to eMMC.
- new patch for adding the 'confirm_yesno' function as suggested by W.Denk
- improved test for existence of RPMB partition.
- fix of coding issues.

Changes in V2:
- use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the 
  RPMB frames.

Pierre Aubert (3):
  eMMC: add support for operations in RPMB partition
  Add the function 'confirm_yesno' for interactive confirmation.
  eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

 README               |   10 ++
 common/cmd_fuse.c    |   11 +--
 common/cmd_mmc.c     |  123 +++++++++++++++++++-
 common/cmd_nand.c    |   16 +--
 common/cmd_otp.c     |   18 +---
 common/console.c     |   28 ++++-
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/common.h     |    2 +-
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 11 files changed, 506 insertions(+), 39 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

-- 
1.7.6.5

[U-Boot] [PATCH V3 1/3] eMMC: add support for operations in RPMB partition

[Pierre Aubert]

This patch adds functions for read, write and authentication
key programming for the Replay Protected Memory Block partition
in the eMMC.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 4 files changed, 336 insertions(+), 1 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
index 931922b..4c6ab9e 100644
--- a/drivers/mmc/Makefile
+++ b/drivers/mmc/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_DWMMC) += dw_mmc.o
 obj-$(CONFIG_EXYNOS_DWMMC) += exynos_dw_mmc.o
 obj-$(CONFIG_ZYNQ_SDHCI) += zynq_sdhci.o
 obj-$(CONFIG_SOCFPGA_DWMMC) += socfpga_dw_mmc.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
 ifdef CONFIG_SPL_BUILD
 obj-$(CONFIG_SPL_MMC_BOOT) += fsl_esdhc_spl.o
 else
diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
new file mode 100644
index 0000000..05936f5
--- /dev/null
+++ b/drivers/mmc/rpmb.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright 2014, Staubli Faverges
+ * Pierre Aubert
+ *
+ * eMMC- Replay Protected Memory Block
+ * According to JEDEC Standard No. 84-A441
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <common.h>
+#include <mmc.h>
+#include <sha256.h>
+#include "mmc_private.h"
+
+/* Request codes */
+#define RPMB_REQ_KEY		1
+#define RPMB_REQ_WCOUNTER	2
+#define RPMB_REQ_WRITE_DATA	3
+#define RPMB_REQ_READ_DATA	4
+#define RPMB_REQ_STATUS		5
+
+/* Response code */
+#define RPMB_RESP_KEY		0x0100
+#define RPMB_RESP_WCOUNTER	0x0200
+#define RPMB_RESP_WRITE_DATA	0x0300
+#define RPMB_RESP_READ_DATA	0x0400
+
+/* Error codes */
+#define RPMB_OK			0
+#define RPMB_ERR_GENERAL	1
+#define RPMB_ERR_AUTH	2
+#define RPMB_ERR_COUNTER	3
+#define RPMB_ERR_ADDRESS	4
+#define RPMB_ERR_WRITE		5
+#define RPMB_ERR_READ		6
+#define RPMB_ERR_KEY		7
+#define RPMB_ERR_CNT_EXPIRED	0x80
+#define RPMB_ERR_MSK		0x7
+
+/* Sizes of RPMB data frame */
+#define RPMB_SZ_STUFF		196
+#define RPMB_SZ_MAC		32
+#define RPMB_SZ_DATA		256
+#define RPMB_SZ_NONCE		16
+
+#define SHA256_BLOCK_SIZE	64
+
+/* Error messages */
+static const char * const rpmb_err_msg[] = {
+	"",
+	"General failure",
+	"Authentication failure",
+	"Counter failure",
+	"Address failure",
+	"Write failure",
+	"Read failure",
+	"Authentication key not yet programmed",
+};
+
+
+/* Structure of RPMB data frame. */
+struct s_rpmb {
+	unsigned char stuff[RPMB_SZ_STUFF];
+	unsigned char mac[RPMB_SZ_MAC];
+	unsigned char data[RPMB_SZ_DATA];
+	unsigned char nonce[RPMB_SZ_NONCE];
+	unsigned long write_counter;
+	unsigned short address;
+	unsigned short block_count;
+	unsigned short result;
+	unsigned short request;
+};
+
+static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
+			      bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+
+	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
+	cmd.cmdarg = blockcount & 0x0000FFFF;
+	if (is_rel_write)
+		cmd.cmdarg |= 1 << 31;
+	cmd.resp_type = MMC_RSP_R1;
+
+	return mmc_send_cmd(mmc, &cmd, NULL);
+}
+static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
+			    unsigned int count, bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, count, is_rel_write);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+
+	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1b;
+
+	data.src = (const char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_WRITE;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+	return 0;
+}
+static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
+			     unsigned short expected)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, 1, false);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1;
+
+	data.dest = (char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_READ;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	/* Check the response and the status */
+	if (be16_to_cpu(s->request) != expected) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:response= %x\n", __func__,
+		       be16_to_cpu(s->request));
+#endif
+		return -1;
+	}
+	ret = be16_to_cpu(s->result);
+	if (ret) {
+		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
+		       (ret & RPMB_ERR_CNT_EXPIRED) ?
+		       "Write counter has expired" : "");
+	}
+
+	/* Return the status of the command */
+	return ret;
+}
+static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	return mmc_rpmb_response(mmc, rpmb_frame, expected);
+}
+static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
+		      unsigned char *output)
+{
+	sha256_context ctx;
+	int i;
+	unsigned char k_ipad[SHA256_BLOCK_SIZE];
+	unsigned char k_opad[SHA256_BLOCK_SIZE];
+
+	sha256_starts(&ctx);
+
+	/* According to RFC 4634, the HMAC transform looks like:
+	   SHA(K XOR opad, SHA(K XOR ipad, text))
+
+	   where K is an n byte key.
+	   ipad is the byte 0x36 repeated blocksize times
+	   opad is the byte 0x5c repeated blocksize times
+	   and text is the data being protected.
+	*/
+
+	for (i = 0; i < RPMB_SZ_MAC; i++) {
+		k_ipad[i] = key[i] ^ 0x36;
+		k_opad[i] = key[i] ^ 0x5c;
+	}
+	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
+	for ( ; i < SHA256_BLOCK_SIZE; i++) {
+		k_ipad[i] = 0x36;
+		k_opad[i] = 0x5c;
+	}
+	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
+	sha256_update(&ctx, buff, len);
+	sha256_finish(&ctx, output);
+
+	/* Init context for second pass */
+	sha256_starts(&ctx);
+
+	/* start with outer pad */
+	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
+
+	/* then results of 1st hash */
+	sha256_update(&ctx, output, RPMB_SZ_MAC);
+
+	/* finish up 2nd pass */
+	sha256_finish(&ctx, output);
+}
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
+{
+	int ret;
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER);
+	if (ret)
+		return ret;
+
+	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
+	return 0;
+}
+int mmc_rpmb_set_key(struct mmc *mmc, void *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
+	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);
+
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+		return -1;
+
+	/* read the operation status */
+	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
+}
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+			break;
+
+		/* Read the result */
+		if (mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_READ_DATA))
+			break;
+
+		/* Check the HMAC if key is provided */
+		if (key) {
+			unsigned char ret_hmac[RPMB_SZ_MAC];
+
+			rpmb_hmac(key, rpmb_frame->data, 284, ret_hmac);
+			if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
+				printf("MAC error on block #%d\n", i);
+				break;
+			}
+		}
+		/* Copy data */
+		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
+	}
+	return i;
+}
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	unsigned long wcount;
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		if (mmc_rpmb_get_counter(mmc, &wcount)) {
+			printf("Cannot read RPMB write counter\n");
+			break;
+		}
+
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->block_count = cpu_to_be16(1);
+		rpmb_frame->write_counter = cpu_to_be32(wcount);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
+		/* Computes HMAC */
+		rpmb_hmac(key, rpmb_frame->data, 284, rpmb_frame->mac);
+
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+			break;
+
+		/* Get status */
+		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
+			break;
+	}
+	return i;
+}
diff --git a/include/mmc.h b/include/mmc.h
index 42d0125..14d296c 100644
--- a/include/mmc.h
+++ b/include/mmc.h
@@ -69,6 +69,7 @@
 #define MMC_CMD_SET_BLOCKLEN		16
 #define MMC_CMD_READ_SINGLE_BLOCK	17
 #define MMC_CMD_READ_MULTIPLE_BLOCK	18
+#define MMC_CMD_SET_BLOCK_COUNT         23
 #define MMC_CMD_WRITE_SINGLE_BLOCK	24
 #define MMC_CMD_WRITE_MULTIPLE_BLOCK	25
 #define MMC_CMD_ERASE_GROUP_START	35
@@ -224,6 +225,7 @@
  * boot partitions (2), general purpose partitions (4) in MMC v4.4.
  */
 #define MMC_NUM_BOOT_PARTITION	2
+#define MMC_PART_RPMB           3       /* RPMB partition number */
 
 struct mmc_cid {
 	unsigned long psn;
@@ -335,7 +337,13 @@ int mmc_set_part_conf(struct mmc *mmc, u8 ack, u8 part_num, u8 access);
 int mmc_set_boot_bus_width(struct mmc *mmc, u8 width, u8 reset, u8 mode);
 /* Function to modify the RST_n_FUNCTION field of EXT_CSD */
 int mmc_set_rst_n_function(struct mmc *mmc, u8 enable);
-
+/* Functions to read / write the RPMB partition */
+int mmc_rpmb_set_key(struct mmc *mmc, void *key);
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *counter);
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key);
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		   unsigned short cnt, unsigned char *key);
 /**
  * Start device initialization and return immediately; it does not block on
  * polling OCR (operation condition register) status.  Then you should call
diff --git a/lib/Makefile b/lib/Makefile
index 27e4f78..2bf90f3 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -35,6 +35,9 @@ obj-y += net_utils.o
 obj-$(CONFIG_PHYSMEM) += physmem.o
 obj-y += qsort.o
 obj-$(CONFIG_SHA1) += sha1.o
+ifdef CONFIG_SUPPORT_EMMC_RPMB
+CONFIG_SHA256 := y
+endif
 obj-$(CONFIG_SHA256) += sha256.o
 obj-y	+= strmhz.o
 obj-$(CONFIG_TPM) += tpm.o
-- 
1.7.6.5

[U-Boot] [PATCH V3 2/3] Add the function 'confirm_yesno' for interactive confirmation.

[Pierre Aubert]

User's confirmation is asked in different commands. This commit adds a
function for such confirmation.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
---
 common/cmd_fuse.c |   11 ++---------
 common/cmd_nand.c |   16 +++++-----------
 common/cmd_otp.c  |   18 +++---------------
 common/console.c  |   28 +++++++++++++++++++++++++++-
 include/common.h  |    2 +-
 5 files changed, 38 insertions(+), 37 deletions(-)

diff --git a/common/cmd_fuse.c b/common/cmd_fuse.c
index 0df57db..abab978 100644
--- a/common/cmd_fuse.c
+++ b/common/cmd_fuse.c
@@ -33,15 +33,8 @@ static int confirm_prog(void)
 					"what you are doing!\n"
 			"\nReally perform this fuse programming? <y/N>\n");
 
-	if (getc() == 'y') {
-		int c;
-
-		putc('y');
-		c = getc();
-		putc('\n');
-		if (c == '\r')
-			return 1;
-	}
+	if (confirm_yesno())
+		return 1;
 
 	puts("Fuse programming aborted\n");
 	return 0;
diff --git a/common/cmd_nand.c b/common/cmd_nand.c
index 04ab0f1..a84f7dc 100644
--- a/common/cmd_nand.c
+++ b/common/cmd_nand.c
@@ -605,22 +605,16 @@ static int do_nand(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		opts.spread = spread;
 
 		if (scrub) {
-			if (!scrub_yes)
-				puts(scrub_warn);
-
-			if (scrub_yes)
+			if (scrub_yes) {
 				opts.scrub = 1;
-			else if (getc() == 'y') {
-				puts("y");
-				if (getc() == '\r')
+			} else {
+				puts(scrub_warn);
+				if (confirm_yesno()) {
 					opts.scrub = 1;
-				else {
+				} else {
 					puts("scrub aborted\n");
 					return 1;
 				}
-			} else {
-				puts("scrub aborted\n");
-				return 1;
 			}
 		}
 		ret = nand_erase_opts(nand, &opts);
diff --git a/common/cmd_otp.c b/common/cmd_otp.c
index 67808aa..593bb8c 100644
--- a/common/cmd_otp.c
+++ b/common/cmd_otp.c
@@ -158,21 +158,9 @@ int do_otp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 			lowup(half + count - 1), page + (half + count - 1) / 2,
 			half + count
 		);
-
-		i = 0;
-		while (1) {
-			if (tstc()) {
-				const char exp_ans[] = "YES\r";
-				char c;
-				putc(c = getc());
-				if (exp_ans[i++] != c) {
-					printf(" Aborting\n");
-					return 1;
-				} else if (!exp_ans[i]) {
-					puts("\n");
-					break;
-				}
-			}
+		if (!confirm_yesno()) {
+			printf(" Aborting\n");
+			return 1;
 		}
 	}
 
diff --git a/common/console.c b/common/console.c
index 2dfb788..5453726 100644
--- a/common/console.c
+++ b/common/console.c
@@ -537,7 +537,33 @@ int ctrlc(void)
 	}
 	return 0;
 }
-
+/* Reads user's confirmation.
+   Returns 1 if user's input is "y", "Y", "yes" or "YES"
+*/
+int confirm_yesno(void)
+{
+	int i;
+	char str_input[5];
+
+	/* Flush input */
+	while (tstc())
+		getc();
+	i = 0;
+	while (i < sizeof(str_input)) {
+		str_input[i] = getc();
+		putc(str_input[i]);
+		if (str_input[i] == '\r')
+			break;
+		i++;
+	}
+	putc('\n');
+	if (strncmp(str_input, "y\r", 2) == 0 ||
+	    strncmp(str_input, "Y\r", 2) == 0 ||
+	    strncmp(str_input, "yes\r", 4) == 0 ||
+	    strncmp(str_input, "YES\r", 4) == 0)
+		return 1;
+	return 0;
+}
 /* pass 1 to disable ctrlc() checking, 0 to enable.
  * returns previous state
  */
diff --git a/include/common.h b/include/common.h
index baf361b..1d922b9 100644
--- a/include/common.h
+++ b/include/common.h
@@ -838,7 +838,7 @@ int	ctrlc (void);
 int	had_ctrlc (void);	/* have we had a Control-C since last clear? */
 void	clear_ctrlc (void);	/* clear the Control-C condition */
 int	disable_ctrlc (int);	/* 1 to disable, 0 to enable Control-C detect */
-
+int confirm_yesno(void);        /*  1 if input is "y", "Y", "yes" or "YES" */
 /*
  * STDIO based functions (can always be used)
  */
-- 
1.7.6.5

[U-Boot] [PATCH V3 3/3] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Pierre Aubert]

This sub-command adds support for the RPMB partition of an eMMC:
* mmc rpmb key <address of the authentication key>
  Programs the authentication key in the eMMC This key can not
  be overwritten.
* mmc rpmb read <address> <block> <#count> [address of key]
  Reads <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. If the optionnal
  address of the authentication key is provided, the
  Message Authentication Code (MAC) is verified on each
  block.
* mmc rpmb write <address> <block> <#count> <address of key>
  Writes <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. The datas are signed
  with the key provided.
* mmc rpmb counter
  Returns the 'Write counter' of the RPMB partition.

The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 README           |   10 ++++
 common/cmd_mmc.c |  123 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 132 insertions(+), 1 deletions(-)

diff --git a/README b/README
index 52a92e7..518e209 100644
--- a/README
+++ b/README
@@ -1493,6 +1493,16 @@ The following options need to be configured:
 			CONFIG_SH_MMCIF_CLK
 			Define the clock frequency for MMCIF
 
+		CONFIG_GENERIC_MMC
+		Enable the generic MMC driver
+
+		CONFIG_SUPPORT_EMMC_BOOT
+		Enable some additional features of the eMMC boot partitions.
+
+		CONFIG_SUPPORT_EMMC_RPMB
+		Enable the commands for reading, writing and programming the
+		key for the Replay Protection Memory Block partition in eMMC.
+
 - USB Device Firmware Update (DFU) class support:
 		CONFIG_DFU_FUNCTION
 		This enables the USB portion of the DFU USB class
diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
index c1916c9..da8a243 100644
--- a/common/cmd_mmc.c
+++ b/common/cmd_mmc.c
@@ -130,7 +130,118 @@ U_BOOT_CMD(
 	"display MMC info",
 	"- display info of the current MMC device"
 );
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+static int confirm_key_prog(void)
+{
+	puts("Warning: Programming authentication key can be done only once !\n"
+	     "         Use this command only if you are sure of what you are doing,\n"
+	     "Really perform the key programming? <y/N> ");
+	if (confirm_yesno())
+		return 1;
+
+	puts("Authentication key programming aborted\n");
+	return 0;
+}
+static int do_mmcrpmb(int argc, char * const argv[])
+{
+	enum rpmb_state {
+		RPMB_INVALID,
+		RPMB_READ,
+		RPMB_WRITE,
+		RPMB_KEY,
+		RPMB_COUNTER,
+	} state;
+	struct mmc *mmc = find_mmc_device(curr_device);
+	void *key_addr;
+	char original_part;
+	int ret;
+
+	state = RPMB_INVALID;
+	if (argc == 4 && strcmp(argv[2], "key") == 0)
+		state = RPMB_KEY;
+	if ((argc == 6 || argc == 7) && strcmp(argv[2], "read") == 0)
+		state = RPMB_READ;
+	else if (argc == 7 && strcmp(argv[2], "write") == 0)
+		state = RPMB_WRITE;
+	else if (argc == 3 && strcmp(argv[2], "counter") == 0)
+		state = RPMB_COUNTER;
+
+	if (state == RPMB_INVALID)
+		return CMD_RET_USAGE;
 
+	if (!mmc) {
+		printf("no mmc device at slot %x\n", curr_device);
+		return CMD_RET_FAILURE;
+	}
+	mmc_init(mmc);
+	if (!(mmc->version & MMC_VERSION_MMC)) {
+		printf("It is not a EMMC device\n");
+		return CMD_RET_FAILURE;
+	}
+	if (mmc->version < MMC_VERSION_4_41) {
+		printf("RPMB not supported before version 4.41\n");
+		return CMD_RET_FAILURE;
+	}
+	/* Switch to the RPMB partition */
+	original_part = mmc->part_num;
+	if (mmc->part_num != MMC_PART_RPMB) {
+		if (mmc_switch_part(curr_device, MMC_PART_RPMB) != 0)
+			return CMD_RET_FAILURE;
+		mmc->part_num = MMC_PART_RPMB;
+	}
+	ret = CMD_RET_SUCCESS;
+	if (state == RPMB_KEY) {
+		key_addr = (void *)simple_strtoul(argv[3], NULL, 16);
+		if (!confirm_key_prog())
+			return CMD_RET_FAILURE;
+		if (mmc_rpmb_set_key(mmc, key_addr)) {
+			printf("ERROR - Key already programmed ?\n");
+			ret = CMD_RET_FAILURE;
+		}
+	} else if (state == RPMB_COUNTER) {
+		unsigned long counter;
+
+		if (mmc_rpmb_get_counter(mmc, &counter))
+			ret = CMD_RET_FAILURE;
+		else
+			printf("Write counter= %lx\n", counter);
+	} else {
+		u16 blk, cnt;
+		void *addr;
+		int n;
+
+		addr = (void *)simple_strtoul(argv[3], NULL, 16);
+		blk = simple_strtoul(argv[4], NULL, 16);
+		cnt = simple_strtoul(argv[5], NULL, 16);
+
+		if (state == RPMB_READ) {
+			key_addr = (argc == 7) ?
+				(void *)simple_strtoul(argv[6],
+						       NULL, 16) :
+				NULL;
+			n =  mmc_rpmb_read(mmc, addr, blk, cnt,
+					   key_addr);
+		} else {
+			key_addr = (void *)simple_strtoul(argv[6],
+							  NULL, 16);
+			n =  mmc_rpmb_write(mmc, addr, blk, cnt,
+					    key_addr);
+		}
+		printf("%d RPMB blocks %s: %s\n",
+		       n, argv[2], (n == cnt) ? "OK" : "ERROR");
+		if (n != cnt)
+			ret = CMD_RET_FAILURE;
+	}
+
+	/* Return to orginal partition */
+	if (mmc->part_num != original_part) {
+		if (mmc_switch_part(curr_device, original_part) != 0)
+			return CMD_RET_FAILURE;
+		mmc->part_num = original_part;
+	}
+	return ret;
+}
+#endif
 static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	enum mmc_state state;
@@ -365,6 +476,10 @@ static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 
 		return mmc_set_rst_n_function(mmc, enable);
 #endif /* CONFIG_SUPPORT_EMMC_BOOT */
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	} else if (strcmp(argv[1], "rpmb") == 0) {
+		return do_mmcrpmb(argc, argv);
+#endif /*  CONFIG_SUPPORT_EMMC_RPMB */
 	}
 
 	else if (argc == 3 && strcmp(argv[1], "setdsr") == 0) {
@@ -454,7 +569,7 @@ static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 }
 
 U_BOOT_CMD(
-	mmc, 6, 1, do_mmcops,
+	mmc, 7, 1, do_mmcops,
 	"MMC sub system",
 	"read addr blk# cnt\n"
 	"mmc write addr blk# cnt\n"
@@ -474,6 +589,12 @@ U_BOOT_CMD(
 	" - Change the RST_n_FUNCTION field of the specified device\n"
 	"   WARNING: This is a write-once field and 0 / 1 / 2 are the only valid values.\n"
 #endif
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	"mmc rpmb read addr blk# cnt [address of auth-key] - block size is 256 bytes\n"
+	"mmc rpmb write addr blk# cnt <address of auth-key> - block size is 256 bytes\n"
+	"mmc rpmb key <address of auth-key> - program the RPMB authentication key.\n"
+	"mmc rpmb counter - read the value of the write counter\n"
+#endif
 	"mmc setdsr - set DSR register value\n"
 	);
 #endif /* !CONFIG_GENERIC_MMC */
-- 
1.7.6.5

[U-Boot] [PATCH V3 3/3] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Wolfgang Denk]

Dear Pierre Aubert,

In message <1398182087-14913-4-git-send-email-p.aubert@staubli.com> you wrote:
> This sub-command adds support for the RPMB partition of an eMMC:
> * mmc rpmb key <address of the authentication key>
>   Programs the authentication key in the eMMC This key can not
>   be overwritten.
> * mmc rpmb read <address> <block> <#count> [address of key]
>   Reads <#count> blocks of 256 bytes in the RPMB partition
>   beginning at block number <block>. If the optionnal
>   address of the authentication key is provided, the
>   Message Authentication Code (MAC) is verified on each
>   block.
> * mmc rpmb write <address> <block> <#count> <address of key>
>   Writes <#count> blocks of 256 bytes in the RPMB partition
>   beginning at block number <block>. The datas are signed
>   with the key provided.
> * mmc rpmb counter
>   Returns the 'Write counter' of the RPMB partition.
> 
> The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB
> 
> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
> CC: Pantelis Antoniou <panto@antoniou-consulting.com>
> ---
>  README           |   10 ++++
>  common/cmd_mmc.c |  123 +++++++++++++++++++++++++++++++++++++++++++++++++++++-
>  2 files changed, 132 insertions(+), 1 deletions(-)

Unfortunatley there is no changelog here.  Please note that this is
mandatory for updated patch versions, see [1] 

[1] http://www.denx.de/wiki/view/U-Boot/Patches#Sending_updated_patch_versions

A changelog in the cover letter is useless, as this does not get
recorded in patchwork.  The changelog must always be in the patch
itself.


...
> +	if (argc == 4 && strcmp(argv[2], "key") == 0)
> +		state = RPMB_KEY;
> +	if ((argc == 6 || argc == 7) && strcmp(argv[2], "read") == 0)
> +		state = RPMB_READ;
> +	else if (argc == 7 && strcmp(argv[2], "write") == 0)
> +		state = RPMB_WRITE;
> +	else if (argc == 3 && strcmp(argv[2], "counter") == 0)
> +		state = RPMB_COUNTER;

As mentiuoned before, these repeated strcmp() should probably be
replaced by a subcommand table.

> +	ret = CMD_RET_SUCCESS;
> +	if (state == RPMB_KEY) {
> +		key_addr = (void *)simple_strtoul(argv[3], NULL, 16);
> +		if (!confirm_key_prog())
> +			return CMD_RET_FAILURE;
> +		if (mmc_rpmb_set_key(mmc, key_addr)) {
> +			printf("ERROR - Key already programmed ?\n");
> +			ret = CMD_RET_FAILURE;

Do a return here, or use some "goto errout" and place the label errout
before the needed cleanup acrtions.

> +		}
> +	} else if (state == RPMB_COUNTER) {

You can then get rid of this "else" level.

> +		unsigned long counter;
> +
> +		if (mmc_rpmb_get_counter(mmc, &counter))
> +			ret = CMD_RET_FAILURE;
> +		else

Ditto.

> +			printf("Write counter= %lx\n", counter);

Should this be a debug() ?

> +#ifdef CONFIG_SUPPORT_EMMC_RPMB
> +	} else if (strcmp(argv[1], "rpmb") == 0) {
> +		return do_mmcrpmb(argc, argv);
> +#endif /*  CONFIG_SUPPORT_EMMC_RPMB */
>  	}

Again, it would be better to switch the code to regular subcommand
processing.

Best regards,

Wolfgang Denk

-- 
DENX Software Engineering GmbH,     MD: Wolfgang Denk & Detlev Zundel
HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd at denx.de
"I knew then (in 1970) that a 4-kbyte minicomputer would cost as much
as a house. So I reasoned  that  after  college,  I'd  have  to  live
cheaply in an apartment and put all my money into owning a computer."
      - Apple co-founder Steve Wozniak, EE Times, June 6, 1988, pg 45

[U-Boot] [PATCH V4 0/3] eMMC: support for Read Protected Memory Block (RPMB)

[Pierre Aubert]

This serie of patches adds some functions and a sub-command of 'mmc' for  programming the authentication key and for reading and writing the RPMB partition of an eMMC according to the JEDEC standard No. 64-A441

The sub-command rpmb is enabled by the flag CONFIG_SUPPORT_EMMC_RPMB defined
in the board configuration file.
It has been tested on a SabreSDP iMX6 board.

Changes in V4:
- use subcommand tables for 'mmc' command and 'mmc rpmb' subcommand.
- add 'mmc info' subcommand, which is the same as mmcinfo.
  mmcinfo is kept for compatibility.
- add changelog in commit comments.

Changes in V3:
- add entries in README for configuration options related to eMMC.
- new patch for adding the 'confirm_yesno' function as suggested by W.Denk
- improved test for existence of RPMB partition.
- fix of coding issues.

Changes in V2:
- use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the 
  RPMB frames.

Pierre Aubert (3):
  eMMC: add support for operations in RPMB partition
  Add the function 'confirm_yesno' for interactive
  eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

 README               |   10 +
 common/cmd_fuse.c    |   11 +-
 common/cmd_mmc.c     |  806 +++++++++++++++++++++++++++++++------------------
 common/cmd_nand.c    |   16 +-
 common/cmd_otp.c     |   18 +-
 common/console.c     |   28 ++-
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++
 include/common.h     |    2 +-
 include/mmc.h        |   10 +-
 lib/Makefile         |    3 +
 11 files changed, 894 insertions(+), 334 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

-- 
1.7.6.5

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Pierre Aubert]

This patch adds functions for read, write and authentication
key programming for the Replay Protected Memory Block partition
in the eMMC.

Changes in V2:
- use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the
  RPMB frames.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    3 +
 4 files changed, 336 insertions(+), 1 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
index 931922b..4c6ab9e 100644
--- a/drivers/mmc/Makefile
+++ b/drivers/mmc/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_DWMMC) += dw_mmc.o
 obj-$(CONFIG_EXYNOS_DWMMC) += exynos_dw_mmc.o
 obj-$(CONFIG_ZYNQ_SDHCI) += zynq_sdhci.o
 obj-$(CONFIG_SOCFPGA_DWMMC) += socfpga_dw_mmc.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
 ifdef CONFIG_SPL_BUILD
 obj-$(CONFIG_SPL_MMC_BOOT) += fsl_esdhc_spl.o
 else
diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
new file mode 100644
index 0000000..05936f5
--- /dev/null
+++ b/drivers/mmc/rpmb.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright 2014, Staubli Faverges
+ * Pierre Aubert
+ *
+ * eMMC- Replay Protected Memory Block
+ * According to JEDEC Standard No. 84-A441
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <common.h>
+#include <mmc.h>
+#include <sha256.h>
+#include "mmc_private.h"
+
+/* Request codes */
+#define RPMB_REQ_KEY		1
+#define RPMB_REQ_WCOUNTER	2
+#define RPMB_REQ_WRITE_DATA	3
+#define RPMB_REQ_READ_DATA	4
+#define RPMB_REQ_STATUS		5
+
+/* Response code */
+#define RPMB_RESP_KEY		0x0100
+#define RPMB_RESP_WCOUNTER	0x0200
+#define RPMB_RESP_WRITE_DATA	0x0300
+#define RPMB_RESP_READ_DATA	0x0400
+
+/* Error codes */
+#define RPMB_OK			0
+#define RPMB_ERR_GENERAL	1
+#define RPMB_ERR_AUTH	2
+#define RPMB_ERR_COUNTER	3
+#define RPMB_ERR_ADDRESS	4
+#define RPMB_ERR_WRITE		5
+#define RPMB_ERR_READ		6
+#define RPMB_ERR_KEY		7
+#define RPMB_ERR_CNT_EXPIRED	0x80
+#define RPMB_ERR_MSK		0x7
+
+/* Sizes of RPMB data frame */
+#define RPMB_SZ_STUFF		196
+#define RPMB_SZ_MAC		32
+#define RPMB_SZ_DATA		256
+#define RPMB_SZ_NONCE		16
+
+#define SHA256_BLOCK_SIZE	64
+
+/* Error messages */
+static const char * const rpmb_err_msg[] = {
+	"",
+	"General failure",
+	"Authentication failure",
+	"Counter failure",
+	"Address failure",
+	"Write failure",
+	"Read failure",
+	"Authentication key not yet programmed",
+};
+
+
+/* Structure of RPMB data frame. */
+struct s_rpmb {
+	unsigned char stuff[RPMB_SZ_STUFF];
+	unsigned char mac[RPMB_SZ_MAC];
+	unsigned char data[RPMB_SZ_DATA];
+	unsigned char nonce[RPMB_SZ_NONCE];
+	unsigned long write_counter;
+	unsigned short address;
+	unsigned short block_count;
+	unsigned short result;
+	unsigned short request;
+};
+
+static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
+			      bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+
+	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
+	cmd.cmdarg = blockcount & 0x0000FFFF;
+	if (is_rel_write)
+		cmd.cmdarg |= 1 << 31;
+	cmd.resp_type = MMC_RSP_R1;
+
+	return mmc_send_cmd(mmc, &cmd, NULL);
+}
+static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
+			    unsigned int count, bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, count, is_rel_write);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+
+	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1b;
+
+	data.src = (const char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_WRITE;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+	return 0;
+}
+static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
+			     unsigned short expected)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, 1, false);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1;
+
+	data.dest = (char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_READ;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	/* Check the response and the status */
+	if (be16_to_cpu(s->request) != expected) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:response= %x\n", __func__,
+		       be16_to_cpu(s->request));
+#endif
+		return -1;
+	}
+	ret = be16_to_cpu(s->result);
+	if (ret) {
+		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
+		       (ret & RPMB_ERR_CNT_EXPIRED) ?
+		       "Write counter has expired" : "");
+	}
+
+	/* Return the status of the command */
+	return ret;
+}
+static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	return mmc_rpmb_response(mmc, rpmb_frame, expected);
+}
+static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
+		      unsigned char *output)
+{
+	sha256_context ctx;
+	int i;
+	unsigned char k_ipad[SHA256_BLOCK_SIZE];
+	unsigned char k_opad[SHA256_BLOCK_SIZE];
+
+	sha256_starts(&ctx);
+
+	/* According to RFC 4634, the HMAC transform looks like:
+	   SHA(K XOR opad, SHA(K XOR ipad, text))
+
+	   where K is an n byte key.
+	   ipad is the byte 0x36 repeated blocksize times
+	   opad is the byte 0x5c repeated blocksize times
+	   and text is the data being protected.
+	*/
+
+	for (i = 0; i < RPMB_SZ_MAC; i++) {
+		k_ipad[i] = key[i] ^ 0x36;
+		k_opad[i] = key[i] ^ 0x5c;
+	}
+	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
+	for ( ; i < SHA256_BLOCK_SIZE; i++) {
+		k_ipad[i] = 0x36;
+		k_opad[i] = 0x5c;
+	}
+	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
+	sha256_update(&ctx, buff, len);
+	sha256_finish(&ctx, output);
+
+	/* Init context for second pass */
+	sha256_starts(&ctx);
+
+	/* start with outer pad */
+	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
+
+	/* then results of 1st hash */
+	sha256_update(&ctx, output, RPMB_SZ_MAC);
+
+	/* finish up 2nd pass */
+	sha256_finish(&ctx, output);
+}
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
+{
+	int ret;
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER);
+	if (ret)
+		return ret;
+
+	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
+	return 0;
+}
+int mmc_rpmb_set_key(struct mmc *mmc, void *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
+	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);
+
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+		return -1;
+
+	/* read the operation status */
+	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
+}
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+			break;
+
+		/* Read the result */
+		if (mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_READ_DATA))
+			break;
+
+		/* Check the HMAC if key is provided */
+		if (key) {
+			unsigned char ret_hmac[RPMB_SZ_MAC];
+
+			rpmb_hmac(key, rpmb_frame->data, 284, ret_hmac);
+			if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
+				printf("MAC error on block #%d\n", i);
+				break;
+			}
+		}
+		/* Copy data */
+		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
+	}
+	return i;
+}
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	unsigned long wcount;
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		if (mmc_rpmb_get_counter(mmc, &wcount)) {
+			printf("Cannot read RPMB write counter\n");
+			break;
+		}
+
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->block_count = cpu_to_be16(1);
+		rpmb_frame->write_counter = cpu_to_be32(wcount);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
+		/* Computes HMAC */
+		rpmb_hmac(key, rpmb_frame->data, 284, rpmb_frame->mac);
+
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+			break;
+
+		/* Get status */
+		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
+			break;
+	}
+	return i;
+}
diff --git a/include/mmc.h b/include/mmc.h
index 42d0125..14d296c 100644
--- a/include/mmc.h
+++ b/include/mmc.h
@@ -69,6 +69,7 @@
 #define MMC_CMD_SET_BLOCKLEN		16
 #define MMC_CMD_READ_SINGLE_BLOCK	17
 #define MMC_CMD_READ_MULTIPLE_BLOCK	18
+#define MMC_CMD_SET_BLOCK_COUNT         23
 #define MMC_CMD_WRITE_SINGLE_BLOCK	24
 #define MMC_CMD_WRITE_MULTIPLE_BLOCK	25
 #define MMC_CMD_ERASE_GROUP_START	35
@@ -224,6 +225,7 @@
  * boot partitions (2), general purpose partitions (4) in MMC v4.4.
  */
 #define MMC_NUM_BOOT_PARTITION	2
+#define MMC_PART_RPMB           3       /* RPMB partition number */
 
 struct mmc_cid {
 	unsigned long psn;
@@ -335,7 +337,13 @@ int mmc_set_part_conf(struct mmc *mmc, u8 ack, u8 part_num, u8 access);
 int mmc_set_boot_bus_width(struct mmc *mmc, u8 width, u8 reset, u8 mode);
 /* Function to modify the RST_n_FUNCTION field of EXT_CSD */
 int mmc_set_rst_n_function(struct mmc *mmc, u8 enable);
-
+/* Functions to read / write the RPMB partition */
+int mmc_rpmb_set_key(struct mmc *mmc, void *key);
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *counter);
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key);
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		   unsigned short cnt, unsigned char *key);
 /**
  * Start device initialization and return immediately; it does not block on
  * polling OCR (operation condition register) status.  Then you should call
diff --git a/lib/Makefile b/lib/Makefile
index 27e4f78..2bf90f3 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -35,6 +35,9 @@ obj-y += net_utils.o
 obj-$(CONFIG_PHYSMEM) += physmem.o
 obj-y += qsort.o
 obj-$(CONFIG_SHA1) += sha1.o
+ifdef CONFIG_SUPPORT_EMMC_RPMB
+CONFIG_SHA256 := y
+endif
 obj-$(CONFIG_SHA256) += sha256.o
 obj-y	+= strmhz.o
 obj-$(CONFIG_TPM) += tpm.o
-- 
1.7.6.5

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Wolfgang Denk]

Dear Pierre Aubert,

In message <1398321641-7113-2-git-send-email-p.aubert@staubli.com> you wrote:
> This patch adds functions for read, write and authentication
> key programming for the Replay Protected Memory Block partition
> in the eMMC.
> 
> Changes in V2:
> - use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the
>   RPMB frames.

The changelog goes into the comment section (i. e. below the "---"
line), not into the commit message.  Also, this being V4 of the patch,
I wonder what was changed in V3 and V4?

> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
> CC: Pantelis Antoniou <panto@antoniou-consulting.com>
> ---

Add changelog here!

>  drivers/mmc/Makefile |    1 +
>  drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
>  include/mmc.h        |   10 ++-
>  lib/Makefile         |    3 +
>  4 files changed, 336 insertions(+), 1 deletions(-)
>  create mode 100644 drivers/mmc/rpmb.c


Best regards,

Wolfgang Denk

-- 
DENX Software Engineering GmbH,     MD: Wolfgang Denk & Detlev Zundel
HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd at denx.de
It usually takes more than three weeks to prepare  a  good  impromptu
speech.                                                  - Mark Twain

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Pierre AUBERT]

Hello Wolfgang,

Le 24/04/2014 08:55, Wolfgang Denk a ?crit :
> Dear Pierre Aubert,
>
> In message <1398321641-7113-2-git-send-email-p.aubert@staubli.com> you wrote:
>> This patch adds functions for read, write and authentication
>> key programming for the Replay Protected Memory Block partition
>> in the eMMC.
>>
>> Changes in V2:
>> - use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the
>>    RPMB frames.
> The changelog goes into the comment section (i. e. below the "---"
I will fix it in V5. I made the same mistake for the patch 3/3.
> line), not into the commit message.  Also, this being V4 of the patch,
> I wonder what was changed in V3 and V4?
There's no change for this patch in V3 and V4. I have resubmitted all 
the patchset.
>
>> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
>> CC: Pantelis Antoniou <panto@antoniou-consulting.com>
>> ---
> Add changelog here!
Ok
>
>>   drivers/mmc/Makefile |    1 +
>>   drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
>>   include/mmc.h        |   10 ++-
>>   lib/Makefile         |    3 +
>>   4 files changed, 336 insertions(+), 1 deletions(-)
>>   create mode 100644 drivers/mmc/rpmb.c
>
> Best regards,
>
> Wolfgang Denk
>
Best regards
Pierre Aubert

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Wolfgang Denk]

Dear Pierre,

In message <5358BA6A.3030604@staubli.com> you wrote:
> 
> > The changelog goes into the comment section (i. e. below the "---"
> I will fix it in V5. I made the same mistake for the patch 3/3.

Thanks.

Actually I cannot see a V4 of patch 3/3 ?

> > line), not into the commit message.  Also, this being V4 of the patch,
> > I wonder what was changed in V3 and V4?
> There's no change for this patch in V3 and V4. I have resubmitted all 
> the patchset.

Then the changelog should say so (like: "V3, V4: no changes").

Best regards,

Wolfgang Denk

-- 
DENX Software Engineering GmbH,     MD: Wolfgang Denk & Detlev Zundel
HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd at denx.de
"A little knowledge is a dangerous thing."                - Doug Gwyn

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Pierre AUBERT]

Hello Wolfgang,

Le 24/04/2014 09:33, Wolfgang Denk a ?crit :
> Dear Pierre,
>
> In message <5358BA6A.3030604@staubli.com> you wrote:
>>> The changelog goes into the comment section (i. e. below the "---"
>> I will fix it in V5. I made the same mistake for the patch 3/3.
> Thanks.
>
> Actually I cannot see a V4 of patch 3/3 ?
It's strange. On my side, I can see it in the mailing list and in patchwork.
>
>>> line), not into the commit message.  Also, this being V4 of the patch,
>>> I wonder what was changed in V3 and V4?
>> There's no change for this patch in V3 and V4. I have resubmitted all
>> the patchset.
> Then the changelog should say so (like: "V3, V4: no changes").
Ok, I will fix it.
>
> Best regards,
>
> Wolfgang Denk
>
Best regards
Pierre

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Wolfgang Denk]

Dear Pierre.

In message <1398321641-7113-2-git-send-email-p.aubert@staubli.com> you wrote:
>
...
> --- a/lib/Makefile
> +++ b/lib/Makefile
> @@ -35,6 +35,9 @@ obj-y += net_utils.o
>  obj-$(CONFIG_PHYSMEM) += physmem.o
>  obj-y += qsort.o
>  obj-$(CONFIG_SHA1) += sha1.o
> +ifdef CONFIG_SUPPORT_EMMC_RPMB
> +CONFIG_SHA256 := y
> +endif

This is a pretty ugly way to tweak the configuration.  Why do we need
this?

I gess all you really want is

	obj-$(CONFIG_SUPPORT_EMMC_RPMB) += sha256.o

?

Best regards,

Wolfgang Denk

-- 
DENX Software Engineering GmbH,     MD: Wolfgang Denk & Detlev Zundel
HRB 165235 Munich, Office: Kirchenstr.5, D-82194 Groebenzell, Germany
Phone: (+49)-8142-66989-10 Fax: (+49)-8142-66989-80 Email: wd at denx.de
PLEASE NOTE: Some Quantum Physics Theories Suggest That When the Con-
sumer Is Not Directly Observing This Product, It May Cease  to  Exist
or Will Exist Only in a Vague and Undetermined State.

[U-Boot] [PATCH V4 1/3] eMMC: add support for operations in RPMB partition

[Pierre AUBERT]

Hello Wolfgang,

Le 24/04/2014 08:59, Wolfgang Denk a ?crit :
> Dear Pierre.
>
> In message <1398321641-7113-2-git-send-email-p.aubert@staubli.com> you wrote:
> ...
>> --- a/lib/Makefile
>> +++ b/lib/Makefile
>> @@ -35,6 +35,9 @@ obj-y += net_utils.o
>>   obj-$(CONFIG_PHYSMEM) += physmem.o
>>   obj-y += qsort.o
>>   obj-$(CONFIG_SHA1) += sha1.o
>> +ifdef CONFIG_SUPPORT_EMMC_RPMB
>> +CONFIG_SHA256 := y
>> +endif
> This is a pretty ugly way to tweak the configuration.  Why do we need
> this?
>
> I gess all you really want is
>
> 	obj-$(CONFIG_SUPPORT_EMMC_RPMB) += sha256.o
>
> ?
Yes. I will fix it.
> Best regards,
>
> Wolfgang Denk
>
Best regards
Pierre Aubert

[U-Boot] [PATCH V4 2/3] Add the function 'confirm_yesno' for interactive

[Pierre Aubert]

User's confirmation is asked in different commands. This commit adds a
function for such confirmation.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
---
 common/cmd_fuse.c |   11 ++---------
 common/cmd_nand.c |   16 +++++-----------
 common/cmd_otp.c  |   18 +++---------------
 common/console.c  |   28 +++++++++++++++++++++++++++-
 include/common.h  |    2 +-
 5 files changed, 38 insertions(+), 37 deletions(-)

diff --git a/common/cmd_fuse.c b/common/cmd_fuse.c
index 0df57db..abab978 100644
--- a/common/cmd_fuse.c
+++ b/common/cmd_fuse.c
@@ -33,15 +33,8 @@ static int confirm_prog(void)
 					"what you are doing!\n"
 			"\nReally perform this fuse programming? <y/N>\n");
 
-	if (getc() == 'y') {
-		int c;
-
-		putc('y');
-		c = getc();
-		putc('\n');
-		if (c == '\r')
-			return 1;
-	}
+	if (confirm_yesno())
+		return 1;
 
 	puts("Fuse programming aborted\n");
 	return 0;
diff --git a/common/cmd_nand.c b/common/cmd_nand.c
index 04ab0f1..a84f7dc 100644
--- a/common/cmd_nand.c
+++ b/common/cmd_nand.c
@@ -605,22 +605,16 @@ static int do_nand(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		opts.spread = spread;
 
 		if (scrub) {
-			if (!scrub_yes)
-				puts(scrub_warn);
-
-			if (scrub_yes)
+			if (scrub_yes) {
 				opts.scrub = 1;
-			else if (getc() == 'y') {
-				puts("y");
-				if (getc() == '\r')
+			} else {
+				puts(scrub_warn);
+				if (confirm_yesno()) {
 					opts.scrub = 1;
-				else {
+				} else {
 					puts("scrub aborted\n");
 					return 1;
 				}
-			} else {
-				puts("scrub aborted\n");
-				return 1;
 			}
 		}
 		ret = nand_erase_opts(nand, &opts);
diff --git a/common/cmd_otp.c b/common/cmd_otp.c
index 67808aa..593bb8c 100644
--- a/common/cmd_otp.c
+++ b/common/cmd_otp.c
@@ -158,21 +158,9 @@ int do_otp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 			lowup(half + count - 1), page + (half + count - 1) / 2,
 			half + count
 		);
-
-		i = 0;
-		while (1) {
-			if (tstc()) {
-				const char exp_ans[] = "YES\r";
-				char c;
-				putc(c = getc());
-				if (exp_ans[i++] != c) {
-					printf(" Aborting\n");
-					return 1;
-				} else if (!exp_ans[i]) {
-					puts("\n");
-					break;
-				}
-			}
+		if (!confirm_yesno()) {
+			printf(" Aborting\n");
+			return 1;
 		}
 	}
 
diff --git a/common/console.c b/common/console.c
index 2dfb788..5453726 100644
--- a/common/console.c
+++ b/common/console.c
@@ -537,7 +537,33 @@ int ctrlc(void)
 	}
 	return 0;
 }
-
+/* Reads user's confirmation.
+   Returns 1 if user's input is "y", "Y", "yes" or "YES"
+*/
+int confirm_yesno(void)
+{
+	int i;
+	char str_input[5];
+
+	/* Flush input */
+	while (tstc())
+		getc();
+	i = 0;
+	while (i < sizeof(str_input)) {
+		str_input[i] = getc();
+		putc(str_input[i]);
+		if (str_input[i] == '\r')
+			break;
+		i++;
+	}
+	putc('\n');
+	if (strncmp(str_input, "y\r", 2) == 0 ||
+	    strncmp(str_input, "Y\r", 2) == 0 ||
+	    strncmp(str_input, "yes\r", 4) == 0 ||
+	    strncmp(str_input, "YES\r", 4) == 0)
+		return 1;
+	return 0;
+}
 /* pass 1 to disable ctrlc() checking, 0 to enable.
  * returns previous state
  */
diff --git a/include/common.h b/include/common.h
index baf361b..1d922b9 100644
--- a/include/common.h
+++ b/include/common.h
@@ -838,7 +838,7 @@ int	ctrlc (void);
 int	had_ctrlc (void);	/* have we had a Control-C since last clear? */
 void	clear_ctrlc (void);	/* clear the Control-C condition */
 int	disable_ctrlc (int);	/* 1 to disable, 0 to enable Control-C detect */
-
+int confirm_yesno(void);        /*  1 if input is "y", "Y", "yes" or "YES" */
 /*
  * STDIO based functions (can always be used)
  */
-- 
1.7.6.5

[U-Boot] [PATCH V4 3/3] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Pierre Aubert]

This sub-command adds support for the RPMB partition of an eMMC:
* mmc rpmb key <address of the authentication key>
  Programs the authentication key in the eMMC This key can not
  be overwritten.
* mmc rpmb read <address> <block> <#count> [address of key]
  Reads <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. If the optionnal
  address of the authentication key is provided, the
  Message Authentication Code (MAC) is verified on each
  block.
* mmc rpmb write <address> <block> <#count> <address of key>
  Writes <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. The datas are signed
  with the key provided.
* mmc rpmb counter
  Returns the 'Write counter' of the RPMB partition.

The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB

Changes in V4:
- use subcommand table for 'mmc' command and 'mmc rpmb' subcommand.
- add 'mmc info' subcommand, which is the same as mmcinfo.
  mmcinfo is kept for compatibility.
- add changelog in commit comments.

Changes in V3:
- add entries in README for configuration options related to eMMC.
- improved test for existence of RPMB partition.
- fix of coding issues.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
CC: Wolfgang Denk <wd@denx.de>
---
 README           |   10 +
 common/cmd_mmc.c |  806 ++++++++++++++++++++++++++++++++++--------------------
 2 files changed, 520 insertions(+), 296 deletions(-)

diff --git a/README b/README
index 52a92e7..518e209 100644
--- a/README
+++ b/README
@@ -1493,6 +1493,16 @@ The following options need to be configured:
 			CONFIG_SH_MMCIF_CLK
 			Define the clock frequency for MMCIF
 
+		CONFIG_GENERIC_MMC
+		Enable the generic MMC driver
+
+		CONFIG_SUPPORT_EMMC_BOOT
+		Enable some additional features of the eMMC boot partitions.
+
+		CONFIG_SUPPORT_EMMC_RPMB
+		Enable the commands for reading, writing and programming the
+		key for the Replay Protection Memory Block partition in eMMC.
+
 - USB Device Firmware Update (DFU) class support:
 		CONFIG_DFU_FUNCTION
 		This enables the USB portion of the DFU USB class
diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
index c1916c9..55af295 100644
--- a/common/cmd_mmc.c
+++ b/common/cmd_mmc.c
@@ -71,12 +71,6 @@ U_BOOT_CMD(
 );
 #else /* !CONFIG_GENERIC_MMC */
 
-enum mmc_state {
-	MMC_INVALID,
-	MMC_READ,
-	MMC_WRITE,
-	MMC_ERASE,
-};
 static void print_mmcinfo(struct mmc *mmc)
 {
 	printf("Device: %s\n", mmc->cfg->name);
@@ -98,7 +92,18 @@ static void print_mmcinfo(struct mmc *mmc)
 
 	printf("Bus Width: %d-bit\n", mmc->bus_width);
 }
-
+static struct mmc *init_mmc_device(int dev)
+{
+	struct mmc *mmc;
+	mmc = find_mmc_device(dev);
+	if (!mmc) {
+		printf("no mmc device at slot %x\n", dev);
+		return NULL;
+	}
+	if (mmc_init(mmc))
+		return NULL;
+	return mmc;
+}
 static int do_mmcinfo(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	struct mmc *mmc;
@@ -112,351 +117,546 @@ static int do_mmcinfo(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		}
 	}
 
-	mmc = find_mmc_device(curr_device);
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-	if (mmc) {
-		mmc_init(mmc);
+	print_mmcinfo(mmc);
+	return CMD_RET_SUCCESS;
+}
 
-		print_mmcinfo(mmc);
-		return 0;
-	} else {
-		printf("no mmc device at slot %x\n", curr_device);
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+static int confirm_key_prog(void)
+{
+	puts("Warning: Programming authentication key can be done only once !\n"
+	     "         Use this command only if you are sure of what you are doing,\n"
+	     "Really perform the key programming? <y/N> ");
+	if (confirm_yesno())
 		return 1;
+
+	puts("Authentication key programming aborted\n");
+	return 0;
+}
+static int do_mmcrpmb_key(cmd_tbl_t *cmdtp, int flag,
+			  int argc, char * const argv[])
+{
+	void *key_addr;
+	struct mmc *mmc = find_mmc_device(curr_device);
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+
+	key_addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	if (!confirm_key_prog())
+		return CMD_RET_FAILURE;
+	if (mmc_rpmb_set_key(mmc, key_addr)) {
+		printf("ERROR - Key already programmed ?\n");
+		return CMD_RET_FAILURE;
 	}
+	return CMD_RET_SUCCESS;
 }
+static int do_mmcrpmb_read(cmd_tbl_t *cmdtp, int flag,
+			   int argc, char * const argv[])
+{
+	u16 blk, cnt;
+	void *addr;
+	int n;
+	void *key_addr = NULL;
+	struct mmc *mmc = find_mmc_device(curr_device);
 
-U_BOOT_CMD(
-	mmcinfo, 1, 0, do_mmcinfo,
-	"display MMC info",
-	"- display info of the current MMC device"
-);
+	if (argc < 4)
+		return CMD_RET_USAGE;
 
-static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
+
+	if (argc == 5)
+		key_addr = (void *)simple_strtoul(argv[4], NULL, 16);
+
+	printf("\nMMC RPMB read: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
+	n =  mmc_rpmb_read(mmc, addr, blk, cnt, key_addr);
+
+	printf("%d RPMB blocks read: %s\n", n, (n == cnt) ? "OK" : "ERROR");
+	if (n != cnt)
+		return CMD_RET_FAILURE;
+	return CMD_RET_SUCCESS;
+}
+static int do_mmcrpmb_write(cmd_tbl_t *cmdtp, int flag,
+			    int argc, char * const argv[])
 {
-	enum mmc_state state;
+	u16 blk, cnt;
+	void *addr;
+	int n;
+	void *key_addr;
+	struct mmc *mmc = find_mmc_device(curr_device);
 
-	if (argc < 2)
+	if (argc != 5)
 		return CMD_RET_USAGE;
 
-	if (curr_device < 0) {
-		if (get_mmc_num() > 0)
-			curr_device = 0;
-		else {
-			puts("No MMC device available\n");
-			return 1;
-		}
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
+	key_addr = (void *)simple_strtoul(argv[4], NULL, 16);
+
+	printf("\nMMC RPMB write: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
+	n =  mmc_rpmb_write(mmc, addr, blk, cnt, key_addr);
+
+	printf("%d RPMB blocks written: %s\n", n, (n == cnt) ? "OK" : "ERROR");
+	if (n != cnt)
+		return CMD_RET_FAILURE;
+	return CMD_RET_SUCCESS;
+}
+static int do_mmcrpmb_counter(cmd_tbl_t *cmdtp, int flag,
+			      int argc, char * const argv[])
+{
+	unsigned long counter;
+	struct mmc *mmc = find_mmc_device(curr_device);
+
+	if (mmc_rpmb_get_counter(mmc, &counter))
+		return CMD_RET_FAILURE;
+	printf("RPMB Write counter= %lx\n", counter);
+	return CMD_RET_SUCCESS;
+}
+
+static cmd_tbl_t cmd_rpmb[] = {
+	U_BOOT_CMD_MKENT(key, 2, 0, do_mmcrpmb_key, "", ""),
+	U_BOOT_CMD_MKENT(read, 5, 1, do_mmcrpmb_read, "", ""),
+	U_BOOT_CMD_MKENT(write, 5, 0, do_mmcrpmb_write, "", ""),
+	U_BOOT_CMD_MKENT(counter, 1, 1, do_mmcrpmb_counter, "", ""),
+};
+
+static int do_mmcrpmb(cmd_tbl_t *cmdtp, int flag,
+		      int argc, char * const argv[])
+{
+	cmd_tbl_t *cp;
+	struct mmc *mmc;
+	char original_part;
+	int ret;
+
+	cp = find_cmd_tbl(argv[1], cmd_rpmb, ARRAY_SIZE(cmd_rpmb));
+
+	/* Drop the rpmb subcommand */
+	argc--;
+	argv++;
+
+	if (cp == NULL || argc > cp->maxargs)
+		return CMD_RET_USAGE;
+	if (flag == CMD_FLAG_REPEAT && !cp->repeatable)
+		return CMD_RET_SUCCESS;
+
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (!(mmc->version & MMC_VERSION_MMC)) {
+		printf("It is not a EMMC device\n");
+		return CMD_RET_FAILURE;
+	}
+	if (mmc->version < MMC_VERSION_4_41) {
+		printf("RPMB not supported before version 4.41\n");
+		return CMD_RET_FAILURE;
 	}
+	/* Switch to the RPMB partition */
+	original_part = mmc->part_num;
+	if (mmc->part_num != MMC_PART_RPMB) {
+		if (mmc_switch_part(curr_device, MMC_PART_RPMB) != 0)
+			return CMD_RET_FAILURE;
+		mmc->part_num = MMC_PART_RPMB;
+	}
+	ret = cp->cmd(cmdtp, flag, argc, argv);
 
-	if (strcmp(argv[1], "rescan") == 0) {
-		struct mmc *mmc;
+	/* Return to original partition */
+	if (mmc->part_num != original_part) {
+		if (mmc_switch_part(curr_device, original_part) != 0)
+			return CMD_RET_FAILURE;
+		mmc->part_num = original_part;
+	}
+	return ret;
+}
+#endif
 
-		if (argc != 2)
-			return CMD_RET_USAGE;
+static int do_mmc_read(cmd_tbl_t *cmdtp, int flag,
+		       int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 blk, cnt, n;
+	void *addr;
 
-		mmc = find_mmc_device(curr_device);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
+	if (argc != 4)
+		return CMD_RET_USAGE;
 
-		mmc->has_init = 0;
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
 
-		if (mmc_init(mmc))
-			return 1;
-		else
-			return 0;
-	} else if (strcmp(argv[1], "part") == 0) {
-		block_dev_desc_t *mmc_dev;
-		struct mmc *mmc;
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-		if (argc != 2)
-			return CMD_RET_USAGE;
+	printf("\nMMC read: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
 
-		mmc = find_mmc_device(curr_device);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
-		mmc_init(mmc);
-		mmc_dev = mmc_get_dev(curr_device);
-		if (mmc_dev != NULL &&
-				mmc_dev->type != DEV_TYPE_UNKNOWN) {
-			print_part(mmc_dev);
-			return 0;
-		}
+	n = mmc->block_dev.block_read(curr_device, blk, cnt, addr);
+	/* flush cache after read */
+	flush_cache((ulong)addr, cnt * 512); /* FIXME */
+	printf("%d blocks read: %s\n", n, (n == cnt) ? "OK" : "ERROR");
 
-		puts("get mmc type error!\n");
-		return 1;
-	} else if (strcmp(argv[1], "list") == 0) {
-		if (argc != 2)
-			return CMD_RET_USAGE;
-		print_mmc_devices('\n');
-		return 0;
-	} else if (strcmp(argv[1], "dev") == 0) {
-		int dev, part = -1;
-		struct mmc *mmc;
-
-		if (argc == 2)
-			dev = curr_device;
-		else if (argc == 3)
-			dev = simple_strtoul(argv[2], NULL, 10);
-		else if (argc == 4) {
-			dev = (int)simple_strtoul(argv[2], NULL, 10);
-			part = (int)simple_strtoul(argv[3], NULL, 10);
-			if (part > PART_ACCESS_MASK) {
-				printf("#part_num shouldn't be larger"
-					" than %d\n", PART_ACCESS_MASK);
-				return 1;
-			}
-		} else
-			return CMD_RET_USAGE;
+	return (n == cnt) ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+static int do_mmc_write(cmd_tbl_t *cmdtp, int flag,
+			int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 blk, cnt, n;
+	void *addr;
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	if (argc != 4)
+		return CMD_RET_USAGE;
 
-		mmc_init(mmc);
-		if (part != -1) {
-			int ret;
-			if (mmc->part_config == MMCPART_NOAVAILABLE) {
-				printf("Card doesn't support part_switch\n");
-				return 1;
-			}
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
 
-			if (part != mmc->part_num) {
-				ret = mmc_switch_part(dev, part);
-				if (!ret)
-					mmc->part_num = part;
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-				printf("switch to partitions #%d, %s\n",
-						part, (!ret) ? "OK" : "ERROR");
-			}
-		}
-		curr_device = dev;
-		if (mmc->part_config == MMCPART_NOAVAILABLE)
-			printf("mmc%d is current device\n", curr_device);
-		else
-			printf("mmc%d(part %d) is current device\n",
-				curr_device, mmc->part_num);
+	printf("\nMMC write: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
 
-		return 0;
-#ifdef CONFIG_SUPPORT_EMMC_BOOT
-	} else if (strcmp(argv[1], "partconf") == 0) {
-		int dev;
-		struct mmc *mmc;
-		u8 ack, part_num, access;
-
-		if (argc == 6) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			ack = simple_strtoul(argv[3], NULL, 10);
-			part_num = simple_strtoul(argv[4], NULL, 10);
-			access = simple_strtoul(argv[5], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	if (mmc_getwp(mmc) == 1) {
+		printf("Error: card is write protected!\n");
+		return CMD_RET_FAILURE;
+	}
+	n = mmc->block_dev.block_write(curr_device, blk, cnt, addr);
+	printf("%d blocks written: %s\n", n, (n == cnt) ? "OK" : "ERROR");
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	return (n == cnt) ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+static int do_mmc_erase(cmd_tbl_t *cmdtp, int flag,
+			int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 blk, cnt, n;
 
-		if (IS_SD(mmc)) {
-			puts("PARTITION_CONFIG only exists on eMMC\n");
-			return 1;
-		}
+	if (argc != 3)
+		return CMD_RET_USAGE;
 
-		/* acknowledge to be sent during boot operation */
-		return mmc_set_part_conf(mmc, ack, part_num, access);
-	} else if (strcmp(argv[1], "bootbus") == 0) {
-		int dev;
-		struct mmc *mmc;
-		u8 width, reset, mode;
-
-		if (argc == 6) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			width = simple_strtoul(argv[3], NULL, 10);
-			reset = simple_strtoul(argv[4], NULL, 10);
-			mode = simple_strtoul(argv[5], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	blk = simple_strtoul(argv[1], NULL, 16);
+	cnt = simple_strtoul(argv[2], NULL, 16);
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-		if (IS_SD(mmc)) {
-			puts("BOOT_BUS_WIDTH only exists on eMMC\n");
-			return 1;
-		}
+	printf("\nMMC erase: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
 
-		/* acknowledge to be sent during boot operation */
-		return mmc_set_boot_bus_width(mmc, width, reset, mode);
-	} else if (strcmp(argv[1], "bootpart-resize") == 0) {
-		int dev;
-		struct mmc *mmc;
-		u32 bootsize, rpmbsize;
-
-		if (argc == 5) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			bootsize = simple_strtoul(argv[3], NULL, 10);
-			rpmbsize = simple_strtoul(argv[4], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	if (mmc_getwp(mmc) == 1) {
+		printf("Error: card is write protected!\n");
+		return CMD_RET_FAILURE;
+	}
+	n = mmc->block_dev.block_erase(curr_device, blk, cnt);
+	printf("%d blocks erased: %s\n", n, (n == cnt) ? "OK" : "ERROR");
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	return (n == cnt) ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+static int do_mmc_rescan(cmd_tbl_t *cmdtp, int flag,
+			 int argc, char * const argv[])
+{
+	struct mmc *mmc;
 
-		if (IS_SD(mmc)) {
-			printf("It is not a EMMC device\n");
-			return 1;
-		}
+	mmc = find_mmc_device(curr_device);
+	if (!mmc) {
+		printf("no mmc device at slot %x\n", curr_device);
+		return CMD_RET_FAILURE;
+	}
 
-		if (0 == mmc_boot_partition_size_change(mmc,
-							bootsize, rpmbsize)) {
-			printf("EMMC boot partition Size %d MB\n", bootsize);
-			printf("EMMC RPMB partition Size %d MB\n", rpmbsize);
-			return 0;
-		} else {
-			printf("EMMC boot partition Size change Failed.\n");
-			return 1;
-		}
-	} else if (strcmp(argv[1], "rst-function") == 0) {
-		/*
-		 * Set the RST_n_ENABLE bit of RST_n_FUNCTION
-		 * The only valid values are 0x0, 0x1 and 0x2 and writing
-		 * a value of 0x1 or 0x2 sets the value permanently.
-		 */
-		int dev;
-		struct mmc *mmc;
-		u8 enable;
-
-		if (argc == 4) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			enable = simple_strtoul(argv[3], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	mmc->has_init = 0;
 
-		if (enable > 2 || enable < 0) {
-			puts("Invalid RST_n_ENABLE value\n");
-			return CMD_RET_USAGE;
+	if (mmc_init(mmc))
+		return CMD_RET_FAILURE;
+	return CMD_RET_SUCCESS;
+}
+static int do_mmc_part(cmd_tbl_t *cmdtp, int flag,
+		       int argc, char * const argv[])
+{
+	block_dev_desc_t *mmc_dev;
+	struct mmc *mmc;
+
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	mmc_dev = mmc_get_dev(curr_device);
+	if (mmc_dev != NULL && mmc_dev->type != DEV_TYPE_UNKNOWN) {
+		print_part(mmc_dev);
+		return CMD_RET_SUCCESS;
+	}
+
+	puts("get mmc type error!\n");
+	return CMD_RET_FAILURE;
+}
+static int do_mmc_dev(cmd_tbl_t *cmdtp, int flag,
+		      int argc, char * const argv[])
+{
+	int dev, part = -1;
+	struct mmc *mmc;
+
+	if (argc == 1) {
+		dev = curr_device;
+	} else if (argc == 2) {
+		dev = simple_strtoul(argv[1], NULL, 10);
+	} else if (argc == 3) {
+		dev = (int)simple_strtoul(argv[1], NULL, 10);
+		part = (int)simple_strtoul(argv[2], NULL, 10);
+		if (part > PART_ACCESS_MASK) {
+			printf("#part_num shouldn't be larger than %d\n",
+			       PART_ACCESS_MASK);
+			return CMD_RET_FAILURE;
 		}
+	} else {
+		return CMD_RET_USAGE;
+	}
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (part != -1) {
+		int ret;
+		if (mmc->part_config == MMCPART_NOAVAILABLE) {
+			printf("Card doesn't support part_switch\n");
+			return CMD_RET_FAILURE;
 		}
 
-		if (IS_SD(mmc)) {
-			puts("RST_n_FUNCTION only exists on eMMC\n");
-			return 1;
+		if (part != mmc->part_num) {
+			ret = mmc_switch_part(dev, part);
+			if (!ret)
+				mmc->part_num = part;
+
+			printf("switch to partitions #%d, %s\n",
+			       part, (!ret) ? "OK" : "ERROR");
 		}
+	}
+	curr_device = dev;
+	if (mmc->part_config == MMCPART_NOAVAILABLE)
+		printf("mmc%d is current device\n", curr_device);
+	else
+		printf("mmc%d(part %d) is current device\n",
+		       curr_device, mmc->part_num);
 
-		return mmc_set_rst_n_function(mmc, enable);
-#endif /* CONFIG_SUPPORT_EMMC_BOOT */
+	return CMD_RET_SUCCESS;
+}
+static int do_mmc_list(cmd_tbl_t *cmdtp, int flag,
+		       int argc, char * const argv[])
+{
+	print_mmc_devices('\n');
+	return CMD_RET_SUCCESS;
+}
+#ifdef CONFIG_SUPPORT_EMMC_BOOT
+static int do_mmc_bootbus(cmd_tbl_t *cmdtp, int flag,
+			  int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u8 width, reset, mode;
+
+	if (argc != 5)
+		return CMD_RET_USAGE;
+	dev = simple_strtoul(argv[1], NULL, 10);
+	width = simple_strtoul(argv[2], NULL, 10);
+	reset = simple_strtoul(argv[3], NULL, 10);
+	mode = simple_strtoul(argv[4], NULL, 10);
+
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		puts("BOOT_BUS_WIDTH only exists on eMMC\n");
+		return CMD_RET_FAILURE;
 	}
 
-	else if (argc == 3 && strcmp(argv[1], "setdsr") == 0) {
-		struct mmc *mmc = find_mmc_device(curr_device);
-		u32 val = simple_strtoul(argv[2], NULL, 16);
-		int ret;
+	/* acknowledge to be sent during boot operation */
+	return mmc_set_boot_bus_width(mmc, width, reset, mode);
+}
+static int do_mmc_boot_resize(cmd_tbl_t *cmdtp, int flag,
+			      int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u32 bootsize, rpmbsize;
 
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
-		ret = mmc_set_dsr(mmc, val);
-		printf("set dsr %s\n", (!ret) ? "OK, force rescan" : "ERROR");
-		if (!ret) {
-			mmc->has_init = 0;
-			if (mmc_init(mmc))
-				return 1;
-			else
-				return 0;
-		}
-		return ret;
+	if (argc != 4)
+		return CMD_RET_USAGE;
+	dev = simple_strtoul(argv[1], NULL, 10);
+	bootsize = simple_strtoul(argv[2], NULL, 10);
+	rpmbsize = simple_strtoul(argv[3], NULL, 10);
+
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		printf("It is not a EMMC device\n");
+		return CMD_RET_FAILURE;
 	}
 
-	state = MMC_INVALID;
-	if (argc == 5 && strcmp(argv[1], "read") == 0)
-		state = MMC_READ;
-	else if (argc == 5 && strcmp(argv[1], "write") == 0)
-		state = MMC_WRITE;
-	else if (argc == 4 && strcmp(argv[1], "erase") == 0)
-		state = MMC_ERASE;
-
-	if (state != MMC_INVALID) {
-		struct mmc *mmc = find_mmc_device(curr_device);
-		int idx = 2;
-		u32 blk, cnt, n;
-		void *addr;
-
-		if (state != MMC_ERASE) {
-			addr = (void *)simple_strtoul(argv[idx], NULL, 16);
-			++idx;
-		} else
-			addr = NULL;
-		blk = simple_strtoul(argv[idx], NULL, 16);
-		cnt = simple_strtoul(argv[idx + 1], NULL, 16);
-
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
+	if (mmc_boot_partition_size_change(mmc, bootsize, rpmbsize)) {
+		printf("EMMC boot partition Size change Failed.\n");
+		return CMD_RET_FAILURE;
+	}
 
-		printf("\nMMC %s: dev # %d, block # %d, count %d ... ",
-				argv[1], curr_device, blk, cnt);
+	printf("EMMC boot partition Size %d MB\n", bootsize);
+	printf("EMMC RPMB partition Size %d MB\n", rpmbsize);
+	return CMD_RET_SUCCESS;
+}
+static int do_mmc_partconf(cmd_tbl_t *cmdtp, int flag,
+			   int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u8 ack, part_num, access;
 
-		mmc_init(mmc);
+	if (argc != 5)
+		return CMD_RET_USAGE;
 
-		if ((state == MMC_WRITE || state == MMC_ERASE)) {
-			if (mmc_getwp(mmc) == 1) {
-				printf("Error: card is write protected!\n");
-				return 1;
-			}
-		}
+	dev = simple_strtoul(argv[1], NULL, 10);
+	ack = simple_strtoul(argv[2], NULL, 10);
+	part_num = simple_strtoul(argv[3], NULL, 10);
+	access = simple_strtoul(argv[4], NULL, 10);
 
-		switch (state) {
-		case MMC_READ:
-			n = mmc->block_dev.block_read(curr_device, blk,
-						      cnt, addr);
-			/* flush cache after read */
-			flush_cache((ulong)addr, cnt * 512); /* FIXME */
-			break;
-		case MMC_WRITE:
-			n = mmc->block_dev.block_write(curr_device, blk,
-						      cnt, addr);
-			break;
-		case MMC_ERASE:
-			n = mmc->block_dev.block_erase(curr_device, blk, cnt);
-			break;
-		default:
-			BUG();
-		}
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		puts("PARTITION_CONFIG only exists on eMMC\n");
+		return CMD_RET_FAILURE;
+	}
+
+	/* acknowledge to be sent during boot operation */
+	return mmc_set_part_conf(mmc, ack, part_num, access);
+}
+static int do_mmc_rst_func(cmd_tbl_t *cmdtp, int flag,
+			   int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u8 enable;
+
+	/*
+	 * Set the RST_n_ENABLE bit of RST_n_FUNCTION
+	 * The only valid values are 0x0, 0x1 and 0x2 and writing
+	 * a value of 0x1 or 0x2 sets the value permanently.
+	 */
+	if (argc != 3)
+		return CMD_RET_USAGE;
+
+	dev = simple_strtoul(argv[1], NULL, 10);
+	enable = simple_strtoul(argv[2], NULL, 10);
+
+	if (enable > 2 || enable < 0) {
+		puts("Invalid RST_n_ENABLE value\n");
+		return CMD_RET_USAGE;
+	}
+
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		puts("RST_n_FUNCTION only exists on eMMC\n");
+		return CMD_RET_FAILURE;
+	}
 
-		printf("%d blocks %s: %s\n",
-				n, argv[1], (n == cnt) ? "OK" : "ERROR");
-		return (n == cnt) ? 0 : 1;
+	return mmc_set_rst_n_function(mmc, enable);
+}
+#endif
+static int do_mmc_setdsr(cmd_tbl_t *cmdtp, int flag,
+			 int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 val;
+	int ret;
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+	val = simple_strtoul(argv[2], NULL, 16);
+
+	mmc = find_mmc_device(curr_device);
+	if (!mmc) {
+		printf("no mmc device@slot %x\n", curr_device);
+		return CMD_RET_FAILURE;
+	}
+	ret = mmc_set_dsr(mmc, val);
+	printf("set dsr %s\n", (!ret) ? "OK, force rescan" : "ERROR");
+	if (!ret) {
+		mmc->has_init = 0;
+		if (mmc_init(mmc))
+			return CMD_RET_FAILURE;
+		else
+			return CMD_RET_SUCCESS;
 	}
+	return ret;
+}
+
+static cmd_tbl_t cmd_mmc[] = {
+	U_BOOT_CMD_MKENT(info, 1, 0, do_mmcinfo, "", ""),
+	U_BOOT_CMD_MKENT(read, 4, 1, do_mmc_read, "", ""),
+	U_BOOT_CMD_MKENT(write, 4, 0, do_mmc_write, "", ""),
+	U_BOOT_CMD_MKENT(erase, 3, 0, do_mmc_erase, "", ""),
+	U_BOOT_CMD_MKENT(rescan, 1, 1, do_mmc_rescan, "", ""),
+	U_BOOT_CMD_MKENT(part, 1, 1, do_mmc_part, "", ""),
+	U_BOOT_CMD_MKENT(dev, 3, 0, do_mmc_dev, "", ""),
+	U_BOOT_CMD_MKENT(list, 1, 1, do_mmc_list, "", ""),
+#ifdef CONFIG_SUPPORT_EMMC_BOOT
+	U_BOOT_CMD_MKENT(bootbus, 5, 0, do_mmc_bootbus, "", ""),
+	U_BOOT_CMD_MKENT(bootpart-resize, 3, 0, do_mmc_boot_resize, "", ""),
+	U_BOOT_CMD_MKENT(partconf, 5, 0, do_mmc_partconf, "", ""),
+	U_BOOT_CMD_MKENT(rst-function, 3, 0, do_mmc_rst_func, "", ""),
+#endif
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	U_BOOT_CMD_MKENT(rpmb, CONFIG_SYS_MAXARGS, 1, do_mmcrpmb, "", ""),
+#endif
+	U_BOOT_CMD_MKENT(setdsr, 2, 0, do_mmc_setdsr, "", ""),
+};
+
+static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	cmd_tbl_t *cp;
+
+	cp = find_cmd_tbl(argv[1], cmd_mmc, ARRAY_SIZE(cmd_mmc));
+
+	/* Drop the mmc command */
+	argc--;
+	argv++;
+
+	if (cp == NULL || argc > cp->maxargs)
+		return CMD_RET_USAGE;
+	if (flag == CMD_FLAG_REPEAT && !cp->repeatable)
+		return CMD_RET_SUCCESS;
 
-	return CMD_RET_USAGE;
+	if (curr_device < 0) {
+		if (get_mmc_num() > 0) {
+			curr_device = 0;
+		} else {
+			puts("No MMC device available\n");
+			return CMD_RET_FAILURE;
+		}
+	}
+	return cp->cmd(cmdtp, flag, argc, argv);
 }
 
 U_BOOT_CMD(
-	mmc, 6, 1, do_mmcops,
+	mmc, 7, 1, do_mmcops,
 	"MMC sub system",
-	"read addr blk# cnt\n"
+	"info - display info of the current MMC device\n"
+	"mmc read addr blk# cnt\n"
 	"mmc write addr blk# cnt\n"
 	"mmc erase blk# cnt\n"
 	"mmc rescan\n"
@@ -474,6 +674,20 @@ U_BOOT_CMD(
 	" - Change the RST_n_FUNCTION field of the specified device\n"
 	"   WARNING: This is a write-once field and 0 / 1 / 2 are the only valid values.\n"
 #endif
-	"mmc setdsr - set DSR register value\n"
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	"mmc rpmb read addr blk# cnt [address of auth-key] - block size is 256 bytes\n"
+	"mmc rpmb write addr blk# cnt <address of auth-key> - block size is 256 bytes\n"
+	"mmc rpmb key <address of auth-key> - program the RPMB authentication key.\n"
+	"mmc rpmb counter - read the value of the write counter\n"
+#endif
+	"mmc setdsr <value> - set DSR register value\n"
 	);
+
+/* Old command kept for compatibility. Same as 'mmc info' */
+U_BOOT_CMD(
+	mmcinfo, 1, 0, do_mmcinfo,
+	"display MMC info",
+	"- display info of the current MMC device"
+);
+
 #endif /* !CONFIG_GENERIC_MMC */
-- 
1.7.6.5

[U-Boot] [PATCH V5 0/3] eMMC: support for Read Protected Memory Block (RPMB)

[Pierre Aubert]

This serie of patches adds some functions and a sub-command of 'mmc' for  programming the authentication key and for reading and writing the RPMB partition of an eMMC according to the JEDEC standard No. 64-A441

The sub-command rpmb is enabled by the flag CONFIG_SUPPORT_EMMC_RPMB defined
in the board configuration file.
It has been tested on a SabreSDP iMX6 board.

Changes in V5:
- move changelog to the right place for each patches
- add empty changelog for patches with no change
- change lib/Makefile for adding sha256 when CONFIG_SUPPORT_EMMC_RPMB is 
  defined

Changes in V4:
- use subcommand tables for 'mmc' command and 'mmc rpmb' subcommand.
- add 'mmc info' subcommand, which is the same as mmcinfo.
  mmcinfo is kept for compatibility.
- add changelog in commit comments.

Changes in V3:
- add entries in README for configuration options related to eMMC.
- new patch for adding the 'confirm_yesno' function as suggested by W.Denk
- improved test for existence of RPMB partition.
- fix of coding issues.

Changes in V2:
- use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the 
  RPMB frames.

Pierre Aubert (3):
  eMMC: add support for operations in RPMB partition
  Add the function 'confirm_yesno' for interactive
  eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

 README               |   10 +
 common/cmd_fuse.c    |   11 +-
 common/cmd_mmc.c     |  806 +++++++++++++++++++++++++++++++------------------
 common/cmd_nand.c    |   16 +-
 common/cmd_otp.c     |   18 +-
 common/console.c     |   28 ++-
 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++
 include/common.h     |    2 +-
 include/mmc.h        |   10 +-
 lib/Makefile         |    1 +
 11 files changed, 892 insertions(+), 334 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

-- 
1.7.6.5

[U-Boot] [PATCH V5 1/3] eMMC: add support for operations in RPMB partition

[Pierre Aubert]

This patch adds functions for read, write and authentication
key programming for the Replay Protected Memory Block partition
in the eMMC.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
---
Changes in V5:
- move changelog to the right place
- change lib/Makefile for adding sha256 when CONFIG_SUPPORT_EMMC_RPMB is 
  defined

V3, V4: no changes

Changes in V2:
- use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the
  RPMB frames.

 drivers/mmc/Makefile |    1 +
 drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
 include/mmc.h        |   10 ++-
 lib/Makefile         |    1 +
 4 files changed, 334 insertions(+), 1 deletions(-)
 create mode 100644 drivers/mmc/rpmb.c

diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
index 931922b..4c6ab9e 100644
--- a/drivers/mmc/Makefile
+++ b/drivers/mmc/Makefile
@@ -30,6 +30,7 @@ obj-$(CONFIG_DWMMC) += dw_mmc.o
 obj-$(CONFIG_EXYNOS_DWMMC) += exynos_dw_mmc.o
 obj-$(CONFIG_ZYNQ_SDHCI) += zynq_sdhci.o
 obj-$(CONFIG_SOCFPGA_DWMMC) += socfpga_dw_mmc.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
 ifdef CONFIG_SPL_BUILD
 obj-$(CONFIG_SPL_MMC_BOOT) += fsl_esdhc_spl.o
 else
diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
new file mode 100644
index 0000000..05936f5
--- /dev/null
+++ b/drivers/mmc/rpmb.c
@@ -0,0 +1,323 @@
+/*
+ * Copyright 2014, Staubli Faverges
+ * Pierre Aubert
+ *
+ * eMMC- Replay Protected Memory Block
+ * According to JEDEC Standard No. 84-A441
+ *
+ * SPDX-License-Identifier:	GPL-2.0+
+ */
+
+#include <config.h>
+#include <common.h>
+#include <mmc.h>
+#include <sha256.h>
+#include "mmc_private.h"
+
+/* Request codes */
+#define RPMB_REQ_KEY		1
+#define RPMB_REQ_WCOUNTER	2
+#define RPMB_REQ_WRITE_DATA	3
+#define RPMB_REQ_READ_DATA	4
+#define RPMB_REQ_STATUS		5
+
+/* Response code */
+#define RPMB_RESP_KEY		0x0100
+#define RPMB_RESP_WCOUNTER	0x0200
+#define RPMB_RESP_WRITE_DATA	0x0300
+#define RPMB_RESP_READ_DATA	0x0400
+
+/* Error codes */
+#define RPMB_OK			0
+#define RPMB_ERR_GENERAL	1
+#define RPMB_ERR_AUTH	2
+#define RPMB_ERR_COUNTER	3
+#define RPMB_ERR_ADDRESS	4
+#define RPMB_ERR_WRITE		5
+#define RPMB_ERR_READ		6
+#define RPMB_ERR_KEY		7
+#define RPMB_ERR_CNT_EXPIRED	0x80
+#define RPMB_ERR_MSK		0x7
+
+/* Sizes of RPMB data frame */
+#define RPMB_SZ_STUFF		196
+#define RPMB_SZ_MAC		32
+#define RPMB_SZ_DATA		256
+#define RPMB_SZ_NONCE		16
+
+#define SHA256_BLOCK_SIZE	64
+
+/* Error messages */
+static const char * const rpmb_err_msg[] = {
+	"",
+	"General failure",
+	"Authentication failure",
+	"Counter failure",
+	"Address failure",
+	"Write failure",
+	"Read failure",
+	"Authentication key not yet programmed",
+};
+
+
+/* Structure of RPMB data frame. */
+struct s_rpmb {
+	unsigned char stuff[RPMB_SZ_STUFF];
+	unsigned char mac[RPMB_SZ_MAC];
+	unsigned char data[RPMB_SZ_DATA];
+	unsigned char nonce[RPMB_SZ_NONCE];
+	unsigned long write_counter;
+	unsigned short address;
+	unsigned short block_count;
+	unsigned short result;
+	unsigned short request;
+};
+
+static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
+			      bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+
+	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
+	cmd.cmdarg = blockcount & 0x0000FFFF;
+	if (is_rel_write)
+		cmd.cmdarg |= 1 << 31;
+	cmd.resp_type = MMC_RSP_R1;
+
+	return mmc_send_cmd(mmc, &cmd, NULL);
+}
+static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
+			    unsigned int count, bool is_rel_write)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, count, is_rel_write);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+
+	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1b;
+
+	data.src = (const char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_WRITE;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return 1;
+	}
+	return 0;
+}
+static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
+			     unsigned short expected)
+{
+	struct mmc_cmd cmd = {0};
+	struct mmc_data data;
+	int ret;
+
+	ret = mmc_set_blockcount(mmc, 1, false);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
+	cmd.cmdarg = 0;
+	cmd.resp_type = MMC_RSP_R1;
+
+	data.dest = (char *)s;
+	data.blocks = 1;
+	data.blocksize = MMC_MAX_BLOCK_LEN;
+	data.flags = MMC_DATA_READ;
+
+	ret = mmc_send_cmd(mmc, &cmd, &data);
+	if (ret) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
+#endif
+		return -1;
+	}
+	/* Check the response and the status */
+	if (be16_to_cpu(s->request) != expected) {
+#ifdef CONFIG_MMC_RPMB_TRACE
+		printf("%s:response= %x\n", __func__,
+		       be16_to_cpu(s->request));
+#endif
+		return -1;
+	}
+	ret = be16_to_cpu(s->result);
+	if (ret) {
+		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
+		       (ret & RPMB_ERR_CNT_EXPIRED) ?
+		       "Write counter has expired" : "");
+	}
+
+	/* Return the status of the command */
+	return ret;
+}
+static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	return mmc_rpmb_response(mmc, rpmb_frame, expected);
+}
+static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
+		      unsigned char *output)
+{
+	sha256_context ctx;
+	int i;
+	unsigned char k_ipad[SHA256_BLOCK_SIZE];
+	unsigned char k_opad[SHA256_BLOCK_SIZE];
+
+	sha256_starts(&ctx);
+
+	/* According to RFC 4634, the HMAC transform looks like:
+	   SHA(K XOR opad, SHA(K XOR ipad, text))
+
+	   where K is an n byte key.
+	   ipad is the byte 0x36 repeated blocksize times
+	   opad is the byte 0x5c repeated blocksize times
+	   and text is the data being protected.
+	*/
+
+	for (i = 0; i < RPMB_SZ_MAC; i++) {
+		k_ipad[i] = key[i] ^ 0x36;
+		k_opad[i] = key[i] ^ 0x5c;
+	}
+	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
+	for ( ; i < SHA256_BLOCK_SIZE; i++) {
+		k_ipad[i] = 0x36;
+		k_opad[i] = 0x5c;
+	}
+	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
+	sha256_update(&ctx, buff, len);
+	sha256_finish(&ctx, output);
+
+	/* Init context for second pass */
+	sha256_starts(&ctx);
+
+	/* start with outer pad */
+	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
+
+	/* then results of 1st hash */
+	sha256_update(&ctx, output, RPMB_SZ_MAC);
+
+	/* finish up 2nd pass */
+	sha256_finish(&ctx, output);
+}
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
+{
+	int ret;
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+		return -1;
+
+	/* Read the result */
+	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER);
+	if (ret)
+		return ret;
+
+	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
+	return 0;
+}
+int mmc_rpmb_set_key(struct mmc *mmc, void *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	/* Fill the request */
+	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
+	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);
+
+	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+		return -1;
+
+	/* read the operation status */
+	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
+}
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
+			break;
+
+		/* Read the result */
+		if (mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_READ_DATA))
+			break;
+
+		/* Check the HMAC if key is provided */
+		if (key) {
+			unsigned char ret_hmac[RPMB_SZ_MAC];
+
+			rpmb_hmac(key, rpmb_frame->data, 284, ret_hmac);
+			if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
+				printf("MAC error on block #%d\n", i);
+				break;
+			}
+		}
+		/* Copy data */
+		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
+	}
+	return i;
+}
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key)
+{
+	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
+	unsigned long wcount;
+	int i;
+
+	for (i = 0; i < cnt; i++) {
+		if (mmc_rpmb_get_counter(mmc, &wcount)) {
+			printf("Cannot read RPMB write counter\n");
+			break;
+		}
+
+		/* Fill the request */
+		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
+		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
+		rpmb_frame->address = cpu_to_be16(blk + i);
+		rpmb_frame->block_count = cpu_to_be16(1);
+		rpmb_frame->write_counter = cpu_to_be32(wcount);
+		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
+		/* Computes HMAC */
+		rpmb_hmac(key, rpmb_frame->data, 284, rpmb_frame->mac);
+
+		if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
+			break;
+
+		/* Get status */
+		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
+			break;
+	}
+	return i;
+}
diff --git a/include/mmc.h b/include/mmc.h
index 42d0125..14d296c 100644
--- a/include/mmc.h
+++ b/include/mmc.h
@@ -69,6 +69,7 @@
 #define MMC_CMD_SET_BLOCKLEN		16
 #define MMC_CMD_READ_SINGLE_BLOCK	17
 #define MMC_CMD_READ_MULTIPLE_BLOCK	18
+#define MMC_CMD_SET_BLOCK_COUNT         23
 #define MMC_CMD_WRITE_SINGLE_BLOCK	24
 #define MMC_CMD_WRITE_MULTIPLE_BLOCK	25
 #define MMC_CMD_ERASE_GROUP_START	35
@@ -224,6 +225,7 @@
  * boot partitions (2), general purpose partitions (4) in MMC v4.4.
  */
 #define MMC_NUM_BOOT_PARTITION	2
+#define MMC_PART_RPMB           3       /* RPMB partition number */
 
 struct mmc_cid {
 	unsigned long psn;
@@ -335,7 +337,13 @@ int mmc_set_part_conf(struct mmc *mmc, u8 ack, u8 part_num, u8 access);
 int mmc_set_boot_bus_width(struct mmc *mmc, u8 width, u8 reset, u8 mode);
 /* Function to modify the RST_n_FUNCTION field of EXT_CSD */
 int mmc_set_rst_n_function(struct mmc *mmc, u8 enable);
-
+/* Functions to read / write the RPMB partition */
+int mmc_rpmb_set_key(struct mmc *mmc, void *key);
+int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *counter);
+int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
+		  unsigned short cnt, unsigned char *key);
+int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
+		   unsigned short cnt, unsigned char *key);
 /**
  * Start device initialization and return immediately; it does not block on
  * polling OCR (operation condition register) status.  Then you should call
diff --git a/lib/Makefile b/lib/Makefile
index 27e4f78..377ab13 100644
--- a/lib/Makefile
+++ b/lib/Makefile
@@ -35,6 +35,7 @@ obj-y += net_utils.o
 obj-$(CONFIG_PHYSMEM) += physmem.o
 obj-y += qsort.o
 obj-$(CONFIG_SHA1) += sha1.o
+obj-$(CONFIG_SUPPORT_EMMC_RPMB) += sha256.o
 obj-$(CONFIG_SHA256) += sha256.o
 obj-y	+= strmhz.o
 obj-$(CONFIG_TPM) += tpm.o
-- 
1.7.6.5

[U-Boot] [PATCH V5 1/3] eMMC: add support for operations in RPMB partition

[Pantelis Antoniou]

Hi Pierre,

On Apr 24, 2014, at 11:30 AM, Pierre Aubert wrote:

> This patch adds functions for read, write and authentication
> key programming for the Replay Protected Memory Block partition
> in the eMMC.
> 
> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
> CC: Pantelis Antoniou <panto@antoniou-consulting.com>
> ---
> Changes in V5:
> - move changelog to the right place
> - change lib/Makefile for adding sha256 when CONFIG_SUPPORT_EMMC_RPMB is 
>  defined
> 
> V3, V4: no changes
> 
> Changes in V2:
> - use ALLOC_CACHE_ALIGN_BUFFER in rpmb.c instead of a static buffer for the
>  RPMB frames.
> 
> drivers/mmc/Makefile |    1 +
> drivers/mmc/rpmb.c   |  323 ++++++++++++++++++++++++++++++++++++++++++++++++++
> include/mmc.h        |   10 ++-
> lib/Makefile         |    1 +
> 4 files changed, 334 insertions(+), 1 deletions(-)
> create mode 100644 drivers/mmc/rpmb.c
> 
> diff --git a/drivers/mmc/Makefile b/drivers/mmc/Makefile
> index 931922b..4c6ab9e 100644
> --- a/drivers/mmc/Makefile
> +++ b/drivers/mmc/Makefile
> @@ -30,6 +30,7 @@ obj-$(CONFIG_DWMMC) += dw_mmc.o
> obj-$(CONFIG_EXYNOS_DWMMC) += exynos_dw_mmc.o
> obj-$(CONFIG_ZYNQ_SDHCI) += zynq_sdhci.o
> obj-$(CONFIG_SOCFPGA_DWMMC) += socfpga_dw_mmc.o
> +obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
> ifdef CONFIG_SPL_BUILD
> obj-$(CONFIG_SPL_MMC_BOOT) += fsl_esdhc_spl.o
> else
> diff --git a/drivers/mmc/rpmb.c b/drivers/mmc/rpmb.c
> new file mode 100644
> index 0000000..05936f5
> --- /dev/null
> +++ b/drivers/mmc/rpmb.c
> @@ -0,0 +1,323 @@
> +/*
> + * Copyright 2014, Staubli Faverges
> + * Pierre Aubert
> + *
> + * eMMC- Replay Protected Memory Block
> + * According to JEDEC Standard No. 84-A441
> + *
> + * SPDX-License-Identifier:	GPL-2.0+
> + */
> +
> +#include <config.h>
> +#include <common.h>
> +#include <mmc.h>
> +#include <sha256.h>
> +#include "mmc_private.h"
> +
> +/* Request codes */
> +#define RPMB_REQ_KEY		1
> +#define RPMB_REQ_WCOUNTER	2
> +#define RPMB_REQ_WRITE_DATA	3
> +#define RPMB_REQ_READ_DATA	4
> +#define RPMB_REQ_STATUS		5
> +
> +/* Response code */
> +#define RPMB_RESP_KEY		0x0100
> +#define RPMB_RESP_WCOUNTER	0x0200
> +#define RPMB_RESP_WRITE_DATA	0x0300
> +#define RPMB_RESP_READ_DATA	0x0400
> +
> +/* Error codes */
> +#define RPMB_OK			0
> +#define RPMB_ERR_GENERAL	1
> +#define RPMB_ERR_AUTH	2
> +#define RPMB_ERR_COUNTER	3
> +#define RPMB_ERR_ADDRESS	4
> +#define RPMB_ERR_WRITE		5
> +#define RPMB_ERR_READ		6
> +#define RPMB_ERR_KEY		7
> +#define RPMB_ERR_CNT_EXPIRED	0x80
> +#define RPMB_ERR_MSK		0x7
> +
> +/* Sizes of RPMB data frame */
> +#define RPMB_SZ_STUFF		196
> +#define RPMB_SZ_MAC		32
> +#define RPMB_SZ_DATA		256
> +#define RPMB_SZ_NONCE		16
> +
> +#define SHA256_BLOCK_SIZE	64
> +
> +/* Error messages */
> +static const char * const rpmb_err_msg[] = {
> +	"",
> +	"General failure",
> +	"Authentication failure",
> +	"Counter failure",
> +	"Address failure",
> +	"Write failure",
> +	"Read failure",
> +	"Authentication key not yet programmed",
> +};
> +
> +
> +/* Structure of RPMB data frame. */
> +struct s_rpmb {
> +	unsigned char stuff[RPMB_SZ_STUFF];
> +	unsigned char mac[RPMB_SZ_MAC];
> +	unsigned char data[RPMB_SZ_DATA];
> +	unsigned char nonce[RPMB_SZ_NONCE];
> +	unsigned long write_counter;
> +	unsigned short address;
> +	unsigned short block_count;
> +	unsigned short result;
> +	unsigned short request;
> +};
> +
> +static int mmc_set_blockcount(struct mmc *mmc, unsigned int blockcount,
> +			      bool is_rel_write)
> +{
> +	struct mmc_cmd cmd = {0};
> +
> +	cmd.cmdidx = MMC_CMD_SET_BLOCK_COUNT;
> +	cmd.cmdarg = blockcount & 0x0000FFFF;
> +	if (is_rel_write)
> +		cmd.cmdarg |= 1 << 31;
> +	cmd.resp_type = MMC_RSP_R1;
> +
> +	return mmc_send_cmd(mmc, &cmd, NULL);
> +}
> +static int mmc_rpmb_request(struct mmc *mmc, const struct s_rpmb *s,
> +			    unsigned int count, bool is_rel_write)
> +{
> +	struct mmc_cmd cmd = {0};
> +	struct mmc_data data;
> +	int ret;
> +
> +	ret = mmc_set_blockcount(mmc, count, is_rel_write);
> +	if (ret) {
> +#ifdef CONFIG_MMC_RPMB_TRACE
> +		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
> +#endif
> +		return 1;
> +	}
> +
> +	cmd.cmdidx = MMC_CMD_WRITE_MULTIPLE_BLOCK;
> +	cmd.cmdarg = 0;
> +	cmd.resp_type = MMC_RSP_R1b;
> +
> +	data.src = (const char *)s;
> +	data.blocks = 1;
> +	data.blocksize = MMC_MAX_BLOCK_LEN;
> +	data.flags = MMC_DATA_WRITE;
> +
> +	ret = mmc_send_cmd(mmc, &cmd, &data);
> +	if (ret) {
> +#ifdef CONFIG_MMC_RPMB_TRACE
> +		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
> +#endif
> +		return 1;
> +	}
> +	return 0;
> +}
> +static int mmc_rpmb_response(struct mmc *mmc, struct s_rpmb *s,
> +			     unsigned short expected)
> +{
> +	struct mmc_cmd cmd = {0};
> +	struct mmc_data data;
> +	int ret;
> +
> +	ret = mmc_set_blockcount(mmc, 1, false);
> +	if (ret) {
> +#ifdef CONFIG_MMC_RPMB_TRACE
> +		printf("%s:mmc_set_blockcount-> %d\n", __func__, ret);
> +#endif
> +		return -1;
> +	}
> +	cmd.cmdidx = MMC_CMD_READ_MULTIPLE_BLOCK;
> +	cmd.cmdarg = 0;
> +	cmd.resp_type = MMC_RSP_R1;
> +
> +	data.dest = (char *)s;
> +	data.blocks = 1;
> +	data.blocksize = MMC_MAX_BLOCK_LEN;
> +	data.flags = MMC_DATA_READ;
> +
> +	ret = mmc_send_cmd(mmc, &cmd, &data);
> +	if (ret) {
> +#ifdef CONFIG_MMC_RPMB_TRACE
> +		printf("%s:mmc_send_cmd-> %d\n", __func__, ret);
> +#endif
> +		return -1;
> +	}
> +	/* Check the response and the status */
> +	if (be16_to_cpu(s->request) != expected) {
> +#ifdef CONFIG_MMC_RPMB_TRACE
> +		printf("%s:response= %x\n", __func__,
> +		       be16_to_cpu(s->request));
> +#endif
> +		return -1;
> +	}
> +	ret = be16_to_cpu(s->result);
> +	if (ret) {
> +		printf("%s %s\n", rpmb_err_msg[ret & RPMB_ERR_MSK],
> +		       (ret & RPMB_ERR_CNT_EXPIRED) ?
> +		       "Write counter has expired" : "");
> +	}
> +
> +	/* Return the status of the command */
> +	return ret;
> +}
> +static int mmc_rpmb_status(struct mmc *mmc, unsigned short expected)
> +{
> +	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
> +
> +	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
> +	rpmb_frame->request = cpu_to_be16(RPMB_REQ_STATUS);
> +	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
> +		return -1;
> +
> +	/* Read the result */
> +	return mmc_rpmb_response(mmc, rpmb_frame, expected);
> +}
> +static void rpmb_hmac(unsigned char *key, unsigned char *buff, int len,
> +		      unsigned char *output)
> +{
> +	sha256_context ctx;
> +	int i;
> +	unsigned char k_ipad[SHA256_BLOCK_SIZE];
> +	unsigned char k_opad[SHA256_BLOCK_SIZE];
> +
> +	sha256_starts(&ctx);
> +
> +	/* According to RFC 4634, the HMAC transform looks like:
> +	   SHA(K XOR opad, SHA(K XOR ipad, text))
> +
> +	   where K is an n byte key.
> +	   ipad is the byte 0x36 repeated blocksize times
> +	   opad is the byte 0x5c repeated blocksize times
> +	   and text is the data being protected.
> +	*/
> +
> +	for (i = 0; i < RPMB_SZ_MAC; i++) {
> +		k_ipad[i] = key[i] ^ 0x36;
> +		k_opad[i] = key[i] ^ 0x5c;
> +	}
> +	/* remaining pad bytes are '\0' XOR'd with ipad and opad values */
> +	for ( ; i < SHA256_BLOCK_SIZE; i++) {
> +		k_ipad[i] = 0x36;
> +		k_opad[i] = 0x5c;
> +	}
> +	sha256_update(&ctx, k_ipad, SHA256_BLOCK_SIZE);
> +	sha256_update(&ctx, buff, len);
> +	sha256_finish(&ctx, output);
> +
> +	/* Init context for second pass */
> +	sha256_starts(&ctx);
> +
> +	/* start with outer pad */
> +	sha256_update(&ctx, k_opad, SHA256_BLOCK_SIZE);
> +
> +	/* then results of 1st hash */
> +	sha256_update(&ctx, output, RPMB_SZ_MAC);
> +
> +	/* finish up 2nd pass */
> +	sha256_finish(&ctx, output);
> +}
> +int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *pcounter)
> +{
> +	int ret;
> +	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
> +
> +	/* Fill the request */
> +	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
> +	rpmb_frame->request = cpu_to_be16(RPMB_REQ_WCOUNTER);
> +	if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
> +		return -1;
> +
> +	/* Read the result */
> +	ret = mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_WCOUNTER);
> +	if (ret)
> +		return ret;
> +
> +	*pcounter = be32_to_cpu(rpmb_frame->write_counter);
> +	return 0;
> +}
> +int mmc_rpmb_set_key(struct mmc *mmc, void *key)
> +{
> +	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
> +	/* Fill the request */
> +	memset(rpmb_frame, 0, sizeof(struct s_rpmb));
> +	rpmb_frame->request = cpu_to_be16(RPMB_REQ_KEY);
> +	memcpy(rpmb_frame->mac, key, RPMB_SZ_MAC);
> +
> +	if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
> +		return -1;
> +
> +	/* read the operation status */
> +	return mmc_rpmb_status(mmc, RPMB_RESP_KEY);
> +}
> +int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
> +		  unsigned short cnt, unsigned char *key)
> +{
> +	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
> +	int i;
> +
> +	for (i = 0; i < cnt; i++) {
> +		/* Fill the request */
> +		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
> +		rpmb_frame->address = cpu_to_be16(blk + i);
> +		rpmb_frame->request = cpu_to_be16(RPMB_REQ_READ_DATA);
> +		if (mmc_rpmb_request(mmc, rpmb_frame, 1, false))
> +			break;
> +
> +		/* Read the result */
> +		if (mmc_rpmb_response(mmc, rpmb_frame, RPMB_RESP_READ_DATA))
> +			break;
> +
> +		/* Check the HMAC if key is provided */
> +		if (key) {
> +			unsigned char ret_hmac[RPMB_SZ_MAC];
> +
> +			rpmb_hmac(key, rpmb_frame->data, 284, ret_hmac);
> +			if (memcmp(ret_hmac, rpmb_frame->mac, RPMB_SZ_MAC)) {
> +				printf("MAC error on block #%d\n", i);
> +				break;
> +			}
> +		}
> +		/* Copy data */
> +		memcpy(addr + i * RPMB_SZ_DATA, rpmb_frame->data, RPMB_SZ_DATA);
> +	}
> +	return i;
> +}
> +int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
> +		  unsigned short cnt, unsigned char *key)
> +{
> +	ALLOC_CACHE_ALIGN_BUFFER(struct s_rpmb, rpmb_frame, 1);
> +	unsigned long wcount;
> +	int i;
> +
> +	for (i = 0; i < cnt; i++) {
> +		if (mmc_rpmb_get_counter(mmc, &wcount)) {
> +			printf("Cannot read RPMB write counter\n");
> +			break;
> +		}
> +
> +		/* Fill the request */
> +		memset(rpmb_frame, 0, sizeof(struct s_rpmb));
> +		memcpy(rpmb_frame->data, addr + i * RPMB_SZ_DATA, RPMB_SZ_DATA);
> +		rpmb_frame->address = cpu_to_be16(blk + i);
> +		rpmb_frame->block_count = cpu_to_be16(1);
> +		rpmb_frame->write_counter = cpu_to_be32(wcount);
> +		rpmb_frame->request = cpu_to_be16(RPMB_REQ_WRITE_DATA);
> +		/* Computes HMAC */
> +		rpmb_hmac(key, rpmb_frame->data, 284, rpmb_frame->mac);
> +
> +		if (mmc_rpmb_request(mmc, rpmb_frame, 1, true))
> +			break;
> +
> +		/* Get status */
> +		if (mmc_rpmb_status(mmc, RPMB_RESP_WRITE_DATA))
> +			break;
> +	}
> +	return i;
> +}
> diff --git a/include/mmc.h b/include/mmc.h
> index 42d0125..14d296c 100644
> --- a/include/mmc.h
> +++ b/include/mmc.h
> @@ -69,6 +69,7 @@
> #define MMC_CMD_SET_BLOCKLEN		16
> #define MMC_CMD_READ_SINGLE_BLOCK	17
> #define MMC_CMD_READ_MULTIPLE_BLOCK	18
> +#define MMC_CMD_SET_BLOCK_COUNT         23
> #define MMC_CMD_WRITE_SINGLE_BLOCK	24
> #define MMC_CMD_WRITE_MULTIPLE_BLOCK	25
> #define MMC_CMD_ERASE_GROUP_START	35
> @@ -224,6 +225,7 @@
>  * boot partitions (2), general purpose partitions (4) in MMC v4.4.
>  */
> #define MMC_NUM_BOOT_PARTITION	2
> +#define MMC_PART_RPMB           3       /* RPMB partition number */
> 
> struct mmc_cid {
> 	unsigned long psn;
> @@ -335,7 +337,13 @@ int mmc_set_part_conf(struct mmc *mmc, u8 ack, u8 part_num, u8 access);
> int mmc_set_boot_bus_width(struct mmc *mmc, u8 width, u8 reset, u8 mode);
> /* Function to modify the RST_n_FUNCTION field of EXT_CSD */
> int mmc_set_rst_n_function(struct mmc *mmc, u8 enable);
> -
> +/* Functions to read / write the RPMB partition */
> +int mmc_rpmb_set_key(struct mmc *mmc, void *key);
> +int mmc_rpmb_get_counter(struct mmc *mmc, unsigned long *counter);
> +int mmc_rpmb_read(struct mmc *mmc, void *addr, unsigned short blk,
> +		  unsigned short cnt, unsigned char *key);
> +int mmc_rpmb_write(struct mmc *mmc, void *addr, unsigned short blk,
> +		   unsigned short cnt, unsigned char *key);
> /**
>  * Start device initialization and return immediately; it does not block on
>  * polling OCR (operation condition register) status.  Then you should call
> diff --git a/lib/Makefile b/lib/Makefile
> index 27e4f78..377ab13 100644
> --- a/lib/Makefile
> +++ b/lib/Makefile
> @@ -35,6 +35,7 @@ obj-y += net_utils.o
> obj-$(CONFIG_PHYSMEM) += physmem.o
> obj-y += qsort.o
> obj-$(CONFIG_SHA1) += sha1.o
> +obj-$(CONFIG_SUPPORT_EMMC_RPMB) += sha256.o
> obj-$(CONFIG_SHA256) += sha256.o
> obj-y	+= strmhz.o
> obj-$(CONFIG_TPM) += tpm.o
> -- 
> 1.7.6.5
> 

Looks good.

Applied

Thanks

-- Pantelis

Acked-by: Pantelis Antoniou <panto@antoniou-consulting.com>

> _______________________________________________
> U-Boot mailing list
> U-Boot at lists.denx.de
> http://lists.denx.de/mailman/listinfo/u-boot

[U-Boot] [PATCH V5 2/3] Add the function 'confirm_yesno' for interactive

[Pierre Aubert]

User's confirmation is asked in different commands. This commit adds a
function for such confirmation.

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
---

V3, V4, V5: no changes

Patch added in V2

 common/cmd_fuse.c |   11 ++---------
 common/cmd_nand.c |   16 +++++-----------
 common/cmd_otp.c  |   18 +++---------------
 common/console.c  |   28 +++++++++++++++++++++++++++-
 include/common.h  |    2 +-
 5 files changed, 38 insertions(+), 37 deletions(-)

diff --git a/common/cmd_fuse.c b/common/cmd_fuse.c
index 0df57db..abab978 100644
--- a/common/cmd_fuse.c
+++ b/common/cmd_fuse.c
@@ -33,15 +33,8 @@ static int confirm_prog(void)
 					"what you are doing!\n"
 			"\nReally perform this fuse programming? <y/N>\n");
 
-	if (getc() == 'y') {
-		int c;
-
-		putc('y');
-		c = getc();
-		putc('\n');
-		if (c == '\r')
-			return 1;
-	}
+	if (confirm_yesno())
+		return 1;
 
 	puts("Fuse programming aborted\n");
 	return 0;
diff --git a/common/cmd_nand.c b/common/cmd_nand.c
index 04ab0f1..a84f7dc 100644
--- a/common/cmd_nand.c
+++ b/common/cmd_nand.c
@@ -605,22 +605,16 @@ static int do_nand(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		opts.spread = spread;
 
 		if (scrub) {
-			if (!scrub_yes)
-				puts(scrub_warn);
-
-			if (scrub_yes)
+			if (scrub_yes) {
 				opts.scrub = 1;
-			else if (getc() == 'y') {
-				puts("y");
-				if (getc() == '\r')
+			} else {
+				puts(scrub_warn);
+				if (confirm_yesno()) {
 					opts.scrub = 1;
-				else {
+				} else {
 					puts("scrub aborted\n");
 					return 1;
 				}
-			} else {
-				puts("scrub aborted\n");
-				return 1;
 			}
 		}
 		ret = nand_erase_opts(nand, &opts);
diff --git a/common/cmd_otp.c b/common/cmd_otp.c
index 67808aa..593bb8c 100644
--- a/common/cmd_otp.c
+++ b/common/cmd_otp.c
@@ -158,21 +158,9 @@ int do_otp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 			lowup(half + count - 1), page + (half + count - 1) / 2,
 			half + count
 		);
-
-		i = 0;
-		while (1) {
-			if (tstc()) {
-				const char exp_ans[] = "YES\r";
-				char c;
-				putc(c = getc());
-				if (exp_ans[i++] != c) {
-					printf(" Aborting\n");
-					return 1;
-				} else if (!exp_ans[i]) {
-					puts("\n");
-					break;
-				}
-			}
+		if (!confirm_yesno()) {
+			printf(" Aborting\n");
+			return 1;
 		}
 	}
 
diff --git a/common/console.c b/common/console.c
index 2dfb788..5453726 100644
--- a/common/console.c
+++ b/common/console.c
@@ -537,7 +537,33 @@ int ctrlc(void)
 	}
 	return 0;
 }
-
+/* Reads user's confirmation.
+   Returns 1 if user's input is "y", "Y", "yes" or "YES"
+*/
+int confirm_yesno(void)
+{
+	int i;
+	char str_input[5];
+
+	/* Flush input */
+	while (tstc())
+		getc();
+	i = 0;
+	while (i < sizeof(str_input)) {
+		str_input[i] = getc();
+		putc(str_input[i]);
+		if (str_input[i] == '\r')
+			break;
+		i++;
+	}
+	putc('\n');
+	if (strncmp(str_input, "y\r", 2) == 0 ||
+	    strncmp(str_input, "Y\r", 2) == 0 ||
+	    strncmp(str_input, "yes\r", 4) == 0 ||
+	    strncmp(str_input, "YES\r", 4) == 0)
+		return 1;
+	return 0;
+}
 /* pass 1 to disable ctrlc() checking, 0 to enable.
  * returns previous state
  */
diff --git a/include/common.h b/include/common.h
index baf361b..1d922b9 100644
--- a/include/common.h
+++ b/include/common.h
@@ -838,7 +838,7 @@ int	ctrlc (void);
 int	had_ctrlc (void);	/* have we had a Control-C since last clear? */
 void	clear_ctrlc (void);	/* clear the Control-C condition */
 int	disable_ctrlc (int);	/* 1 to disable, 0 to enable Control-C detect */
-
+int confirm_yesno(void);        /*  1 if input is "y", "Y", "yes" or "YES" */
 /*
  * STDIO based functions (can always be used)
  */
-- 
1.7.6.5

[U-Boot] [PATCH V5 2/3] Add the function 'confirm_yesno' for interactive

[Pantelis Antoniou]

Hi Pierre,

On Apr 24, 2014, at 11:30 AM, Pierre Aubert wrote:

> User's confirmation is asked in different commands. This commit adds a
> function for such confirmation.
> 
> Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
> ---
> 
> V3, V4, V5: no changes
> 
> Patch added in V2
> 
> common/cmd_fuse.c |   11 ++---------
> common/cmd_nand.c |   16 +++++-----------
> common/cmd_otp.c  |   18 +++---------------
> common/console.c  |   28 +++++++++++++++++++++++++++-
> include/common.h  |    2 +-
> 5 files changed, 38 insertions(+), 37 deletions(-)
> 
> diff --git a/common/cmd_fuse.c b/common/cmd_fuse.c
> index 0df57db..abab978 100644
> --- a/common/cmd_fuse.c
> +++ b/common/cmd_fuse.c
> @@ -33,15 +33,8 @@ static int confirm_prog(void)
> 					"what you are doing!\n"
> 			"\nReally perform this fuse programming? <y/N>\n");
> 
> -	if (getc() == 'y') {
> -		int c;
> -
> -		putc('y');
> -		c = getc();
> -		putc('\n');
> -		if (c == '\r')
> -			return 1;
> -	}
> +	if (confirm_yesno())
> +		return 1;
> 
> 	puts("Fuse programming aborted\n");
> 	return 0;
> diff --git a/common/cmd_nand.c b/common/cmd_nand.c
> index 04ab0f1..a84f7dc 100644
> --- a/common/cmd_nand.c
> +++ b/common/cmd_nand.c
> @@ -605,22 +605,16 @@ static int do_nand(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
> 		opts.spread = spread;
> 
> 		if (scrub) {
> -			if (!scrub_yes)
> -				puts(scrub_warn);
> -
> -			if (scrub_yes)
> +			if (scrub_yes) {
> 				opts.scrub = 1;
> -			else if (getc() == 'y') {
> -				puts("y");
> -				if (getc() == '\r')
> +			} else {
> +				puts(scrub_warn);
> +				if (confirm_yesno()) {
> 					opts.scrub = 1;
> -				else {
> +				} else {
> 					puts("scrub aborted\n");
> 					return 1;
> 				}
> -			} else {
> -				puts("scrub aborted\n");
> -				return 1;
> 			}
> 		}
> 		ret = nand_erase_opts(nand, &opts);
> diff --git a/common/cmd_otp.c b/common/cmd_otp.c
> index 67808aa..593bb8c 100644
> --- a/common/cmd_otp.c
> +++ b/common/cmd_otp.c
> @@ -158,21 +158,9 @@ int do_otp(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
> 			lowup(half + count - 1), page + (half + count - 1) / 2,
> 			half + count
> 		);
> -
> -		i = 0;
> -		while (1) {
> -			if (tstc()) {
> -				const char exp_ans[] = "YES\r";
> -				char c;
> -				putc(c = getc());
> -				if (exp_ans[i++] != c) {
> -					printf(" Aborting\n");
> -					return 1;
> -				} else if (!exp_ans[i]) {
> -					puts("\n");
> -					break;
> -				}
> -			}
> +		if (!confirm_yesno()) {
> +			printf(" Aborting\n");
> +			return 1;
> 		}
> 	}
> 
> diff --git a/common/console.c b/common/console.c
> index 2dfb788..5453726 100644
> --- a/common/console.c
> +++ b/common/console.c
> @@ -537,7 +537,33 @@ int ctrlc(void)
> 	}
> 	return 0;
> }
> -
> +/* Reads user's confirmation.
> +   Returns 1 if user's input is "y", "Y", "yes" or "YES"
> +*/
> +int confirm_yesno(void)
> +{
> +	int i;
> +	char str_input[5];
> +
> +	/* Flush input */
> +	while (tstc())
> +		getc();
> +	i = 0;
> +	while (i < sizeof(str_input)) {
> +		str_input[i] = getc();
> +		putc(str_input[i]);
> +		if (str_input[i] == '\r')
> +			break;
> +		i++;
> +	}
> +	putc('\n');
> +	if (strncmp(str_input, "y\r", 2) == 0 ||
> +	    strncmp(str_input, "Y\r", 2) == 0 ||
> +	    strncmp(str_input, "yes\r", 4) == 0 ||
> +	    strncmp(str_input, "YES\r", 4) == 0)
> +		return 1;

I guess you could convert to upper/lower case and reduce the comparisons but I'd
let it pass.

Maybe follow up later with a patch just for that.

> +	return 0;
> +}
> /* pass 1 to disable ctrlc() checking, 0 to enable.
>  * returns previous state
>  */
> diff --git a/include/common.h b/include/common.h
> index baf361b..1d922b9 100644
> --- a/include/common.h
> +++ b/include/common.h
> @@ -838,7 +838,7 @@ int	ctrlc (void);
> int	had_ctrlc (void);	/* have we had a Control-C since last clear? */
> void	clear_ctrlc (void);	/* clear the Control-C condition */
> int	disable_ctrlc (int);	/* 1 to disable, 0 to enable Control-C detect */
> -
> +int confirm_yesno(void);        /*  1 if input is "y", "Y", "yes" or "YES" */
> /*
>  * STDIO based functions (can always be used)
>  */
> -- 
> 1.7.6.5
> 

This is reasonable, and I've applied it to my mmc tree,

Applied,

Thanks

-- Pantelis

Acked-by: Pantelis Antoniou <panto@antoniou-consulting.com>

[U-Boot] [PATCH V5 3/3] eMMC: cmd_mmc.c adds the 'rpmb' sub-command for the 'mmc' command

[Pierre Aubert]

This sub-command adds support for the RPMB partition of an eMMC:
* mmc rpmb key <address of the authentication key>
  Programs the authentication key in the eMMC This key can not
  be overwritten.
* mmc rpmb read <address> <block> <#count> [address of key]
  Reads <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. If the optionnal
  address of the authentication key is provided, the
  Message Authentication Code (MAC) is verified on each
  block.
* mmc rpmb write <address> <block> <#count> <address of key>
  Writes <#count> blocks of 256 bytes in the RPMB partition
  beginning at block number <block>. The datas are signed
  with the key provided.
* mmc rpmb counter
  Returns the 'Write counter' of the RPMB partition.

The sub-command is conditional on compilation flag CONFIG_SUPPORT_EMMC_RPMB

Signed-off-by: Pierre Aubert <p.aubert@staubli.com>
CC: Pantelis Antoniou <panto@antoniou-consulting.com>
CC: Wolfgang Denk <wd@denx.de>
---
Changes in V5: 
- move changelog to the right place

Changes in V4:
- use subcommand table for 'mmc' command and 'mmc rpmb' subcommand.
- add 'mmc info' subcommand, which is the same as mmcinfo.
  mmcinfo is kept for compatibility.
- add changelog in commit comments.

Changes in V3:
- add entries in README for configuration options related to eMMC.
- improved test for existence of RPMB partition.
- fix of coding issues.

V2: no changes

 README           |   10 +
 common/cmd_mmc.c |  806 ++++++++++++++++++++++++++++++++++--------------------
 2 files changed, 520 insertions(+), 296 deletions(-)

diff --git a/README b/README
index 52a92e7..518e209 100644
--- a/README
+++ b/README
@@ -1493,6 +1493,16 @@ The following options need to be configured:
 			CONFIG_SH_MMCIF_CLK
 			Define the clock frequency for MMCIF
 
+		CONFIG_GENERIC_MMC
+		Enable the generic MMC driver
+
+		CONFIG_SUPPORT_EMMC_BOOT
+		Enable some additional features of the eMMC boot partitions.
+
+		CONFIG_SUPPORT_EMMC_RPMB
+		Enable the commands for reading, writing and programming the
+		key for the Replay Protection Memory Block partition in eMMC.
+
 - USB Device Firmware Update (DFU) class support:
 		CONFIG_DFU_FUNCTION
 		This enables the USB portion of the DFU USB class
diff --git a/common/cmd_mmc.c b/common/cmd_mmc.c
index c1916c9..55af295 100644
--- a/common/cmd_mmc.c
+++ b/common/cmd_mmc.c
@@ -71,12 +71,6 @@ U_BOOT_CMD(
 );
 #else /* !CONFIG_GENERIC_MMC */
 
-enum mmc_state {
-	MMC_INVALID,
-	MMC_READ,
-	MMC_WRITE,
-	MMC_ERASE,
-};
 static void print_mmcinfo(struct mmc *mmc)
 {
 	printf("Device: %s\n", mmc->cfg->name);
@@ -98,7 +92,18 @@ static void print_mmcinfo(struct mmc *mmc)
 
 	printf("Bus Width: %d-bit\n", mmc->bus_width);
 }
-
+static struct mmc *init_mmc_device(int dev)
+{
+	struct mmc *mmc;
+	mmc = find_mmc_device(dev);
+	if (!mmc) {
+		printf("no mmc device at slot %x\n", dev);
+		return NULL;
+	}
+	if (mmc_init(mmc))
+		return NULL;
+	return mmc;
+}
 static int do_mmcinfo(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	struct mmc *mmc;
@@ -112,351 +117,546 @@ static int do_mmcinfo(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 		}
 	}
 
-	mmc = find_mmc_device(curr_device);
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-	if (mmc) {
-		mmc_init(mmc);
+	print_mmcinfo(mmc);
+	return CMD_RET_SUCCESS;
+}
 
-		print_mmcinfo(mmc);
-		return 0;
-	} else {
-		printf("no mmc device at slot %x\n", curr_device);
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+static int confirm_key_prog(void)
+{
+	puts("Warning: Programming authentication key can be done only once !\n"
+	     "         Use this command only if you are sure of what you are doing,\n"
+	     "Really perform the key programming? <y/N> ");
+	if (confirm_yesno())
 		return 1;
+
+	puts("Authentication key programming aborted\n");
+	return 0;
+}
+static int do_mmcrpmb_key(cmd_tbl_t *cmdtp, int flag,
+			  int argc, char * const argv[])
+{
+	void *key_addr;
+	struct mmc *mmc = find_mmc_device(curr_device);
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+
+	key_addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	if (!confirm_key_prog())
+		return CMD_RET_FAILURE;
+	if (mmc_rpmb_set_key(mmc, key_addr)) {
+		printf("ERROR - Key already programmed ?\n");
+		return CMD_RET_FAILURE;
 	}
+	return CMD_RET_SUCCESS;
 }
+static int do_mmcrpmb_read(cmd_tbl_t *cmdtp, int flag,
+			   int argc, char * const argv[])
+{
+	u16 blk, cnt;
+	void *addr;
+	int n;
+	void *key_addr = NULL;
+	struct mmc *mmc = find_mmc_device(curr_device);
 
-U_BOOT_CMD(
-	mmcinfo, 1, 0, do_mmcinfo,
-	"display MMC info",
-	"- display info of the current MMC device"
-);
+	if (argc < 4)
+		return CMD_RET_USAGE;
 
-static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
+
+	if (argc == 5)
+		key_addr = (void *)simple_strtoul(argv[4], NULL, 16);
+
+	printf("\nMMC RPMB read: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
+	n =  mmc_rpmb_read(mmc, addr, blk, cnt, key_addr);
+
+	printf("%d RPMB blocks read: %s\n", n, (n == cnt) ? "OK" : "ERROR");
+	if (n != cnt)
+		return CMD_RET_FAILURE;
+	return CMD_RET_SUCCESS;
+}
+static int do_mmcrpmb_write(cmd_tbl_t *cmdtp, int flag,
+			    int argc, char * const argv[])
 {
-	enum mmc_state state;
+	u16 blk, cnt;
+	void *addr;
+	int n;
+	void *key_addr;
+	struct mmc *mmc = find_mmc_device(curr_device);
 
-	if (argc < 2)
+	if (argc != 5)
 		return CMD_RET_USAGE;
 
-	if (curr_device < 0) {
-		if (get_mmc_num() > 0)
-			curr_device = 0;
-		else {
-			puts("No MMC device available\n");
-			return 1;
-		}
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
+	key_addr = (void *)simple_strtoul(argv[4], NULL, 16);
+
+	printf("\nMMC RPMB write: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
+	n =  mmc_rpmb_write(mmc, addr, blk, cnt, key_addr);
+
+	printf("%d RPMB blocks written: %s\n", n, (n == cnt) ? "OK" : "ERROR");
+	if (n != cnt)
+		return CMD_RET_FAILURE;
+	return CMD_RET_SUCCESS;
+}
+static int do_mmcrpmb_counter(cmd_tbl_t *cmdtp, int flag,
+			      int argc, char * const argv[])
+{
+	unsigned long counter;
+	struct mmc *mmc = find_mmc_device(curr_device);
+
+	if (mmc_rpmb_get_counter(mmc, &counter))
+		return CMD_RET_FAILURE;
+	printf("RPMB Write counter= %lx\n", counter);
+	return CMD_RET_SUCCESS;
+}
+
+static cmd_tbl_t cmd_rpmb[] = {
+	U_BOOT_CMD_MKENT(key, 2, 0, do_mmcrpmb_key, "", ""),
+	U_BOOT_CMD_MKENT(read, 5, 1, do_mmcrpmb_read, "", ""),
+	U_BOOT_CMD_MKENT(write, 5, 0, do_mmcrpmb_write, "", ""),
+	U_BOOT_CMD_MKENT(counter, 1, 1, do_mmcrpmb_counter, "", ""),
+};
+
+static int do_mmcrpmb(cmd_tbl_t *cmdtp, int flag,
+		      int argc, char * const argv[])
+{
+	cmd_tbl_t *cp;
+	struct mmc *mmc;
+	char original_part;
+	int ret;
+
+	cp = find_cmd_tbl(argv[1], cmd_rpmb, ARRAY_SIZE(cmd_rpmb));
+
+	/* Drop the rpmb subcommand */
+	argc--;
+	argv++;
+
+	if (cp == NULL || argc > cp->maxargs)
+		return CMD_RET_USAGE;
+	if (flag == CMD_FLAG_REPEAT && !cp->repeatable)
+		return CMD_RET_SUCCESS;
+
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (!(mmc->version & MMC_VERSION_MMC)) {
+		printf("It is not a EMMC device\n");
+		return CMD_RET_FAILURE;
+	}
+	if (mmc->version < MMC_VERSION_4_41) {
+		printf("RPMB not supported before version 4.41\n");
+		return CMD_RET_FAILURE;
 	}
+	/* Switch to the RPMB partition */
+	original_part = mmc->part_num;
+	if (mmc->part_num != MMC_PART_RPMB) {
+		if (mmc_switch_part(curr_device, MMC_PART_RPMB) != 0)
+			return CMD_RET_FAILURE;
+		mmc->part_num = MMC_PART_RPMB;
+	}
+	ret = cp->cmd(cmdtp, flag, argc, argv);
 
-	if (strcmp(argv[1], "rescan") == 0) {
-		struct mmc *mmc;
+	/* Return to original partition */
+	if (mmc->part_num != original_part) {
+		if (mmc_switch_part(curr_device, original_part) != 0)
+			return CMD_RET_FAILURE;
+		mmc->part_num = original_part;
+	}
+	return ret;
+}
+#endif
 
-		if (argc != 2)
-			return CMD_RET_USAGE;
+static int do_mmc_read(cmd_tbl_t *cmdtp, int flag,
+		       int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 blk, cnt, n;
+	void *addr;
 
-		mmc = find_mmc_device(curr_device);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
+	if (argc != 4)
+		return CMD_RET_USAGE;
 
-		mmc->has_init = 0;
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
 
-		if (mmc_init(mmc))
-			return 1;
-		else
-			return 0;
-	} else if (strcmp(argv[1], "part") == 0) {
-		block_dev_desc_t *mmc_dev;
-		struct mmc *mmc;
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-		if (argc != 2)
-			return CMD_RET_USAGE;
+	printf("\nMMC read: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
 
-		mmc = find_mmc_device(curr_device);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
-		mmc_init(mmc);
-		mmc_dev = mmc_get_dev(curr_device);
-		if (mmc_dev != NULL &&
-				mmc_dev->type != DEV_TYPE_UNKNOWN) {
-			print_part(mmc_dev);
-			return 0;
-		}
+	n = mmc->block_dev.block_read(curr_device, blk, cnt, addr);
+	/* flush cache after read */
+	flush_cache((ulong)addr, cnt * 512); /* FIXME */
+	printf("%d blocks read: %s\n", n, (n == cnt) ? "OK" : "ERROR");
 
-		puts("get mmc type error!\n");
-		return 1;
-	} else if (strcmp(argv[1], "list") == 0) {
-		if (argc != 2)
-			return CMD_RET_USAGE;
-		print_mmc_devices('\n');
-		return 0;
-	} else if (strcmp(argv[1], "dev") == 0) {
-		int dev, part = -1;
-		struct mmc *mmc;
-
-		if (argc == 2)
-			dev = curr_device;
-		else if (argc == 3)
-			dev = simple_strtoul(argv[2], NULL, 10);
-		else if (argc == 4) {
-			dev = (int)simple_strtoul(argv[2], NULL, 10);
-			part = (int)simple_strtoul(argv[3], NULL, 10);
-			if (part > PART_ACCESS_MASK) {
-				printf("#part_num shouldn't be larger"
-					" than %d\n", PART_ACCESS_MASK);
-				return 1;
-			}
-		} else
-			return CMD_RET_USAGE;
+	return (n == cnt) ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+static int do_mmc_write(cmd_tbl_t *cmdtp, int flag,
+			int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 blk, cnt, n;
+	void *addr;
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	if (argc != 4)
+		return CMD_RET_USAGE;
 
-		mmc_init(mmc);
-		if (part != -1) {
-			int ret;
-			if (mmc->part_config == MMCPART_NOAVAILABLE) {
-				printf("Card doesn't support part_switch\n");
-				return 1;
-			}
+	addr = (void *)simple_strtoul(argv[1], NULL, 16);
+	blk = simple_strtoul(argv[2], NULL, 16);
+	cnt = simple_strtoul(argv[3], NULL, 16);
 
-			if (part != mmc->part_num) {
-				ret = mmc_switch_part(dev, part);
-				if (!ret)
-					mmc->part_num = part;
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-				printf("switch to partitions #%d, %s\n",
-						part, (!ret) ? "OK" : "ERROR");
-			}
-		}
-		curr_device = dev;
-		if (mmc->part_config == MMCPART_NOAVAILABLE)
-			printf("mmc%d is current device\n", curr_device);
-		else
-			printf("mmc%d(part %d) is current device\n",
-				curr_device, mmc->part_num);
+	printf("\nMMC write: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
 
-		return 0;
-#ifdef CONFIG_SUPPORT_EMMC_BOOT
-	} else if (strcmp(argv[1], "partconf") == 0) {
-		int dev;
-		struct mmc *mmc;
-		u8 ack, part_num, access;
-
-		if (argc == 6) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			ack = simple_strtoul(argv[3], NULL, 10);
-			part_num = simple_strtoul(argv[4], NULL, 10);
-			access = simple_strtoul(argv[5], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	if (mmc_getwp(mmc) == 1) {
+		printf("Error: card is write protected!\n");
+		return CMD_RET_FAILURE;
+	}
+	n = mmc->block_dev.block_write(curr_device, blk, cnt, addr);
+	printf("%d blocks written: %s\n", n, (n == cnt) ? "OK" : "ERROR");
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	return (n == cnt) ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+static int do_mmc_erase(cmd_tbl_t *cmdtp, int flag,
+			int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 blk, cnt, n;
 
-		if (IS_SD(mmc)) {
-			puts("PARTITION_CONFIG only exists on eMMC\n");
-			return 1;
-		}
+	if (argc != 3)
+		return CMD_RET_USAGE;
 
-		/* acknowledge to be sent during boot operation */
-		return mmc_set_part_conf(mmc, ack, part_num, access);
-	} else if (strcmp(argv[1], "bootbus") == 0) {
-		int dev;
-		struct mmc *mmc;
-		u8 width, reset, mode;
-
-		if (argc == 6) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			width = simple_strtoul(argv[3], NULL, 10);
-			reset = simple_strtoul(argv[4], NULL, 10);
-			mode = simple_strtoul(argv[5], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	blk = simple_strtoul(argv[1], NULL, 16);
+	cnt = simple_strtoul(argv[2], NULL, 16);
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
 
-		if (IS_SD(mmc)) {
-			puts("BOOT_BUS_WIDTH only exists on eMMC\n");
-			return 1;
-		}
+	printf("\nMMC erase: dev # %d, block # %d, count %d ... ",
+	       curr_device, blk, cnt);
 
-		/* acknowledge to be sent during boot operation */
-		return mmc_set_boot_bus_width(mmc, width, reset, mode);
-	} else if (strcmp(argv[1], "bootpart-resize") == 0) {
-		int dev;
-		struct mmc *mmc;
-		u32 bootsize, rpmbsize;
-
-		if (argc == 5) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			bootsize = simple_strtoul(argv[3], NULL, 10);
-			rpmbsize = simple_strtoul(argv[4], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	if (mmc_getwp(mmc) == 1) {
+		printf("Error: card is write protected!\n");
+		return CMD_RET_FAILURE;
+	}
+	n = mmc->block_dev.block_erase(curr_device, blk, cnt);
+	printf("%d blocks erased: %s\n", n, (n == cnt) ? "OK" : "ERROR");
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
-		}
+	return (n == cnt) ? CMD_RET_SUCCESS : CMD_RET_FAILURE;
+}
+static int do_mmc_rescan(cmd_tbl_t *cmdtp, int flag,
+			 int argc, char * const argv[])
+{
+	struct mmc *mmc;
 
-		if (IS_SD(mmc)) {
-			printf("It is not a EMMC device\n");
-			return 1;
-		}
+	mmc = find_mmc_device(curr_device);
+	if (!mmc) {
+		printf("no mmc device at slot %x\n", curr_device);
+		return CMD_RET_FAILURE;
+	}
 
-		if (0 == mmc_boot_partition_size_change(mmc,
-							bootsize, rpmbsize)) {
-			printf("EMMC boot partition Size %d MB\n", bootsize);
-			printf("EMMC RPMB partition Size %d MB\n", rpmbsize);
-			return 0;
-		} else {
-			printf("EMMC boot partition Size change Failed.\n");
-			return 1;
-		}
-	} else if (strcmp(argv[1], "rst-function") == 0) {
-		/*
-		 * Set the RST_n_ENABLE bit of RST_n_FUNCTION
-		 * The only valid values are 0x0, 0x1 and 0x2 and writing
-		 * a value of 0x1 or 0x2 sets the value permanently.
-		 */
-		int dev;
-		struct mmc *mmc;
-		u8 enable;
-
-		if (argc == 4) {
-			dev = simple_strtoul(argv[2], NULL, 10);
-			enable = simple_strtoul(argv[3], NULL, 10);
-		} else {
-			return CMD_RET_USAGE;
-		}
+	mmc->has_init = 0;
 
-		if (enable > 2 || enable < 0) {
-			puts("Invalid RST_n_ENABLE value\n");
-			return CMD_RET_USAGE;
+	if (mmc_init(mmc))
+		return CMD_RET_FAILURE;
+	return CMD_RET_SUCCESS;
+}
+static int do_mmc_part(cmd_tbl_t *cmdtp, int flag,
+		       int argc, char * const argv[])
+{
+	block_dev_desc_t *mmc_dev;
+	struct mmc *mmc;
+
+	mmc = init_mmc_device(curr_device);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	mmc_dev = mmc_get_dev(curr_device);
+	if (mmc_dev != NULL && mmc_dev->type != DEV_TYPE_UNKNOWN) {
+		print_part(mmc_dev);
+		return CMD_RET_SUCCESS;
+	}
+
+	puts("get mmc type error!\n");
+	return CMD_RET_FAILURE;
+}
+static int do_mmc_dev(cmd_tbl_t *cmdtp, int flag,
+		      int argc, char * const argv[])
+{
+	int dev, part = -1;
+	struct mmc *mmc;
+
+	if (argc == 1) {
+		dev = curr_device;
+	} else if (argc == 2) {
+		dev = simple_strtoul(argv[1], NULL, 10);
+	} else if (argc == 3) {
+		dev = (int)simple_strtoul(argv[1], NULL, 10);
+		part = (int)simple_strtoul(argv[2], NULL, 10);
+		if (part > PART_ACCESS_MASK) {
+			printf("#part_num shouldn't be larger than %d\n",
+			       PART_ACCESS_MASK);
+			return CMD_RET_FAILURE;
 		}
+	} else {
+		return CMD_RET_USAGE;
+	}
 
-		mmc = find_mmc_device(dev);
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", dev);
-			return 1;
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (part != -1) {
+		int ret;
+		if (mmc->part_config == MMCPART_NOAVAILABLE) {
+			printf("Card doesn't support part_switch\n");
+			return CMD_RET_FAILURE;
 		}
 
-		if (IS_SD(mmc)) {
-			puts("RST_n_FUNCTION only exists on eMMC\n");
-			return 1;
+		if (part != mmc->part_num) {
+			ret = mmc_switch_part(dev, part);
+			if (!ret)
+				mmc->part_num = part;
+
+			printf("switch to partitions #%d, %s\n",
+			       part, (!ret) ? "OK" : "ERROR");
 		}
+	}
+	curr_device = dev;
+	if (mmc->part_config == MMCPART_NOAVAILABLE)
+		printf("mmc%d is current device\n", curr_device);
+	else
+		printf("mmc%d(part %d) is current device\n",
+		       curr_device, mmc->part_num);
 
-		return mmc_set_rst_n_function(mmc, enable);
-#endif /* CONFIG_SUPPORT_EMMC_BOOT */
+	return CMD_RET_SUCCESS;
+}
+static int do_mmc_list(cmd_tbl_t *cmdtp, int flag,
+		       int argc, char * const argv[])
+{
+	print_mmc_devices('\n');
+	return CMD_RET_SUCCESS;
+}
+#ifdef CONFIG_SUPPORT_EMMC_BOOT
+static int do_mmc_bootbus(cmd_tbl_t *cmdtp, int flag,
+			  int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u8 width, reset, mode;
+
+	if (argc != 5)
+		return CMD_RET_USAGE;
+	dev = simple_strtoul(argv[1], NULL, 10);
+	width = simple_strtoul(argv[2], NULL, 10);
+	reset = simple_strtoul(argv[3], NULL, 10);
+	mode = simple_strtoul(argv[4], NULL, 10);
+
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		puts("BOOT_BUS_WIDTH only exists on eMMC\n");
+		return CMD_RET_FAILURE;
 	}
 
-	else if (argc == 3 && strcmp(argv[1], "setdsr") == 0) {
-		struct mmc *mmc = find_mmc_device(curr_device);
-		u32 val = simple_strtoul(argv[2], NULL, 16);
-		int ret;
+	/* acknowledge to be sent during boot operation */
+	return mmc_set_boot_bus_width(mmc, width, reset, mode);
+}
+static int do_mmc_boot_resize(cmd_tbl_t *cmdtp, int flag,
+			      int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u32 bootsize, rpmbsize;
 
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
-		ret = mmc_set_dsr(mmc, val);
-		printf("set dsr %s\n", (!ret) ? "OK, force rescan" : "ERROR");
-		if (!ret) {
-			mmc->has_init = 0;
-			if (mmc_init(mmc))
-				return 1;
-			else
-				return 0;
-		}
-		return ret;
+	if (argc != 4)
+		return CMD_RET_USAGE;
+	dev = simple_strtoul(argv[1], NULL, 10);
+	bootsize = simple_strtoul(argv[2], NULL, 10);
+	rpmbsize = simple_strtoul(argv[3], NULL, 10);
+
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		printf("It is not a EMMC device\n");
+		return CMD_RET_FAILURE;
 	}
 
-	state = MMC_INVALID;
-	if (argc == 5 && strcmp(argv[1], "read") == 0)
-		state = MMC_READ;
-	else if (argc == 5 && strcmp(argv[1], "write") == 0)
-		state = MMC_WRITE;
-	else if (argc == 4 && strcmp(argv[1], "erase") == 0)
-		state = MMC_ERASE;
-
-	if (state != MMC_INVALID) {
-		struct mmc *mmc = find_mmc_device(curr_device);
-		int idx = 2;
-		u32 blk, cnt, n;
-		void *addr;
-
-		if (state != MMC_ERASE) {
-			addr = (void *)simple_strtoul(argv[idx], NULL, 16);
-			++idx;
-		} else
-			addr = NULL;
-		blk = simple_strtoul(argv[idx], NULL, 16);
-		cnt = simple_strtoul(argv[idx + 1], NULL, 16);
-
-		if (!mmc) {
-			printf("no mmc device at slot %x\n", curr_device);
-			return 1;
-		}
+	if (mmc_boot_partition_size_change(mmc, bootsize, rpmbsize)) {
+		printf("EMMC boot partition Size change Failed.\n");
+		return CMD_RET_FAILURE;
+	}
 
-		printf("\nMMC %s: dev # %d, block # %d, count %d ... ",
-				argv[1], curr_device, blk, cnt);
+	printf("EMMC boot partition Size %d MB\n", bootsize);
+	printf("EMMC RPMB partition Size %d MB\n", rpmbsize);
+	return CMD_RET_SUCCESS;
+}
+static int do_mmc_partconf(cmd_tbl_t *cmdtp, int flag,
+			   int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u8 ack, part_num, access;
 
-		mmc_init(mmc);
+	if (argc != 5)
+		return CMD_RET_USAGE;
 
-		if ((state == MMC_WRITE || state == MMC_ERASE)) {
-			if (mmc_getwp(mmc) == 1) {
-				printf("Error: card is write protected!\n");
-				return 1;
-			}
-		}
+	dev = simple_strtoul(argv[1], NULL, 10);
+	ack = simple_strtoul(argv[2], NULL, 10);
+	part_num = simple_strtoul(argv[3], NULL, 10);
+	access = simple_strtoul(argv[4], NULL, 10);
 
-		switch (state) {
-		case MMC_READ:
-			n = mmc->block_dev.block_read(curr_device, blk,
-						      cnt, addr);
-			/* flush cache after read */
-			flush_cache((ulong)addr, cnt * 512); /* FIXME */
-			break;
-		case MMC_WRITE:
-			n = mmc->block_dev.block_write(curr_device, blk,
-						      cnt, addr);
-			break;
-		case MMC_ERASE:
-			n = mmc->block_dev.block_erase(curr_device, blk, cnt);
-			break;
-		default:
-			BUG();
-		}
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		puts("PARTITION_CONFIG only exists on eMMC\n");
+		return CMD_RET_FAILURE;
+	}
+
+	/* acknowledge to be sent during boot operation */
+	return mmc_set_part_conf(mmc, ack, part_num, access);
+}
+static int do_mmc_rst_func(cmd_tbl_t *cmdtp, int flag,
+			   int argc, char * const argv[])
+{
+	int dev;
+	struct mmc *mmc;
+	u8 enable;
+
+	/*
+	 * Set the RST_n_ENABLE bit of RST_n_FUNCTION
+	 * The only valid values are 0x0, 0x1 and 0x2 and writing
+	 * a value of 0x1 or 0x2 sets the value permanently.
+	 */
+	if (argc != 3)
+		return CMD_RET_USAGE;
+
+	dev = simple_strtoul(argv[1], NULL, 10);
+	enable = simple_strtoul(argv[2], NULL, 10);
+
+	if (enable > 2 || enable < 0) {
+		puts("Invalid RST_n_ENABLE value\n");
+		return CMD_RET_USAGE;
+	}
+
+	mmc = init_mmc_device(dev);
+	if (!mmc)
+		return CMD_RET_FAILURE;
+
+	if (IS_SD(mmc)) {
+		puts("RST_n_FUNCTION only exists on eMMC\n");
+		return CMD_RET_FAILURE;
+	}
 
-		printf("%d blocks %s: %s\n",
-				n, argv[1], (n == cnt) ? "OK" : "ERROR");
-		return (n == cnt) ? 0 : 1;
+	return mmc_set_rst_n_function(mmc, enable);
+}
+#endif
+static int do_mmc_setdsr(cmd_tbl_t *cmdtp, int flag,
+			 int argc, char * const argv[])
+{
+	struct mmc *mmc;
+	u32 val;
+	int ret;
+
+	if (argc != 2)
+		return CMD_RET_USAGE;
+	val = simple_strtoul(argv[2], NULL, 16);
+
+	mmc = find_mmc_device(curr_device);
+	if (!mmc) {
+		printf("no mmc device@slot %x\n", curr_device);
+		return CMD_RET_FAILURE;
+	}
+	ret = mmc_set_dsr(mmc, val);
+	printf("set dsr %s\n", (!ret) ? "OK, force rescan" : "ERROR");
+	if (!ret) {
+		mmc->has_init = 0;
+		if (mmc_init(mmc))
+			return CMD_RET_FAILURE;
+		else
+			return CMD_RET_SUCCESS;
 	}
+	return ret;
+}
+
+static cmd_tbl_t cmd_mmc[] = {
+	U_BOOT_CMD_MKENT(info, 1, 0, do_mmcinfo, "", ""),
+	U_BOOT_CMD_MKENT(read, 4, 1, do_mmc_read, "", ""),
+	U_BOOT_CMD_MKENT(write, 4, 0, do_mmc_write, "", ""),
+	U_BOOT_CMD_MKENT(erase, 3, 0, do_mmc_erase, "", ""),
+	U_BOOT_CMD_MKENT(rescan, 1, 1, do_mmc_rescan, "", ""),
+	U_BOOT_CMD_MKENT(part, 1, 1, do_mmc_part, "", ""),
+	U_BOOT_CMD_MKENT(dev, 3, 0, do_mmc_dev, "", ""),
+	U_BOOT_CMD_MKENT(list, 1, 1, do_mmc_list, "", ""),
+#ifdef CONFIG_SUPPORT_EMMC_BOOT
+	U_BOOT_CMD_MKENT(bootbus, 5, 0, do_mmc_bootbus, "", ""),
+	U_BOOT_CMD_MKENT(bootpart-resize, 3, 0, do_mmc_boot_resize, "", ""),
+	U_BOOT_CMD_MKENT(partconf, 5, 0, do_mmc_partconf, "", ""),
+	U_BOOT_CMD_MKENT(rst-function, 3, 0, do_mmc_rst_func, "", ""),
+#endif
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	U_BOOT_CMD_MKENT(rpmb, CONFIG_SYS_MAXARGS, 1, do_mmcrpmb, "", ""),
+#endif
+	U_BOOT_CMD_MKENT(setdsr, 2, 0, do_mmc_setdsr, "", ""),
+};
+
+static int do_mmcops(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	cmd_tbl_t *cp;
+
+	cp = find_cmd_tbl(argv[1], cmd_mmc, ARRAY_SIZE(cmd_mmc));
+
+	/* Drop the mmc command */
+	argc--;
+	argv++;
+
+	if (cp == NULL || argc > cp->maxargs)
+		return CMD_RET_USAGE;
+	if (flag == CMD_FLAG_REPEAT && !cp->repeatable)
+		return CMD_RET_SUCCESS;
 
-	return CMD_RET_USAGE;
+	if (curr_device < 0) {
+		if (get_mmc_num() > 0) {
+			curr_device = 0;
+		} else {
+			puts("No MMC device available\n");
+			return CMD_RET_FAILURE;
+		}
+	}
+	return cp->cmd(cmdtp, flag, argc, argv);
 }
 
 U_BOOT_CMD(
-	mmc, 6, 1, do_mmcops,
+	mmc, 7, 1, do_mmcops,
 	"MMC sub system",
-	"read addr blk# cnt\n"
+	"info - display info of the current MMC device\n"
+	"mmc read addr blk# cnt\n"
 	"mmc write addr blk# cnt\n"
 	"mmc erase blk# cnt\n"
 	"mmc rescan\n"
@@ -474,6 +674,20 @@ U_BOOT_CMD(
 	" - Change the RST_n_FUNCTION field of the specified device\n"
 	"   WARNING: This is a write-once field and 0 / 1 / 2 are the only valid values.\n"
 #endif
-	"mmc setdsr - set DSR register value\n"
+#ifdef CONFIG_SUPPORT_EMMC_RPMB
+	"mmc rpmb read addr blk# cnt [address of auth-key] - block size is 256 bytes\n"
+	"mmc rpmb write addr blk# cnt <address of auth-key> - block size is 256 bytes\n"
+	"mmc rpmb key <address of auth-key> - program the RPMB authentication key.\n"
+	"mmc rpmb counter - read the value of the write counter\n"
+#endif
+	"mmc setdsr <value> - set DSR register value\n"
 	);
+
+/* Old command kept for compatibility. Same as 'mmc info' */
+U_BOOT_CMD(
+	mmcinfo, 1, 0, do_mmcinfo,
+	"display MMC info",
+	"- display info of the current MMC device"
+);
+
 #endif /* !CONFIG_GENERIC_MMC */
-- 
1.7.6.5