tee-supplicant/src/rpmb.c

/*
 * Copyright (c) 2016, Linaro Limited
 * 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.
 *
 * 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 COPYRIGHT HOLDERS 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 COPYRIGHT HOLDER 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 <dirent.h>
#include <fcntl.h>
#include <linux/types.h>
#include <linux/mmc/ioctl.h>
#include <netinet/in.h>
#include <pthread.h>
#include <rpmb.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <tee_client_api.h>
#include <teec_trace.h>
#include <tee_supplicant.h>
#include <unistd.h>

#ifdef RPMB_EMU
#include <stdarg.h>
#include "hmac_sha2.h"
#else
#include <errno.h>
#endif

/*
 * Request and response definitions must be in sync with the secure side
 */

/* Request */
struct rpmb_req {
	uint16_t cmd;
#define RPMB_CMD_DATA_REQ      0x00
#define RPMB_CMD_GET_DEV_INFO  0x01
	uint16_t dev_id;
	uint16_t block_count;
	/* Optional data frames (rpmb_data_frame) follow */
};
#define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))

#define RPMB_CID_SZ 16

/* Response to device info request */
struct rpmb_dev_info {
	uint8_t cid[RPMB_CID_SZ];
	uint8_t rpmb_size_mult;	/* EXT CSD-slice 168: RPMB Size */
	uint8_t rel_wr_sec_c;	/* EXT CSD-slice 222: Reliable Write Sector */
				/*                    Count */
	uint8_t ret_code;
#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
};

/*
 * This structure is shared with OP-TEE and the MMC ioctl layer.
 * It is the "data frame for RPMB access" defined by JEDEC, minus the
 * start and stop bits.
 */
struct rpmb_data_frame {
	uint8_t stuff_bytes[196];
	uint8_t key_mac[32];
	uint8_t data[256];
	uint8_t nonce[16];
	uint32_t write_counter;
	uint16_t address;
	uint16_t block_count;
	uint16_t op_result;
#define RPMB_RESULT_OK				0x00
#define RPMB_RESULT_GENERAL_FAILURE		0x01
#define RPMB_RESULT_AUTH_FAILURE		0x02
#define RPMB_RESULT_ADDRESS_FAILURE		0x04
#define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED	0x07
	uint16_t msg_type;
#define RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM		0x0001
#define RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ	0x0002
#define RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE		0x0003
#define RPMB_MSG_TYPE_REQ_AUTH_DATA_READ		0x0004
#define RPMB_MSG_TYPE_REQ_RESULT_READ			0x0005
#define RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM		0x0100
#define RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ	0x0200
#define RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE		0x0300
#define RPMB_MSG_TYPE_RESP_AUTH_DATA_READ		0x0400
};


static pthread_mutex_t rpmb_mutex = PTHREAD_MUTEX_INITIALIZER;

/*
 * ioctl() interface
 * Comes from: uapi/linux/major.h, linux/mmc/core.h
 */

#define MMC_BLOCK_MAJOR	179

/* mmc_ioc_cmd.opcode */
#define MMC_READ_MULTIPLE_BLOCK		18
#define MMC_WRITE_MULTIPLE_BLOCK	25

/* mmc_ioc_cmd.flags */
#define MMC_RSP_PRESENT	(1 << 0)
#define MMC_RSP_136     (1 << 1)	/* 136 bit response */
#define MMC_RSP_CRC	(1 << 2)	/* Expect valid CRC */
#define MMC_RSP_OPCODE	(1 << 4)	/* Response contains opcode */

#define MMC_RSP_R1      (MMC_RSP_PRESENT|MMC_RSP_CRC|MMC_RSP_OPCODE)

#define MMC_CMD_ADTC	(1 << 5)	/* Addressed data transfer command */

/* mmc_ioc_cmd.write_flag */
#define MMC_CMD23_ARG_REL_WR	(1 << 31) /* CMD23 reliable write */

