rpmb emulation hack. Breaks proper hardware supportrpmb_hack2

Signed-off-by: Ilias Apalodimas <ilias.apalodimas@linaro.org>

12 files changed, 1291 insertions, 22 deletions

diff --git a/arch/arm/include/asm/gpio.h b/arch/arm/include/asm/gpio.h
index 333e407b66..a8413b2544 100644
--- a/arch/arm/include/asm/gpio.h
+++ b/arch/arm/include/asm/gpio.h
@@ -6,7 +6,7 @@
 	!defined(CONFIG_ARCH_LS1088A) && !defined(CONFIG_ARCH_ASPEED) && \
 	!defined(CONFIG_ARCH_LS1012A) && !defined(CONFIG_ARCH_LS1043A) && \
 	!defined(CONFIG_ARCH_LS1046A) && !defined(CONFIG_ARCH_U8500) && \
-	!defined(CONFIG_CORTINA_PLATFORM)
+	!defined(CONFIG_CORTINA_PLATFORM) && !defined(CONFIG_ARCH_QEMU)
 #include <asm/arch/gpio.h>
 #endif
 #include <asm-generic/gpio.h>
diff --git a/arch/arm/include/asm/ioctl.h b/arch/arm/include/asm/ioctl.h
new file mode 100644
index 0000000000..b279fe06df
--- /dev/null
+++ b/arch/arm/include/asm/ioctl.h
@@ -0,0 +1 @@
+#include <asm-generic/ioctl.h>
diff --git a/configs/qemu_tfa_mm_defconfig b/configs/qemu_tfa_mm_defconfig
new file mode 100644
index 0000000000..0b841276bf
--- /dev/null
+++ b/configs/qemu_tfa_mm_defconfig
@@ -0,0 +1,53 @@
+CONFIG_ARM=y
+CONFIG_POSITION_INDEPENDENT=y
+CONFIG_ARCH_QEMU=y
+CONFIG_TFABOOT=y
+CONFIG_ENV_SIZE=0x40000
+CONFIG_ENV_SECT_SIZE=0x40000
+CONFIG_NR_DRAM_BANKS=1
+CONFIG_AHCI=y
+CONFIG_DISTRO_DEFAULTS=y
+CONFIG_FIT=y
+CONFIG_FIT_SIGNATURE=y
+CONFIG_FIT_VERBOSE=y
+CONFIG_FIT_BEST_MATCH=y
+CONFIG_LEGACY_IMAGE_FORMAT=y
+CONFIG_USE_PREBOOT=y
+CONFIG_PREBOOT="pci enum"
+# CONFIG_DISPLAY_CPUINFO is not set
+# CONFIG_DISPLAY_BOARDINFO is not set
+CONFIG_CMD_BOOTEFI_SELFTEST=y
+CONFIG_CMD_NVEDIT_EFI=y
+CONFIG_CMD_PCI=y
+CONFIG_CMD_USB=y
+CONFIG_CMD_EFIDEBUG=y
+CONFIG_OF_BOARD=y
+CONFIG_ENV_IS_IN_FLASH=y
+CONFIG_ENV_ADDR=0x4000000
+CONFIG_SCSI_AHCI=y
+CONFIG_AHCI_PCI=y
+CONFIG_DM_MMC=y
+CONFIG_SUPPORT_EMMC_RPMB=y
+CONFIG_DM_MTD=y
+CONFIG_MTD_NOR_FLASH=y
+CONFIG_FLASH_CFI_DRIVER=y
+CONFIG_CFI_FLASH=y
+CONFIG_SYS_FLASH_USE_BUFFER_WRITE=y
+CONFIG_SYS_FLASH_CFI=y
+CONFIG_DM_ETH=y
+CONFIG_E1000=y
+CONFIG_NVME=y
+CONFIG_PCI=y
+CONFIG_DM_PCI=y
+CONFIG_PCIE_ECAM_GENERIC=y
+CONFIG_SCSI=y
+CONFIG_DM_SCSI=y
+CONFIG_SYSRESET=y
+CONFIG_SYSRESET_PSCI=y
+CONFIG_TEE=y
+CONFIG_OPTEE=y
+CONFIG_USB=y
+CONFIG_DM_USB=y
+CONFIG_USB_EHCI_HCD=y
+CONFIG_USB_EHCI_PCI=y
+CONFIG_EFI_MM_COMM_TEE=y
diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
index 928d3f8002..28108536d2 100644
--- a/drivers/tee/optee/Makefile
+++ b/drivers/tee/optee/Makefile
@@ -3,3 +3,4 @@
 obj-y += core.o
 obj-y += supplicant.o
 obj-$(CONFIG_SUPPORT_EMMC_RPMB) += rpmb.o
