Enable UFS RPMB-backed OP-TEE secure storage on Genio 720

index d9d35a06439..508eb74d976 100644
@@ -157,16 +157,25 @@ int ufs_rpmb_route_frames(struct udevice *scsi_dev, unsigned int region,
 	int ret;
 
 	if (!scsi_dev || reqlen % RPMB_FRAME_SIZE ||
-	    rsplen % RPMB_FRAME_SIZE || !n_req)
+	    rsplen % RPMB_FRAME_SIZE || !n_req || !n_rsp)
 		return -EINVAL;
 
 	request = get_unaligned_be16((u8 *)req + RPMB_FRAME_REQ_OFFSET);
 
+	/*
+	 * Enforce Linux's frame counts before any I/O: a key-programming
+	 * request and a write counter read are a single frame answered by a
+	 * single frame, a data write may span several frames but returns one
+	 * result, and a data read is one request returning several frames.
+	 */
 	switch (request) {
 	case RPMB_REQ_KEY:
 	case RPMB_REQ_WRITE_DATA: {
 		u8 status_frame[RPMB_FRAME_SIZE];
 
+		if (n_rsp != 1 || (request == RPMB_REQ_KEY && n_req != 1))
+			return -EINVAL;
+
 		ret = ufs_rpmb_send(scsi_dev, region, req, n_req);
 		if (ret)
 			return ret;

@@ -178,20 +187,17 @@ int ufs_rpmb_route_frames(struct udevice *scsi_dev, unsigned int region,
 		if (ret)
 			return ret;
 
-		if (n_rsp < 1)
-			return -EINVAL;
-
 		return ufs_rpmb_recv(scsi_dev, region, rsp, 1);
 	}
 	case RPMB_REQ_WCOUNTER:
 	case RPMB_REQ_READ_DATA:
+		if (n_req != 1 || (request == RPMB_REQ_WCOUNTER && n_rsp != 1))
+			return -EINVAL;
+
 		ret = ufs_rpmb_send(scsi_dev, region, req, 1);
 		if (ret)
 			return ret;
 
-		if (n_rsp < 1)
-			return -EINVAL;
-
 		return ufs_rpmb_recv(scsi_dev, region, rsp, n_rsp);
 	default:
 		debug("ufs-rpmb: unsupported request 0x%x\n", request);

index 508eb74d976..8b61760f071 100644
@@ -270,6 +270,8 @@ static void ufs_rpmb_string_to_ascii(const u8 *raw, char *out, size_t outsz)
 	for (i = 0; i < nchars && n < (int)outsz - 1; i++) {
 		u16 c = get_unaligned_be16(raw + QUERY_DESC_HDR_SIZE + i * 2);
 
+		if (!c)
+			break;
 		out[n++] = (c >= 0x20 && c <= 0x7e) ? (char)c : ' ';
 	}
 	out[n] = '\0';

index a47c76811cd..2abdfd99e5d 100644
@@ -829,6 +829,8 @@ static int optee_probe(struct udevice *dev)
 	u32 sec_caps;
 	int ret;
 
+	optee_suppl_rpmb_init(dev);
+
 	if (!is_optee_api(pdata->invoke_fn)) {
 		dev_err(dev, "OP-TEE api uid mismatch\n");
 		return -ENOENT;

index 5c72b280058..3f9fea532e4 100644
@@ -34,7 +34,8 @@
 /**
  * struct optee_private - OP-TEE driver private data
  * @rpmb_next_region:	next UFS RPMB region to report on PROBE_NEXT
- * @rpmb_cur_region:	UFS RPMB region selected by the last PROBE_NEXT
+ * @rpmb_cur_region:	UFS RPMB region selected by the last PROBE_NEXT, or
+ *			UFS_RPMB_NUM_REGIONS while no region is selected
  */
 struct optee_private {
 	unsigned int rpmb_next_region;

@@ -97,6 +98,8 @@ static inline void optee_suppl_rpmb_release(struct udevice *dev)
 #endif
 
 #ifdef CONFIG_SUPPORT_UFS_RPMB
+void optee_suppl_rpmb_init(struct udevice *dev);
+
 void optee_suppl_cmd_rpmb_probe_reset(struct udevice *dev,
 				      struct optee_msg_arg *arg);
 

@@ -106,6 +109,10 @@ void optee_suppl_cmd_rpmb_probe_next(struct udevice *dev,
 void optee_suppl_cmd_rpmb_frames(struct udevice *dev,
 				 struct optee_msg_arg *arg);
 #else
+static inline void optee_suppl_rpmb_init(struct udevice *dev)
+{
+}
+
 static inline void optee_suppl_cmd_rpmb_probe_reset(struct udevice *dev,
 						    struct optee_msg_arg *arg)
 {

index 0c820e9595d..fbb56305609 100644
@@ -8,6 +8,13 @@
 #include "optee_msg_supplicant.h"
 #include "optee_private.h"
 
+void optee_suppl_rpmb_init(struct udevice *dev)
+{
+	struct optee_private *priv = dev_get_priv(dev);
+
+	priv->rpmb_cur_region = UFS_RPMB_NUM_REGIONS;
+}
+
 static int optee_rpmb_get_dev(struct udevice **scsi_devp)
 {
 	struct udevice *ufs_dev, *scsi_dev;

@@ -32,6 +39,7 @@ void optee_suppl_cmd_rpmb_probe_reset(struct udevice *dev,
 {
 	struct optee_private *priv = dev_get_priv(dev);
 
+	priv->rpmb_cur_region = UFS_RPMB_NUM_REGIONS;
 	if (arg->num_params != 1 ||
 	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT) {
 		arg->ret = TEE_ERROR_BAD_PARAMETERS;

@@ -39,7 +47,6 @@ void optee_suppl_cmd_rpmb_probe_reset(struct udevice *dev,
 	}
 
 	priv->rpmb_next_region = 0;
-	priv->rpmb_cur_region = 0;
 
 	arg->params[0].u.value.a = OPTEE_MSG_RPC_SHM_TYPE_APPL;
 	arg->ret = TEE_SUCCESS;

@@ -57,6 +64,7 @@ void optee_suppl_cmd_rpmb_probe_next(struct udevice *dev,
 	ulong cid_size;
 	int ret;
 
+	priv->rpmb_cur_region = UFS_RPMB_NUM_REGIONS;
 	if (arg->num_params != 2 ||
 	    arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT ||
 	    arg->params[1].attr != OPTEE_MSG_ATTR_TYPE_RMEM_OUTPUT) {

@@ -78,7 +86,7 @@ void optee_suppl_cmd_rpmb_probe_next(struct udevice *dev,
 	}
 
 	while (priv->rpmb_next_region < UFS_RPMB_NUM_REGIONS) {
-		unsigned int region = priv->rpmb_next_region++;
+		unsigned int region = priv->rpmb_next_region;
 
 		ret = ufs_rpmb_get_region_info(scsi_dev, region, &size_mult,
 					       &rel_wr, cid_buf);

@@ -86,6 +94,7 @@ void optee_suppl_cmd_rpmb_probe_next(struct udevice *dev,
 			arg->ret = TEE_ERROR_GENERIC;
 			return;
 		}
+		priv->rpmb_next_region++;
 		if (!ret)
 			continue;
 

@@ -120,6 +129,11 @@ void optee_suppl_cmd_rpmb_frames(struct udevice *dev,
 		return;
 	}
 
+	if (priv->rpmb_cur_region >= UFS_RPMB_NUM_REGIONS) {
+		arg->ret = TEE_ERROR_ITEM_NOT_FOUND;
+		return;
+	}
+
 	if (optee_rpmb_get_dev(&scsi_dev)) {
 		arg->ret = TEE_ERROR_ITEM_NOT_FOUND;
 		return;

index 2abdfd99e5d..53806670b6a 100644
@@ -194,13 +194,17 @@ static int bind_service_drivers(struct udevice *dev)
 	int ret, ret2, ret3 = 0;
 	u32 tee_sess;
 
+	if (!ll_entry_count(struct optee_service, optee_service))
+		return 0;
+
 	ret = optee_open_enum_session(dev, &tee_sess);
 	if (ret)
 		return ret;
 
 	ret = optee_bind_services(dev, tee_sess, PTA_CMD_GET_DEVICES);
 	ret2 = optee_bind_services(dev, tee_sess, PTA_CMD_GET_DEVICES_SUPP);
-	if (CONFIG_IS_ENABLED(SUPPORT_EMMC_RPMB))
+	if (CONFIG_IS_ENABLED(SUPPORT_EMMC_RPMB) ||
+	    CONFIG_IS_ENABLED(SUPPORT_UFS_RPMB))
 		ret3 = optee_bind_services(dev, tee_sess,
 					   PTA_CMD_GET_DEVICES_RPMB);