/* Maximum number of commands used in a multiple ioc command request */
#define RPMB_MAX_IOC_MULTI_CMDS		3

#ifndef RPMB_EMU

#define IOCTL(fd, request, ...)					   \
	({							   \
		int ret;					   \
		ret = ioctl((fd), (request), ##__VA_ARGS__);	   \
		if (ret < 0)					   \
			EMSG("ioctl ret=%d errno=%d", ret, errno); \
		ret;						   \
	})


/* Open and/or return file descriptor to RPMB partition of device dev_id */
static int mmc_rpmb_fd(uint16_t dev_id)
{
	static int id;
	static int fd = -1;
	char path[PATH_MAX] = { 0 };

	DMSG("dev_id = %u", dev_id);
	if (fd < 0) {
		snprintf(path, sizeof(path), "/dev/mmcblk%urpmb", dev_id);
		fd = open(path, O_RDWR);
		if (fd < 0) {
			EMSG("Could not open %s (%s)", path, strerror(errno));
			return -1;
		}
		id = dev_id;
	}
	if (id != dev_id) {
		EMSG("Only one MMC device is supported");
		return -1;
	}
	return fd;
}

/*
 * Read @n bytes from @fd, takes care of short reads and EINTR.
 * Adapted from “Advanced Programming In the UNIX Environment” by W. Richard
 * Stevens and Stephen A. Rago, 2013, 3rd Edition, Addison-Wesley
 * (EINTR handling was added)
 */
static ssize_t readn(int fd, void *ptr, size_t n)
{
	size_t nleft = n;
	ssize_t nread = 0;
	uint8_t *p = ptr;

	while (nleft > 0) {
		if ((nread = read(fd, p, nleft)) < 0) {
			if (errno == EINTR)
				continue;
			if (nleft == n)
				return -1; /* error, nothing read, return -1 */
			else
				break; /* error, return amount read so far */
		} else if (nread == 0) {
			break; /* EOF */
		}
		nleft -= nread;
		p += nread;
	}
	return n - nleft; /* return >= 0 */
}

/* Size of CID printed in hexadecimal */
#define CID_STR_SZ (2 * RPMB_CID_SZ)

static TEEC_Result read_cid_str(uint16_t dev_id, char cid[CID_STR_SZ + 1])
{
	TEEC_Result res = TEEC_ERROR_GENERIC;
	char path[48] = { 0 };
	int fd = 0;
	int st = 0;

	snprintf(path, sizeof(path),
		 "/sys/class/mmc_host/mmc%u/mmc%u:0001/cid", dev_id, dev_id);
	fd = open(path, O_RDONLY);
	if (fd < 0)
		return TEEC_ERROR_ITEM_NOT_FOUND;
	st = readn(fd, cid, CID_STR_SZ);
	if (st != CID_STR_SZ) {
		EMSG("Read CID error");
		if (errno)
			EMSG("%s", strerror(errno));
		res = TEEC_ERROR_NO_DATA;
		goto out;
	}
	res = TEEC_SUCCESS;
out:
	close(fd);
	return res;
}

static int hexchar2int(char c)
{
	if (c >= '0' && c <= '9')
		return c - '0';
	if (c >= 'a' && c <= 'f')
		return c - 'a' + 10;
	if (c >= 'A' && c <= 'F')
		return c - 'A' + 10;
	return -1;
}

static int hexbyte2int(char *hex)
{
	int v1 = hexchar2int(hex[0]);
	int v2 = hexchar2int(hex[1]);

	if (v1 < 0 || v2 < 0)
		return -1;
	return 16 * v1 + v2;
}

/* Device Identification (CID) register is 16 bytes. It is read from sysfs. */
static TEEC_Result read_cid(uint16_t dev_id, uint8_t *cid)
{
	TEEC_Result res = TEEC_ERROR_GENERIC;
	char cid_str[CID_STR_SZ + 1] = { };
	int i = 0;
	int v = 0;

	res = read_cid_str(dev_id, cid_str);
	if (res)
		return res;

	for (i = 0; i < RPMB_CID_SZ; i++) {
		v = hexbyte2int(cid_str + 2 * i);
		if (v < 0) {
			EMSG("Invalid CID string: %s", cid_str);
			return TEEC_ERROR_NO_DATA;
		}
		cid[i] = v;
	}
	return TEEC_SUCCESS;
}

static TEEC_Result read_mmc_sysfs_hex(uint16_t dev_id, const char *file, uint8_t *value)
{
	TEEC_Result status = TEEC_SUCCESS;
	char path[255] = { 0 };
	char buf[255] = { 0 };
	char *endp = buf;
	int fd = 0;
	int ret = 0;

	snprintf(path, sizeof(path), "/sys/class/mmc_host/mmc%u/mmc%u:0001/%s",
		 dev_id, dev_id, file);

	fd = open(path, O_RDONLY);
	if (fd < 0) {
		EMSG("Could not open %s (%s)", path, strerror(errno));
		return TEEC_ERROR_ITEM_NOT_FOUND;
	}

	ret = readn(fd, buf, sizeof(buf));
	if (ret < 0) {
		EMSG("Read error (%s)", strerror(errno));
		status = TEEC_ERROR_NO_DATA;
		goto out;
	}

	errno = 0;
	*value = strtol(buf, &endp, 16);
	if (errno || endp == buf)
		status = TEEC_ERROR_GENERIC;

out:
	close(fd);
	return status;
}

static TEEC_Result read_size_mult(uint16_t dev_id, uint8_t *value)
{
	return read_mmc_sysfs_hex(dev_id, "raw_rpmb_size_mult", value);
}

static TEEC_Result read_rel_wr_sec_c(uint16_t dev_id, uint8_t *value)
{
	return read_mmc_sysfs_hex(dev_id, "rel_sectors", value);
}

/*
 * - If --rpmb-cid is given, find the eMMC RPMB device number with the specified
 * CID, cache the number, copy it to @ndev_id and return true. If not found
 * return false.
 * - If --rpmb-cid is not given, copy @dev_id to @ndev_id and return true.
 */
static bool remap_rpmb_dev_id(uint16_t dev_id, uint16_t *ndev_id)
{
	TEEC_Result res = TEEC_ERROR_GENERIC;
	static bool found = false;
	static bool err = false;
	static uint16_t id = 0;
	char cid[CID_STR_SZ + 1] = { };
	struct dirent *dent = NULL;
	DIR *dir = NULL;
	int num = 0;

	if (err || found)
		goto out;

	if (!supplicant_params.rpmb_cid) {
		id = dev_id;
		found = true;
		goto out;
	}

	dir = opendir("/sys/class/mmc_host");
	if (!dir) {
		EMSG("Could not open /sys/class/mmc_host (%s)",
		     strerror(errno));
		err = true;
		goto out;
	}

	while ((dent = readdir(dir))) {
		if (sscanf(dent->d_name, "%*[^0123456789]%d", &num) != 1)
			continue;
		if (num > UINT16_MAX) {
			EMSG("Too many MMC devices");
			err = true;
			break;
		}
		id = (uint16_t)num;
		res = read_cid_str(id, cid);
		if (res)
			continue;
		if (strcmp(cid, supplicant_params.rpmb_cid))
			continue;
		IMSG("RPMB device %s is at /dev/mmcblk%urpmb\n", cid, id);
		found = true;
		break;
	}

	closedir(dir);

	if (!found)
		err = true;
out:
	if (found)
		*ndev_id = id;
	return found;
}

#else /* RPMB_EMU */

#define IOCTL(fd, request, ...) ioctl_emu((fd), (request), ##__VA_ARGS__)

/* Emulated rel_wr_sec_c value (reliable write size, *256 bytes) */
#define EMU_RPMB_REL_WR_SEC_C	1
/* Emulated rpmb_size_mult value (RPMB size, *128 kB) */
#define EMU_RPMB_SIZE_MULT	2

#define EMU_RPMB_SIZE_BYTES	(EMU_RPMB_SIZE_MULT * 128 * 1024)

/* Emulated eMMC device state */
struct rpmb_emu {
	uint8_t buf[EMU_RPMB_SIZE_BYTES];
	size_t size;
	uint8_t key[32];
	bool key_set;
	uint8_t nonce[16];
	uint32_t write_counter;
	struct {
		uint16_t msg_type;
		uint16_t op_result;
		uint16_t address;
	} last_op;
};
static struct rpmb_emu rpmb_emu = {
	.size = EMU_RPMB_SIZE_BYTES
};

static struct rpmb_emu *mem_for_fd(int fd)
{
	static int sfd = -1;

	if (sfd == -1)
		sfd = fd;
	if (sfd != fd) {
		EMSG("Emulating more than 1 RPMB partition is not supported");
		return NULL;
	}

	return &rpmb_emu;
}

#if (DEBUGLEVEL >= TRACE_FLOW)
static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr,
			bool to_mmc)
{
	char msg[100] = { 0 };
	size_t i = 0;

	for (i = 0; i < numblk; i++) {
		snprintf(msg, sizeof(msg), "%s MMC block %zu",
			 to_mmc ? "Write" : "Read", startblk + i);
		dump_buffer(msg, ptr, 256);
		ptr += 256;
	}
}
#else
static void dump_blocks(size_t startblk, size_t numblk, uint8_t *ptr,
			bool to_mmc)
{
	(void)startblk;
	(void)numblk;
	(void)ptr;
	(void)to_mmc;
}
#endif