+obj-y += sha2.o hmac_sha2.o rpmb_emu.o rpmb.o
diff --git a/drivers/tee/optee/hmac_sha2.c b/drivers/tee/optee/hmac_sha2.c
new file mode 100644
index 0000000000..61b24b128f
--- /dev/null
+++ b/drivers/tee/optee/hmac_sha2.c
@@ -0,0 +1,126 @@
+/*
+ * HMAC-SHA-224/256/384/512 implementation
+ * Last update: 06/15/2005
+ * Issue date:  06/15/2005
+ *
+ * Copyright (C) 2005 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * 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.
+ * 3. Neither the name of the project nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT 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 PROJECT 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 "hmac_sha2.h"
+
+/* HMAC-SHA-256 functions */
+
+void hmac_sha256_init(hmac_sha256_ctx *ctx, const unsigned char *key,
+                      unsigned int key_size)
+{
+    unsigned int fill = 0;
+    unsigned int num = 0;
+    const unsigned char *key_used = NULL;
+    unsigned char key_temp[SHA256_DIGEST_SIZE] = { 0 };
+    int i = 0;
+
+    if (key_size == SHA256_BLOCK_SIZE) {
+        key_used = key;
+        num = SHA256_BLOCK_SIZE;
+    } else {
+        if (key_size > SHA256_BLOCK_SIZE){
+            num = SHA256_DIGEST_SIZE;
+            sha256(key, key_size, key_temp);
+            key_used = key_temp;
+        } else { /* key_size > SHA256_BLOCK_SIZE */
+            key_used = key;
+            num = key_size;
+        }
+        fill = SHA256_BLOCK_SIZE - num;
+
+        memset(ctx->block_ipad + num, 0x36, fill);
+        memset(ctx->block_opad + num, 0x5c, fill);
+    }
+
+    for (i = 0; i < (int) num; i++) {
+        ctx->block_ipad[i] = key_used[i] ^ 0x36;
+        ctx->block_opad[i] = key_used[i] ^ 0x5c;
+    }
+
+    sha256_init(&ctx->ctx_inside);
+    sha256_update_tee(&ctx->ctx_inside, ctx->block_ipad, SHA256_BLOCK_SIZE);
+
+    sha256_init(&ctx->ctx_outside);
+    sha256_update_tee(&ctx->ctx_outside, ctx->block_opad,
+                  SHA256_BLOCK_SIZE);
+
+    /* for hmac_reinit */
+    memcpy(&ctx->ctx_inside_reinit, &ctx->ctx_inside,
+           sizeof(sha256_ctx));
+    memcpy(&ctx->ctx_outside_reinit, &ctx->ctx_outside,
+           sizeof(sha256_ctx));
+}
+
+void hmac_sha256_reinit(hmac_sha256_ctx *ctx)
+{
+    memcpy(&ctx->ctx_inside, &ctx->ctx_inside_reinit,
+           sizeof(sha256_ctx));
+    memcpy(&ctx->ctx_outside, &ctx->ctx_outside_reinit,
+           sizeof(sha256_ctx));
+}
+
+void hmac_sha256_update(hmac_sha256_ctx *ctx, const unsigned char *message,
+                        unsigned int message_len)
+{
+    sha256_update_tee(&ctx->ctx_inside, message, message_len);
+}
+
+void hmac_sha256_final(hmac_sha256_ctx *ctx, unsigned char *mac,
+                       unsigned int mac_size)
+{
+    unsigned char digest_inside[SHA256_DIGEST_SIZE] = { 0 };
+    unsigned char mac_temp[SHA256_DIGEST_SIZE] = { 0 };
+
+    sha256_final(&ctx->ctx_inside, digest_inside);
+    sha256_update_tee(&ctx->ctx_outside, digest_inside, SHA256_DIGEST_SIZE);
+    sha256_final(&ctx->ctx_outside, mac_temp);
+    memcpy(mac, mac_temp, mac_size);
+}
+
+void hmac_sha256(const unsigned char *key, unsigned int key_size,
+          const unsigned char *message, unsigned int message_len,
+          unsigned char *mac, unsigned mac_size)
+{
+    hmac_sha256_ctx ctx;
+
+    memset(&ctx, 0, sizeof(ctx));
+
+    hmac_sha256_init(&ctx, key, key_size);
+    hmac_sha256_update(&ctx, message, message_len);
+    hmac_sha256_final(&ctx, mac, mac_size);
+}
diff --git a/drivers/tee/optee/hmac_sha2.h b/drivers/tee/optee/hmac_sha2.h
new file mode 100644
index 0000000000..1044524d75
--- /dev/null
+++ b/drivers/tee/optee/hmac_sha2.h
@@ -0,0 +1,74 @@
+/*
+ * HMAC-SHA-224/256/384/512 implementation
+ * Last update: 06/15/2005
+ * Issue date:  06/15/2005
+ *
+ * Copyright (C) 2005 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * 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.
+ * 3. Neither the name of the project nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT 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 PROJECT 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.
+ */
+
+#ifndef HMAC_SHA2_H
+#define HMAC_SHA2_H
+
+#include "sha2.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    sha256_ctx ctx_inside;
+    sha256_ctx ctx_outside;
+
+    /* for hmac_reinit */
+    sha256_ctx ctx_inside_reinit;
+    sha256_ctx ctx_outside_reinit;
+
+    unsigned char block_ipad[SHA256_BLOCK_SIZE];
+    unsigned char block_opad[SHA256_BLOCK_SIZE];
+} hmac_sha256_ctx;
+
+void hmac_sha256_init(hmac_sha256_ctx *ctx, const unsigned char *key,
+                      unsigned int key_size);
+void hmac_sha256_reinit(hmac_sha256_ctx *ctx);
+void hmac_sha256_update(hmac_sha256_ctx *ctx, const unsigned char *message,
+                        unsigned int message_len);
+void hmac_sha256_final(hmac_sha256_ctx *ctx, unsigned char *mac,
+                       unsigned int mac_size);
+void hmac_sha256(const unsigned char *key, unsigned int key_size,
+                 const unsigned char *message, unsigned int message_len,
+                 unsigned char *mac, unsigned mac_size);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* !HMAC_SHA2_H */
+
diff --git a/drivers/tee/optee/rpmb.c b/drivers/tee/optee/rpmb.c
index 0804fc963c..275f2112f1 100644
--- a/drivers/tee/optee/rpmb.c
+++ b/drivers/tee/optee/rpmb.c
@@ -12,35 +12,15 @@
 
 #include "optee_msg.h"
 #include "optee_private.h"
