VRPMB-BE: add RPMB protocol

This patch implements RPMB protocol into vRPMB backend
VBS-U. It will handle RPMB request from VRPMB FE.

Signed-off-by: weideng <wei.a.deng@intel.com>
Signed-off-by: yingbinx <yingbinx.zeng@intel.com>
Acked-by: Zhu Bing <bing.zhu@intel.com>

devicemodel/hw/pci/virtio/virtio_rpmb.c

@@ -56,6 +56,7 @@
 #include <fcntl.h>
 #include <sys/ioctl.h>
 #include <linux/types.h>
+#include <rpmb.h>
 
 #define VIRTIO_RPMB_RINGSZ	64
 
@@ -63,6 +64,8 @@ static int virtio_rpmb_debug;
 #define DPRINTF(params) do { if (virtio_rpmb_debug) printf params; } while (0)
 #define WPRINTF(params) (printf params)
 
+#define SEQ_CMD_MAX	3	/*support up to 3 cmds*/
+
 /*
  * virtio-rpmb struct
  */
@@ -77,16 +80,238 @@ struct virtio_rpmb {
 	int vmid;
 };
 
+struct virtio_rpmb_ioctl_cmd {
+	unsigned int cmd;	/*ioctl cmd*/
+	int result;		/* result for ioctl cmd*/
+};
+
+struct virtio_rpmb_ioc_seq_cmd {
+	struct virtio_rpmb_ioctl_cmd ioc;
+	union {
+		__u64 num_of_cmds;	/*num of  seq cmds*/
+		__u64 seq_base;
+	};
+};
+
+struct virtio_rpmb_ioc_seq_data {
+	struct virtio_rpmb_ioc_seq_cmd seq_cmd;
+	struct rpmb_cmd cmds[SEQ_CMD_MAX + 1];
+};
+
+/*
+ * get start address of seq cmds.
+ */
+static struct rpmb_cmd *
+virtio_rpmb_get_seq_cmds(void *pdata)
+{
+	struct virtio_rpmb_ioc_seq_data *seq_data;
+
+	if (!pdata) {
+		DPRINTF(("error, invalid args!\n"));
+		return NULL;
+	}
+
+	seq_data = (struct virtio_rpmb_ioc_seq_data *)pdata;
+
+	return (struct rpmb_cmd *)&seq_data->cmds[0];
+}
+
+/*
+ * get address of seq specail frames.
+ * index:	the index of cmds.
+ */
+static struct rpmb_frame *
+virtio_rpmb_get_seq_frames(void *pdata, __u32 index)
+{
+	struct virtio_rpmb_ioc_seq_data *seq_data;
+	struct rpmb_frame *frames;
+	struct rpmb_cmd *cmds, *cmd;
+	__u64 ncmds;
+	__u32 offset = 0;
+	__u32 num = 0;
+
+	if (!pdata || index > SEQ_CMD_MAX) {
+		DPRINTF(("error, invalid args!\n"));
+		return NULL;
+	}
+
+	seq_data = (struct virtio_rpmb_ioc_seq_data *)pdata;
+
+	/* get number of cmds.*/
+	ncmds = seq_data->seq_cmd.num_of_cmds;
+	if (ncmds > SEQ_CMD_MAX) {
+		DPRINTF(("error, ncmds(%llu) > max\n", ncmds));
+		return NULL;
+	}
+
+	/* get start address of cmds.*/
+	cmds = virtio_rpmb_get_seq_cmds(pdata);
+	if (!cmds) {
+		DPRINTF(("fail to get seq cmds.\n"));
+		return NULL;
+	}
+
+	/* get start address of frames.*/
+	frames = (struct rpmb_frame *)&seq_data->cmds[ncmds + 1];
+	if (!frames) {
+		DPRINTF(("error, invalid frames ptr.\n"));
+		return NULL;
+	}
+
+	for (num = 0; num < index; num++) {
+		cmd = &cmds[num];
+		if (!cmd) {
+			DPRINTF(("error, invalid cmd ptr.\n"));
+			return NULL;
+		}
+		offset += cmd->nframes;
+	}
+
+	return (struct rpmb_frame *)&frames[offset];
+}
+
+/*
+ * get address of all seq data.
+ */
+static void *
+virtio_rpmb_get_seq_data(void *pdata)
+{
+	struct virtio_rpmb_ioc_seq_data *seq_data;
+
+	if (!pdata) {
+		DPRINTF(("error, invalid args!\n"));
+		return NULL;
+	}
+
+	seq_data = (struct virtio_rpmb_ioc_seq_data *)pdata;
+
+	return (void *)&seq_data->seq_cmd.seq_base;
+}
+
+/*
+ * Address space is different between
+ * SOS VBS-U and UOS kernel.
+ * Update frames_ptr to current space.
+ */
+static int
+virtio_rpmb_map_seq_frames(struct iovec *iov)
+{
+	struct virtio_rpmb_ioc_seq_cmd *seq_cmd = NULL;
+	struct rpmb_cmd *cmds, *cmd;
+	__u64 index;
+
+	if (!iov) {
+		DPRINTF(("error, invalid arg!\n"));
+		return -1;
+	}
+
+	seq_cmd = (struct virtio_rpmb_ioc_seq_cmd *)(iov->iov_base);
+	if (!seq_cmd || seq_cmd->num_of_cmds > SEQ_CMD_MAX) {
+		DPRINTF(("found invalid data.\n"));
+		return -1;
+	}
+
+	/* get start address of cmds.*/
+	cmds = virtio_rpmb_get_seq_cmds(iov->iov_base);
+	if (!cmds) {
+		DPRINTF(("fail to get seq cmds.\n"));
+		return -1;
+	}
+
+	/* set frames_ptr to VBS-U space.*/
+	for (index = 0; index < seq_cmd->num_of_cmds; index++) {
+		cmd = &cmds[index];
+		if (!cmd) {
+			DPRINTF(("error, invalid cmd ptr.\n"));
+			return -1;
+		}
+		/* set frames address.*/
+		cmd->frames = virtio_rpmb_get_seq_frames(iov->iov_base, index);
+		if (!cmd->frames) {
+			DPRINTF(("fail to get frames[%llu] ptr!\n", index));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int
+virtio_rpmb_seq_handler(struct virtio_rpmb *rpmb, struct iovec *iov)
+{
+	struct virtio_rpmb_ioctl_cmd *ioc = NULL;
+	int fd;
+	void *pdata;
+	int rc;
+
+	if (!rpmb || !iov) {
+		DPRINTF(("found invalid args!!!\n"));
+		return -1;
+	}
+
+	/* update frames_ptr to curent space*/
+	rc = virtio_rpmb_map_seq_frames(iov);
+	if (rc) {
+		DPRINTF(("fail to map seq frames\n"));
+		return rc;
+	}
+
+	ioc = (struct virtio_rpmb_ioctl_cmd *)(iov->iov_base);
+	if (!ioc) {
+		DPRINTF(("error, get ioc is NULL!\n"));
+		return -1;
+	}
+
+	pdata = virtio_rpmb_get_seq_data(iov->iov_base);
+	if (!pdata) {
+		DPRINTF(("fail to get seq data\n"));
+		return -1;
+	}
+
+	/* open rpmb device.*/
+	rc = open("/dev/rpmb0", O_RDWR | O_NONBLOCK);
+	if (rc < 0) {
+		DPRINTF(("failed to open /dev/rpmb0.\n"));
+		return rc;
+	}
+	fd = rc;
+
+	/* send ioctl cmd.*/
+	rc = ioctl(fd, ioc->cmd, pdata);
+	if (rc)
+		DPRINTF(("seq ioctl cmd failed(%d).\n", rc));
+
+	/* close rpmb device.*/
+	close(fd);
+
+	return rc;
+}
+
 static void
 virtio_rpmb_notify(void *base, struct virtio_vq_info *vq)
 {
 	struct iovec iov;
 	int len;
 	uint16_t idx;
+	struct virtio_rpmb *rpmb = (struct virtio_rpmb *)base;
+	struct virtio_rpmb_ioctl_cmd *ioc;
 
 	while (vq_has_descs(vq)) {
 		vq_getchain(vq, &idx, &iov, 1, NULL);
 
+		ioc = (struct virtio_rpmb_ioctl_cmd *)(iov.iov_base);
+
+		switch (ioc->cmd) {
+		case RPMB_IOC_SEQ_CMD:
+			ioc->result = virtio_rpmb_seq_handler(rpmb, &iov);
+			break;
+
+		default:
+			DPRINTF(("found unsupported ioctl(%u)!\n", ioc->cmd));
+			ioc->result = -1;
+			break;
+		}
+
 		len = iov.iov_len;
 		assert(len > 0);