#define CUC(x) ((const unsigned char *)(x))
static void hmac_update_frm(hmac_sha256_ctx *ctx, struct rpmb_data_frame *frm)
{
	hmac_sha256_update(ctx, CUC(frm->data), 256);
	hmac_sha256_update(ctx, CUC(frm->nonce), 16);
	hmac_sha256_update(ctx, CUC(&frm->write_counter), 4);
	hmac_sha256_update(ctx, CUC(&frm->address), 2);
	hmac_sha256_update(ctx, CUC(&frm->block_count), 2);
	hmac_sha256_update(ctx, CUC(&frm->op_result), 2);
	hmac_sha256_update(ctx, CUC(&frm->msg_type), 2);
}

static bool is_hmac_valid(struct rpmb_emu *mem, struct rpmb_data_frame *frm,
		   size_t nfrm)
{
	uint8_t mac[32] = { 0 };
	size_t i = 0;
	hmac_sha256_ctx ctx;

	memset(&ctx, 0, sizeof(ctx));

	if (!mem->key_set) {
		EMSG("Cannot check MAC (key not set)");
		return false;
	}

	hmac_sha256_init(&ctx, mem->key, sizeof(mem->key));
	for (i = 0; i < nfrm; i++, frm++)
		hmac_update_frm(&ctx, frm);
	frm--;
	hmac_sha256_final(&ctx, mac, 32);

	if (memcmp(mac, frm->key_mac, 32)) {
		EMSG("Invalid MAC");
		return false;
	}
	return true;
}