+#include "rpmb_emu.h"
 
 /*
  * Request and response definitions must be in sync with the secure side of
  * OP-TEE.
  */
 
-/* Request */
-struct rpmb_req {
-	u16 cmd;
-#define RPMB_CMD_DATA_REQ      0x00
-#define RPMB_CMD_GET_DEV_INFO  0x01
-	u16 dev_id;
-	u16 block_count;
-	/* Optional data frames (rpmb_data_frame) follow */
-};
-
 #define RPMB_REQ_DATA(req) ((void *)((struct rpmb_req *)(req) + 1))
 
-/* Response to device info request */
-struct rpmb_dev_info {
-	u8 cid[16];
-	u8 rpmb_size_mult;	/* EXT CSD-slice 168: RPMB Size */
-	u8 rel_wr_sec_c;	/* EXT CSD-slice 222: Reliable Write Sector */
-				/*                    Count */
-	u8 ret_code;
-#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
-#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
-};
-
 static void release_mmc(struct optee_private *priv)
 {
 	int rc;
@@ -175,8 +155,13 @@ void optee_suppl_cmd_rpmb(struct udevice *dev, struct optee_msg_arg *arg)
 	rsp_buf = (u8 *)rsp_shm->addr + arg->params[1].u.rmem.offs;
 	rsp_size = arg->params[1].u.rmem.size;
 
+#ifdef EMU
 	arg->ret = rpmb_process_request(dev_get_priv(dev), req_buf, req_size,
 					rsp_buf, rsp_size);
+#else
+	arg->ret = rpmb_process_request_emu(req_buf, req_size, rsp_buf,
+					    rsp_size);
+#endif
 }
 
 void optee_suppl_rpmb_release(struct udevice *dev)
