[PATCH v5 4/6] ufs: derive the per-region RPMB CID and size for OP-TEE

[Jorge Ramirez-Ortiz <jorge.ramirez@oss.qualcomm.com>]

OP-TEE derives its RPMB authentication key from a device identifier, so
U-Boot must supply the exact identifier the Linux kernel uses; otherwise
the keys differ and OP-TEE cannot access RPMB storage provisioned under
Linux, or vice versa. The RPMB probe also needs each region's size and
reliable-write count to enumerate the regions. Provide both so secure
storage stays interoperable between U-Boot and Linux on the same device.

Signed-off-by: Jorge Ramirez-Ortiz <jorge.ramirez@oss.qualcomm.com>
---
 drivers/ufs/Kconfig    |   7 +-
 drivers/ufs/ufs-rpmb.c | 181 +++++++++++++++++++++++++++++++++++++++++
 include/ufs.h          |   6 ++
 3 files changed, 192 insertions(+), 2 deletions(-)

diff --git a/drivers/ufs/Kconfig b/drivers/ufs/Kconfig
index a0067b1f203..648b58b46e4 100644
--- a/drivers/ufs/Kconfig
+++ b/drivers/ufs/Kconfig
@@ -95,11 +95,14 @@ config UFS_TI_J721E
 config SUPPORT_UFS_RPMB
 	bool "Enable UFS RPMB (Replay Protected Memory Block) support"
 	depends on UFS && OPTEE && !SUPPORT_EMMC_RPMB
+	select BLAKE2
 	help
 	  Route OP-TEE RPMB requests to the UFS RPMB Well-Known LUN using
 	  SCSI SECURITY PROTOCOL IN/OUT commands. Required for OP-TEE secure
 	  storage (CFG_RPMB_FS) on UFS-based platforms that have no eMMC.
-	  The OP-TEE supplicant handles a single RPMB transport, so this is
-	  mutually exclusive with the eMMC RPMB supplicant (SUPPORT_EMMC_RPMB).
+	  BLAKE2 is used to derive the fixed-length RPMB CID that matches the
+	  Linux kernel UFS device_id ABI. The OP-TEE supplicant handles a
+	  single RPMB transport, so this is mutually exclusive with the eMMC
+	  RPMB supplicant (SUPPORT_EMMC_RPMB).
 
 endmenu
diff --git a/drivers/ufs/ufs-rpmb.c b/drivers/ufs/ufs-rpmb.c
index e6e0b2aec33..18230786abe 100644
--- a/drivers/ufs/ufs-rpmb.c
+++ b/drivers/ufs/ufs-rpmb.c
@@ -1,13 +1,16 @@
 // SPDX-License-Identifier: GPL-2.0+
 #include <dm.h>
+#include <hexdump.h>
 #include <log.h>
 #include <malloc.h>
 #include <scsi.h>
 #include <ufs.h>
+#include <vsprintf.h>
 #include <asm/cache.h>
 #include <asm/unaligned.h>
 #include <linux/errno.h>
 #include <linux/string.h>
+#include <u-boot/blake2.h>
 #include "ufs.h"
 
 #define RPMB_REQ_KEY		1
@@ -24,6 +27,17 @@
 #define SEC_PROTOCOL_UFS		0xEC
 #define UFS_RPMB_SEC_PROTOCOL_ID	0x01
 
+#define GEOMETRY_DESC_RPMB_RW_SIZE	0x17
+
+#define RPMB_UNIT_DESC_LOGICAL_BLK_SIZE		0x0A
+#define RPMB_UNIT_DESC_LOGICAL_BLK_COUNT	0x0B
+#define RPMB_UNIT_DESC_REGION0_SIZE		0x13
+#define RPMB_UNIT_DESC_REGION1_SIZE		0x14
+#define RPMB_UNIT_DESC_REGION2_SIZE		0x15
+#define RPMB_UNIT_DESC_REGION3_SIZE		0x16
+#define UFS_RPMB_LEGACY_SPEC_VER		0x0220
+#define UFS_RPMB_REGION_UNIT_SHIFT		17
+
 static u8 ufs_rpmb_sense_key(const struct scsi_cmd *pccb)
 {
 	const u8 *sense = pccb->sense_buf;
@@ -165,3 +179,170 @@ int ufs_rpmb_route_frames(struct udevice *scsi_dev, unsigned int region,
 		return -EINVAL;
 	}
 }