static uint16_t gen_msb1st_result(uint8_t byte)
{
	return (uint16_t)byte << 8;
}

static uint16_t compute_hmac(struct rpmb_emu *mem, struct rpmb_data_frame *frm,
			     size_t nfrm)
{
	size_t i = 0;
	hmac_sha256_ctx ctx;

	memset(&ctx, 0, sizeof(ctx));

	if (!mem->key_set) {
		EMSG("Cannot compute MAC (key not set)");
		return gen_msb1st_result(RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED);
	}

	hmac_sha256_init(&ctx, mem->key, sizeof(mem->key));
	for (i = 0; i < nfrm; i++, frm++)
		hmac_update_frm(&ctx, frm);
	frm--;
	hmac_sha256_final(&ctx, frm->key_mac, 32);

	return gen_msb1st_result(RPMB_RESULT_OK);
}

static uint16_t ioctl_emu_mem_transfer(struct rpmb_emu *mem,
				       struct rpmb_data_frame *frm,
				       size_t nfrm, int to_mmc)
{
	size_t start = mem->last_op.address * 256;
	size_t size = nfrm * 256;
	size_t i = 0;
	uint8_t *memptr = NULL;

	if (start > mem->size || start + size > mem->size) {
		EMSG("Transfer bounds exceeed emulated memory");
		return gen_msb1st_result(RPMB_RESULT_ADDRESS_FAILURE);
	}
	if (to_mmc && !is_hmac_valid(mem, frm, nfrm))
		return gen_msb1st_result(RPMB_RESULT_AUTH_FAILURE);

	DMSG("Transferring %zu 256-byte data block%s %s MMC (block offset=%zu)",
	     nfrm, (nfrm > 1) ? "s" : "", to_mmc ? "to" : "from", start / 256);
	for (i = 0; i < nfrm; i++) {
		memptr = mem->buf + start + i * 256;
		if (to_mmc) {
			memcpy(memptr, frm[i].data, 256);
			mem->write_counter++;
			frm[i].write_counter = htonl(mem->write_counter);
			frm[i].msg_type =
				htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE);
		} else {
			memcpy(frm[i].data, memptr, 256);
			frm[i].msg_type =
				htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_READ);
			frm[i].address = htons(mem->last_op.address);
			frm[i].block_count = nfrm;
			memcpy(frm[i].nonce, mem->nonce, 16);
		}
		frm[i].op_result = gen_msb1st_result(RPMB_RESULT_OK);
	}
	dump_blocks(mem->last_op.address, nfrm, mem->buf + start, to_mmc);

	if (!to_mmc)
		compute_hmac(mem, frm, nfrm);

	return gen_msb1st_result(RPMB_RESULT_OK);
}

