diff mbox series

[isar-cip-core,4/7] add u-boot patch for qemu to support RPMB

Message ID 20221123152906.75323-5-sven.schultschik@siemens.com (mailing list archive)
State Deferred
Headers show
Series Secureboot on QEMU with EDK2, OP-TEE and RPMB | expand

Commit Message

Schultschik, Sven Nov. 23, 2022, 3:29 p.m. UTC
From: Sven Schultschik <sven.schultschik@siemens.com>

Qemu itself is missing a implemented emulation of an RPBM (replay protected memory) which is part of an emmc. Therefore currently a u-boot patch, which adds a RPMB emulation, is needed which breaks the u-boot hardware support. The patch is only included into the qemu u-boot recipes and can be removed if there is an official qemu rpmb emulation, which currently does not have any ETA.

Signed-off-by: Sven Schultschik <sven.schultschik@siemens.com>
---
 ...hack.-Breaks-proper-hardware-support.patch | 1375 +++++++++++++++++
 recipes-bsp/u-boot/u-boot-qemu-common.inc     |    3 +
 2 files changed, 1378 insertions(+)
 create mode 100644 recipes-bsp/u-boot/files/0002-rpmb-emulation-hack.-Breaks-proper-hardware-support.patch
diff mbox series

Patch

diff --git a/recipes-bsp/u-boot/files/0002-rpmb-emulation-hack.-Breaks-proper-hardware-support.patch b/recipes-bsp/u-boot/files/0002-rpmb-emulation-hack.-Breaks-proper-hardware-support.patch
new file mode 100644
index 0000000..26266b5
--- /dev/null
+++ b/recipes-bsp/u-boot/files/0002-rpmb-emulation-hack.-Breaks-proper-hardware-support.patch
@@ -0,0 +1,1375 @@ 
+From a4179f663673dbfa48f79761acc3ff781ee9b2b8 Mon Sep 17 00:00:00 2001
+From: Ilias Apalodimas <ilias.apalodimas@linaro.org>
+Date: Thu, 12 Nov 2020 09:44:54 +0200
+Subject: [PATCH] irpmb patch hack
+
+Signed-off-by: Ilias Apalodimas <ilias.apalodimas@linaro.org>
+---
+ arch/arm/include/asm/gpio.h   |   3 +-
+ arch/arm/include/asm/ioctl.h  |   1 +
+ configs/qemu_tfa_mm_defconfig |  53 ++++
+ drivers/tee/optee/Makefile    |   1 +
+ drivers/tee/optee/hmac_sha2.c | 126 ++++++++
+ drivers/tee/optee/hmac_sha2.h |  74 +++++
+ drivers/tee/optee/rpmb.c      |  27 +-
+ drivers/tee/optee/rpmb.h      |   1 +
+ drivers/tee/optee/rpmb_emu.c  | 563 ++++++++++++++++++++++++++++++++++
+ drivers/tee/optee/rpmb_emu.h  | 141 +++++++++
+ drivers/tee/optee/sha2.c      | 249 +++++++++++++++
+ drivers/tee/optee/sha2.h      |  75 +++++
+ 12 files changed, 1292 insertions(+), 22 deletions(-)
+ create mode 100644 arch/arm/include/asm/ioctl.h
+ create mode 100644 configs/qemu_tfa_mm_defconfig
+ create mode 100644 drivers/tee/optee/hmac_sha2.c
+ create mode 100644 drivers/tee/optee/hmac_sha2.h
+ create mode 100644 drivers/tee/optee/rpmb.h
+ create mode 100644 drivers/tee/optee/rpmb_emu.c
+ create mode 100644 drivers/tee/optee/rpmb_emu.h
+ create mode 100644 drivers/tee/optee/sha2.c
+ create mode 100644 drivers/tee/optee/sha2.h
+
+diff --git a/arch/arm/include/asm/ioctl.h b/arch/arm/include/asm/ioctl.h
+new file mode 100644
+index 000000000000..b279fe06dfe5
+--- /dev/null
++++ b/arch/arm/include/asm/ioctl.h
+@@ -0,0 +1 @@
++#include <asm-generic/ioctl.h>
+diff --git a/drivers/tee/optee/Makefile b/drivers/tee/optee/Makefile
+index 928d3f80027f..28108536d231 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 000000000000..61b24b128f1d
+--- /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 000000000000..1044524d75c5
+--- /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 0804fc963cf5..275f2112f102 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 000000000000..8b137891791f
+--- /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 000000000000..629f36ee6b29
+--- /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 000000000000..3471eecf63b5
+--- /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 000000000000..a9acd7244947
+--- /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 000000000000..4ce0f3cd5231
+--- /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 */
++
+-- 
+2.29.2
+
diff --git a/recipes-bsp/u-boot/u-boot-qemu-common.inc b/recipes-bsp/u-boot/u-boot-qemu-common.inc
index 802fc50..6e7158b 100644
--- a/recipes-bsp/u-boot/u-boot-qemu-common.inc
+++ b/recipes-bsp/u-boot/u-boot-qemu-common.inc
@@ -13,6 +13,9 @@  require recipes-bsp/u-boot/u-boot-common.inc
 
 U_BOOT_BIN = "u-boot.bin"
 
+SRC_URI_append_secureboot = " \
+    file://0002-rpmb-emulation-hack.-Breaks-proper-hardware-support.patch;patch=1"
+
 DEPENDS_append_secureboot = " optee-os-${MACHINE}"
 
 do_deploy[dirs] = "${DEPLOY_DIR_IMAGE}"