DM: Refactor RPMB files

Move rpmb_sim.c and rpmb_backend.c to hw/platform/rpmb/ Signed-off-by: Huang Yang <yang.huang@intel.com> Reviewed-by: Kevin Tian <kevin.tian@intel.com>
2026-06-07 09:41:30 +00:00 · 2018-05-27 17:32:06 -04:00
parent 113ece2854
commit 7003e50e4e
3 changed files with 2 additions and 2 deletions
--- a/devicemodel/hw/platform/rpmb/rpmb_backend.c
+++ b/devicemodel/hw/platform/rpmb/rpmb_backend.c
@@ -0,0 +1,530 @@
+/*
+ * Copyright (C) 2018 Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    this position and unchanged.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <string.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <stdbool.h>
+
+#include "rpmb.h"
+#include "rpmb_sim.h"
+#include "vrpmb.h"
+#include "rpmb_backend.h"
+
+static int virtio_rpmb_debug = 1;
+#define DPRINTF(params) do { if (virtio_rpmb_debug) printf params; } while (0)
+#define WPRINTF(params) (printf params)
+
+#define READ_STR_LEN 10
+#define WRITE_STR_LEN 11
+static uint32_t phy_counter = 0;
+static uint32_t virt_counter = 0;
+static uint8_t rpmb_key[RPMB_KEY_32_LEN] = {0};
+static uint8_t virt_rpmb_key[RPMB_KEY_32_LEN] = {0};
+static uint16_t g_rpmb_mode = RPMB_SIM_MODE;
+static const char READ_DATA_STR[READ_STR_LEN] = "read data";
+static const char WRITE_DATA_STR[WRITE_STR_LEN] = "write data";
+
+//TODO: will be read from config file.
+static uint16_t get_uos_count(void)
+{
+	return 1;
+}
+
+//TODO: will be predefined and read from config file.
+static uint16_t get_rpmb_blocks(void)
+{
+	return rpmb_get_blocks();
+}
+
+//TODO: hardcode keybox size. It will be read from keybox header from RPMB.
+static uint16_t get_common_blocks(void)
+{
+	uint16_t kb_blocks;
+	uint32_t kb_size = 15872;
+
+	kb_blocks = (kb_size + (RPMB_BLOCK_SIZE -1)) / RPMB_BLOCK_SIZE;
+	//reserve for simulated rpmb + KBox header + padding
+	return kb_blocks + 1 + 1 + 1;
+}
+
+static uint16_t get_accessible_blocks(void)
+{
+	return (get_rpmb_blocks() - get_common_blocks()) /
+			get_uos_count() + get_common_blocks();
+}
+
+/* Todo: To get the uos number, e.g. No.0 or No.1, which is
+  used for calculating UOS RPMB range address.
+  But this will be removed after config file is supported.
+  We plan to predefine such info and save to config file.
+*/
+static uint8_t get_uos_id(void)
+{
+	return (get_uos_count() - 1);
+}
+
+void rpmb_mode_init(uint16_t mode)
+{
+	g_rpmb_mode = mode;
+	DPRINTF(("%s: rpmb mode is %d\n", __func__, g_rpmb_mode));
+}
+
+void rpmb_counter_init(uint32_t counter)
+{
+	phy_counter = counter;
+}
+
+static bool is_key_programmed(void)
+{
+	if (g_rpmb_mode == RPMB_PHY_MODE) {
+		return true;
+	} else if (g_rpmb_mode == RPMB_SIM_MODE) {
+		if (is_use_sim_rpmb())
+			return true;
+	}
+
+	DPRINTF(("%s: rpmb mode 0x%x is unsupported\n", __func__, g_rpmb_mode));
+	return false;
+}
+
+static uint16_t get_phy_addr(uint8_t uos_id, uint16_t vaddr)
+{
+	uint16_t common_blocks = get_common_blocks();
+	uint16_t accessible_blocks = get_accessible_blocks();
+
+	if (vaddr < get_common_blocks()) {
+		return vaddr;
+	} else {
+		return (((accessible_blocks - common_blocks) * uos_id) + vaddr);
+	}
+}
+
+int get_virt_rpmb_key(void)
+{
+	int rc = -1;
+	uint8_t key[RPMB_KEY_LEN];
+
+	rc = get_vrpmb_key(key, sizeof(key));
+	if (rc == 0){
+		DPRINTF(("%s: get uos key fail\n", __func__));
+	}
+
+	memcpy(virt_rpmb_key, key, RPMB_KEY_32_LEN);
+	memset(key, 0, RPMB_KEY_LEN);
+	return rc;
+}
+
+static int rpmb_replace_frame(struct rpmb_frame *frames, uint32_t frame_cnt,
+	const uint8_t *key, const uint8_t *nonce, const uint32_t *write_counter,
+	const uint16_t *addr, const uint16_t *block_count, const int *result, const int *req_resp)
+{
+	uint32_t i;
+
+	for (i = 0; i < frame_cnt; i++) {
+		if (nonce)
+			memcpy(frames[i].nonce, nonce, sizeof(frames[i].nonce));
+		if (write_counter)
+			frames[i].write_counter = swap32(*write_counter);
+		if (addr)
+			frames[i].addr = swap16(*addr);
+		if (block_count)
+			frames[i].block_count = swap16(*block_count);
+		if (result)
+			frames[i].result = swap16(*result);
+		if (req_resp)
+			frames[i].req_resp = swap16(*req_resp);
+	}
+
+	if (key) {
+		if (rpmb_mac(key, frames, frame_cnt, frames[frame_cnt - 1].key_mac)) {
+			DPRINTF(("%s: rpmb_mac failed\n", __func__));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int rpmb_check_frame(const char *cmd_str, int *err,
+		const struct rpmb_frame *frames, uint32_t frame_cnt,
+		const uint8_t *key, const uint32_t *write_counter,
+		const uint16_t *addr, const uint16_t *block_count)
+{
+	uint32_t i;
+	uint8_t mac[32];
+
+	for (i = 0; i < frame_cnt; i++) {
+		if (write_counter && *write_counter != swap32(frames[i].write_counter)) {
+			*err = RPMB_RES_COUNT_FAILURE;
+			DPRINTF(("%s: Bad write counter %u\n", cmd_str, *write_counter));
+			return -1;
+		}
+	}
+
+	if (addr && *addr >= get_accessible_blocks()) {
+		*err = RPMB_RES_ADDR_FAILURE;
+		 DPRINTF(("%s: Bad addr, got %u, expected %u\n",
+			cmd_str, swap16(frames[i].addr), *addr));
+		return -1;
+	}
+
+	if (addr && block_count && (*addr + *block_count) > get_accessible_blocks()) {
+		*err = RPMB_RES_GENERAL_FAILURE;
+		 DPRINTF(("%s: Bad block count %u\n",
+			cmd_str, *block_count));
+		return -1;
+	}
+
+	if (addr && !memcmp(cmd_str, WRITE_DATA_STR, sizeof(WRITE_DATA_STR))) {
+		if (*addr < get_common_blocks()) {
+			*err = RPMB_RES_WRITE_FAILURE;
+			DPRINTF(("%s: Common block is read only\n", cmd_str));
+			return -1;
+		}
+	}
+
+	if (key) {
+		if (rpmb_mac(key, frames, frame_cnt, mac)) {
+			*err = RPMB_RES_GENERAL_FAILURE;
+			DPRINTF(("%s: rpmb_mac failed\n", cmd_str));
+			return -1;
+		}
+
+		if (memcmp(frames[frame_cnt - 1].key_mac, mac, sizeof(mac))) {
+			*err = RPMB_RES_AUTH_FAILURE;
+			DPRINTF(("%s: Bad MAC\n", cmd_str));
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+static int rpmb_phy_ioctl(uint32_t ioc_cmd, void* seq_data)
+{
+	int rc = -1;
+	int fd;
+
+	if (seq_data == NULL) {
+		DPRINTF(("%s: seq_data is NULL\n", __func__));
+		return rc;
+	}
+
+	/* open rpmb device.*/
+	fd = open(RPMB_PHY_PATH_NAME, O_RDWR | O_NONBLOCK);
+	if (fd < 0) {
+		DPRINTF(("%s: failed to open %s.\n", __func__, RPMB_PHY_PATH_NAME));
+		return fd;
+	}
+
+	/* send ioctl cmd.*/
+	rc = ioctl(fd, ioc_cmd, seq_data);
+	if (rc)
+		DPRINTF(("%s: seq ioctl cmd failed(%d).\n", __func__, rc));
+
+	/* close rpmb device.*/
+	close(fd);
+
+	return rc;
+}
+
+static int rpmb_sim_ioctl(uint32_t ioc_cmd, void* seq_data)
+{
+	if (seq_data == NULL) {
+		DPRINTF(("%s: seq_data is NULL\n", __func__));
+		return -1;
+	}
+
+	switch (ioc_cmd) {
+	case RPMB_IOC_REQ_CMD:
+	case RPMB_IOC_SEQ_CMD:
+		return rpmb_sim_send(seq_data);
+	default:
+		DPRINTF(("%s: ioctl 0x%x is unsupported\n", __func__, ioc_cmd));
+		return -1;
+	}
+}
+
+static int rpmb_virt_ioctl(uint32_t ioc_cmd, void* seq_data)
+{
+	if (g_rpmb_mode == RPMB_PHY_MODE) {
+		return rpmb_phy_ioctl(ioc_cmd, seq_data);
+	} else if (g_rpmb_mode == RPMB_SIM_MODE) {
+		return rpmb_sim_ioctl(ioc_cmd, seq_data);
+	}
+
+	DPRINTF(("%s: rpmb mode 0x%x is unsupported\n", __func__, g_rpmb_mode));
+	return -1;
+}
+
+static int rpmb_virt_write(uint32_t ioc_cmd, void* seq_data,
+		struct rpmb_frame* in_frame, uint32_t in_cnt,
+		struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int err = RPMB_RES_WRITE_FAILURE;
+	int resp = RPMB_RESP_DATA_WRITE;
+	uint16_t vaddr;
+	uint16_t paddr;
+	uint16_t block_count;
+	uint8_t uos_id;
+	__u16 rpmb_result = 0;
+	int rc;
+
+	if (in_cnt == 0 || in_frame == NULL || seq_data == NULL) {
+		DPRINTF(("%s: in_frame, in_cnt or seq_data is not available.\n", __func__));
+		return -1;
+	}
+
+	if (out_cnt > 0 && out_frame != NULL) {
+		memset(out_frame, 0, out_cnt * RPMB_FRAME_SIZE);
+	} else {
+		DPRINTF(("%s: vrpmb must be aware of the result in out_frame.\n", __func__));
+		return -1;
+	}
+
+	uos_id = get_uos_id();
+
+	vaddr = swap16(in_frame->addr);
+	block_count = in_cnt;
+	if (rpmb_check_frame(WRITE_DATA_STR, &err, in_frame, in_cnt,
+			virt_rpmb_key, &virt_counter, &vaddr, &block_count))
+		goto out;
+
+	paddr = get_phy_addr(uos_id, vaddr);
+
+	if (rpmb_replace_frame(in_frame, in_cnt, rpmb_key, NULL,
+			&phy_counter, &paddr, NULL, NULL, NULL))
+	{
+		err = RPMB_RES_GENERAL_FAILURE;
+		goto out;
+	}
+
+	if (rpmb_virt_ioctl(ioc_cmd, seq_data)) {
+		DPRINTF(("%s: rpmb virt ioctl failed.\n", __func__));
+		return -1;
+	}
+
+	if (out_frame->result == swap16(RPMB_RES_COUNT_FAILURE)) {
+		memset(out_frame, 0, out_cnt * RPMB_FRAME_SIZE);
+		rc = rpmb_get_counter(g_rpmb_mode, rpmb_key, &phy_counter, &rpmb_result);
+		if (rc) {
+			DPRINTF(("%s: rpmb_get_counter failed(0x%x)\n", __func__, rpmb_result));
+			return -1;
+		}
+
+		/* Since phy_counter has changed, so we have to generate mac again*/
+		if (rpmb_replace_frame(in_frame, in_cnt, rpmb_key, NULL,
+				&phy_counter, &paddr, NULL, NULL, NULL))
+		{
+			err = RPMB_RES_GENERAL_FAILURE;
+			goto out;
+		}
+
+		if (rpmb_virt_ioctl(ioc_cmd, seq_data)) {
+			DPRINTF(("%s: rpmb virt retry ioctl failed.\n", __func__));
+			return -1;
+		}
+	}
+
+	if (out_frame->result == RPMB_RES_OK) {
+		phy_counter++;
+		virt_counter++;
+	}
+
+	rpmb_replace_frame(out_frame, out_cnt, virt_rpmb_key, NULL,
+			&virt_counter, &vaddr, NULL, NULL, NULL);
+
+	return 0;
+out:
+	rpmb_replace_frame(out_frame, out_cnt, virt_rpmb_key, in_frame[0].nonce,
+			&virt_counter, &vaddr, &block_count, &err, &resp);
+
+	return 0;
+}
+
+static int rpmb_virt_read(uint32_t ioc_cmd, void* seq_data,
+		struct rpmb_frame* in_frame, uint32_t in_cnt,
+		struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int err = RPMB_RES_READ_FAILURE;
+	int resp = RPMB_RESP_DATA_READ;
+	uint16_t vaddr;
+	uint16_t paddr;
+	uint16_t block_count;
+	uint8_t uos_id;
+
+	if (in_cnt == 0 || in_frame == NULL) {
+		DPRINTF(("%s: in_frame, in_cnt or seq_data is not available\n", __func__));
+		return -1;
+	}
+
+	if (out_cnt == 0 || out_frame == NULL) {
+		DPRINTF(("%s: out_frame or out_cnt is not available\n", __func__));
+		return -1;
+	}
+
+	uos_id = get_uos_id();
+
+	memset(out_frame, 0, out_cnt * RPMB_FRAME_SIZE);
+	vaddr = swap16(in_frame->addr);
+	block_count = out_cnt;
+
+	if (rpmb_check_frame(READ_DATA_STR, &err, in_frame,
+			in_cnt, NULL, NULL, &vaddr, &block_count))
+		goto out;
+
+	paddr = get_phy_addr(uos_id, vaddr);
+	if (rpmb_replace_frame(in_frame, in_cnt, NULL,
+			NULL, NULL, &paddr, NULL, NULL, NULL )) {
+		err = RPMB_RES_GENERAL_FAILURE;
+		goto out;
+	}
+
+	if (rpmb_virt_ioctl(ioc_cmd, seq_data)) {
+		DPRINTF(("%s: rpmb ioctl failed\n", __func__));
+		return -1;
+	}
+
+	rpmb_replace_frame(out_frame, out_cnt, virt_rpmb_key, NULL,
+				NULL, &vaddr, NULL, NULL, NULL);
+
+	return 0;
+out:
+	rpmb_replace_frame(out_frame, out_cnt, virt_rpmb_key, in_frame[0].nonce,
+					NULL, &vaddr, &block_count, &err, &resp);
+
+	return 0;
+}
+
+static int rpmb_virt_get_counter(struct rpmb_frame* in_frame, uint32_t in_cnt,
+				struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int err = RPMB_RES_OK;
+	int resp = RPMB_RESP_GET_COUNTER;
+
+	if (in_cnt == 0 || in_frame == NULL) {
+		DPRINTF(("%s: in_frame or in_cnt is not available\n", __func__));
+		return -1;
+	}
+
+	if (out_cnt == 0 || out_frame == NULL) {
+		DPRINTF(("%s: out_frame or out_cnt is not available\n", __func__));
+		return -1;
+	}
+
+	memset(out_frame, 0, out_cnt * RPMB_FRAME_SIZE);
+
+	if (!is_key_programmed()) {
+		DPRINTF(("%s: rpmb key is not programmed\n", __func__));
+		err = RPMB_RES_NO_AUTH_KEY;
+		goto out;
+	}
+
+	rpmb_replace_frame(out_frame, out_cnt, virt_rpmb_key,
+			in_frame[0].nonce, &virt_counter, NULL, NULL, &err, &resp);
+
+	return 0;
+
+out:
+	rpmb_replace_frame(out_frame, out_cnt, virt_rpmb_key,
+			in_frame[0].nonce, NULL, NULL, NULL, &err, &resp);
+
+	return 0;
+}
+
+int rpmb_handler(uint32_t cmd, void *r)
+{
+	int rc = -1;
+	uint16_t i;
+	uint32_t write_cnt = 0;
+	uint32_t rel_write_cnt = 0;
+	uint32_t read_cnt = 0;
+	struct rpmb_frame *frame_write = NULL;
+	struct rpmb_frame *frame_rel_write = NULL;
+	struct rpmb_frame *frame_read = NULL;
+	struct rpmb_ioc_cmd *ioc_cmd = NULL;
+	struct rpmb_ioc_seq_data *iseq = r;
+
+	if (r == NULL) {
+		DPRINTF(("%s: rpmb iocctl seq data is NULL\n", __func__));
+		goto err_response;
+	}
+
+	for (i = 0; i < iseq->h.num_of_cmds; i++) {
+		ioc_cmd = (struct rpmb_ioc_cmd *)(&iseq->cmd[i]);
+		if (ioc_cmd->flags == 0) {
+			frame_read = (struct rpmb_frame *)ioc_cmd->frames_ptr;
+			read_cnt = ioc_cmd->nframes;
+		} else if (ioc_cmd->flags == RPMB_F_WRITE) {
+			frame_write = (struct rpmb_frame *)ioc_cmd->frames_ptr;
+			write_cnt = ioc_cmd->nframes;
+		} else if (ioc_cmd->flags == (RPMB_F_WRITE | RPMB_F_REL_WRITE)) {
+			frame_rel_write = (struct rpmb_frame *)ioc_cmd->frames_ptr;
+			rel_write_cnt = ioc_cmd->nframes;
+		} else {
+			DPRINTF(("%s: rpmb_ioc_cmd is invalid in the rpmb_ioc_seq_data\n", __func__));
+			goto err_response;
+		}
+	}
+
+	if (rel_write_cnt) {
+		if (frame_rel_write[0].req_resp == swap16(RPMB_REQ_DATA_WRITE)) {
+			if (write_cnt && (frame_write->req_resp == swap16(RPMB_REQ_RESULT_READ)))
+				rc = rpmb_virt_write(cmd, r, frame_rel_write,
+						rel_write_cnt, frame_read, read_cnt);
+			else
+				rc = rpmb_virt_write(cmd, r, frame_rel_write, rel_write_cnt, NULL, 0);
+		} else if (frame_rel_write[0].req_resp == swap16(RPMB_REQ_PROGRAM_KEY)) {
+			DPRINTF(("%s: rpmb grogram key is unsupported\n", __func__));
+			goto err_response;
+		} else {
+			DPRINTF(("%s: rpmb ioctl frame is invalid\n", __func__));
+			goto err_response;
+		}
+	} else if (write_cnt) {
+		if (frame_write[0].req_resp == swap16(RPMB_REQ_DATA_READ)) {
+			rc = rpmb_virt_read(cmd, r, frame_write, 1, frame_read, read_cnt);
+		} else if (frame_write[0].req_resp == swap16(RPMB_REQ_GET_COUNTER)) {
+			rc = rpmb_virt_get_counter(frame_write, 1, frame_read, 1);
+		} else {
+			DPRINTF(("%s: rpmb get counter frame is invalid\n", __func__));
+			goto err_response;
+		}
+	} else {
+		DPRINTF(("%s: rpmb ioctl frame is invalid\n", __func__));
+		goto err_response;
+	}
+
+	return rc;
+
+err_response:
+	return -1;
+}
--- a/devicemodel/hw/platform/rpmb/rpmb_sim.c
+++ b/devicemodel/hw/platform/rpmb/rpmb_sim.c
@@ -0,0 +1,657 @@
+/*
+ * Copyright (c) 2018 Intel Corporation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    this position and unchanged.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *	  notice, this list of conditions and the following disclaimer in the
+ *	  documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <ctype.h>
+#include <errno.h>
+#include <stdbool.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <fcntl.h>
+#include <inttypes.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <openssl/hmac.h>
+
+#include "rpmb.h"
+#include "rpmb_sim.h"
+
+static FILE *rpmb_fd = NULL;
+
+/*
+ * 0~6 is magic
+ * 7~38 is rpmb key
+ * 39~41 is write counter
+ */
+#define KEY_MAGIC		"key_sim"
+#define KEY_MAGIC_ADDR		0
+#define KEY_MAGIC_LENGTH	7
+
+#define KEY_ADDR		7
+#define KEY_LENGTH		32
+
+#define WRITER_COUNTER_ADDR	39
+
+#define TEEDATA_SIZE		(4*1024*1024) //4M
+#define TEEDATA_BLOCK_COUNT		(TEEDATA_SIZE/256)
+
+#define offsetof(s, m)		(size_t) &(((s *) 0)->m)
+
+static int virtio_rpmb_debug = 1;
+#define DPRINTF(params) do { if (virtio_rpmb_debug) printf params; } while (0)
+#define WPRINTF(params) (printf params)
+
+int rpmb_mac(const uint8_t *key, const struct rpmb_frame *frames,
+			size_t frame_cnt, uint8_t *mac)
+{
+	int i;
+	int hmac_ret;
+	unsigned int md_len;
+	struct hmac_ctx_st hmac_ctx;
+
+	HMAC_CTX_init(&hmac_ctx);
+	hmac_ret = HMAC_Init_ex(&hmac_ctx, key, 32, EVP_sha256(), NULL);
+	if (!hmac_ret) {
+		DPRINTF(("HMAC_Init_ex failed\n"));
+		goto err;
+	}
+
+	for (i = 0; i < frame_cnt; i++) {
+		hmac_ret = HMAC_Update(&hmac_ctx, frames[i].data, 284);
+		if (!hmac_ret) {
+			DPRINTF(("HMAC_Update failed\n"));
+			goto err;
+		}
+	}
+
+	hmac_ret = HMAC_Final(&hmac_ctx, mac, &md_len);
+	if (md_len != 32) {
+		DPRINTF(("bad md_len %d != 32.\n", md_len));
+		goto err;
+	}
+
+	if (!hmac_ret) {
+		DPRINTF(("HMAC_Final failed\n"));
+		goto err;
+	}
+
+err:
+	HMAC_CTX_cleanup(&hmac_ctx);
+
+	return hmac_ret ? 0 : -1;
+}
+
+static void rpmb_sim_close(void)
+{
+	fclose(rpmb_fd);
+	rpmb_fd = NULL;
+}
+
+static int file_write(FILE *fp, const void *buf, size_t size, off_t offset)
+{
+	size_t rc = 0;
+
+	if (fseek(fp, offset, SEEK_SET)) {
+		DPRINTF(("%s:Seek to %ld failed.\n", __func__, offset));
+		return -1;
+	}
+
+	rc = fwrite(buf, sizeof(char), size, fp);
+	if (rc != size) {
+		return -1;
+	}
+
+	if (fflush(fp) < 0) {
+		return -1;
+	}
+
+	return rc;
+}
+
+static int file_read(FILE *fp, void *buf, size_t size, off_t offset)
+{
+	size_t rc = 0;
+
+	if (fseek(fp, offset, SEEK_SET)) {
+		DPRINTF(("%s:Seek to %ld failed.\n", __func__, offset));
+		return -1;
+	}
+
+	rc = fread(buf, sizeof(char), size, fp);
+	if (rc == size) {
+		return rc;
+	} else {
+		return -1;
+	}
+}
+
+static int rpmb_sim_open(const char *rpmb_devname)
+{
+	uint8_t data = 0;
+	rpmb_fd = fopen(rpmb_devname, "rb+");
+
+	if (rpmb_fd == NULL) {
+		/*if the rpmb device file does not exist, create a new file*/
+		rpmb_fd = fopen(rpmb_devname, "wb+");
+		DPRINTF(("rpmb device file(%s) does not exist, create a new file\n", rpmb_devname));
+		/* Write 0 to the last byte to enable 4MB length access */
+		if (file_write(rpmb_fd, &data, 1, TEEDATA_SIZE - 1) < 0) {
+			DPRINTF(("Failed to initialize simulated rpmb to 0.\n"));
+			rpmb_fd = NULL;
+		}
+	}
+
+	if (rpmb_fd == NULL) {
+		DPRINTF(("%s: unable (%d) to open rpmb device '%s': %s\n",
+			__func__, errno, rpmb_devname, strerror(errno)));
+		return -1;
+	}
+
+	return 0;
+}
+
+static int get_counter(uint32_t *counter)
+{
+	int rc = 0;
+
+	rc = file_read(rpmb_fd, counter, sizeof(*counter), WRITER_COUNTER_ADDR);
+	if (rc < 0)
+	{
+		DPRINTF(("%s failed.\n", __func__));
+		return -1;
+	}
+
+	swap32(*counter);
+
+	return 0;
+}
+
+static int set_counter(const uint32_t *counter)
+{
+	int rc = 0;
+	uint32_t cnt = *counter;
+
+	swap32(cnt);
+	rc = file_write(rpmb_fd, &cnt, sizeof(cnt), WRITER_COUNTER_ADDR);
+	if (rc < 0)
+	{
+		DPRINTF(("%s failed.\n", __func__));
+		return -1;
+	}
+
+	return 0;
+}
+
+static int is_key_programmed(void)
+{
+	int rc = 0;
+	uint8_t magic[KEY_MAGIC_LENGTH] = {0};
+
+	rc = file_read(rpmb_fd, magic, KEY_MAGIC_LENGTH, KEY_MAGIC_ADDR);
+	if (rc < 0)
+	{
+		DPRINTF(("%s read magic failed.\n", __func__));
+		return 0;
+	}
+
+	if (memcmp(KEY_MAGIC, magic, KEY_MAGIC_LENGTH))
+		return 0;
+
+	return 1;
+}
+
+static int get_key(uint8_t *key)
+{
+	int rc = 0;
+
+	rc = file_read(rpmb_fd, key, 32, KEY_ADDR);
+	if (rc < 0)
+	{
+		DPRINTF(("%s failed.\n", __func__));
+		return -1;
+	}
+
+	return 0;
+}
+
+static int program_key(const uint8_t *key)
+{
+	int rc = 0;
+
+	rc = file_write(rpmb_fd, key, 32, KEY_ADDR);
+	if (rc < 0)
+	{
+		DPRINTF(("%s failed at set key.\n", __func__));
+		return -1;
+	}
+
+	rc = file_write(rpmb_fd, KEY_MAGIC, KEY_MAGIC_LENGTH, KEY_MAGIC_ADDR);
+	if (rc < 0)
+	{
+		DPRINTF(("%s failed at set magic.\n", __func__));
+		return -1;
+	}
+
+	return 0;
+}
+
+static int rpmb_sim_program_key(const struct rpmb_frame* in_frame, uint32_t in_cnt,
+							struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int ret = 0;
+	int err = RPMB_RES_WRITE_FAILURE;
+	uint32_t counter = 0;
+
+	if (in_cnt == 0  || in_frame == NULL)
+		return -EINVAL;
+
+	if (is_key_programmed())
+		err = RPMB_RES_GENERAL_FAILURE;
+	else
+		ret = program_key(in_frame->key_mac);
+
+	if (ret)
+		goto out;
+
+	ret = set_counter(&counter);
+	if (ret)
+		goto out;
+
+	err = RPMB_RES_OK;
+
+out:
+	if (out_frame) {
+		memset(out_frame, 0, out_cnt*sizeof(*out_frame));
+		out_frame->req_resp = swap16(RPMB_RESP_PROGRAM_KEY);
+		out_frame->result = swap16(err);
+	}
+
+	return ret;
+}
+
+static int rpmb_sim_write(const struct rpmb_frame* in_frame, uint32_t in_cnt,
+					  struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int ret = 0;
+	int err = RPMB_RES_WRITE_FAILURE;
+	uint32_t i;
+	uint8_t key[32];
+	uint8_t mac[32];
+	uint32_t counter;
+	uint16_t addr;
+	uint16_t block_count;
+	uint8_t data[256*in_cnt];
+
+	if (in_cnt == 0 || in_frame == NULL)
+		return -EINVAL;
+
+	if (in_frame[0].req_resp != swap16(RPMB_REQ_DATA_WRITE))
+		return -EINVAL;
+
+	if (in_cnt > 2) {
+		err = RPMB_RES_GENERAL_FAILURE;
+		goto out;
+	}
+
+	addr = swap16(in_frame[0].addr);
+	block_count = swap16(in_frame[0].block_count);
+
+	if (addr >= TEEDATA_BLOCK_COUNT) {
+		err = RPMB_RES_ADDR_FAILURE;
+		goto out;
+	}
+
+	if (addr + block_count > TEEDATA_BLOCK_COUNT)
+		goto out;
+
+	if (block_count == 0 || block_count > in_cnt) {
+		ret = -EINVAL;
+		err = RPMB_RES_GENERAL_FAILURE;
+		goto out;
+	}
+
+	if (!is_key_programmed()) {
+		err = RPMB_RES_NO_AUTH_KEY;
+		goto out;
+	}
+
+	if (get_counter(&counter))
+		goto out;
+
+	if (counter == 0xFFFFFFFF) {
+		err = RPMB_RES_WRITE_COUNTER_EXPIRED;
+		goto out;
+	}
+
+	if (counter != swap32(in_frame[0].write_counter)) {
+		err = RPMB_RES_COUNT_FAILURE;
+		goto out;
+	}
+
+	if (get_key(key)) {
+		err = RPMB_RES_GENERAL_FAILURE;
+		goto out;
+	}
+
+	if (rpmb_mac(key, in_frame, in_cnt, mac)) {
+		err = RPMB_RES_GENERAL_FAILURE;
+		goto out;
+	}
+
+	if (memcmp(in_frame[in_cnt - 1].key_mac, mac, 32)) {
+		DPRINTF(("%s wrong mac.\n", __func__));
+		err = RPMB_RES_AUTH_FAILURE;
+		goto out;
+	}
+
+	for (i = 0; i < in_cnt; i++)
+		memcpy(data + i * 256, in_frame[i].data, 256);
+
+	if (file_write(rpmb_fd, data, sizeof(data), 256 * addr) < 0) {
+		DPRINTF(("%s write_with_retry failed.\n", __func__));
+		goto out;
+	}
+
+	++counter;
+	if (set_counter(&counter)) {
+		DPRINTF(("%s set_counter failed.\n", __func__));
+		goto out;
+	}
+
+	err = RPMB_RES_OK;
+
+out:
+	if (out_frame) {
+		memset(out_frame, 0, out_cnt*sizeof(*out_frame));
+		out_frame->req_resp = swap16(RPMB_RESP_DATA_WRITE);
+		out_frame->result = swap16(err);
+		if (err == RPMB_RES_OK) {
+			out_frame->addr = swap16(addr);
+			out_frame->write_counter = swap32(counter);
+			rpmb_mac(key, out_frame, 1, out_frame->key_mac);
+		}
+	}
+
+	return ret;
+}
+
+static int rpmb_sim_read(const struct rpmb_frame* in_frame, uint32_t in_cnt,
+					 struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int ret = 0;
+	uint32_t i;
+	int err = RPMB_RES_READ_FAILURE;
+	uint8_t key[32];
+	uint8_t mac[32];
+	uint16_t addr;
+	uint8_t data[256*out_cnt];
+
+	if (in_cnt != 1 || in_frame == NULL)
+		return -EINVAL;
+
+	if (in_frame->req_resp != swap16(RPMB_REQ_DATA_READ))
+		return -EINVAL;
+
+	addr = swap16(in_frame->addr);
+
+	if (addr >= TEEDATA_BLOCK_COUNT) {
+		err = RPMB_RES_ADDR_FAILURE;
+		goto out;
+	}
+
+	if (addr + out_cnt > TEEDATA_BLOCK_COUNT)
+		goto out;
+
+	if (!is_key_programmed()) {
+		err = RPMB_RES_NO_AUTH_KEY;
+		goto out;
+	}
+
+	if (file_read(rpmb_fd, data, sizeof(data), 256 * addr) < 0) {
+		DPRINTF(("%s read_with_retry failed.\n", __func__));
+		goto out;
+	}
+
+	err = RPMB_RES_OK;
+
+out:
+	if (out_frame) {
+		memset(out_frame, 0, out_cnt*sizeof(*out_frame));
+		for (i = 0; i < out_cnt; i++) {
+			memcpy(out_frame[i].nonce, in_frame[0].nonce, sizeof(in_frame[0].nonce));
+			out_frame[i].req_resp = swap16(RPMB_RESP_DATA_READ);
+			out_frame[i].block_count = swap16(out_cnt);
+			out_frame[i].addr = in_frame[0].addr;
+			memcpy(out_frame[i].data, data+256*i, 256);
+		}
+		if (get_key(key))
+			DPRINTF(("%s, get_key failed.\n", __func__));
+
+		out_frame[out_cnt - 1].result = swap16(err);
+		rpmb_mac(key, out_frame, out_cnt, mac);
+		memcpy(out_frame[out_cnt - 1].key_mac, mac, sizeof(mac));
+	}
+
+	return ret;
+}
+
+static int rpmb_sim_get_counter(const struct rpmb_frame* in_frame, uint32_t in_cnt,
+							struct rpmb_frame* out_frame, uint32_t out_cnt)
+{
+	int ret = 0;
+	int err = RPMB_RES_COUNT_FAILURE;
+	uint8_t key[32];
+	uint32_t counter;
+
+	if (in_cnt != 1 || in_frame == NULL)
+		return -EINVAL;
+
+	if (in_frame->req_resp != swap16(RPMB_REQ_GET_COUNTER))
+		return -EINVAL;
+
+	if (!is_key_programmed()) {
+		err = RPMB_RES_NO_AUTH_KEY;
+		goto out;
+	}
+
+	if (get_key(key))
+		goto out;
+
+	if (get_counter(&counter))
+		goto out;
+
+	err = RPMB_RES_OK;
+
+out:
+	if (out_frame) {
+		memset(out_frame, 0, sizeof(*out_frame)*out_cnt);
+		out_frame->result = swap16(err);
+		out_frame->req_resp = swap16(RPMB_RESP_GET_COUNTER);
+		memcpy(out_frame->nonce, in_frame[0].nonce, sizeof(in_frame[0].nonce));
+
+	if (err == RPMB_RES_OK) {
+			out_frame->write_counter = swap32(counter);
+			rpmb_mac(key, out_frame, out_cnt, out_frame->key_mac);
+		}
+	}
+
+	return ret;
+}
+
+int is_use_sim_rpmb(void)
+{
+	int ret;
+
+	ret = rpmb_sim_open(RPMB_SIM_PATH_NAME);
+	if (ret) {
+		DPRINTF(("%s: rpmb_sim_open failed\n", __func__));
+		return 0;
+	}
+
+	ret = is_key_programmed();
+
+	rpmb_sim_close();
+
+	return ret;
+}
+
+int rpmb_sim_key_init(uint8_t *key)
+{
+	int ret;
+	uint32_t counter = 0;
+
+	ret = rpmb_sim_open(RPMB_SIM_PATH_NAME);
+	if (ret) {
+		DPRINTF(("%s: rpmb_sim_open failed\n", __func__));
+		return ret;
+	}
+
+	if (!is_key_programmed()) {
+		ret = program_key(key);
+		if (ret) {
+			DPRINTF(("%s: program_key failed\n", __func__));
+			goto out;
+		}
+	}
+
+	ret = get_counter(&counter);
+	if (ret) {
+		counter = 0;
+		ret = set_counter(&counter);
+		if (ret) {
+			DPRINTF(("%s: set_counter failed\n", __func__));
+			goto out;
+		}
+	}
+
+out:
+	rpmb_sim_close();
+
+	return ret;
+}
+
+/*
+ *                rel_write       write      read
+ * RPMB_READ          0             1        1~N
+ * RPMB_WRITE        1~N            1         1
+ * GET_COUNTER        0             1         1
+ * PROGRAM_KEY        1             1         1
+ */
+static int rpmb_sim_operations(const void *rel_write_data, size_t rel_write_size,
+						const void *write_data, size_t write_size,
+						void *read_buf, size_t read_size)
+{
+	int ret = -1;
+
+	if (rel_write_size) {
+		size_t nframe = rel_write_size/RPMB_FRAME_SIZE;
+		struct rpmb_frame rel_write_frame[nframe];
+		memcpy(rel_write_frame, rel_write_data, sizeof(rel_write_frame));
+		if (rel_write_frame[0].req_resp == swap16(RPMB_REQ_DATA_WRITE))  {
+			if (write_size/RPMB_FRAME_SIZE &&
+					((struct rpmb_frame*)write_data)->req_resp == swap16(RPMB_REQ_RESULT_READ))
+				ret = rpmb_sim_write(rel_write_frame, nframe, read_buf, read_size/RPMB_FRAME_SIZE);
+			else
+				ret = rpmb_sim_write(rel_write_frame, nframe, NULL, 0);
+		}
+		else if (rel_write_frame[0].req_resp == swap16(RPMB_REQ_PROGRAM_KEY)) {
+			if (write_size/RPMB_FRAME_SIZE &&
+					((struct rpmb_frame*)write_data)->req_resp == swap16(RPMB_REQ_RESULT_READ))
+				ret = rpmb_sim_program_key(rel_write_frame, 1, read_buf, read_size/RPMB_FRAME_SIZE);
+			else
+				ret = rpmb_sim_program_key(rel_write_frame, 1, NULL, 0);
+		}
+	}
+	else if (write_size) {
+		struct rpmb_frame write_frame[write_size/RPMB_FRAME_SIZE];
+		memcpy(write_frame, write_data, sizeof(write_frame));
+		if (write_frame[0].req_resp == swap16(RPMB_REQ_DATA_READ)) {
+			ret = rpmb_sim_read(write_frame, 1, read_buf, read_size/RPMB_FRAME_SIZE);
+		}
+		else if (write_frame[0].req_resp == swap16(RPMB_REQ_GET_COUNTER)) {
+			ret = rpmb_sim_get_counter(write_frame, 1, read_buf, 1);
+		}
+	}
+
+	return ret;
+}
+
+int rpmb_sim_send(const void *r)
+{
+	int ret;
+	uint16_t i;
+	uint32_t write_size = 0;
+	uint32_t rel_write_size = 0;
+	uint32_t read_size = 0;
+	struct rpmb_frame *frame_write = NULL;
+	struct rpmb_frame *frame_rel_write = NULL;
+	struct rpmb_frame *frame_read = NULL;
+	struct rpmb_ioc_cmd *ioc_cmd = NULL;
+	const struct rpmb_ioc_seq_data *iseq = r;
+
+	for (i = 0; i < iseq->h.num_of_cmds; i++) {
+	ioc_cmd = (struct rpmb_ioc_cmd *)(&iseq->cmd[i]);
+		if (ioc_cmd->flags == 0) {
+			frame_read = (struct rpmb_frame *)ioc_cmd->frames_ptr;
+			read_size = ioc_cmd->nframes * RPMB_FRAME_SIZE;
+		} else if (ioc_cmd->flags == RPMB_F_WRITE) {
+			frame_write = (struct rpmb_frame *)ioc_cmd->frames_ptr;
+			write_size = ioc_cmd->nframes * RPMB_FRAME_SIZE;
+		} else if (ioc_cmd->flags == (RPMB_F_WRITE | RPMB_F_REL_WRITE)) {
+			frame_rel_write = (struct rpmb_frame *)ioc_cmd->frames_ptr;
+			rel_write_size = ioc_cmd->nframes * RPMB_FRAME_SIZE;
+		} else {
+			DPRINTF(("%s: rpmb_ioc_cmd is invalid in the rpmb_ioc_seq_data\n", __func__));
+			goto err_response;
+		}
+	}
+
+	ret = rpmb_sim_open(RPMB_SIM_PATH_NAME);
+	if (ret) {
+		DPRINTF(("%s: rpmb_sim_open failed\n", __func__));
+		goto err_response;
+	}
+
+	/* execute rpmb command */
+	ret = rpmb_sim_operations(frame_rel_write, rel_write_size,
+							 frame_write, write_size,
+							 frame_read, read_size);
+	rpmb_sim_close();
+
+	if (ret) {
+		DPRINTF(("%s: rpmb_sim_operations failed\n", __func__));
+		goto err_response;
+	}
+
+	return 0;
+
+err_response:
+	return -1;
+}