static void ioctl_emu_get_write_result(struct rpmb_emu *mem,
				       struct rpmb_data_frame *frm)
{
	frm->msg_type =	htons(RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE);
	frm->op_result = mem->last_op.op_result;
	frm->address = htons(mem->last_op.address);
	frm->write_counter = htonl(mem->write_counter);
	compute_hmac(mem, frm, 1);
}

static uint16_t ioctl_emu_setkey(struct rpmb_emu *mem,
				 struct rpmb_data_frame *frm)
{
	if (mem->key_set) {
		EMSG("Key already set");
		return gen_msb1st_result(RPMB_RESULT_GENERAL_FAILURE);
	}
	dump_buffer("Setting key", frm->key_mac, 32);
	memcpy(mem->key, frm->key_mac, 32);
	mem->key_set = true;

	return gen_msb1st_result(RPMB_RESULT_OK);
}

static void ioctl_emu_get_keyprog_result(struct rpmb_emu *mem,
					 struct rpmb_data_frame *frm)
{
	frm->msg_type =
		htons(RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM);
	frm->op_result = mem->last_op.op_result;
}

static void ioctl_emu_read_ctr(struct rpmb_emu *mem,
			       struct rpmb_data_frame *frm)
{
	DMSG("Reading counter");
	frm->msg_type = htons(RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ);
	frm->write_counter = htonl(mem->write_counter);
	memcpy(frm->nonce, mem->nonce, 16);
	frm->op_result = compute_hmac(mem, frm, 1);
}