+
+static int ufs_rpmb_read_geometry(struct udevice *scsi_dev, u8 *rpmb_rw_size)
+{
+	struct ufs_hba *hba = dev_get_uclass_priv(scsi_dev->parent);
+	u8 desc[QUERY_DESC_GEOMETRY_DEF_SIZE];
+	int ret;
+
+	ret = ufshcd_read_desc_param(hba, QUERY_DESC_IDN_GEOMETRY, 0, 0,
+				     desc, sizeof(desc));
+	if (ret)
+		return ret;
+
+	*rpmb_rw_size = desc[GEOMETRY_DESC_RPMB_RW_SIZE];
+
+	return 0;
+}
+
+static int ufs_rpmb_read_region_sizes(struct ufs_hba *hba, u16 spec_ver,
+				      u8 sizes[UFS_RPMB_NUM_REGIONS])
+{
+	u8 unit[QUERY_DESC_UNIT_DEF_SIZE] = { };
+	int ret;
+
+	ret = ufshcd_read_desc_param(hba, QUERY_DESC_IDN_UNIT,
+				     UFS_UPIU_RPMB_WLUN, 0, unit, sizeof(unit));
+	if (ret)
+		return ret;
+
+	memset(sizes, 0, UFS_RPMB_NUM_REGIONS);
+
+	if (spec_ver > UFS_RPMB_LEGACY_SPEC_VER) {
+		sizes[0] = unit[RPMB_UNIT_DESC_REGION0_SIZE];
+		sizes[1] = unit[RPMB_UNIT_DESC_REGION1_SIZE];
+		sizes[2] = unit[RPMB_UNIT_DESC_REGION2_SIZE];
+		sizes[3] = unit[RPMB_UNIT_DESC_REGION3_SIZE];
+	} else {
+		u8 blk_shift = unit[RPMB_UNIT_DESC_LOGICAL_BLK_SIZE];
+		u64 region;
+
+		/*
+		 * bLogicalBlockSize is a device-supplied log2 block size;
+		 * reject values that would make the shift below undefined.
+		 */
+		if (blk_shift >= 64)
+			return -EINVAL;
+
+		region = get_unaligned_be64(unit +
+					    RPMB_UNIT_DESC_LOGICAL_BLK_COUNT);
+		region <<= blk_shift;
+		region >>= UFS_RPMB_REGION_UNIT_SHIFT;
+
+		sizes[0] = region > 0xff ? 0xff : region;
+	}
+
+	return 0;
+}
+
+static void ufs_rpmb_string_to_ascii(const u8 *raw, char *out, size_t outsz)
+{
+	int nchars = ((int)raw[QUERY_DESC_LENGTH_OFFSET] - QUERY_DESC_HDR_SIZE);
+	int i, n = 0;
+
+	nchars = nchars > 0 ? nchars / 2 : 0;
+	for (i = 0; i < nchars && n < (int)outsz - 1; i++) {
+		u16 c = get_unaligned_be16(raw + QUERY_DESC_HDR_SIZE + i * 2);
+
+		out[n++] = (c >= 0x20 && c <= 0x7e) ? (char)c : ' ';
+	}
+	out[n] = '\0';
+}
+
+static int ufs_rpmb_build_cid(struct ufs_hba *hba, const u8 *dev_desc,
+			      unsigned int region, u8 *cid)
+{
+	char serial_hex[QUERY_DESC_MAX_SIZE * 2 + 1];
+	u16 manf_id, spec_ver, dev_ver, manf_date;
+	u8 serial[QUERY_DESC_MAX_SIZE] = { };
+	char idstr[QUERY_DESC_MAX_SIZE * 3];
+	char model[MAX_MODEL_LEN * 8];
+	u8 raw[QUERY_DESC_MAX_SIZE];
+	u8 blen;
+	int ret;
+
+	manf_date = get_unaligned_be16(dev_desc + DEVICE_DESC_PARAM_MANF_DATE);
+	spec_ver = get_unaligned_be16(dev_desc + DEVICE_DESC_PARAM_SPEC_VER);
+	manf_id = get_unaligned_be16(dev_desc + DEVICE_DESC_PARAM_MANF_ID);
+	dev_ver = get_unaligned_be16(dev_desc + DEVICE_DESC_PARAM_DEV_VER);
+
+	ret = ufshcd_read_desc_param(hba, QUERY_DESC_IDN_STRING,
+				     dev_desc[DEVICE_DESC_PARAM_PRDCT_NAME], 0,
+				     raw, sizeof(raw));
+	if (ret)
+		return ret;
+
+	ufs_rpmb_string_to_ascii(raw, model, sizeof(model));
+
+	ret = ufshcd_read_desc_param(hba, QUERY_DESC_IDN_STRING,
+				     dev_desc[DEVICE_DESC_PARAM_SN], 0,
+				     raw, sizeof(raw));
+	if (ret)
+		return ret;
+
+	blen = raw[QUERY_DESC_LENGTH_OFFSET];
+	if (blen < QUERY_DESC_HDR_SIZE)
+		return -EINVAL;
+
+	memcpy(serial, raw + QUERY_DESC_HDR_SIZE, blen - QUERY_DESC_HDR_SIZE);
+	/*
+	 * The identifier format is an ABI shared with the secure world. The
+	 * kernel's ufshcd_create_device_id() hex-encodes the full descriptor
+	 * length (header included), so the serial always carries two trailing
+	 * zero bytes; reproduce that here so the derived CID matches.
+	 */
+	bin2hex(serial_hex, serial, blen);
+	serial_hex[blen * 2] = '\0';
+
+	snprintf(idstr, sizeof(idstr), "%04X-%04X-%s-%s-%04X-%04X-R%u",
+		 manf_id, spec_ver, model, serial_hex, dev_ver, manf_date,
+		 region);
+
+	if (blake2b(cid, UFS_RPMB_CID_SIZE, idstr, strlen(idstr), NULL, 0))
+		return -EIO;
+
+	return 0;
+}
+
+int ufs_rpmb_get_region_info(struct udevice *scsi_dev, unsigned int region,
+			     u8 *size_mult, u8 *rel_wr, u8 *cid)
+{
+	struct ufs_hba *hba = dev_get_uclass_priv(scsi_dev->parent);
+	u8 dev_desc[QUERY_DESC_DEVICE_DEF_SIZE] = { };
+	u8 sizes[UFS_RPMB_NUM_REGIONS];
+	u16 spec_ver;
+	int ret;
+
+	if (region >= UFS_RPMB_NUM_REGIONS)
+		return 0;
+
+	ret = ufshcd_read_desc_param(hba, QUERY_DESC_IDN_DEVICE, 0, 0,
+				     dev_desc, sizeof(dev_desc));
+	if (ret)
+		return ret;
+
+	spec_ver = get_unaligned_be16(dev_desc + DEVICE_DESC_PARAM_SPEC_VER);
+
+	ret = ufs_rpmb_read_region_sizes(hba, spec_ver, sizes);
+	if (ret)
+		return ret;
+
+	if (!sizes[region])
+		return 0;
+
+	ret = ufs_rpmb_read_geometry(scsi_dev, rel_wr);
+	if (ret)
+		return ret;
+
+	if (!*rel_wr)
+		*rel_wr = 1;
+
+	ret = ufs_rpmb_build_cid(hba, dev_desc, region, cid);
+	if (ret)
+		return ret;
+
+	*size_mult = sizes[region];
+
+	return 1;
+}
diff --git a/include/ufs.h b/include/ufs.h
index 1bec3ce73a4..a8b7d370f78 100644
--- a/include/ufs.h
+++ b/include/ufs.h
@@ -20,8 +20,14 @@ int ufs_probe(void);
  */
 int ufs_probe_dev(int index);
 
+#define UFS_RPMB_CID_SIZE	16
+#define UFS_RPMB_NUM_REGIONS	4
+
 int ufs_rpmb_route_frames(struct udevice *scsi_dev, unsigned int region,
 			  void *req, unsigned long reqlen, void *rsp,
 			  unsigned long rsplen);
 
+int ufs_rpmb_get_region_info(struct udevice *scsi_dev, unsigned int region,
+			     u8 *size_mult, u8 *rel_wr, u8 *cid);
+
 #endif
-- 
2.54.0