diff --git a/drivers/tee/optee/rpmb.h b/drivers/tee/optee/rpmb.h
new file mode 100644
index 0000000000..8b13789179
--- /dev/null
+++ b/drivers/tee/optee/rpmb.h
@@ -0,0 +1 @@
+
diff --git a/drivers/tee/optee/rpmb_emu.c b/drivers/tee/optee/rpmb_emu.c
new file mode 100644
index 0000000000..7f4f688573
--- /dev/null
+++ b/drivers/tee/optee/rpmb_emu.c
@@ -0,0 +1,563 @@
+// SPDX-License-Identifier: BSD-2-Clause
+/*
+ * Copyright (c) 2020 Linaro Limited
+ */
+
+#include <common.h>
+#include <dm.h>
+#include <hexdump.h>
+#include <log.h>
+#include <tee.h>
+#include <mmc.h>
+#include <dm/device_compat.h>
+
+#include "optee_msg.h"
+#include "optee_private.h"
+#include "sha2.h"
+#include "hmac_sha2.h"
+#include "rpmb_emu.h"
+
+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) {
+		printf("Emulating more than 1 RPMB partition is not supported\n");
+		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);
+		//print_hex_dump_bytes("", DUMP_PREFIX_OFFSET, 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) {
+		printf("Cannot check MAC (key not set)\n");
+		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)) {
+		printf("Invalid MAC\n");
+		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) {
+		printf("Cannot compute MAC (key not set)\n");
+		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) {
+		printf("Transfer bounds exceeed emulated memory\n");
+		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);
+
+	//printf("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) {
+		printf("Key already set\n");
+		return gen_msb1st_result(RPMB_RESULT_GENERAL_FAILURE);
+	}
+	print_hex_dump_bytes("Setting Key:", DUMP_PREFIX_OFFSET, 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)
+{
+	printf("Reading counter\n");
+	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 TEE_SUCCESS;
+}
+
+static void ioctl_emu_set_ext_csd(uint8_t *ext_csd)
+{
+	ext_csd[168] = EMU_RPMB_SIZE_MULT;
+	ext_csd[222] = EMU_RPMB_REL_WR_SEC_C;
+}
+
+/* A crude emulation of the MMC ioctls we need for RPMB */
+static int ioctl_emu(int fd, unsigned long request, ...)
+{
+	struct mmc_ioc_cmd *cmd = NULL;
+	struct rpmb_data_frame *frm = NULL;
+	uint16_t msg_type = 0;
+	struct rpmb_emu *mem = mem_for_fd(fd);
+	va_list ap;
+
+	if (request != MMC_IOC_CMD) {
+		printf("Unsupported ioctl: 0x%lx\n", request);
+		return -1;
+	}
+	if (!mem)
+		return -1;
+
+	va_start(ap, request);
+	cmd = va_arg(ap, struct mmc_ioc_cmd *);
+	va_end(ap);
+
+	switch (cmd->opcode) {
+	case MMC_SEND_EXT_CSD:
+		ioctl_emu_set_ext_csd((uint8_t *)(uintptr_t)cmd->data_ptr);
+		break;
+
+	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:
+			printf("Unexpected\n");
+			break;
+		}
+		break;
+
+	default:
+		printf("Unsupported ioctl opcode 0x%08x\n", cmd->opcode);
+		return -1;
+	}
+
+	return 0;
+}
+
+static int mmc_rpmb_fd(uint16_t dev_id)
+{
+	(void)dev_id;
+
+	/* Any value != -1 will do in test mode */
+	return 0;
+}
+
+static int mmc_fd(uint16_t dev_id)
+{
+	(void)dev_id;
+
+	return 0;
+}
+
+static void close_mmc_fd(int fd)
+{
+	(void)fd;
+}
+
+/*
+ * Extended CSD Register is 512 bytes and defines device properties
+ * and selected modes.
+ */
+static uint32_t read_ext_csd(int fd, uint8_t *ext_csd)
+{
+	int st = 0;
+	struct mmc_ioc_cmd cmd = {
+		.blksz = 512,
+		.blocks = 1,
+		.flags = MMC_RSP_R1 | MMC_CMD_ADTC,
+		.opcode = MMC_SEND_EXT_CSD,
+	};
+
+	mmc_ioc_cmd_set_data(cmd, ext_csd);
+
+	st = IOCTL(fd, MMC_IOC_CMD, &cmd);
+	if (st < 0)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+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)
+{
+	int st = 0;
+	size_t i = 0;
+	uint16_t msg_type = ntohs(req_frm->msg_type);
+	struct mmc_ioc_cmd cmd = {
+		.blksz = 512,
+		.blocks = req_nfrm,
+		.data_ptr = (uintptr_t)req_frm,
+		.flags = MMC_RSP_R1 | MMC_CMD_ADTC,
+		.opcode = MMC_WRITE_MULTIPLE_BLOCK,
+		.write_flag = 1,
+	};
+
+	for (i = 1; i < req_nfrm; i++) {
+		if (req_frm[i].msg_type != msg_type) {
+			printf("All request frames shall be of the same type\n");
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	}
+
+	//printf("Req: %zu frame(s) of type 0x%04x", req_nfrm, msg_type);
+	//printf("Rsp: %zu frame(s)", rsp_nfrm);
+
+	switch(msg_type) {
+	case RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM:
+	case RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE:
+		if (rsp_nfrm != 1) {
+			printf("Expected only one response frame\n");
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+
+		/* Send write request frame(s) */
+		cmd.write_flag |= 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;
+		st = IOCTL(fd, MMC_IOC_CMD, &cmd);
+		if (st < 0)
+			return TEE_ERROR_GENERIC;
+		cmd.postsleep_min_us = 0;
+		cmd.postsleep_max_us = 0;
+
+		/* Send result request frame */
+		memset(rsp_frm, 0, 1);
+		rsp_frm->msg_type = htons(RPMB_MSG_TYPE_REQ_RESULT_READ);
+		cmd.data_ptr = (uintptr_t)rsp_frm;
+		cmd.write_flag &= ~MMC_CMD23_ARG_REL_WR;
+		st = IOCTL(fd, MMC_IOC_CMD, &cmd);
+		if (st < 0)
+			return TEE_ERROR_GENERIC;
+
+		/* Read response frame */
+		cmd.opcode = MMC_READ_MULTIPLE_BLOCK;
+		cmd.write_flag = 0;
+		cmd.blocks = rsp_nfrm;
+		st = IOCTL(fd, MMC_IOC_CMD, &cmd);
+		if (st < 0)
+			return TEE_ERROR_GENERIC;
+		break;
+
+	case RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ:
+		if (rsp_nfrm != 1) {
+			printf("Expected only one response frame\n");
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+#if __GNUC__ > 6
+		__attribute__((fallthrough));
+#endif
+
+	case RPMB_MSG_TYPE_REQ_AUTH_DATA_READ:
+		if (req_nfrm != 1) {
+			printf("Expected only one request frame\n");
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+
+		/* Send request frame */
+		st = IOCTL(fd, MMC_IOC_CMD, &cmd);
+		if (st < 0)
+			return TEE_ERROR_GENERIC;
+
+		/* Read response frames */
+		cmd.data_ptr = (uintptr_t)rsp_frm;
+		cmd.opcode = MMC_READ_MULTIPLE_BLOCK;
+		cmd.write_flag = 0;
+		cmd.blocks = rsp_nfrm;
+		st = IOCTL(fd, MMC_IOC_CMD, &cmd);
+		if (st < 0)
+			return TEE_ERROR_GENERIC;
+		break;
+
+	default:
+		printf("Unsupported message type: %d", msg_type);
+		return TEE_ERROR_GENERIC;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static uint32_t rpmb_get_dev_info(uint16_t dev_id, struct rpmb_dev_info *info)
+{
+	int fd = 0;
+	uint32_t res = 0;
+	uint8_t ext_csd[512] = { 0 };
+
+	res = read_cid(dev_id, info->cid);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	fd = mmc_fd(dev_id);
+	if (fd < 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = read_ext_csd(fd, ext_csd);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	info->rel_wr_sec_c = ext_csd[222];
+	info->rpmb_size_mult = ext_csd[168];
+	info->ret_code = RPMB_CMD_GET_DEV_INFO_RET_OK;
+
+err:
+	close_mmc_fd(fd);
+	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
+ */
+uint32_t rpmb_process_request_emu(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;
+	uint32_t res = 0;
+	int fd = 0;
+
+	if (req_size < sizeof(*sreq))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	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(sreq->dev_id);
+		if (fd < 0)
+			return TEE_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)) {
+			printf("Invalid req/rsp size");
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+		res = rpmb_get_dev_info(sreq->dev_id,
+					(struct rpmb_dev_info *)rsp);
+		break;
+
+	default:
+		printf("Unsupported RPMB command: %d", sreq->cmd);
+		res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	}
+
+	return res;
+}
diff --git a/drivers/tee/optee/rpmb_emu.h b/drivers/tee/optee/rpmb_emu.h
new file mode 100644
index 0000000000..3471eecf63
--- /dev/null
+++ b/drivers/tee/optee/rpmb_emu.h
@@ -0,0 +1,141 @@
+#include <linux/ioctl.h>
+
+/* mmc_ioc_cmd.opcode */
+#define MMC_SEND_EXT_CSD                 8
+#define MMC_READ_MULTIPLE_BLOCK         18
+#define MMC_WRITE_MULTIPLE_BLOCK        25
+
+#define IOCTL(fd, request, ...) ioctl_emu((fd), (request), ##__VA_ARGS__)
+#define mmc_ioc_cmd_set_data(ic, ptr) ic.data_ptr = (__u64)(unsigned long) ptr
+#define MMC_CMD23_ARG_REL_WR    (1 << 31) /* CMD23 reliable write */
+
+/* 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)
+
+struct mmc_ioc_cmd {
+	/* Implies direction of data.  true = write, false = read */
+	int write_flag;
+
+	/* Application-specific command.  true = precede with CMD55 */
+	int is_acmd;
+
+	uint32_t opcode;
+	uint32_t arg;
+	uint32_t response[4];  /* CMD response */
+	unsigned int flags;
+	unsigned int blksz;
+	unsigned int blocks;
+
+	/*
+	 * Sleep at least postsleep_min_us useconds, and at most
+	 * postsleep_max_us useconds *after* issuing command.  Needed for
+	 * some read commands for which cards have no other way of indicating
+	 * they're ready for the next command (i.e. there is no equivalent of
+	 * a "busy" indicator for read operations).
+	 */
+	unsigned int postsleep_min_us;
+	unsigned int postsleep_max_us;
+
+	/*
+	 * Override driver-computed timeouts.  Note the difference in units!
+	 */
+	unsigned int data_timeout_ns;
+	unsigned int cmd_timeout_ms;
+
+	/*
+	 * For 64-bit machines, the next member, ``__u64 data_ptr``, wants to
+	 * be 8-byte aligned.  Make sure this struct is the same size when
+	 * built for 32-bit.
+	 */
+	uint32_t __pad;
+
+	/* DAT buffer */
+	uint32_t data_ptr;
+};
+#define MMC_BLOCK_MAJOR		179
+#define MMC_IOC_CMD _IOWR(MMC_BLOCK_MAJOR, 0, struct mmc_ioc_cmd)
+
+/* 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))
+
+/* Response to device info request */
+struct rpmb_dev_info {
+	uint8_t cid[16];
+	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
+};
+/* 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 */
+
+
+/* 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;
+};
+
+/*
+ * 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
+};
+
+uint32_t rpmb_process_request_emu(void *req, size_t req_size,
+                                  void *rsp, size_t rsp_size);
diff --git a/drivers/tee/optee/sha2.c b/drivers/tee/optee/sha2.c
new file mode 100644
index 0000000000..a9acd72449
--- /dev/null
+++ b/drivers/tee/optee/sha2.c
@@ -0,0 +1,249 @@
+/*
+ * FIPS 180-2 SHA-224/256/384/512 implementation
+ * Last update: 02/02/2007
+ * Issue date:  04/30/2005
+ *
+ * Copyright (C) 2005, 2007 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * 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.
+ * 3. Neither the name of the project nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT 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 PROJECT 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 "sha2.h"
+
+#define SHFR(x, n)    (x >> n)
+#define ROTR(x, n)   ((x >> n) | (x << ((sizeof(x) << 3) - n)))
+#define ROTL(x, n)   ((x << n) | (x >> ((sizeof(x) << 3) - n)))
+#define CH(x, y, z)  ((x & y) ^ (~x & z))
+#define MAJ(x, y, z) ((x & y) ^ (x & z) ^ (y & z))
+
+#define SHA256_F1(x) (ROTR(x,  2) ^ ROTR(x, 13) ^ ROTR(x, 22))
+#define SHA256_F2(x) (ROTR(x,  6) ^ ROTR(x, 11) ^ ROTR(x, 25))
+#define SHA256_F3(x) (ROTR(x,  7) ^ ROTR(x, 18) ^ SHFR(x,  3))
+#define SHA256_F4(x) (ROTR(x, 17) ^ ROTR(x, 19) ^ SHFR(x, 10))
+
+#define UNPACK32(x, str)                      \
+{                                             \
+    *((str) + 3) = (uint8) ((x)      );       \
+    *((str) + 2) = (uint8) ((x) >>  8);       \
+    *((str) + 1) = (uint8) ((x) >> 16);       \
+    *((str) + 0) = (uint8) ((x) >> 24);       \
+}
+
+#define PACK32(str, x)                        \
+{                                             \
+    *(x) =   ((uint32) *((str) + 3)      )    \
+           | ((uint32) *((str) + 2) <<  8)    \
+           | ((uint32) *((str) + 1) << 16)    \
+           | ((uint32) *((str) + 0) << 24);   \
+}
+
+#define UNPACK64(x, str)                      \
+{                                             \
+    *((str) + 7) = (uint8) ((x)      );       \
+    *((str) + 6) = (uint8) ((x) >>  8);       \
+    *((str) + 5) = (uint8) ((x) >> 16);       \
+    *((str) + 4) = (uint8) ((x) >> 24);       \
+    *((str) + 3) = (uint8) ((x) >> 32);       \
+    *((str) + 2) = (uint8) ((x) >> 40);       \
+    *((str) + 1) = (uint8) ((x) >> 48);       \
+    *((str) + 0) = (uint8) ((x) >> 56);       \
+}
+
+#define PACK64(str, x)                        \
+{                                             \
+    *(x) =   ((uint64) *((str) + 7)      )    \
+           | ((uint64) *((str) + 6) <<  8)    \
+           | ((uint64) *((str) + 5) << 16)    \
+           | ((uint64) *((str) + 4) << 24)    \
+           | ((uint64) *((str) + 3) << 32)    \
+           | ((uint64) *((str) + 2) << 40)    \
+           | ((uint64) *((str) + 1) << 48)    \
+           | ((uint64) *((str) + 0) << 56);   \
+}
+
+#define SHA256_SCR(i)                         \
+{                                             \
+    w[i] =  SHA256_F4(w[i -  2]) + w[i -  7]  \
+          + SHA256_F3(w[i - 15]) + w[i - 16]; \
+}
+
+uint32 sha256_h0[8] =
+            {0x6a09e667, 0xbb67ae85, 0x3c6ef372, 0xa54ff53a,
+             0x510e527f, 0x9b05688c, 0x1f83d9ab, 0x5be0cd19};
+
+uint32 sha256_k[64] =
+            {0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
+             0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
+             0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
+             0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
+             0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
+             0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
+             0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
+             0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
+             0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
+             0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
+             0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
+             0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
+             0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
+             0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
+             0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
+             0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2};
+
+/* SHA-256 functions */
+
+static void sha256_transf(sha256_ctx *ctx, const unsigned char *message,
+                          unsigned int block_nb)
+{
+    uint32 w[64] = { 0 };
+    uint32 wv[8] = { 0 };
+    uint32 t1 = 0;
+    uint32 t2 = 0;
+    const unsigned char *sub_block = NULL;
+    int i = 0;
+    int j = 0;
+
+    for (i = 0; i < (int) block_nb; i++) {
+        sub_block = message + (i << 6);
+
+        for (j = 0; j < 16; j++) {
+            PACK32(&sub_block[j << 2], &w[j]);
+        }
+
+        for (j = 16; j < 64; j++) {
+            SHA256_SCR(j);
+        }
+
+        for (j = 0; j < 8; j++) {
+            wv[j] = ctx->h[j];
+        }
+
+        for (j = 0; j < 64; j++) {
+            t1 = wv[7] + SHA256_F2(wv[4]) + CH(wv[4], wv[5], wv[6])
+                + sha256_k[j] + w[j];
+            t2 = SHA256_F1(wv[0]) + MAJ(wv[0], wv[1], wv[2]);
+            wv[7] = wv[6];
+            wv[6] = wv[5];
+            wv[5] = wv[4];
+            wv[4] = wv[3] + t1;
+            wv[3] = wv[2];
+            wv[2] = wv[1];
+            wv[1] = wv[0];
+            wv[0] = t1 + t2;
+        }
+
+        for (j = 0; j < 8; j++) {
+            ctx->h[j] += wv[j];
+        }
+    }
+}
+
+void sha256(const unsigned char *message, unsigned int len,
+	    unsigned char *digest)
+{
+    sha256_ctx ctx;
+
+    memset(&ctx, 0, sizeof(ctx));
+
+    sha256_init(&ctx);
+    sha256_update_tee(&ctx, message, len);
+    sha256_final(&ctx, digest);
+}
+
+void sha256_init(sha256_ctx *ctx)
+{
+    int i = 0;
+
+    for (i = 0; i < 8; i++) {
+        ctx->h[i] = sha256_h0[i];
+    }
+
+    ctx->len = 0;
+    ctx->tot_len = 0;
+}
+
+void sha256_update_tee(sha256_ctx *ctx, const unsigned char *message,
+                   unsigned int len)
+{
+    unsigned int block_nb = 0;
+    unsigned int new_len = 0;
+    unsigned int rem_len = 0;
+    unsigned int tmp_len = 0;
+    const unsigned char *shifted_message = NULL;
+
+    tmp_len = SHA256_BLOCK_SIZE - ctx->len;
+    rem_len = len < tmp_len ? len : tmp_len;
+
+    memcpy(&ctx->block[ctx->len], message, rem_len);
+
+    if (ctx->len + len < SHA256_BLOCK_SIZE) {
+        ctx->len += len;
+        return;
+    }
+
+    new_len = len - rem_len;
+    block_nb = new_len / SHA256_BLOCK_SIZE;
+
+    shifted_message = message + rem_len;
+
+    sha256_transf(ctx, ctx->block, 1);
+    sha256_transf(ctx, shifted_message, block_nb);
+
+    rem_len = new_len % SHA256_BLOCK_SIZE;
+
+    memcpy(ctx->block, &shifted_message[block_nb << 6],
+           rem_len);
+
+    ctx->len = rem_len;
+    ctx->tot_len += (block_nb + 1) << 6;
+}
+
+void sha256_final(sha256_ctx *ctx, unsigned char *digest)
+{
+    unsigned int block_nb = 0;
+    unsigned int pm_len = 0;
+    unsigned int len_b = 0;
+    int i = 0;
+
+    block_nb = (1 + ((SHA256_BLOCK_SIZE - 9)
+                     < (ctx->len % SHA256_BLOCK_SIZE)));
+
+    len_b = (ctx->tot_len + ctx->len) << 3;
+    pm_len = block_nb << 6;
+
+    memset(ctx->block + ctx->len, 0, pm_len - ctx->len);
+    ctx->block[ctx->len] = 0x80;
+    UNPACK32(len_b, ctx->block + pm_len - 4);
+
+    sha256_transf(ctx, ctx->block, block_nb);
+
+    for (i = 0 ; i < 8; i++) {
+        UNPACK32(ctx->h[i], &digest[i << 2]);
+    }
+}
diff --git a/drivers/tee/optee/sha2.h b/drivers/tee/optee/sha2.h
new file mode 100644
index 0000000000..4ce0f3cd52
--- /dev/null
+++ b/drivers/tee/optee/sha2.h
@@ -0,0 +1,75 @@
+/*
+ * FIPS 180-2 SHA-224/256/384/512 implementation
+ * Last update: 02/02/2007
+ * Issue date:  04/30/2005
+ *
+ * Copyright (C) 2005, 2007 Olivier Gay <olivier.gay@a3.epfl.ch>
+ * All rights reserved.
+ *
+ * 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.
+ * 3. Neither the name of the project nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE PROJECT 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 PROJECT 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.
+ */
+
+#ifndef SHA2_H
+#define SHA2_H
+
+#define SHA256_DIGEST_SIZE ( 256 / 8)
+#define SHA256_BLOCK_SIZE  ( 512 / 8)
+
+#ifndef SHA2_TYPES
+#define SHA2_TYPES
+typedef unsigned char uint8;
+typedef unsigned int  uint32;
+typedef unsigned long long uint64;
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+    unsigned int tot_len;
+    unsigned int len;
+    unsigned char block[2 * SHA256_BLOCK_SIZE];
+    uint32 h[8];
+} sha256_ctx;
+
+typedef sha256_ctx sha224_ctx;
+
+void sha256_init(sha256_ctx * ctx);
+void sha256_update_tee(sha256_ctx *ctx, const unsigned char *message,
+                   unsigned int len);
+void sha256_final(sha256_ctx *ctx, unsigned char *digest);
+void sha256(const unsigned char *message, unsigned int len,
+            unsigned char *digest);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* !SHA2_H */
+