static uint32_t read_cid(uint16_t dev_id, uint8_t *cid)
{
	/* Taken from an actual eMMC chip */
	static const uint8_t test_cid[] = {
		/* MID (Manufacturer ID): Micron */
		0xfe,
		/* CBX (Device/BGA): BGA */
		0x01,
		/* OID (OEM/Application ID) */
		0x4e,
		/* PNM (Product name) "MMC04G" */
		0x4d, 0x4d, 0x43, 0x30, 0x34, 0x47,
		/* PRV (Product revision): 4.2 */
		0x42,
		/* PSN (Product serial number) */
		0xc8, 0xf6, 0x55, 0x2a,
		/*
		 * MDT (Manufacturing date):
		 * June, 2014
		 */
		0x61,
		/* (CRC7 (0xA) << 1) | 0x1 */
		0x15
	};

	(void)dev_id;
	memcpy(cid, test_cid, sizeof(test_cid));

	return TEEC_SUCCESS;
}

/* A crude emulation of the MMC ioc commands we need for RPMB */
static int ioctl_emu_cmd(int fd, struct mmc_ioc_cmd *cmd)
{
	struct rpmb_data_frame *frm = NULL;
	uint16_t msg_type = 0;
	struct rpmb_emu *mem = mem_for_fd(fd);

	if (!mem)
		return -1;

	switch (cmd->opcode) {
	case MMC_WRITE_MULTIPLE_BLOCK:
		frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr;
		msg_type = ntohs(frm->msg_type);

		switch (msg_type) {
		case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
			mem->last_op.msg_type = msg_type;
			mem->last_op.op_result = ioctl_emu_setkey(mem, frm);
			break;

		case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
			mem->last_op.msg_type = msg_type;
			mem->last_op.address = ntohs(frm->address);
			mem->last_op.op_result =
					ioctl_emu_mem_transfer(mem, frm,
							       cmd->blocks, 1);
			break;

		case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
		case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
			memcpy(mem->nonce, frm->nonce, 16);
			mem->last_op.msg_type = msg_type;
			mem->last_op.address = ntohs(frm->address);
			break;
		default:
			break;
		}
		break;

	case MMC_READ_MULTIPLE_BLOCK:
		frm = (struct rpmb_data_frame *)(uintptr_t)cmd->data_ptr;
		msg_type = ntohs(frm->msg_type);

		switch (mem->last_op.msg_type) {
		case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
			ioctl_emu_get_keyprog_result(mem, frm);
			break;

		case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
			ioctl_emu_get_write_result(mem, frm);
			break;

		case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
			ioctl_emu_read_ctr(mem, frm);
			break;

		case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
			ioctl_emu_mem_transfer(mem, frm, cmd->blocks, 0);
			break;

		default:
			EMSG("Unexpected");
			break;
		}
		break;

	default:
		EMSG("Unsupported ioctl opcode 0x%08x", cmd->opcode);
		return -1;
	}

	return 0;
}

static int ioctl_emu(int fd, unsigned long request, ...)
{
	struct mmc_ioc_multi_cmd *mcmd = NULL;
	struct mmc_ioc_cmd *cmd = NULL;
	size_t i = 0;
	int res = 0;
	va_list ap;

	if (request == MMC_IOC_CMD) {
		va_start(ap, request);
		cmd = va_arg(ap, struct mmc_ioc_cmd *);
		va_end(ap);

		res = ioctl_emu_cmd(fd, cmd);
	} else if (request == MMC_IOC_MULTI_CMD) {
		va_start(ap, request);
		mcmd = va_arg(ap, struct mmc_ioc_multi_cmd *);
		va_end(ap);

		for (i = 0; i < mcmd->num_of_cmds; i++) {
			res = ioctl_emu_cmd(fd, &mcmd->cmds[i]);
			if (res)
				return res;
		}
	} else {
		EMSG("Unsupported ioctl: 0x%lx", request);
		return -1;
	}

	return res;
}

static int mmc_rpmb_fd(uint16_t dev_id)
{
	(void)dev_id;

	/* Any value != -1 will do in test mode */
	return 0;
}

static TEEC_Result read_size_mult(uint16_t dev_id, uint8_t *value)
{
	(void)dev_id;

	*value = EMU_RPMB_SIZE_MULT;
	return TEEC_SUCCESS;
}

static TEEC_Result read_rel_wr_sec_c(uint16_t dev_id, uint8_t *value)
{
	(void)dev_id;

	*value = EMU_RPMB_REL_WR_SEC_C;
	return TEEC_SUCCESS;
}

static bool remap_rpmb_dev_id(uint16_t dev_id, uint16_t *ndev_id)
{
	*ndev_id = dev_id;
	return true;
}

#endif /* RPMB_EMU */

static inline void set_mmc_io_cmd(struct mmc_ioc_cmd *cmd, unsigned int blocks,
				  __u32 opcode, int write_flag)
{
	cmd->blksz = 512;
	cmd->blocks = blocks;
	cmd->flags = MMC_RSP_R1 | MMC_CMD_ADTC;
	cmd->opcode = opcode;
	cmd->write_flag = write_flag;
}

static uint32_t rpmb_data_req(int fd, struct rpmb_data_frame *req_frm,
			      size_t req_nfrm, struct rpmb_data_frame *rsp_frm,
			      size_t rsp_nfrm)
{
	TEEC_Result res = TEEC_SUCCESS;
	int st = 0;
	size_t i = 0;
	uint16_t msg_type = ntohs(req_frm->msg_type);
	struct mmc_ioc_multi_cmd *mcmd = NULL;
	struct mmc_ioc_cmd *cmd = NULL;

	for (i = 1; i < req_nfrm; i++) {
		if (req_frm[i].msg_type != msg_type) {
			EMSG("All request frames shall be of the same type");
			return TEEC_ERROR_BAD_PARAMETERS;
		}
	}

	DMSG("Req: %zu frame(s) of type 0x%04x", req_nfrm, msg_type);
	DMSG("Rsp: %zu frame(s)", rsp_nfrm);

	mcmd = (struct mmc_ioc_multi_cmd *)
		calloc(1, sizeof(struct mmc_ioc_multi_cmd) +
			RPMB_MAX_IOC_MULTI_CMDS * sizeof(struct mmc_ioc_cmd));
	if (!mcmd)
		return TEEC_ERROR_OUT_OF_MEMORY;

	switch(msg_type) {
	case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
	case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
		if (rsp_nfrm != 1) {
			EMSG("Expected only one response frame");
			res = TEEC_ERROR_BAD_PARAMETERS;
			goto out;
		}

		mcmd->num_of_cmds = 3;

		/* Send write request frame(s) */
		cmd = &mcmd->cmds[0];
		set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK,
			       1 | MMC_CMD23_ARG_REL_WR);
		/*
		 * Black magic: tested on a HiKey board with a HardKernel eMMC
		 * module. When postsleep values are zero, the kernel logs
		 * random errors: "mmc_blk_ioctl_cmd: Card Status=0x00000E00"
		 * and ioctl() fails.
		 */
		cmd->postsleep_min_us = 20000;
		cmd->postsleep_max_us = 50000;
		mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm);

		/* Send result request frame */
		cmd = &mcmd->cmds[1];
		set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1);
		memset(rsp_frm, 0, sizeof(*rsp_frm));
		rsp_frm->msg_type = htons(RPMB_MSG_TYPE_REQ_RESULT_READ);
		mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);

		/* Read response frame */
		cmd = &mcmd->cmds[2];
		set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0);
		mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
		break;

	case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
		if (rsp_nfrm != 1) {
			EMSG("Expected only one response frame");
			res = TEEC_ERROR_BAD_PARAMETERS;
			goto out;
		}
#if __GNUC__ > 6
		__attribute__((fallthrough));
#endif

	case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
		if (req_nfrm != 1) {
			EMSG("Expected only one request frame");
			res = TEEC_ERROR_BAD_PARAMETERS;
			goto out;
		}

		mcmd->num_of_cmds = 2;

		/* Send request frame */
		cmd = &mcmd->cmds[0];
		set_mmc_io_cmd(cmd, req_nfrm, MMC_WRITE_MULTIPLE_BLOCK, 1);
		mmc_ioc_cmd_set_data((*cmd), (uintptr_t)req_frm);

		/* Read response frames */
		cmd = &mcmd->cmds[1];
		set_mmc_io_cmd(cmd, rsp_nfrm, MMC_READ_MULTIPLE_BLOCK, 0);
		mmc_ioc_cmd_set_data((*cmd), (uintptr_t)rsp_frm);
		break;

	default:
		EMSG("Unsupported message type: %d", msg_type);
		res = TEEC_ERROR_GENERIC;
		goto out;
	}

	st = IOCTL(fd, MMC_IOC_MULTI_CMD, mcmd);
	if (st < 0)
		res = TEEC_ERROR_GENERIC;

out:
	free(mcmd);

	return res;
}

static uint32_t rpmb_get_dev_info(uint16_t dev_id, struct rpmb_dev_info *info)
{
	TEEC_Result res = TEEC_SUCCESS;
	uint8_t rpmb_size_mult = 0;
	uint8_t rel_wr_sec_c = 0;

	res = read_cid(dev_id, info->cid);
	if (res != TEEC_SUCCESS)
		return res;

	res = read_size_mult(dev_id, &rpmb_size_mult);
	if (res != TEEC_SUCCESS)
		return res;
	info->rpmb_size_mult = rpmb_size_mult;

	res = read_rel_wr_sec_c(dev_id, &rel_wr_sec_c);
	if (res != TEEC_SUCCESS)
		return res;
	info->rel_wr_sec_c = rel_wr_sec_c;

	info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;

	return res;
}


/*
 * req is one struct rpmb_req followed by one or more struct rpmb_data_frame
 * rsp is either one struct rpmb_dev_info or one or more struct rpmb_data_frame
 */
static uint32_t rpmb_process_request_unlocked(void *req, size_t req_size,
					      void *rsp, size_t rsp_size)
{
	struct rpmb_req *sreq = req;
	size_t req_nfrm = 0;
	size_t rsp_nfrm = 0;
	uint16_t dev_id = 0;
	uint32_t res = 0;
	int fd = 0;

	if (req_size < sizeof(*sreq))
		return TEEC_ERROR_BAD_PARAMETERS;

	if (!remap_rpmb_dev_id(sreq->dev_id, &dev_id))
		return TEEC_ERROR_ITEM_NOT_FOUND;

	switch (sreq->cmd) {
	case RPMB_CMD_DATA_REQ:
		req_nfrm = (req_size - sizeof(struct rpmb_req)) / 512;
		rsp_nfrm = rsp_size / 512;
		fd = mmc_rpmb_fd(dev_id);
		if (fd < 0)
			return TEEC_ERROR_BAD_PARAMETERS;
		res = rpmb_data_req(fd, RPMB_REQ_DATA(req), req_nfrm, rsp,
				    rsp_nfrm);
		break;

	case RPMB_CMD_GET_DEV_INFO:
		if (req_size != sizeof(struct rpmb_req) ||
		    rsp_size != sizeof(struct rpmb_dev_info)) {
			EMSG("Invalid req/rsp size");
			return TEEC_ERROR_BAD_PARAMETERS;
		}
		res = rpmb_get_dev_info(dev_id, (struct rpmb_dev_info *)rsp);
		break;

	default:
		EMSG("Unsupported RPMB command: %d", sreq->cmd);
		res = TEEC_ERROR_BAD_PARAMETERS;
		break;
	}

	return res;
}


uint32_t rpmb_process_request(void *req, size_t req_size, void *rsp,
			      size_t rsp_size)
{
	uint32_t res = 0;

	tee_supp_mutex_lock(&rpmb_mutex);
	res = rpmb_process_request_unlocked(req, req_size, rsp, rsp_size);
	tee_supp_mutex_unlock(&rpmb_mutex);

	return res;
}