Initial commit with upstream sources

Change-Id: Ie9460111f21fc955102fd8732a0173b2d0499a4a

41 files changed, 15128 insertions, 0 deletions

diff --git a/core/tee/se/aid.c b/core/tee/se/aid.c
new file mode 100644
index 0000000..47c5c94
--- /dev/null
+++ b/core/tee/se/aid.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <kernel/panic.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/aid.h>
+#include <tee/se/util.h>
+
+#include "aid_priv.h"
+
+TEE_Result tee_se_aid_create(const char *name, struct tee_se_aid **aid)
+{
+	size_t str_length = strlen(name);
+	size_t aid_length = str_length / 2;
+
+	assert(aid);
+	if (*aid)
+		panic("aid already allocated");
+
+	if (str_length < MIN_AID_LENGTH || str_length > MAX_AID_LENGTH)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	*aid = malloc(sizeof(struct tee_se_aid));
+	if (!(*aid))
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	hex_decode(name, str_length, (*aid)->aid);
+	(*aid)->length = aid_length;
+	(*aid)->refcnt = 1;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_aid_create_from_buffer(uint8_t *id, size_t length,
+		struct tee_se_aid **aid)
+{
+	*aid = malloc(sizeof(struct tee_se_aid));
+	if (!(*aid))
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memcpy((*aid)->aid, id, length);
+	(*aid)->length = length;
+	(*aid)->refcnt = 1;
+	return TEE_SUCCESS;
+}
+
+void tee_se_aid_acquire(struct tee_se_aid *aid)
+{
+	assert(aid);
+	aid->refcnt++;
+}
+
+int tee_se_aid_get_refcnt(struct tee_se_aid *aid)
+{
+	assert(aid);
+	return aid->refcnt;
+}
+
+void tee_se_aid_release(struct tee_se_aid *aid)
+{
+	assert(aid);
+	if (aid->refcnt <= 0)
+		panic();
+	aid->refcnt--;
+	if (!aid->refcnt)
+		free(aid);
+}
diff --git a/core/tee/se/aid_priv.h b/core/tee/se/aid_priv.h
new file mode 100644
index 0000000..555246f
--- /dev/null
+++ b/core/tee/se/aid_priv.h
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef TEE_SE_AID_PRIV_H
+#define TEE_SE_AID_PRIV_H
+
+struct tee_se_aid {
+	uint8_t aid[MAX_AID_LENGTH];
+	size_t length;
+	int refcnt;
+};
+
+int tee_se_aid_get_refcnt(struct tee_se_aid *aid);
+
+TEE_Result tee_se_aid_create(const char *name, struct tee_se_aid **aid);
+
+void tee_se_aid_acquire(struct tee_se_aid *aid);
+
+#endif
diff --git a/core/tee/se/apdu.c b/core/tee/se/apdu.c
new file mode 100644
index 0000000..ddef30d
--- /dev/null
+++ b/core/tee/se/apdu.c
@@ -0,0 +1,183 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/apdu.h>
+#include <tee/se/util.h>
+
+#include "apdu_priv.h"
+
+/*
+ * APDU format, [..] means optional fields
+ *
+ * CMD_APDU: CLA, INS, P1, P2, [LC, DATA, LE]
+ * RESP_APDU: [DATA], SW1, SW2
+ *
+ */
+#define CMD_APDU_SIZE(lc) ((lc) + 4)
+#define RESP_APDU_SIZE(le) ((le) + 2)
+
+struct cmd_apdu *alloc_cmd_apdu(uint8_t cla, uint8_t ins, uint8_t p1,
+		uint8_t p2, uint8_t lc, uint8_t le, uint8_t *data)
+{
+	size_t apdu_length = CMD_APDU_SIZE(lc);
+	size_t total_length;
+	struct cmd_apdu *apdu;
+	uint8_t *buf;
+
+	/*
+	 * check if we need to reserve space for LC/LE
+	 * (both fields are optional)
+	 */
+	if (lc)
+		apdu_length++;
+	if (le)
+		apdu_length++;
+
+	total_length = sizeof(struct cmd_apdu) + apdu_length;
+	apdu = malloc(total_length);
+
+	if (!apdu)
+		return NULL;
+
+	apdu->base.length = apdu_length;
+	apdu->base.data_buf = (uint8_t *)(apdu + 1);
+	apdu->base.refcnt = 1;
+
+	buf = apdu->base.data_buf;
+	buf[CLA] = cla;
+	buf[INS] = ins;
+	buf[P1] = p1;
+	buf[P2] = p2;
+	if (lc)
+		buf[LC] = lc;
+	if (data != NULL)
+		memmove(&buf[CDATA], data, lc);
+	if (le)
+		buf[CDATA + lc + OFF_LE] = le;
+
+	return apdu;
+}
+
+struct cmd_apdu *alloc_cmd_apdu_from_buf(uint8_t *buf, size_t length)
+{
+	struct cmd_apdu *apdu = malloc(sizeof(struct cmd_apdu));
+
+	if (!apdu)
+		return NULL;
+	apdu->base.length = length;
+	apdu->base.data_buf = buf;
+	apdu->base.refcnt = 1;
+	return apdu;
+}
+
+struct resp_apdu *alloc_resp_apdu(uint8_t le)
+{
+	size_t total_length = sizeof(struct resp_apdu) + RESP_APDU_SIZE(le);
+	struct resp_apdu *apdu;
+
+	apdu = malloc(total_length);
+
+	if (!apdu)
+		return NULL;
+
+	apdu->base.length = RESP_APDU_SIZE(le);
+	apdu->base.data_buf = (uint8_t *)(apdu + 1);
+	apdu->base.refcnt = 1;
+
+	return apdu;
+}
+
+uint8_t *resp_apdu_get_data(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->resp_data;
+}
+
+size_t resp_apdu_get_data_len(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->resp_data_len;
+}
+
+uint8_t resp_apdu_get_sw1(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->sw1;
+}
+
+uint8_t resp_apdu_get_sw2(struct resp_apdu *apdu)
+{
+	assert(apdu);
+	return apdu->sw2;
+}
+
+uint8_t *apdu_get_data(struct apdu_base *apdu)
+{
+	assert(apdu);
+	return apdu->data_buf;
+}
+size_t apdu_get_length(struct apdu_base *apdu)
+{
+	assert(apdu);
+	return apdu->length;
+}
+int apdu_get_refcnt(struct apdu_base *apdu)
+{
+	assert(apdu);
+	return apdu->refcnt;
+}
+void apdu_acquire(struct apdu_base *apdu)
+{
+	assert(apdu);
+	apdu->refcnt++;
+}
+void apdu_release(struct apdu_base *apdu)
+{
+	assert(apdu);
+	apdu->refcnt--;
+	if (apdu->refcnt == 0)
+		free(apdu);
+}
+
+void parse_resp_apdu(struct resp_apdu *apdu)
+{
+	uint8_t *buf = apdu->base.data_buf;
+	/* resp data length =  resp buf length - SW1 - SW2 */
+	apdu->resp_data_len = apdu->base.length - 2;
+	if (apdu->resp_data_len > 0)
+		apdu->resp_data = &buf[RDATA];
+	else
+		apdu->resp_data = NULL;
+	apdu->sw1 = buf[RDATA + apdu->resp_data_len + OFF_SW1];
+	apdu->sw2 = buf[RDATA + apdu->resp_data_len + OFF_SW2];
+}
diff --git a/core/tee/se/apdu_priv.h b/core/tee/se/apdu_priv.h
new file mode 100644
index 0000000..3d30050
--- /dev/null
+++ b/core/tee/se/apdu_priv.h
@@ -0,0 +1,69 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef TEE_SE_APDU_PRIV_H
+#define TEE_SE_APDU_PRIV_H
+
+enum {
+	/* command APDU */
+	CLA = 0,
+	INS = 1,
+	P1 = 2,
+	P2 = 3,
+	LC = 4,
+	CDATA = 5,
+	OFF_LE = 0,
+
+	/* response APDU */
+	RDATA = 0,
+	OFF_SW1 = 0,
+	OFF_SW2 = 1,
+};
+
+struct apdu_base {
+	uint8_t *data_buf;
+	size_t length;
+	int refcnt;
+};
+
+struct cmd_apdu {
+	struct apdu_base base;
+};
+
+struct resp_apdu {
+	struct apdu_base base;
+	uint8_t sw1;
+	uint8_t sw2;
+	uint8_t *resp_data;
+	size_t resp_data_len;
+};
+
+void parse_resp_apdu(struct resp_apdu *apdu);
+
+int apdu_get_refcnt(struct apdu_base *apdu);
+
+#endif
diff --git a/core/tee/se/channel.c b/core/tee/se/channel.c
new file mode 100644
index 0000000..5df7084
--- /dev/null
+++ b/core/tee/se/channel.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/session.h>
+#include <tee/se/channel.h>
+#include <tee/se/iso7816.h>
+#include <tee/se/aid.h>
+#include <tee/se/apdu.h>
+
+#include <stdlib.h>
+#include <string.h>
+
+#include "aid_priv.h"
+#include "channel_priv.h"
+
+struct tee_se_channel *tee_se_channel_alloc(struct tee_se_session *s,
+		int channel_id)
+{
+	struct tee_se_channel *c;
+
+	c = malloc(sizeof(struct tee_se_channel));
+	if (c) {
+		c->session = s;
+		c->channel_id = channel_id;
+		c->aid = NULL;
+		c->select_resp = NULL;
+	}
+	return c;
+}
+
+void tee_se_channel_free(struct tee_se_channel *c)
+{
+	assert(c);
+	if (c->aid)
+		tee_se_aid_release(c->aid);
+	if (c->select_resp)
+		apdu_release(to_apdu_base(c->select_resp));
+}
+
+struct tee_se_session *tee_se_channel_get_session(struct tee_se_channel *c)
+{
+	assert(c);
+	return c->session;
+}
+
+int tee_se_channel_get_id(struct tee_se_channel *c)
+{
+	assert(c);
+	return c->channel_id;
+}
+
+void tee_se_channel_set_select_response(struct tee_se_channel *c,
+		struct resp_apdu *resp)
+{
+	assert(c);
+
+	if (c->select_resp)
+		apdu_release(to_apdu_base(c->select_resp));
+	apdu_acquire(to_apdu_base(resp));
+	c->select_resp = resp;
+}
+
+TEE_Result tee_se_channel_get_select_response(struct tee_se_channel *c,
+		struct resp_apdu **resp)
+{
+	assert(c && resp);
+
+	if (c->select_resp) {
+		*resp = c->select_resp;
+		return TEE_SUCCESS;
+	} else {
+		return TEE_ERROR_NO_DATA;
+	}
+}
+
+void tee_se_channel_set_aid(struct tee_se_channel *c,
+		struct tee_se_aid *aid)
+{
+	assert(c);
+	if (c->aid)
+		tee_se_aid_release(c->aid);
+	tee_se_aid_acquire(aid);
+	c->aid = aid;
+}
+
+
+TEE_Result tee_se_channel_select(struct tee_se_channel *c,
+		struct tee_se_aid *aid)
+{
+	assert(c);
+	return iso7816_select(c, aid);
+}
+
+TEE_Result tee_se_channel_select_next(struct tee_se_channel *c)
+{
+	assert(c);
+	return iso7816_select_next(c);
+}
+
+TEE_Result tee_se_channel_transmit(struct tee_se_channel *c,
+		struct cmd_apdu *cmd_apdu, struct resp_apdu *resp_apdu)
+{
+	struct tee_se_session *s;
+	uint8_t *cmd_buf;
+	int cla_channel;
+
+	assert(c && cmd_apdu && resp_apdu);
+
+	s = c->session;
+	cla_channel = iso7816_get_cla_channel(c->channel_id);
+	cmd_buf = apdu_get_data(to_apdu_base(cmd_apdu));
+	cmd_buf[ISO7816_CLA_OFFSET] = ISO7816_CLA | cla_channel;
+	return tee_se_session_transmit(s, cmd_apdu, resp_apdu);
+}
diff --git a/core/tee/se/channel_priv.h b/core/tee/se/channel_priv.h
new file mode 100644
index 0000000..984a365
--- /dev/null
+++ b/core/tee/se/channel_priv.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef TEE_SE_CHANNEL_PRIV_H
+#define TEE_SE_CHANNEL_PRIV_H
+
+struct tee_se_aid;
+
+struct tee_se_channel {
+	int channel_id;
+	struct tee_se_session *session;
+	struct tee_se_aid *aid;
+	struct resp_apdu *select_resp;
+
+	TAILQ_ENTRY(tee_se_channel) link;
+};
+
+struct tee_se_channel *tee_se_channel_alloc(struct tee_se_session *s,
+		int channel_id);
+
+void tee_se_channel_free(struct tee_se_channel *c);
+
+#endif
diff --git a/core/tee/se/iso7816.c b/core/tee/se/iso7816.c
new file mode 100644
index 0000000..329c425
--- /dev/null
+++ b/core/tee/se/iso7816.c
@@ -0,0 +1,205 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <kernel/panic.h>
+#include <malloc.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <tee/se/reader.h>
+#include <tee/se/session.h>
+#include <tee/se/iso7816.h>
+#include <tee/se/aid.h>
+#include <tee/se/apdu.h>
+#include <tee/se/channel.h>
+#include <tee/se/util.h>
+#include <tee/se/reader/interface.h>
+#include <trace.h>
+
+#include "session_priv.h"
+#include "aid_priv.h"
+#include "apdu_priv.h"
+
+TEE_Result iso7816_exchange_apdu(struct tee_se_reader_proxy *proxy,
+		struct cmd_apdu *cmd, struct resp_apdu *resp)
+{
+	TEE_Result ret;
+
+	assert(cmd && resp);
+	ret = tee_se_reader_transmit(proxy,
+			cmd->base.data_buf, cmd->base.length,
+			resp->base.data_buf, &resp->base.length);
+
+	if (ret == TEE_SUCCESS)
+		parse_resp_apdu(resp);
+
+	return ret;
+}
+
+int iso7816_get_cla_channel(int channel_id)
+{
+	int cla_channel;
+	/*
+	 * From GP Card Spec,
+	 * the logical channel number 0~3 should have CLA: 0x00 ~ 0x03,
+	 * for channel number 4~19 should have CLA: 0x40 ~ 0x4f
+	 */
+	if (channel_id < 4)
+		cla_channel = channel_id;
+	else
+		cla_channel = 0x40 | (channel_id - 4);
+
+	return cla_channel;
+}
+
+static TEE_Result internal_select(struct tee_se_channel *c,
+		struct tee_se_aid *aid, int select_ops)
+{
+	struct cmd_apdu *cmd;
+	struct resp_apdu *resp;
+	struct tee_se_session *s;
+	TEE_Result ret;
+	TEE_SEReaderProperties prop;
+	size_t rx_buf_len = 0;
+	int channel_id;
+	uint8_t cla_channel;
+
+	assert(c);
+
+	s = tee_se_channel_get_session(c);
+	channel_id = tee_se_channel_get_id(c);
+
+	if (channel_id >= MAX_LOGICAL_CHANNEL)
+		panic("invalid channel id");
+
+	cla_channel = iso7816_get_cla_channel(channel_id);
+	if (select_ops == FIRST_OR_ONLY_OCCURRENCE) {
+		assert(aid);
+		cmd = alloc_cmd_apdu(ISO7816_CLA | cla_channel,
+				SELECT_CMD, SELECT_BY_AID,
+				select_ops, aid->length,
+				rx_buf_len, aid->aid);
+	} else {
+		cmd = alloc_cmd_apdu(ISO7816_CLA | cla_channel,
+				SELECT_CMD, SELECT_BY_AID,
+				select_ops, 0, rx_buf_len, NULL);
+	}
+
+	resp = alloc_resp_apdu(rx_buf_len);
+
+	ret = tee_se_session_transmit(s, cmd, resp);
+	if (ret != TEE_SUCCESS) {
+		EMSG("exchange apdu failed: %d", ret);
+		return ret;
+	}
+
+	tee_se_reader_get_properties(s->reader_proxy, &prop);
+	if (prop.selectResponseEnable)
+		tee_se_channel_set_select_response(c, resp);
+	if (aid)
+		tee_se_channel_set_aid(c, aid);
+
+	if (resp->sw1 == CMD_OK_SW1 && resp->sw2 == CMD_OK_SW2) {
+		ret = TEE_SUCCESS;
+	} else {
+		EMSG("operation failed, sw1:%02X, sw2:%02X",
+				resp->sw1, resp->sw2);
+		if (resp->sw1 == 0x6A && resp->sw2 == 0x83)
+			ret = TEE_ERROR_ITEM_NOT_FOUND;
+		else
+			ret = TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	apdu_release(to_apdu_base(cmd));
+	apdu_release(to_apdu_base(resp));
+
+	return ret;
+}
+
+static TEE_Result internal_manage_channel(struct tee_se_session *s,
+		bool open_ops, int *channel_id)
+{
+	struct cmd_apdu *cmd;
+	struct resp_apdu *resp;
+	TEE_Result ret;
+	size_t tx_buf_len = 0, rx_buf_len = 1;
+
+	uint8_t open_flag = (open_ops) ? OPEN_CHANNEL : CLOSE_CHANNEL;
+	uint8_t channel_flag =
+		(open_flag == OPEN_CHANNEL) ? OPEN_NEXT_AVAILABLE : *channel_id;
+
+	assert(s);
+
+	cmd = alloc_cmd_apdu(ISO7816_CLA, MANAGE_CHANNEL_CMD, open_flag,
+			channel_flag, tx_buf_len, rx_buf_len, NULL);
+
+	resp = alloc_resp_apdu(rx_buf_len);
+
+	ret = tee_se_session_transmit(s, cmd, resp);
+	if (ret != TEE_SUCCESS) {
+		EMSG("exchange apdu failed: %d", ret);
+		return ret;
+	}
+
+	if (resp->sw1 == CMD_OK_SW1 && resp->sw2 == CMD_OK_SW2) {
+		if (open_ops)
+			*channel_id = resp->base.data_buf[0];
+		ret = TEE_SUCCESS;
+	} else {
+		EMSG("operation failed, sw1:%02X, sw2:%02X",
+				resp->sw1, resp->sw2);
+		ret = TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	apdu_release(to_apdu_base(cmd));
+	apdu_release(to_apdu_base(resp));
+
+	return ret;
+}
+
+TEE_Result iso7816_open_available_logical_channel(struct tee_se_session *s,
+		int *channel_id)
+{
+	return internal_manage_channel(s, true, channel_id);
+}
+
+TEE_Result iso7816_close_logical_channel(struct tee_se_session *s,
+		int channel_id)
+{
+	return internal_manage_channel(s, false, &channel_id);
+}
+
+TEE_Result iso7816_select(struct tee_se_channel *c, struct tee_se_aid *aid)
+{
+	return internal_select(c, aid, FIRST_OR_ONLY_OCCURRENCE);
+}
+
+TEE_Result iso7816_select_next(struct tee_se_channel *c)
+{
+	return internal_select(c, NULL, NEXT_OCCURRENCE);
+}
diff --git a/core/tee/se/manager.c b/core/tee/se/manager.c
new file mode 100644
index 0000000..30b35f6
--- /dev/null
+++ b/core/tee/se/manager.c
@@ -0,0 +1,153 @@
+/*
+ * Copyright (c) 2014, 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 <initcall.h>
+#include <trace.h>
+#include <kernel/mutex.h>
+#include <tee/se/manager.h>
+#include <tee/se/session.h>
+#include <tee/se/reader.h>
+#include <tee/se/reader/interface.h>
+
+#include <stdlib.h>
+#include <sys/queue.h>
+
+#include "reader_priv.h"
+#include "session_priv.h"
+
+TAILQ_HEAD(reader_proxy_head, tee_se_reader_proxy);
+
+struct tee_se_manager_ctx {
+	/* number of readers registered */
+	size_t reader_count;
+	/* mutex to pretect the reader proxy list */
+	struct mutex mutex;
+	/* list of registered readers */
+	struct reader_proxy_head reader_proxies;
+};
+static struct tee_se_manager_ctx se_manager_ctx;
+
+TEE_Result tee_se_manager_register_reader(struct tee_se_reader *r)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *proxy =
+		malloc(sizeof(struct tee_se_reader_proxy));
+	if (!proxy)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	proxy->reader = r;
+	proxy->refcnt = 0;
+	proxy->basic_channel_locked = false;
+	mutex_init(&proxy->mutex);
+
+	mutex_lock(&ctx->mutex);
+	TAILQ_INSERT_TAIL(&ctx->reader_proxies, proxy, link);
+	ctx->reader_count++;
+	mutex_unlock(&ctx->mutex);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_manager_unregister_reader(struct tee_se_reader *r)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *proxy;
+
+	mutex_lock(&ctx->mutex);
+	TAILQ_FOREACH(proxy, &ctx->reader_proxies, link)
+	{
+		if (proxy->reader == r)
+			TAILQ_REMOVE(&ctx->reader_proxies, proxy, link);
+		free(proxy);
+	}
+	ctx->reader_count--;
+	mutex_unlock(&ctx->mutex);
+
+	return TEE_SUCCESS;
+}
+
+size_t tee_se_manager_get_reader_count(void)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+
+	return ctx->reader_count;
+}
+
+TEE_Result tee_se_manager_get_readers(
+		struct tee_se_reader_proxy **proxy_list,
+		size_t *proxy_list_size)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *proxy;
+	size_t i = 0;
+
+	if (TAILQ_EMPTY(&ctx->reader_proxies))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	TAILQ_FOREACH(proxy, &ctx->reader_proxies, link) {
+		if (i >= *proxy_list_size)
+			return TEE_ERROR_SHORT_BUFFER;
+
+		proxy_list[i] = proxy;
+		i++;
+	}
+	*proxy_list_size = i;
+
+	return TEE_SUCCESS;
+}
+
+bool tee_se_manager_is_reader_proxy_valid(
+		struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+	struct tee_se_reader_proxy *h;
+
+	TAILQ_FOREACH(h, &ctx->reader_proxies, link) {
+		if (h == proxy)
+			return true;
+	}
+
+	return false;
+}
+
+static void context_init(struct tee_se_manager_ctx *ctx)
+{
+	TAILQ_INIT(&ctx->reader_proxies);
+	mutex_init(&ctx->mutex);
+	ctx->reader_count = 0;
+}
+
+static TEE_Result tee_se_manager_init(void)
+{
+	struct tee_se_manager_ctx *ctx = &se_manager_ctx;
+
+	context_init(ctx);
+
+	return TEE_SUCCESS;
+}
+
+service_init(tee_se_manager_init);
diff --git a/core/tee/se/reader.c b/core/tee/se/reader.c
new file mode 100644
index 0000000..3f0aa98
--- /dev/null
+++ b/core/tee/se/reader.c
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <string.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <tee/se/reader.h>
+#include <tee/se/reader/interface.h>
+
+#include "reader_priv.h"
+#include "session_priv.h"
+
+TEE_Result tee_se_reader_check_state(struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_reader *r;
+
+	if (proxy->refcnt == 0)
+		return TEE_ERROR_BAD_STATE;
+
+	r = proxy->reader;
+	if (r->ops->get_state) {
+		enum tee_se_reader_state state;
+
+		mutex_lock(&proxy->mutex);
+		state = r->ops->get_state(r);
+		mutex_unlock(&proxy->mutex);
+
+		if (state != READER_STATE_SE_INSERTED)
+			return TEE_ERROR_COMMUNICATION;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_reader_get_name(struct tee_se_reader_proxy *proxy,
+		char **reader_name, size_t *reader_name_len)
+{
+	size_t name_len;
+
+	assert(proxy && proxy->reader);
+	name_len = strlen(proxy->reader->name);
+	*reader_name = proxy->reader->name;
+	*reader_name_len = name_len;
+
+	return TEE_SUCCESS;
+}
+
+void tee_se_reader_get_properties(struct tee_se_reader_proxy *proxy,
+		TEE_SEReaderProperties *prop)
+{
+	assert(proxy && proxy->reader);
+	*prop = proxy->reader->prop;
+}
+
+int tee_se_reader_get_refcnt(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy && proxy->reader);
+	return proxy->refcnt;
+}
+
+TEE_Result tee_se_reader_attach(struct tee_se_reader_proxy *proxy)
+{
+	TEE_Result ret;
+
+	mutex_lock(&proxy->mutex);
+	if (proxy->refcnt == 0) {
+		struct tee_se_reader *r = proxy->reader;
+
+		if (r->ops->open) {
+			ret = r->ops->open(r);
+			if (ret != TEE_SUCCESS) {
+				mutex_unlock(&proxy->mutex);
+				return ret;
+			}
+		}
+	}
+	proxy->refcnt++;
+	mutex_unlock(&proxy->mutex);
+	return TEE_SUCCESS;
+}
+
+void tee_se_reader_detach(struct tee_se_reader_proxy *proxy)
+{
+	if (proxy->refcnt <= 0)
+		panic("invalid refcnf");
+
+	mutex_lock(&proxy->mutex);
+	proxy->refcnt--;
+	if (proxy->refcnt == 0) {
+		struct tee_se_reader *r = proxy->reader;
+
+		if (r->ops->close)
+			r->ops->close(r);
+	}
+	mutex_unlock(&proxy->mutex);
+
+}
+
+TEE_Result tee_se_reader_transmit(struct tee_se_reader_proxy *proxy,
+		uint8_t *tx_buf, size_t tx_buf_len,
+		uint8_t *rx_buf, size_t *rx_buf_len)
+{
+	struct tee_se_reader *r;
+	TEE_Result ret;
+
+	assert(proxy && proxy->reader);
+	ret = tee_se_reader_check_state(proxy);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	mutex_lock(&proxy->mutex);
+	r = proxy->reader;
+
+	assert(r->ops->transmit);
+	ret = r->ops->transmit(r, tx_buf, tx_buf_len, rx_buf, rx_buf_len);
+
+	mutex_unlock(&proxy->mutex);
+
+	return ret;
+}
+
+void tee_se_reader_lock_basic_channel(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy);
+
+	mutex_lock(&proxy->mutex);
+	proxy->basic_channel_locked = true;
+	mutex_unlock(&proxy->mutex);
+}
+
+void tee_se_reader_unlock_basic_channel(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy);
+
+	mutex_lock(&proxy->mutex);
+	proxy->basic_channel_locked = false;
+	mutex_unlock(&proxy->mutex);
+}
+
+bool tee_se_reader_is_basic_channel_locked(struct tee_se_reader_proxy *proxy)
+{
+	assert(proxy);
+	return proxy->basic_channel_locked;
+}
+
+TEE_Result tee_se_reader_get_atr(struct tee_se_reader_proxy *proxy,
+		uint8_t **atr, size_t *atr_len)
+{
+	TEE_Result ret;
+	struct tee_se_reader *r;
+
+	assert(proxy && atr && atr_len);
+	ret = tee_se_reader_check_state(proxy);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	mutex_lock(&proxy->mutex);
+	r = proxy->reader;
+
+	assert(r->ops->get_atr);
+	ret = r->ops->get_atr(r, atr, atr_len);
+
+	mutex_unlock(&proxy->mutex);
+	return ret;
+}
+
+TEE_Result tee_se_reader_open_session(struct tee_se_reader_proxy *proxy,
+		struct tee_se_session **session)
+{
+	TEE_Result ret;
+	struct tee_se_session *s;
+
+	assert(session && !*session);
+	assert(proxy && proxy->reader);
+
+	s = tee_se_session_alloc(proxy);
+	if (!s)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ret = tee_se_reader_attach(proxy);
+	if (ret != TEE_SUCCESS)
+		goto err_free_session;
+
+	*session = s;
+
+	return TEE_SUCCESS;
+err_free_session:
+	tee_se_session_free(s);
+	return ret;
+}
diff --git a/core/tee/se/reader/passthru_reader/driver.c b/core/tee/se/reader/passthru_reader/driver.c
new file mode 100644
index 0000000..e3c550e
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/driver.c
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2014, 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 <platform_config.h>
+#include <io.h>
+#include <initcall.h>
+#include <tee/se/reader/interface.h>
+#include <mm/core_memprot.h>
+
+#include <trace.h>
+
+#include <stdlib.h>
+
+#include "pcsc.h"
+#include "reader.h"
+
+struct pcsc_context {
+	uint32_t mmio_base;
+	uint8_t num_readers;
+	struct pcsc_reader *readers;
+};
+static struct pcsc_context pcsc_context;
+
+register_phys_mem(MEM_AREA_IO_SEC, PCSC_BASE, 0x1000);
+
+static uint32_t pcsc_read_reg(struct pcsc_context *ctx, uint8_t offset)
+{
+	return read32(ctx->mmio_base + offset);
+}
+
+static void pcsc_write_reg(struct pcsc_context *ctx, uint8_t offset,
+		uint32_t value) __attribute__((unused));
+static void pcsc_write_reg(struct pcsc_context *ctx, uint8_t offset,
+		uint32_t value)
+{
+	write32(ctx->mmio_base + offset, value);
+}
+
+static TEE_Result populate_readers(struct pcsc_context *ctx)
+{
+	int i;
+	uint32_t reader_mmio_base = ctx->mmio_base + PCSC_REG_MAX;
+	TEE_Result ret;
+
+	ctx->readers = malloc(sizeof(struct pcsc_reader) * ctx->num_readers);
+	if (!ctx->readers)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	for (i = 0; i < ctx->num_readers; i++) {
+		uint32_t mmio_base =
+			reader_mmio_base + (i * PCSC_REG_READER_MAX);
+		struct pcsc_reader *r = &ctx->readers[i];
+
+		init_reader(r, i, mmio_base);
+		ret = tee_se_manager_register_reader(&r->se_reader);
+		if (ret != TEE_SUCCESS)
+			goto err_rollback;
+	}
+
+	return TEE_SUCCESS;
+
+err_rollback:
+	i--;
+	while (i) {
+		tee_se_manager_unregister_reader(&ctx->readers[i].se_reader);
+		i--;
+	}
+	free(ctx->readers);
+	return ret;
+}
+
+static void context_init(struct pcsc_context *ctx)
+{
+	ctx->mmio_base = (vaddr_t)phys_to_virt(PCSC_BASE, MEM_AREA_IO_SEC);
+	if (ctx->mmio_base) {
+		ctx->num_readers = pcsc_read_reg(ctx, PCSC_REG_NUM_READERS);
+		DMSG("%d reader detected", ctx->num_readers);
+	}
+}
+
+static TEE_Result pcsc_passthru_reader_init(void)
+{
+	TEE_Result ret;
+	struct pcsc_context *ctx = &pcsc_context;
+
+	context_init(ctx);
+
+	ret = populate_readers(ctx);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+driver_init(pcsc_passthru_reader_init);
diff --git a/core/tee/se/reader/passthru_reader/pcsc.h b/core/tee/se/reader/passthru_reader/pcsc.h
new file mode 100644
index 0000000..6f0636b
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/pcsc.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef PCSC_H
+#define PCSC_H
+
+/* common control registers */
+#define PCSC_REG_NUM_READERS    0x0
+#define PCSC_REG_IRQ_STATUS     0x4
+#define     PCSC_IRQ_STATE_CHANGE   0x1
+#define PCSC_REG_MAX            0x8
+
+/* per-reader control/status registers */
+#define PCSC_REG_READER_CONTROL 0x0
+/* preferred protocol, directly mapped to pcsclite */
+#define     PCSC_READER_CTL_PROTOCOL_T0     0x0001
+#define     PCSC_READER_CTL_PROTOCOL_T1     0x0002
+#define     PCSC_READER_CTL_PROTOCOL_T15    0x0004
+#define     PCSC_READER_CTL_PROTOCOL_RAW    0x0008
+#define     PCSC_READER_CTL_PROTOCOL_MASK   0x000f
+/* shared mode, directly mapped to pcsclite */
+#define     PCSC_READER_CTL_SHARE_MASK      0x0030
+#define     PCSC_READER_CTL_SHARE_SHIFT     4
+#define     PCSC_READER_CTL_SHARE_EXCLUSIVE 0x0010
+#define     PCSC_READER_CTL_SHARE_SHARED    0x0020
+#define     PCSC_READER_CTL_SHARE_DIRECT    0x0030
+/* disposition mode, directly mapped to pcsclite */
+#define     PCSC_READER_CTL_DISPOSITION_MASK    0x0300
+#define     PCSC_READER_CTL_DISPOSITION_SHIFT   8
+#define     PCSC_READER_CTL_DISPOSITION_LEAVE_CARD  0x0000
+#define     PCSC_READER_CTL_DISPOSITION_RESET_CARD  0x0100
+#define     PCSC_READER_CTL_DISPOSITION_UNPOWER_CARD 0x0200
+#define     PCSC_READER_CTL_DISPOSITION_EJECT_CARD  0x0300
+/* reader commands */
+#define     PCSC_READER_CTL_CONNECT         0x1000
+#define     PCSC_READER_CTL_DISCONNECT      0x2000
+#define     PCSC_READER_CTL_READ_ATR        0x4000
+#define     PCSC_READER_CTL_TRANSMIT        0x8000
+#define PCSC_REG_READER_STATE   0x4
+/* reader state, directly mapped to pcsclite */
+#define     PCSC_READER_STATE_IGNORE    0x0001
+#define     PCSC_READER_STATE_CHANGED   0x0002
+#define     PCSC_READER_STATE_UNKNOWN   0x0004
+#define     PCSC_READER_STATE_UNAVAILABLE   0x0008
+#define     PCSC_READER_STATE_EMPTY     0x0010
+#define     PCSC_READER_STATE_PRESENT   0x0020
+#define     PCSC_READER_STATE_ATRMATCH  0x0040
+#define     PCSC_READER_STATE_EXCLUSIVE 0x0080
+#define     PCSC_READER_STATE_INUSE     0x0100
+#define     PCSC_READER_STATE_MUTE      0x0200
+#define     PCSC_READER_STATE_UNPOWERED 0x0400
+#define PCSC_REG_READER_TX_ADDR    0x8
+#define PCSC_REG_READER_TX_SIZE    0xc
+#define PCSC_REG_READER_RX_ADDR    0x10
+#define PCSC_REG_READER_RX_SIZE    0x14
+#define PCSC_REG_READER_ATR_LEN    0x18
+#define PCSC_REG_READER_MAX     0x1c
+
+#endif
diff --git a/core/tee/se/reader/passthru_reader/reader.c b/core/tee/se/reader/passthru_reader/reader.c
new file mode 100644
index 0000000..4cb13e1
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/reader.c
@@ -0,0 +1,248 @@
+/*
+ * Copyright (c) 2014, 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 <io.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <stdio.h>
+#include <trace.h>
+
+#include <tee/se/util.h>
+#include <tee/se/reader/interface.h>
+
+#include "pcsc.h"
+#include "reader.h"
+
+static void pcsc_reader_dump(struct pcsc_reader *r)
+{
+	DMSG("[%s]:", r->se_reader.name);
+	if (r->state & PCSC_READER_STATE_IGNORE)
+		DMSG("  Ignore this reader");
+
+	if (r->state & PCSC_READER_STATE_UNKNOWN)
+		DMSG("  Reader unknown");
+
+	if (r->state & PCSC_READER_STATE_UNAVAILABLE)
+		DMSG("  Status unavailable");
+
+	if (r->state & PCSC_READER_STATE_EMPTY)
+		DMSG("  Card removed");
+
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		DMSG("  Card inserted");
+
+	if (r->state & PCSC_READER_STATE_ATRMATCH)
+		DMSG("  ATR matches card");
+
+	if (r->state & PCSC_READER_STATE_EXCLUSIVE)
+		DMSG("  Exclusive Mode");
+
+	if (r->state & PCSC_READER_STATE_INUSE)
+		DMSG("  Shared Mode");
+
+	if (r->state & PCSC_READER_STATE_MUTE)
+		DMSG("  Unresponsive card");
+
+	if (r->state & PCSC_READER_STATE_UNPOWERED)
+		DMSG("  Reader Unpowered,");
+
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		DMSG("Card Connected: [%s]",
+				r->connected ? "Yes" : "No");
+
+	if (r->connected) {
+		char dumpbuf[DUMP_BUF_MAX], *buf = dumpbuf;
+		size_t remain = DUMP_BUF_MAX;
+
+		buf = print_buf(buf, &remain, "ATR: ");
+		dump_hex(buf, &remain, r->atr, r->atr_len);
+		DMSG("%s", buf);
+	}
+}
+
+static uint32_t pcsc_reader_read_reg(struct pcsc_reader *r, uint32_t offset)
+{
+	return read32(r->mmio_base + offset);
+}
+
+static void pcsc_reader_write_reg(struct pcsc_reader *r, uint32_t offset,
+		uint32_t value)
+{
+	write32(value, r->mmio_base + offset);
+}
+
+static void pcsc_reader_get_atr(struct pcsc_reader *r)
+{
+	uint32_t atr_paddr = 0;
+	uint32_t atr_len = pcsc_reader_read_reg(r, PCSC_REG_READER_ATR_LEN);
+
+	atr_paddr = virt_to_phys((void *)r->atr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_ADDR,
+			atr_paddr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_SIZE,
+			atr_len);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_READ_ATR);
+	r->atr_len = atr_len;
+}
+
+static void pcsc_reader_connect(struct pcsc_reader *r)
+{
+	if (r->connected)
+		panic();
+
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_CONNECT |
+			PCSC_READER_CTL_PROTOCOL_T1 |
+			PCSC_READER_CTL_SHARE_SHARED);
+	r->connected = true;
+	pcsc_reader_get_atr(r);
+}
+
+static void pcsc_reader_disconnect(struct pcsc_reader *r)
+{
+	if (!r->connected)
+		panic();
+
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_DISCONNECT |
+			PCSC_READER_CTL_DISPOSITION_RESET_CARD);
+	r->connected = false;
+	r->atr_len = 0;
+}
+
+static TEE_Result pcsc_reader_transmit(struct pcsc_reader *r, uint8_t *tx_buf,
+		size_t tx_len, uint8_t *rx_buf, size_t *rx_len)
+{
+	uint32_t tx_buf_paddr = 0, rx_buf_paddr = 0;
+
+	if (!r->connected)
+		panic();
+
+	tx_buf_paddr = virt_to_phys((void *)tx_buf);
+	rx_buf_paddr = virt_to_phys((void *)rx_buf);
+
+	pcsc_reader_write_reg(r, PCSC_REG_READER_TX_ADDR,
+			tx_buf_paddr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_TX_SIZE,
+			tx_len);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_ADDR,
+			rx_buf_paddr);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_RX_SIZE,
+			*rx_len);
+	pcsc_reader_write_reg(r, PCSC_REG_READER_CONTROL,
+			PCSC_READER_CTL_TRANSMIT);
+
+	*rx_len = pcsc_reader_read_reg(r, PCSC_REG_READER_RX_SIZE);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result pcsc_passthru_reader_open(struct tee_se_reader *se_reader)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	if (!se_reader->prop.sePresent) {
+		EMSG("SE is not present");
+		return TEE_ERROR_COMMUNICATION;
+	}
+
+	pcsc_reader_connect(r);
+
+	pcsc_reader_dump(r);
+
+	return TEE_SUCCESS;
+}
+
+static void pcsc_passthru_reader_close(struct tee_se_reader *se_reader)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	pcsc_reader_disconnect(r);
+
+	pcsc_reader_dump(r);
+}
+
+static TEE_Result pcsc_passthru_reader_transmit(struct tee_se_reader *se_reader,
+		uint8_t *tx_buf, size_t tx_len, uint8_t *rx_buf, size_t *rx_len)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	return pcsc_reader_transmit(r, tx_buf, tx_len, rx_buf, rx_len);
+}
+
+static enum tee_se_reader_state pcsc_passthru_reader_get_state(
+		struct tee_se_reader *se_reader)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		return READER_STATE_SE_INSERTED;
+	else
+		return READER_STATE_SE_EJECTED;
+}
+
+static TEE_Result pcsc_passthru_reader_get_atr(
+		struct tee_se_reader *se_reader, uint8_t **atr,
+		size_t *atr_len)
+{
+	struct pcsc_reader *r = se_reader->private_data;
+
+	if (r->atr_len > 0) {
+		*atr = r->atr;
+		*atr_len = r->atr_len;
+		return TEE_SUCCESS;
+	} else
+		return TEE_ERROR_COMMUNICATION;
+}
+
+static struct tee_se_reader_ops pcsc_passthru_reader_ops = {
+	.open = pcsc_passthru_reader_open,
+	.close = pcsc_passthru_reader_close,
+	.get_state = pcsc_passthru_reader_get_state,
+	.get_atr = pcsc_passthru_reader_get_atr,
+	.transmit = pcsc_passthru_reader_transmit,
+};
+
+void init_reader(struct pcsc_reader *r, uint8_t index, uint32_t mmio_base)
+{
+	r->index = index;
+	r->mmio_base = mmio_base;
+	r->atr_len = 0;
+	r->state = pcsc_reader_read_reg(r, PCSC_REG_READER_STATE);
+
+	snprintf(r->se_reader.name, TEE_SE_READER_NAME_MAX,
+			"tee_reader_pcsc#%d", index);
+	r->se_reader.ops = &pcsc_passthru_reader_ops;
+	r->se_reader.prop.teeOnly = true;
+	r->se_reader.prop.selectResponseEnable = true;
+	if (r->state & PCSC_READER_STATE_PRESENT)
+		r->se_reader.prop.sePresent = true;
+	else
+		r->se_reader.prop.sePresent = false;
+	r->se_reader.private_data = r;
+}
+
diff --git a/core/tee/se/reader/passthru_reader/reader.h b/core/tee/se/reader/passthru_reader/reader.h
new file mode 100644
index 0000000..cb9541c
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/reader.h
@@ -0,0 +1,45 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef READER_H
+#define READER_H
+
+#define MAX_ATR_SIZE	23
+
+struct pcsc_reader {
+	bool connected;
+	uint8_t index;
+	uint32_t state;
+	uint32_t mmio_base;
+	uint8_t atr[MAX_ATR_SIZE];
+	uint8_t atr_len;
+	struct tee_se_reader se_reader;
+};
+
+void init_reader(struct pcsc_reader *r, uint8_t index, uint32_t mmio_base);
+
+#endif
diff --git a/core/tee/se/reader/passthru_reader/sub.mk b/core/tee/se/reader/passthru_reader/sub.mk
new file mode 100644
index 0000000..f5b1e08
--- /dev/null
+++ b/core/tee/se/reader/passthru_reader/sub.mk
@@ -0,0 +1 @@
+srcs-y	+= driver.c reader.c
diff --git a/core/tee/se/reader/sub.mk b/core/tee/se/reader/sub.mk
new file mode 100644
index 0000000..7899bcc
--- /dev/null
+++ b/core/tee/se/reader/sub.mk
@@ -0,0 +1 @@
+subdirs-${CFG_PCSC_PASSTHRU_READER_DRV} = passthru_reader
diff --git a/core/tee/se/reader_priv.h b/core/tee/se/reader_priv.h
new file mode 100644
index 0000000..00e139e
--- /dev/null
+++ b/core/tee/se/reader_priv.h
@@ -0,0 +1,49 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef TEE_SE_READER_PRIV_H
+#define TEE_SE_READER_PRIV_H
+
+/*
+ * Reader Proxy is used to serialize access from multiple seesions,
+ * and maintain reference counter. All access to the reader should
+ * go through Reader Proxy
+ */
+struct tee_se_reader_proxy {
+	struct tee_se_reader *reader;
+	int refcnt;
+	bool basic_channel_locked;
+	struct mutex mutex;
+
+	TAILQ_ENTRY(tee_se_reader_proxy) link;
+};
+
+TEE_Result tee_se_reader_check_state(struct tee_se_reader_proxy *proxy);
+
+int tee_se_reader_get_refcnt(struct tee_se_reader_proxy *proxy);
+
+#endif
diff --git a/core/tee/se/service.c b/core/tee/se/service.c
new file mode 100644
index 0000000..ae063ed
--- /dev/null
+++ b/core/tee/se/service.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <tee_api_types.h>
+#include <trace.h>
+
+#include <kernel/tee_ta_manager.h>
+#include <kernel/user_ta.h>
+#include <tee/se/service.h>
+#include <tee/se/session.h>
+#include <tee/se/reader.h>
+
+#include "service_priv.h"
+#include "reader_priv.h"
+#include "session_priv.h"
+
+TEE_Result tee_se_service_open(
+		struct tee_se_service **service)
+{
+	TEE_Result ret;
+	struct tee_se_service *h;
+	struct tee_ta_session *sess;
+	struct user_ta_ctx *utc;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	assert(service);
+	if (utc->se_service != NULL)
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	h = malloc(sizeof(struct tee_se_service));
+	if (!h)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	TAILQ_INIT(&h->opened_sessions);
+	TAILQ_INIT(&h->closed_sessions);
+	mutex_init(&h->mutex);
+	*service = h;
+
+	utc->se_service = h;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_service_add_session(
+		struct tee_se_service *service,
+		struct tee_se_session *session)
+{
+	assert(service && session);
+
+	mutex_lock(&service->mutex);
+	TAILQ_INSERT_TAIL(&service->opened_sessions, session, link);
+	mutex_unlock(&service->mutex);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_se_service_is_session_closed(
+		struct tee_se_service *service,
+		struct tee_se_session *session)
+{
+	struct tee_se_session *s;
+
+	TAILQ_FOREACH(s, &service->closed_sessions, link) {
+		if (s == session)
+			return TEE_SUCCESS;
+	}
+
+	return tee_se_reader_check_state(session->reader_proxy);
+}
+
+void tee_se_service_close_session(
+		struct tee_se_service *service,
+		struct tee_se_session *session)
+{
+	assert(service && session);
+
+	tee_se_session_close(session);
+
+	mutex_lock(&service->mutex);
+
+	TAILQ_REMOVE(&service->opened_sessions,
+			session, link);
+	TAILQ_INSERT_TAIL(&service->closed_sessions,
+			session, link);
+
+	mutex_unlock(&service->mutex);
+}
+
+void tee_se_service_close_sessions_by_reader(
+		struct tee_se_service *service,
+		struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_session *s;
+
+	assert(service && proxy);
+
+	TAILQ_FOREACH(s, &service->opened_sessions, link) {
+		if (s->reader_proxy == proxy)
+			tee_se_service_close_session(service, s);
+	}
+}
+
+TEE_Result tee_se_service_close(
+		struct tee_se_service *service __unused)
+{
+	TEE_Result ret;
+	struct tee_se_service *h;
+	struct tee_se_session *s;
+	struct tee_ta_session *sess;
+	struct user_ta_ctx *utc;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	utc = to_user_ta_ctx(sess->ctx);
+	assert(utc->se_service);
+	h = utc->se_service;
+
+	/* clean up all sessions */
+	mutex_lock(&h->mutex);
+	TAILQ_FOREACH(s, &h->opened_sessions, link) {
+		TAILQ_REMOVE(&h->opened_sessions, s, link);
+		tee_se_session_close(s);
+	}
+
+	TAILQ_FOREACH(s, &h->closed_sessions, link)
+		TAILQ_REMOVE(&h->closed_sessions, s, link);
+
+	mutex_unlock(&h->mutex);
+
+	free(h);
+
+	return TEE_SUCCESS;
+}
+
+bool tee_se_service_is_valid(struct tee_se_service *service)
+{
+	TEE_Result ret;
+	struct tee_ta_session *sess;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return false;
+
+	if (to_user_ta_ctx(sess->ctx)->se_service == service)
+		return true;
+	else
+		return false;
+}
+
+bool tee_se_service_is_session_valid(
+		struct tee_se_service *service,
+		struct tee_se_session *session_service)
+{
+	struct tee_se_session *sh;
+
+	TAILQ_FOREACH(sh, &service->opened_sessions, link) {
+		if (sh == session_service)
+			return true;
+	}
+	TAILQ_FOREACH(sh, &service->closed_sessions, link) {
+		if (sh == session_service)
+			return true;
+	}
+	return false;
+}
+
+bool tee_se_service_is_channel_valid(struct tee_se_service *service,
+		struct tee_se_channel *channel)
+{
+	struct tee_se_session *s;
+
+	TAILQ_FOREACH(s, &service->opened_sessions, link) {
+		if (tee_se_session_is_channel_exist(s, channel))
+			return true;
+	}
+
+	return false;
+}
diff --git a/core/tee/se/service_priv.h b/core/tee/se/service_priv.h
new file mode 100644
index 0000000..9ad155a
--- /dev/null
+++ b/core/tee/se/service_priv.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef TEE_SE_SERVICE_PRIV_H
+#define TEE_SE_SERVICE_PRIV_H
+
+TAILQ_HEAD(se_session_head, tee_se_session);
+
+struct tee_se_service {
+	/* list of sessions opened on the service */
+	struct se_session_head opened_sessions;
+	/* list of sessions closed on the service */
+	struct se_session_head closed_sessions;
+	/* mutex to pretect the session lists */
+	struct mutex mutex;
+};
+
+#endif
diff --git a/core/tee/se/session.c b/core/tee/se/session.c
new file mode 100644
index 0000000..ed28aa4
--- /dev/null
+++ b/core/tee/se/session.c
@@ -0,0 +1,194 @@
+/*
+ * Copyright (c) 2014, 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 <assert.h>
+#include <kernel/mutex.h>
+#include <stdlib.h>
+#include <sys/queue.h>
+#include <trace.h>
+
+#include <tee/se/reader.h>
+#include <tee/se/session.h>
+#include <tee/se/channel.h>
+#include <tee/se/iso7816.h>
+
+#include "session_priv.h"
+#include "channel_priv.h"
+
+struct tee_se_session *tee_se_session_alloc(
+		struct tee_se_reader_proxy *proxy)
+{
+	struct tee_se_session *s;
+
+	assert(proxy);
+	s = malloc(sizeof(struct tee_se_session));
+	if (s) {
+		TAILQ_INIT(&s->channels);
+		s->reader_proxy = proxy;
+	}
+	return s;
+}
+
+void tee_se_session_free(struct tee_se_session *s)
+{
+	free(s);
+}
+
+bool tee_se_session_is_channel_exist(struct tee_se_session *s,
+		struct tee_se_channel *c)
+{
+	struct tee_se_channel *c1;
+
+	TAILQ_FOREACH(c1, &s->channels, link) {
+		if (c1 == c)
+			return true;
+	}
+	return false;
+}
+
+TEE_Result tee_se_session_get_atr(struct tee_se_session *s,
+		uint8_t **atr, size_t *atr_len)
+{
+	assert(s && atr && atr_len);
+
+	return tee_se_reader_get_atr(s->reader_proxy, atr, atr_len);
+}
+
+TEE_Result tee_se_session_open_basic_channel(struct tee_se_session *s,
+		struct tee_se_aid *aid, struct tee_se_channel **channel)
+{
+	struct tee_se_channel *c;
+	TEE_Result ret;
+
+	assert(s && channel && !*channel);
+
+	if (tee_se_reader_is_basic_channel_locked(s->reader_proxy)) {
+		*channel = NULL;
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	c = tee_se_channel_alloc(s, 0);
+	if (!c)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	if (aid) {
+		ret = iso7816_select(c, aid);
+		if (ret != TEE_SUCCESS)
+			goto err_free_channel;
+	}
+
+	tee_se_reader_lock_basic_channel(s->reader_proxy);
+	*channel = c;
+	TAILQ_INSERT_TAIL(&s->channels, c, link);
+
+	return TEE_SUCCESS;
+
+err_free_channel:
+	tee_se_channel_free(c);
+	return ret;
+}
+
+TEE_Result tee_se_session_open_logical_channel(struct tee_se_session *s,
+		struct tee_se_aid *aid, struct tee_se_channel **channel)
+{
+	int channel_id;
+	struct tee_se_channel *c;
+	TEE_Result ret;
+
+	assert(s && channel && !*channel);
+
+	ret = iso7816_open_available_logical_channel(s, &channel_id);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	c = tee_se_channel_alloc(s, channel_id);
+	if (!c)
+		goto err_close_channel;
+
+	if (aid != NULL) {
+		ret = iso7816_select(c, aid);
+		if (ret != TEE_SUCCESS)
+			goto err_free_channel;
+	}
+
+	*channel = c;
+	TAILQ_INSERT_TAIL(&s->channels, c, link);
+
+	return TEE_SUCCESS;
+
+err_free_channel:
+	tee_se_channel_free(c);
+err_close_channel:
+	iso7816_close_logical_channel(s, channel_id);
+
+	return ret;
+}
+
+void tee_se_session_close_channel(struct tee_se_session *s,
+		struct tee_se_channel *c)
+{
+	int channel_id;
+
+	assert(s && c);
+	channel_id = tee_se_channel_get_id(c);
+	if (channel_id > 0) {
+		iso7816_close_logical_channel(s, channel_id);
+	} else {
+		tee_se_reader_unlock_basic_channel(s->reader_proxy);
+	}
+
+	TAILQ_REMOVE(&s->channels, c, link);
+	tee_se_channel_free(c);
+}
+
+TEE_Result tee_se_session_transmit(struct tee_se_session *s,
+		struct cmd_apdu *c, struct resp_apdu *r)
+{
+	struct tee_se_reader_proxy *h = s->reader_proxy;
+
+	/*
+	 * This call might block the caller.
+	 * The reader proxy will make sure only 1 session
+	 * is transmitting. Others should wait until the
+	 * activating transation finished.
+	 */
+	return iso7816_exchange_apdu(h, c, r);
+}
+
+void tee_se_session_close(struct tee_se_session *s)
+{
+	struct tee_se_channel *c;
+
+	assert(s);
+
+	TAILQ_FOREACH(c, &s->channels, link)
+		tee_se_session_close_channel(s, c);
+
+	tee_se_reader_detach(s->reader_proxy);
+
+	tee_se_session_free(s);
+}
diff --git a/core/tee/se/session_priv.h b/core/tee/se/session_priv.h
new file mode 100644
index 0000000..81b28b4
--- /dev/null
+++ b/core/tee/se/session_priv.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2014, 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.
+ */
+
+#ifndef TEE_SE_SESSION_PRIV_H
+#define TEE_SE_SESSION_PRIV_H
+
+TAILQ_HEAD(channel_list, tee_se_channel);
+
+struct tee_se_session {
+	struct tee_se_reader_proxy *reader_proxy;
+
+	/* list of channels opened on the session*/
+	struct channel_list channels;
+
+	TAILQ_ENTRY(tee_se_session) link;
+};
+
+struct tee_se_session *tee_se_session_alloc(struct tee_se_reader_proxy *proxy);
+
+void tee_se_session_free(struct tee_se_session *s);
+
+#endif
diff --git a/core/tee/se/sub.mk b/core/tee/se/sub.mk
new file mode 100644
index 0000000..f5ebdd8
--- /dev/null
+++ b/core/tee/se/sub.mk
@@ -0,0 +1,3 @@
+srcs-y += service.c manager.c reader.c iso7816.c session.c channel.c aid.c apdu.c util.c svc.c
+
+subdirs-y += reader
diff --git a/core/tee/se/svc.c b/core/tee/se/svc.c
new file mode 100644
index 0000000..2859989
--- /dev/null
+++ b/core/tee/se/svc.c
@@ -0,0 +1,516 @@
+/*
+ * Copyright (c) 2014, 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 <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <kernel/user_ta.h>
+#include <tee/tee_svc.h>
+#include <tee/se/svc.h>
+#include <trace.h>
+#include <utee_defines.h>
+
+TEE_Result syscall_se_service_open(uint32_t *service_handle)
+{
+	struct tee_ta_session *sess;
+	struct tee_se_service *kservice;
+	TEE_Result ret;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_se_service_open(&kservice);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return tee_svc_copy_kaddr_to_uref(service_handle, kservice);
+}
+
+TEE_Result syscall_se_service_close(unsigned long service_handle)
+{
+	struct tee_se_service *h = tee_svc_uref_to_kaddr(service_handle);
+
+	if (!tee_se_service_is_valid(h))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return tee_se_service_close(h);
+}
+
+TEE_Result syscall_se_service_get_readers(unsigned long service_handle,
+			uint32_t *reader_handles, uint64_t *len)
+{
+	TEE_Result ret;
+	size_t i;
+	size_t tmp_klen;
+	uint64_t klen;
+	struct tee_se_service *h = tee_svc_uref_to_kaddr(service_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_reader_proxy **kreaders;
+	size_t kreaders_size;
+
+	if (!tee_se_service_is_valid(h))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_svc_copy_from_user(&klen, len, sizeof(klen));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	if (klen < tee_se_manager_get_reader_count())
+		return TEE_ERROR_SHORT_BUFFER;
+
+	kreaders_size = klen * sizeof(struct tee_se_reader_proxy *);
+	kreaders = malloc(kreaders_size);
+	if (kreaders == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	tmp_klen = klen;
+	ret = tee_se_manager_get_readers(kreaders, &tmp_klen);
+	if (ret != TEE_SUCCESS)
+		goto err_free_kreaders;
+	klen = tmp_klen;
+
+	for (i = 0; i < klen; i++) {
+		ret = tee_svc_copy_kaddr_to_uref(&reader_handles[i],
+						 kreaders[i]);
+		if (ret != TEE_SUCCESS)
+			goto err_free_kreaders;
+	}
+
+	ret = tee_svc_copy_to_user(len, &klen, sizeof(*len));
+
+err_free_kreaders:
+	free(kreaders);
+
+	return ret;
+}
+
+TEE_Result syscall_se_reader_get_prop(unsigned long reader_handle, uint32_t *p)
+{
+	TEE_Result ret;
+	TEE_SEReaderProperties kprop;
+	uint32_t kp = 0;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_ta_session *sess;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	tee_se_reader_get_properties(r, &kprop);
+	if (kprop.sePresent)
+		kp |= UTEE_SE_READER_PRESENT;
+	if (kprop.teeOnly)
+		kp |= UTEE_SE_READER_TEE_ONLY;
+	if (kprop.selectResponseEnable)
+		kp |= UTEE_SE_READER_SELECT_RESPONE_ENABLE;
+	ret = tee_svc_copy_to_user(p, &kp, sizeof(kp));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_reader_get_name(unsigned long reader_handle,
+			char *name, uint64_t *name_len)
+{
+	TEE_Result ret;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_ta_session *sess;
+	char *kname;
+	size_t kname_len;
+	uint64_t uname_len;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_svc_copy_from_user(&uname_len, name_len, sizeof(uname_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	kname_len = uname_len;
+	tee_se_reader_get_name(r, &kname, &kname_len);
+
+	if (uname_len < kname_len)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	ret = tee_svc_copy_to_user(name, kname, kname_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	uname_len = kname_len;
+	ret = tee_svc_copy_to_user(name_len, &uname_len, sizeof(*name_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_reader_open_session(unsigned long reader_handle,
+			uint32_t *session_handle)
+{
+	TEE_Result ret;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct tee_se_session *ksession = NULL;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	ret = tee_se_reader_open_session(r, &ksession);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	ret = tee_se_service_add_session(service, ksession);
+
+	ret = tee_svc_copy_kaddr_to_uref(session_handle, ksession);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_reader_close_sessions(unsigned long reader_handle)
+{
+	TEE_Result ret;
+	struct tee_se_reader_proxy *r = tee_svc_uref_to_kaddr(reader_handle);
+	struct tee_se_service *service;
+	struct tee_ta_session *sess;
+
+	if (!tee_se_manager_is_reader_proxy_valid(r))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	tee_se_service_close_sessions_by_reader(service, r);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_session_is_closed(unsigned long session_handle)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return tee_se_service_is_session_closed(service, s);
+}
+
+TEE_Result syscall_se_session_get_atr(unsigned long session_handle,
+			void *atr, uint64_t *atr_len)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	size_t katr_len;
+	uint64_t uatr_len;
+	uint8_t *katr;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_svc_copy_from_user(&uatr_len, atr_len, sizeof(uatr_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	katr_len = uatr_len;
+	ret = tee_se_session_get_atr(s, &katr, &katr_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	if (uatr_len < katr_len)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	ret = tee_svc_copy_to_user(atr, katr, katr_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	uatr_len = katr_len;
+	ret = tee_svc_copy_to_user(atr_len, &uatr_len, sizeof(*atr_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_session_open_channel(unsigned long session_handle,
+			unsigned long is_logical, const void *aid_buf,
+			size_t aid_buf_len, uint32_t *channel_handle)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct tee_se_aid *se_aid = NULL;
+	struct tee_se_channel *kc = NULL;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (aid_buf) {
+		ret = tee_se_aid_create_from_buffer((void *)aid_buf,
+						    aid_buf_len, &se_aid);
+		if (ret != TEE_SUCCESS)
+			return ret;
+	}
+
+	if (is_logical)
+		ret = tee_se_session_open_logical_channel(s, se_aid, &kc);
+	else
+		ret = tee_se_session_open_basic_channel(s, se_aid, &kc);
+	if (ret != TEE_SUCCESS)
+		goto error_free_aid;
+
+	ret = tee_svc_copy_kaddr_to_uref(channel_handle, kc);
+	if (ret != TEE_SUCCESS)
+		goto error_free_aid;
+
+	return TEE_SUCCESS;
+
+error_free_aid:
+	if (se_aid)
+		tee_se_aid_release(se_aid);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_session_close(unsigned long session_handle)
+{
+	TEE_Result ret;
+	struct tee_se_session *s = tee_svc_uref_to_kaddr(session_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_session_valid(service, s))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_se_service_close_session(service, s);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_channel_select_next(unsigned long channel_handle)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_se_channel_select_next(c);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_channel_get_select_resp(unsigned long channel_handle,
+			void *resp, uint64_t *resp_len)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct resp_apdu *resp_apdu;
+	size_t kresp_len;
+	uint64_t uresp_len;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_svc_copy_from_user(&uresp_len, resp_len, sizeof(size_t));
+	if (ret != TEE_SUCCESS)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_se_channel_get_select_response(c, &resp_apdu);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	kresp_len = apdu_get_length(to_apdu_base(resp_apdu));
+	if (uresp_len < kresp_len)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	ret = tee_svc_copy_to_user(resp,
+			apdu_get_data(to_apdu_base(resp_apdu)), kresp_len);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	uresp_len = kresp_len;
+	ret = tee_svc_copy_to_user(resp_len, &uresp_len, sizeof(*resp_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_se_channel_transmit(unsigned long channel_handle,
+			void *cmd, unsigned long cmd_len, void *resp,
+			uint64_t *resp_len)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_service *service;
+	struct cmd_apdu *cmd_apdu;
+	struct resp_apdu *resp_apdu;
+	void *kcmd_buf;
+	uint64_t kresp_len;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ret = tee_svc_copy_from_user(&kresp_len, resp_len, sizeof(kresp_len));
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	kcmd_buf = malloc(cmd_len);
+	if (kcmd_buf == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ret = tee_svc_copy_from_user(kcmd_buf, cmd, cmd_len);
+	if (ret != TEE_SUCCESS)
+		goto err_free_cmd_buf;
+
+	cmd_apdu =
+		alloc_cmd_apdu_from_buf(kcmd_buf, cmd_len);
+	if (cmd_apdu == NULL)
+		goto err_free_cmd_buf;
+
+	kresp_len -= 2; /* reserve space for SW1 and SW2 */
+	resp_apdu = alloc_resp_apdu(kresp_len);
+	if (resp_apdu == NULL)
+		goto err_free_cmd_apdu;
+
+	ret = tee_se_channel_transmit(c, cmd_apdu, resp_apdu);
+	if (ret != TEE_SUCCESS)
+		goto err_free_resp_apdu;
+
+	kresp_len = apdu_get_length(to_apdu_base(resp_apdu));
+	ret = tee_svc_copy_to_user(resp_len, &kresp_len, sizeof(*resp_len));
+	if (ret != TEE_SUCCESS)
+		goto err_free_resp_apdu;
+
+	ret = tee_svc_copy_to_user(resp, resp_apdu_get_data(resp_apdu),
+				   kresp_len);
+	if (ret != TEE_SUCCESS)
+		goto err_free_resp_apdu;
+
+	apdu_release(to_apdu_base(resp_apdu));
+	apdu_release(to_apdu_base(cmd_apdu));
+	free(kcmd_buf);
+
+	return TEE_SUCCESS;
+
+err_free_resp_apdu:
+	apdu_release(to_apdu_base(resp_apdu));
+err_free_cmd_apdu:
+	apdu_release(to_apdu_base(cmd_apdu));
+err_free_cmd_buf:
+	free(kcmd_buf);
+	return ret;
+}
+
+TEE_Result syscall_se_channel_close(unsigned long channel_handle)
+{
+	TEE_Result ret;
+	struct tee_se_channel *c = tee_svc_uref_to_kaddr(channel_handle);
+	struct tee_ta_session *sess;
+	struct tee_se_session *s;
+	struct tee_se_service *service;
+
+	ret = tee_ta_get_current_session(&sess);
+	if (ret != TEE_SUCCESS)
+		return ret;
+
+	service = to_user_ta_ctx(sess->ctx)->se_service;
+	if (!tee_se_service_is_channel_valid(service, c))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	s = tee_se_channel_get_session(c);
+
+	tee_se_session_close_channel(s, c);
+
+	return TEE_SUCCESS;
+}
diff --git a/core/tee/se/util.c b/core/tee/se/util.c
new file mode 100644
index 0000000..d118f26
--- /dev/null
+++ b/core/tee/se/util.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2014, 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 <tee_api_types.h>
+#include <trace.h>
+#include <tee/se/util.h>
+
+#include <stdio.h>
+
+char *print_buf(char *buf, size_t *remain_size, const char *fmt, ...)
+{
+	va_list ap;
+	size_t len;
+
+	va_start(ap, fmt);
+	len = vsnprintf(buf, *remain_size, fmt, ap);
+	buf += len;
+	*remain_size -= len;
+	va_end(ap);
+	return buf;
+}
+
+void dump_hex(char *buf, size_t *remain_size, uint8_t *input_buf,
+		size_t input_size)
+{
+	size_t i;
+
+	for (i = 0; i < input_size; i++)
+		buf = print_buf(buf, remain_size, "%02X ", input_buf[i]);
+}
+
+void print_hex(uint8_t *input_buf, size_t input_size)
+{
+	char buf[DUMP_BUF_MAX];
+	size_t remain = sizeof(buf);
+
+	dump_hex(buf, &remain, input_buf, input_size);
+	DMSG("%s", buf);
+}
+
+uint8_t *hex_decode(const char *in, size_t len, uint8_t *out)
+{
+	size_t i, t, hn, ln;
+
+	for (t = 0, i = 0; i < len; i += 2, ++t) {
+		hn = in[i] > '9' ?
+			(in[i] | 32) - 'a' + 10 : in[i] - '0';
+		ln = in[i + 1] > '9' ?
+			(in[i + 1] | 32) - 'a' + 10 : in[i + 1] - '0';
+
+		out[t] = (hn << 4) | ln;
+	}
+	return out;
+}
diff --git a/core/tee/sub.mk b/core/tee/sub.mk
new file mode 100644
index 0000000..32f0f98
--- /dev/null
+++ b/core/tee/sub.mk
@@ -0,0 +1,47 @@
+CFG_CRYPTO ?= y
+
+ifeq (y,$(CFG_CRYPTO))
+
+# HMAC-based Extract-and-Expand Key Derivation Function
+# http://tools.ietf.org/html/rfc5869
+# This is an OP-TEE extension, not part of the GlobalPlatform Internal API v1.0
+CFG_CRYPTO_HKDF ?= y
+
+# NIST SP800-56A Concatenation Key Derivation Function
+# This is an OP-TEE extension
+CFG_CRYPTO_CONCAT_KDF ?= y
+
+# PKCS #5 v2.0 / RFC 2898 key derivation function 2
+# This is an OP-TEE extension
+CFG_CRYPTO_PBKDF2 ?= y
+
+endif
+
+srcs-y += tee_cryp_utl.c
+srcs-$(CFG_CRYPTO_HKDF) += tee_cryp_hkdf.c
+srcs-$(CFG_CRYPTO_CONCAT_KDF) += tee_cryp_concat_kdf.c
+srcs-$(CFG_CRYPTO_PBKDF2) += tee_cryp_pbkdf2.c
+
+ifeq ($(CFG_WITH_USER_TA),y)
+
+srcs-y += tee_svc.c
+cppflags-tee_svc.c-y += -DTEE_IMPL_VERSION=$(TEE_IMPL_VERSION)
+srcs-y += tee_svc_cryp.c
+srcs-y += tee_svc_storage.c
+srcs-$(CFG_RPMB_FS) += tee_rpmb_fs.c
+srcs-$(CFG_REE_FS) += tee_ree_fs.c
+srcs-$(CFG_SQL_FS) += tee_sql_fs.c
+srcs-$(call cfg-one-enabled,CFG_REE_FS CFG_SQL_FS) += tee_fs_rpc.c
+srcs-$(call cfg-one-enabled,CFG_REE_FS CFG_SQL_FS CFG_RPMB_FS) += \
+	tee_fs_rpc_cache.c
+srcs-y += tee_fs_key_manager.c
+srcs-y += tee_obj.c
+srcs-y += tee_pobj.c
+srcs-y += tee_time_generic.c
+
+endif #CFG_WITH_USER_TA,y
+
+srcs-y += uuid.c
+
+subdirs-$(CFG_SE_API) += se
+
diff --git a/core/tee/tee_cryp_concat_kdf.c b/core/tee/tee_cryp_concat_kdf.c
new file mode 100644
index 0000000..b0c7c3d
--- /dev/null
+++ b/core/tee/tee_cryp_concat_kdf.c
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2014, 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 <tee/tee_cryp_concat_kdf.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+#include <stdlib.h>
+#include <string.h>
+
+TEE_Result tee_cryp_concat_kdf(uint32_t hash_id, const uint8_t *shared_secret,
+			       size_t shared_secret_len,
+			       const uint8_t *other_info,
+			       size_t other_info_len, uint8_t *derived_key,
+			       size_t derived_key_len)
+{
+	TEE_Result res;
+	size_t ctx_size, hash_len, i, n, sz;
+	void *ctx = NULL;
+	uint8_t tmp[TEE_MAX_HASH_SIZE];
+	uint32_t be_count;
+	uint8_t *out = derived_key;
+	uint32_t hash_algo = TEE_ALG_HASH_ALGO(hash_id);
+	const struct hash_ops *hash = &crypto_ops.hash;
+
+	if (!hash->get_ctx_size || !hash->init || !hash->update ||
+	    !hash->final) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	res = hash->get_ctx_size(hash_algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = tee_hash_get_digest_size(hash_algo, &hash_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	n = derived_key_len / hash_len;
+	sz = hash_len;
+	for (i = 1; i <= n + 1; i++) {
+		be_count = TEE_U32_TO_BIG_ENDIAN(i);
+
+		res = hash->init(ctx, hash_algo);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = hash->update(ctx, hash_algo, (uint8_t *)&be_count,
+				   sizeof(be_count));
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = hash->update(ctx, hash_algo, shared_secret,
+				   shared_secret_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		if (other_info && other_info_len) {
+			res = hash->update(ctx, hash_algo, other_info,
+					   other_info_len);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+		res = hash->final(ctx, hash_algo, tmp, sizeof(tmp));
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		if (i == n + 1)
+			sz = derived_key_len % hash_len;
+		memcpy(out, tmp, sz);
+		out += sz;
+	}
+	res = TEE_SUCCESS;
+out:
+	free(ctx);
+	return res;
+}
diff --git a/core/tee/tee_cryp_hkdf.c b/core/tee/tee_cryp_hkdf.c
new file mode 100644
index 0000000..d5287d9
--- /dev/null
+++ b/core/tee/tee_cryp_hkdf.c
@@ -0,0 +1,208 @@
+/*
+ * Copyright (c) 2014, 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 <tee/tee_cryp_hkdf.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <stdlib.h>
+#include <string.h>
+#include <utee_defines.h>
+
+
+static const uint8_t zero_salt[TEE_MAX_HASH_SIZE];
+
+static TEE_Result hkdf_extract(uint32_t hash_id, const uint8_t *ikm,
+			       size_t ikm_len, const uint8_t *salt,
+			       size_t salt_len, uint8_t *prk, size_t *prk_len)
+{
+	TEE_Result res;
+	size_t ctx_size;
+	void *ctx = NULL;
+	uint32_t hash_algo = TEE_ALG_HASH_ALGO(hash_id);
+	uint32_t hmac_algo = (TEE_OPERATION_MAC << 28) | hash_id;
+	const struct mac_ops *m = &crypto_ops.mac;
+
+	if (!m->get_ctx_size || !m->init || !m->update) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	if (!salt || !salt_len) {
+		/*
+		 * RFC 5869 section 2.2:
+		 * If not provided, [the salt] is set to a string of HashLen
+		 * zeros
+		 */
+		salt = zero_salt;
+		res = tee_hash_get_digest_size(hash_algo, &salt_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	res = m->get_ctx_size(hmac_algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/*
+	 * RFC 5869 section 2.1: "Note that in the extract step, 'IKM' is used
+	 * as the HMAC input, not as the HMAC key."
+	 * Therefore, salt is the HMAC key in the formula from section 2.2:
+	 * "PRK = HMAC-Hash(salt, IKM)"
+	 */
+	res = m->init(ctx, hmac_algo, salt, salt_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = m->update(ctx, hmac_algo, ikm, ikm_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = m->final(ctx, hmac_algo, prk, *prk_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_hash_get_digest_size(hash_algo, prk_len);
+out:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result hkdf_expand(uint32_t hash_id, const uint8_t *prk,
+			      size_t prk_len, const uint8_t *info,
+			      size_t info_len, uint8_t *okm, size_t okm_len)
+{
+	uint8_t tn[TEE_MAX_HASH_SIZE];
+	size_t tn_len, hash_len, i, n, where, ctx_size;
+	TEE_Result res = TEE_SUCCESS;
+	void *ctx = NULL;
+	const struct mac_ops *m = &crypto_ops.mac;
+	uint32_t hash_algo = TEE_ALG_HASH_ALGO(hash_id);
+	uint32_t hmac_algo = TEE_ALG_HMAC_ALGO(hash_id);
+
+	if (!m->get_ctx_size || !m->init || !m->update || !m->final) {
+		res = TEE_ERROR_NOT_IMPLEMENTED;
+		goto out;
+	}
+
+	res = tee_hash_get_digest_size(hash_algo, &hash_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (!okm || prk_len < hash_len) {
+		res = TEE_ERROR_BAD_STATE;
+		goto out;
+	}
+
+	if (!info)
+		info_len = 0;
+
+	res = m->get_ctx_size(hmac_algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/* N = ceil(L/HashLen) */
+	n = okm_len / hash_len;
+	if ((okm_len % hash_len) != 0)
+		n++;
+
+	if (n > 255) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+
+	/*
+	 * RFC 5869 section 2.3
+	 *   T = T(1) | T(2) | T(3) | ... | T(N)
+	 *   OKM = first L octets of T
+	 *   T(0) = empty string (zero length)
+	 *   T(1) = HMAC-Hash(PRK, T(0) | info | 0x01)
+	 *   T(2) = HMAC-Hash(PRK, T(1) | info | 0x02)
+	 *   T(3) = HMAC-Hash(PRK, T(2) | info | 0x03)
+	 *   ...
+	 */
+	tn_len = 0;
+	where = 0;
+	for (i = 1; i <= n; i++) {
+		uint8_t c = i;
+
+		res = m->init(ctx, hmac_algo, prk, prk_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->update(ctx, hmac_algo, tn, tn_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->update(ctx, hmac_algo, info, info_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->update(ctx, hmac_algo, &c, 1);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = m->final(ctx, hmac_algo, tn, sizeof(tn));
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		memcpy(okm + where, tn, (i < n) ? hash_len : (okm_len - where));
+		where += hash_len;
+		tn_len = hash_len;
+	}
+
+out:
+	free(ctx);
+	return res;
+}
+
+TEE_Result tee_cryp_hkdf(uint32_t hash_id, const uint8_t *ikm, size_t ikm_len,
+			 const uint8_t *salt, size_t salt_len,
+			 const uint8_t *info, size_t info_len, uint8_t *okm,
+			 size_t okm_len)
+{
+	TEE_Result res;
+	uint8_t prk[TEE_MAX_HASH_SIZE];
+	size_t prk_len = sizeof(prk);
+
+	res = hkdf_extract(hash_id, ikm, ikm_len, salt, salt_len, prk,
+			   &prk_len);
+	if (res != TEE_SUCCESS)
+		return res;
+	res = hkdf_expand(hash_id, prk, prk_len, info, info_len, okm,
+			  okm_len);
+
+	return res;
+}
diff --git a/core/tee/tee_cryp_pbkdf2.c b/core/tee/tee_cryp_pbkdf2.c
new file mode 100644
index 0000000..5e6abc1
--- /dev/null
+++ b/core/tee/tee_cryp_pbkdf2.c
@@ -0,0 +1,147 @@
+/*
+ * Copyright (c) 2014, 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 <tee/tee_cryp_pbkdf2.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_cryp_utl.h>
+#include <utee_defines.h>
+#include <stdlib.h>
+#include <string.h>
+
+struct hmac_parms {
+	uint32_t algo;
+	size_t hash_len;
+	void *ctx;
+};
+
+struct pbkdf2_parms {
+	const uint8_t *password;
+	size_t password_len;
+	const uint8_t *salt;
+	size_t salt_len;
+	uint32_t iteration_count;
+};
+
+static TEE_Result pbkdf2_f(uint8_t *out, size_t len, uint32_t idx,
+			   struct hmac_parms *h, struct pbkdf2_parms *p)
+{
+	TEE_Result res;
+	uint8_t u[TEE_MAX_HASH_SIZE];
+	uint32_t be_index;
+	size_t i, j;
+	const struct mac_ops *mac = &crypto_ops.mac;
+
+	memset(out, 0, len);
+	for (i = 1; i <= p->iteration_count; i++) {
+		res = mac->init(h->ctx, h->algo, p->password, p->password_len);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (i == 1) {
+			if (p->salt && p->salt_len) {
+				res = mac->update(h->ctx, h->algo, p->salt,
+						  p->salt_len);
+				if (res != TEE_SUCCESS)
+					return res;
+			}
+
+			be_index = TEE_U32_TO_BIG_ENDIAN(idx);
+
+			res = mac->update(h->ctx, h->algo,
+					  (uint8_t *)&be_index,
+					  sizeof(be_index));
+			if (res != TEE_SUCCESS)
+				return res;
+		} else {
+			res = mac->update(h->ctx, h->algo, u, h->hash_len);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = mac->final(h->ctx, h->algo, u, sizeof(u));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		for (j = 0; j < len; j++)
+			out[j] ^= u[j];
+	}
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_cryp_pbkdf2(uint32_t hash_id, const uint8_t *password,
+			   size_t password_len, const uint8_t *salt,
+			   size_t salt_len, uint32_t iteration_count,
+			   uint8_t *derived_key, size_t derived_key_len)
+{
+	TEE_Result res;
+	size_t ctx_size, i, l, r;
+	uint8_t *out = derived_key;
+	struct pbkdf2_parms pbkdf2_parms;
+	struct hmac_parms hmac_parms = {0, };
+	const struct mac_ops *mac = &crypto_ops.mac;
+
+	if (!mac->get_ctx_size || !mac->init || !mac->update ||
+	    !mac->final)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	hmac_parms.algo = TEE_ALG_HMAC_ALGO(hash_id);
+
+	res = tee_mac_get_digest_size(hmac_parms.algo, &hmac_parms.hash_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = mac->get_ctx_size(hmac_parms.algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	hmac_parms.ctx = malloc(ctx_size);
+	if (!hmac_parms.ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	pbkdf2_parms.password = password;
+	pbkdf2_parms.password_len = password_len;
+	pbkdf2_parms.salt = salt;
+	pbkdf2_parms.salt_len = salt_len;
+	pbkdf2_parms.iteration_count = iteration_count;
+
+	l = derived_key_len / hmac_parms.hash_len;
+	r = derived_key_len % hmac_parms.hash_len;
+
+	for (i = 1; i <= l; i++) {
+		res = pbkdf2_f(out, hmac_parms.hash_len, i, &hmac_parms,
+			       &pbkdf2_parms);
+		if (res != TEE_SUCCESS)
+			goto out;
+		out += hmac_parms.hash_len;
+	}
+	if (r)
+		res = pbkdf2_f(out, r, i, &hmac_parms, &pbkdf2_parms);
+
+out:
+	free(hmac_parms.ctx);
+	return res;
+}
diff --git a/core/tee/tee_cryp_utl.c b/core/tee/tee_cryp_utl.c
new file mode 100644
index 0000000..fa01161
--- /dev/null
+++ b/core/tee/tee_cryp_utl.c
@@ -0,0 +1,403 @@
+/*
+ * Copyright (c) 2014, 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 <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <utee_defines.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee/tee_cryp_provider.h>
+#include <kernel/tee_time.h>
+#include <rng_support.h>
+#include <initcall.h>
+
+#if !defined(CFG_WITH_SOFTWARE_PRNG)
+TEE_Result get_rng_array(void *buffer, int len)
+{
+	char *buf_char = buffer;
+	int i;
+
+
+	if (buf_char == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	for (i = 0; i < len; i++)
+		buf_char[i] = hw_get_random_byte();
+
+	return TEE_SUCCESS;
+}
+#endif
+
+TEE_Result tee_hash_get_digest_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+	case TEE_ALG_MD5:
+	case TEE_ALG_HMAC_MD5:
+		*size = TEE_MD5_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA1:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_DSA_SHA1:
+		*size = TEE_SHA1_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA224:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_DSA_SHA224:
+		*size = TEE_SHA224_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA256:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_DSA_SHA256:
+		*size = TEE_SHA256_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA384:
+	case TEE_ALG_HMAC_SHA384:
+		*size = TEE_SHA384_HASH_SIZE;
+		break;
+	case TEE_ALG_SHA512:
+	case TEE_ALG_HMAC_SHA512:
+		*size = TEE_SHA512_HASH_SIZE;
+		break;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_hash_createdigest(uint32_t algo, const uint8_t *data,
+				 size_t datalen, uint8_t *digest,
+				 size_t digestlen)
+{
+	TEE_Result res = TEE_ERROR_BAD_STATE;
+	void *ctx = NULL;
+	size_t ctxsize;
+
+	if (crypto_ops.hash.get_ctx_size == NULL ||
+	    crypto_ops.hash.init == NULL ||
+	    crypto_ops.hash.update == NULL ||
+	    crypto_ops.hash.final == NULL)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	if (crypto_ops.hash.get_ctx_size(algo, &ctxsize) != TEE_SUCCESS) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto out;
+	}
+
+	ctx = malloc(ctxsize);
+	if (ctx == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	if (crypto_ops.hash.init(ctx, algo) != TEE_SUCCESS)
+		goto out;
+
+	if (datalen != 0) {
+		if (crypto_ops.hash.update(ctx, algo, data, datalen)
+		    != TEE_SUCCESS)
+			goto out;
+	}
+
+	if (crypto_ops.hash.final(ctx, algo, digest, digestlen) != TEE_SUCCESS)
+		goto out;
+
+	res = TEE_SUCCESS;
+
+out:
+	if (ctx)
+		free(ctx);
+
+	return res;
+}
+
+TEE_Result tee_mac_get_digest_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+	case TEE_ALG_HMAC_MD5:
+	case TEE_ALG_HMAC_SHA224:
+	case TEE_ALG_HMAC_SHA1:
+	case TEE_ALG_HMAC_SHA256:
+	case TEE_ALG_HMAC_SHA384:
+	case TEE_ALG_HMAC_SHA512:
+		return tee_hash_get_digest_size(algo, size);
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_AES_CMAC:
+		*size = TEE_AES_BLOCK_SIZE;
+		return TEE_SUCCESS;
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+		*size = TEE_DES_BLOCK_SIZE;
+		return TEE_SUCCESS;
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+}
+
+TEE_Result tee_cipher_get_block_size(uint32_t algo, size_t *size)
+{
+	switch (algo) {
+	case TEE_ALG_AES_CBC_MAC_NOPAD:
+	case TEE_ALG_AES_CBC_MAC_PKCS5:
+	case TEE_ALG_AES_CMAC:
+	case TEE_ALG_AES_ECB_NOPAD:
+	case TEE_ALG_AES_CBC_NOPAD:
+	case TEE_ALG_AES_CTR:
+	case TEE_ALG_AES_CTS:
+	case TEE_ALG_AES_XTS:
+	case TEE_ALG_AES_CCM:
+	case TEE_ALG_AES_GCM:
+		*size = 16;
+		break;
+
+	case TEE_ALG_DES_CBC_MAC_NOPAD:
+	case TEE_ALG_DES_CBC_MAC_PKCS5:
+	case TEE_ALG_DES_ECB_NOPAD:
+	case TEE_ALG_DES_CBC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_NOPAD:
+	case TEE_ALG_DES3_CBC_MAC_PKCS5:
+	case TEE_ALG_DES3_ECB_NOPAD:
+	case TEE_ALG_DES3_CBC_NOPAD:
+		*size = 8;
+		break;
+
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_do_cipher_update(void *ctx, uint32_t algo,
+				TEE_OperationMode mode, bool last_block,
+				const uint8_t *data, size_t len, uint8_t *dst)
+{
+	TEE_Result res;
+	size_t block_size;
+
+	if (mode != TEE_MODE_ENCRYPT && mode != TEE_MODE_DECRYPT)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (crypto_ops.cipher.update == NULL)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	/*
+	 * Check that the block contains the correct number of data, apart
+	 * for the last block in some XTS / CTR / XTS mode
+	 */
+	res = tee_cipher_get_block_size(algo, &block_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	if ((len % block_size) != 0) {
+		if (!last_block)
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		switch (algo) {
+		case TEE_ALG_AES_ECB_NOPAD:
+		case TEE_ALG_DES_ECB_NOPAD:
+		case TEE_ALG_DES3_ECB_NOPAD:
+		case TEE_ALG_AES_CBC_NOPAD:
+		case TEE_ALG_DES_CBC_NOPAD:
+		case TEE_ALG_DES3_CBC_NOPAD:
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		case TEE_ALG_AES_CTR:
+		case TEE_ALG_AES_XTS:
+		case TEE_ALG_AES_CTS:
+			/*
+			 * These modes doesn't require padding for the last
+			 * block.
+			 *
+			 * This isn't entirely true, both XTS and CTS can only
+			 * encrypt minimum one block and also they need at least
+			 * one complete block in the last update to finish the
+			 * encryption. The algorithms are supposed to detect
+			 * that, we're only making sure that all data fed up to
+			 * that point consists of complete blocks.
+			 */
+			break;
+
+		default:
+			return TEE_ERROR_NOT_SUPPORTED;
+		}
+	}
+
+	return crypto_ops.cipher.update(ctx, algo, mode, last_block, data, len,
+					dst);
+}
+
+/*
+ * From http://en.wikipedia.org/wiki/Ciphertext_stealing
+ * CBC ciphertext stealing encryption using a standard
+ * CBC interface:
+ *	1. Pad the last partial plaintext block with 0.
+ *	2. Encrypt the whole padded plaintext using the
+ *	   standard CBC mode.
+ *	3. Swap the last two ciphertext blocks.
+ *	4. Truncate the ciphertext to the length of the
+ *	   original plaintext.
+ *
+ * CBC ciphertext stealing decryption using a standard
+ * CBC interface
+ *	1. Dn = Decrypt (K, Cn-1). Decrypt the second to last
+ *	   ciphertext block.
+ *	2. Cn = Cn || Tail (Dn, B-M). Pad the ciphertext to the
+ *	   nearest multiple of the block size using the last
+ *	   B-M bits of block cipher decryption of the
+ *	   second-to-last ciphertext block.
+ *	3. Swap the last two ciphertext blocks.
+ *	4. Decrypt the (modified) ciphertext using the standard
+ *	   CBC mode.
+ *	5. Truncate the plaintext to the length of the original
+ *	   ciphertext.
+ */
+TEE_Result tee_aes_cbc_cts_update(void *cbc_ctx, void *ecb_ctx,
+				  TEE_OperationMode mode, bool last_block,
+				  const uint8_t *data, size_t len,
+				  uint8_t *dst)
+{
+	TEE_Result res;
+	int nb_blocks, len_last_block, block_size = 16;
+	uint8_t tmp_block[64], tmp2_block[64];
+
+	if (!last_block)
+		return tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					     mode, last_block, data, len, dst);
+
+	/* Compute the last block length and check constraints */
+	nb_blocks = ((len + block_size - 1) / block_size);
+	if (nb_blocks < 2)
+		return TEE_ERROR_BAD_STATE;
+	len_last_block = len % block_size;
+	if (len_last_block == 0)
+		len_last_block = block_size;
+
+	if (mode == TEE_MODE_ENCRYPT) {
+		memcpy(tmp_block,
+		       data + ((nb_blocks - 1) * block_size),
+		       len_last_block);
+		memset(tmp_block + len_last_block,
+		       0,
+		       block_size - len_last_block);
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, data,
+					   (nb_blocks - 1) * block_size, dst);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		memcpy(dst + (nb_blocks - 1) * block_size,
+		       dst + (nb_blocks - 2) * block_size,
+		       len_last_block);
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, tmp_block, block_size,
+					   dst + (nb_blocks - 2) * block_size);
+		if (res != TEE_SUCCESS)
+			return res;
+	} else {
+		/* 1. Decrypt the second to last ciphertext block */
+		res = tee_do_cipher_update(ecb_ctx, TEE_ALG_AES_ECB_NOPAD,
+					   mode, 0,
+					   data + (nb_blocks - 2) * block_size,
+					   block_size, tmp2_block);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		/* 2. Cn = Cn || Tail (Dn, B-M) */
+		memcpy(tmp_block, data + ((nb_blocks - 1) * block_size),
+		       len_last_block);
+		memcpy(tmp_block + len_last_block, tmp2_block + len_last_block,
+		       block_size - len_last_block);
+
+		/* 3. Swap the last two ciphertext blocks */
+		/* done by passing the correct buffers in step 4. */
+
+		/* 4. Decrypt the (modified) ciphertext */
+		if (nb_blocks > 2) {
+			res = tee_do_cipher_update(cbc_ctx,
+						   TEE_ALG_AES_CBC_NOPAD, mode,
+						   0, data,
+						   (nb_blocks - 2) *
+						   block_size, dst);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, tmp_block, block_size,
+					   dst +
+					   ((nb_blocks - 2) * block_size));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_do_cipher_update(cbc_ctx, TEE_ALG_AES_CBC_NOPAD,
+					   mode, 0, data +
+					   ((nb_blocks - 2) * block_size),
+					   block_size, tmp_block);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		/* 5. Truncate the plaintext */
+		memcpy(dst + (nb_blocks - 1) * block_size, tmp_block,
+		       len_last_block);
+	}
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_prng_add_entropy(const uint8_t *in, size_t len)
+{
+	if (crypto_ops.prng.add_entropy)
+		return crypto_ops.prng.add_entropy(in, len);
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Override this in your platform code to feed the PRNG platform-specific
+ * jitter entropy. This implementation does not efficiently deliver entropy
+ * and is here for backwards-compatibility.
+ */
+__weak void plat_prng_add_jitter_entropy(void)
+{
+	TEE_Time current;
+
+	if (tee_time_get_sys_time(&current) == TEE_SUCCESS)
+		tee_prng_add_entropy((uint8_t *)&current, sizeof(current));
+}
+
+static TEE_Result tee_cryp_init(void)
+{
+	if (crypto_ops.init)
+		return crypto_ops.init();
+
+	return TEE_SUCCESS;
+}
+
+service_init(tee_cryp_init);
diff --git a/core/tee/tee_fs_key_manager.c b/core/tee/tee_fs_key_manager.c
new file mode 100644
index 0000000..c827cef
--- /dev/null
+++ b/core/tee/tee_fs_key_manager.c
@@ -0,0 +1,553 @@
+/*
+ * Copyright (c) 2015, 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.
+ */
+
+
+/*
+ * Acronyms:
+ *
+ * FEK - File Encryption Key
+ * SSK - Secure Storage Key
+ * TSK - Trusted app Storage Key
+ * IV  - Initial vector
+ * HUK - Hardware Unique Key
+ * RNG - Random Number Generator
+ */
+
+#include <initcall.h>
+#include <stdlib.h>
+#include <string.h>
+#include <kernel/panic.h>
+#include <kernel/tee_common_otp.h>
+#include <kernel/tee_ta_manager.h>
+#include <tee/tee_cryp_utl.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs_key_manager.h>
+#include <compiler.h>
+#include <trace.h>
+#include <util.h>
+
+struct tee_fs_ssk {
+	bool is_init;
+	uint8_t key[TEE_FS_KM_SSK_SIZE];
+};
+
+struct aad {
+	const uint8_t *encrypted_key;
+	const uint8_t *iv;
+};
+
+struct km_header {
+	struct aad aad;
+	uint8_t *tag;
+};
+
+static struct tee_fs_ssk tee_fs_ssk;
+static uint8_t string_for_ssk_gen[] = "ONLY_FOR_tee_fs_ssk";
+
+
+static TEE_Result do_hmac(uint8_t *out_key, uint32_t out_key_size,
+			  const uint8_t *in_key, uint32_t in_key_size,
+			  const uint8_t *message, uint32_t message_size)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint8_t *ctx = NULL;
+	size_t hash_ctx_size = 0;
+
+	if (!out_key || !in_key || !message)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = crypto_ops.mac.get_ctx_size(TEE_FS_KM_HMAC_ALG, &hash_ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(hash_ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.mac.init(ctx, TEE_FS_KM_HMAC_ALG, in_key, in_key_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.mac.update(ctx, TEE_FS_KM_HMAC_ALG,
+			message, message_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.mac.final(ctx, TEE_FS_KM_HMAC_ALG, out_key,
+				   out_key_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = TEE_SUCCESS;
+
+exit:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result fek_crypt(TEE_OperationMode mode,
+		uint8_t *key, int size)
+{
+	TEE_Result res;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	uint8_t tsk[TEE_FS_KM_TSK_SIZE];
+	uint8_t dst_key[TEE_FS_KM_FEK_SIZE];
+	struct tee_ta_session *sess;
+
+	if (!key)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (size != TEE_FS_KM_FEK_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (tee_fs_ssk.is_init == 0)
+		return TEE_ERROR_GENERIC;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = do_hmac(tsk, sizeof(tsk), tee_fs_ssk.key, TEE_FS_KM_SSK_SIZE,
+		      (uint8_t *)&sess->ctx->uuid, sizeof(TEE_UUID));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = crypto_ops.cipher.get_ctx_size(TEE_FS_KM_ENC_FEK_ALG, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.cipher.init(ctx, TEE_FS_KM_ENC_FEK_ALG, mode, tsk,
+				     sizeof(tsk), NULL, 0, NULL, 0);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.cipher.update(ctx, TEE_FS_KM_ENC_FEK_ALG,
+			mode, true, key, size, dst_key);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	crypto_ops.cipher.final(ctx, TEE_FS_KM_ENC_FEK_ALG);
+
+	memcpy(key, dst_key, sizeof(dst_key));
+
+exit:
+	free(ctx);
+
+	return res;
+}
+
+static TEE_Result generate_fek(uint8_t *key, uint8_t len)
+{
+	return crypto_ops.prng.read(key, len);
+}
+
+static TEE_Result generate_iv(uint8_t *iv, uint8_t len)
+{
+	return crypto_ops.prng.read(iv, len);
+}
+
+static TEE_Result tee_fs_init_key_manager(void)
+{
+	int res = TEE_SUCCESS;
+	struct tee_hw_unique_key huk;
+	uint8_t chip_id[TEE_FS_KM_CHIP_ID_LENGTH];
+	uint8_t message[sizeof(chip_id) + sizeof(string_for_ssk_gen)];
+
+	/* Secure Storage Key Generation:
+	 *
+	 *     SSK = HMAC(HUK, message)
+	 *     message := concatenate(chip_id, static string)
+	 * */
+	tee_otp_get_hw_unique_key(&huk);
+	tee_otp_get_die_id(chip_id, sizeof(chip_id));
+
+	memcpy(message, chip_id, sizeof(chip_id));
+	memcpy(message + sizeof(chip_id), string_for_ssk_gen,
+			sizeof(string_for_ssk_gen));
+
+	res = do_hmac(tee_fs_ssk.key, sizeof(tee_fs_ssk.key),
+			huk.data, sizeof(huk.data),
+			message, sizeof(message));
+
+	if (res == TEE_SUCCESS)
+		tee_fs_ssk.is_init = 1;
+
+	return res;
+}
+
+static TEE_Result do_auth_enc(TEE_OperationMode mode,
+		struct km_header *hdr,
+		uint8_t *fek, int fek_len,
+		const uint8_t *data_in, size_t in_size,
+		uint8_t *data_out, size_t *out_size)
+{
+	TEE_Result res = TEE_SUCCESS;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	size_t tag_len = TEE_FS_KM_MAX_TAG_LEN;
+
+	if ((mode != TEE_MODE_ENCRYPT) && (mode != TEE_MODE_DECRYPT))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (*out_size < in_size) {
+		EMSG("output buffer(%zd) < input buffer(%zd)",
+				*out_size, in_size);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	res = crypto_ops.authenc.get_ctx_size(TEE_FS_KM_AUTH_ENC_ALG,
+			&ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx) {
+		EMSG("request memory size %zu failed", ctx_size);
+		return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	res = crypto_ops.authenc.init(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+			mode, fek, fek_len, hdr->aad.iv,
+			TEE_FS_KM_IV_LEN, TEE_FS_KM_MAX_TAG_LEN,
+			sizeof(struct aad), in_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.authenc.update_aad(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+			mode, (uint8_t *)hdr->aad.encrypted_key,
+			TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.authenc.update_aad(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+			mode, (uint8_t *)hdr->aad.iv,
+			TEE_FS_KM_IV_LEN);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (mode == TEE_MODE_ENCRYPT) {
+		res = crypto_ops.authenc.enc_final(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+				data_in, in_size, data_out, out_size,
+				hdr->tag, &tag_len);
+	} else {
+		res = crypto_ops.authenc.dec_final(ctx, TEE_FS_KM_AUTH_ENC_ALG,
+				data_in, in_size, data_out, out_size,
+				hdr->tag, tag_len);
+	}
+
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	crypto_ops.authenc.final(ctx, TEE_FS_KM_AUTH_ENC_ALG);
+
+exit:
+	free(ctx);
+	return res;
+}
+
+size_t tee_fs_get_header_size(enum tee_fs_file_type type)
+{
+	size_t header_size = 0;
+
+	switch (type) {
+	case META_FILE:
+		header_size = sizeof(struct meta_header);
+		break;
+	case BLOCK_FILE:
+		header_size = sizeof(struct block_header);
+		break;
+	default:
+		panic("Unknown file type");
+	}
+
+	return header_size;
+}
+
+TEE_Result tee_fs_generate_fek(uint8_t *buf, int buf_size)
+{
+	TEE_Result res;
+
+	if (buf_size != TEE_FS_KM_FEK_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = generate_fek(buf, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return fek_crypt(TEE_MODE_ENCRYPT, buf,
+			TEE_FS_KM_FEK_SIZE);
+}
+
+TEE_Result tee_fs_encrypt_file(enum tee_fs_file_type file_type,
+		const uint8_t *data_in, size_t data_in_size,
+		uint8_t *data_out, size_t *data_out_size,
+		const uint8_t *encrypted_fek)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct km_header hdr;
+	uint8_t iv[TEE_FS_KM_IV_LEN];
+	uint8_t tag[TEE_FS_KM_MAX_TAG_LEN];
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+	uint8_t *ciphertext;
+	size_t cipher_size;
+	size_t header_size = tee_fs_get_header_size(file_type);
+
+	/*
+	 * Meta File Format: |Header|Chipertext|
+	 * Header Format:    |AAD|Tag|
+	 * AAD Format:       |Encrypted_FEK|IV|
+	 *
+	 * Block File Format: |Header|Ciphertext|
+	 * Header Format:     |IV|Tag|
+	 *
+	 * TSK = HMAC(SSK, TA_UUID)
+	 * FEK = AES_DECRYPT(TSK, Encrypted_FEK)
+	 * Chipertext = AES_GCM_ENCRYPT(FEK, IV, Meta_Info, AAD)
+	 */
+
+	if (*data_out_size != (header_size + data_in_size))
+		return TEE_ERROR_SHORT_BUFFER;
+
+	res = generate_iv(iv, TEE_FS_KM_IV_LEN);
+	if (res != TEE_SUCCESS)
+		goto fail;
+
+	memcpy(fek, encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	res = fek_crypt(TEE_MODE_DECRYPT, fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto fail;
+
+	ciphertext = data_out + header_size;
+	cipher_size = data_in_size;
+
+	hdr.aad.iv = iv;
+	hdr.aad.encrypted_key = encrypted_fek;
+	hdr.tag = tag;
+
+	res = do_auth_enc(TEE_MODE_ENCRYPT, &hdr,
+			fek, TEE_FS_KM_FEK_SIZE,
+			data_in, data_in_size,
+			ciphertext, &cipher_size);
+
+	if (res == TEE_SUCCESS) {
+		if (file_type == META_FILE) {
+			memcpy(data_out, encrypted_fek, TEE_FS_KM_FEK_SIZE);
+			data_out += TEE_FS_KM_FEK_SIZE;
+		}
+
+		memcpy(data_out, iv, TEE_FS_KM_IV_LEN);
+		data_out += TEE_FS_KM_IV_LEN;
+		memcpy(data_out, tag, TEE_FS_KM_MAX_TAG_LEN);
+
+		*data_out_size = header_size + cipher_size;
+	}
+
+fail:
+	return res;
+}
+
+TEE_Result tee_fs_decrypt_file(enum tee_fs_file_type file_type,
+		const uint8_t *data_in, size_t data_in_size,
+		uint8_t *plaintext, size_t *plaintext_size,
+		uint8_t *encrypted_fek)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct km_header km_hdr;
+	size_t file_hdr_size = tee_fs_get_header_size(file_type);
+	const uint8_t *cipher = data_in + file_hdr_size;
+	int cipher_size = data_in_size - file_hdr_size;
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+
+	if (file_type == META_FILE) {
+		struct meta_header *hdr = (struct meta_header *)data_in;
+
+		km_hdr.aad.encrypted_key = hdr->encrypted_key;
+		km_hdr.aad.iv = hdr->common.iv;
+		km_hdr.tag = hdr->common.tag;
+
+		/* return encrypted FEK to tee_fs which is used for block
+		 * encryption/decryption */
+		memcpy(encrypted_fek, hdr->encrypted_key, TEE_FS_KM_FEK_SIZE);
+	} else {
+		struct block_header *hdr = (struct block_header *)data_in;
+
+		km_hdr.aad.encrypted_key = encrypted_fek;
+		km_hdr.aad.iv = hdr->common.iv;
+		km_hdr.tag = hdr->common.tag;
+	}
+
+	memcpy(fek, km_hdr.aad.encrypted_key, TEE_FS_KM_FEK_SIZE);
+	res = fek_crypt(TEE_MODE_DECRYPT, fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS) {
+		EMSG("Failed to decrypt FEK, res=0x%x", res);
+		return res;
+	}
+
+	return do_auth_enc(TEE_MODE_DECRYPT, &km_hdr, fek, TEE_FS_KM_FEK_SIZE,
+			cipher, cipher_size, plaintext, plaintext_size);
+}
+
+static TEE_Result sha256(uint8_t *out, size_t out_size, const uint8_t *in,
+			 size_t in_size)
+{
+	TEE_Result res;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	uint32_t algo = TEE_ALG_SHA256;
+
+	res = crypto_ops.hash.get_ctx_size(algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.hash.init(ctx, algo);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.hash.update(ctx, algo, in, in_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.hash.final(ctx, algo, out, out_size);
+
+out:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result aes_ecb(uint8_t out[TEE_AES_BLOCK_SIZE],
+			  const uint8_t in[TEE_AES_BLOCK_SIZE],
+			  const uint8_t *key, size_t key_size)
+{
+	TEE_Result res;
+	uint8_t *ctx = NULL;
+	size_t ctx_size;
+	uint32_t algo = TEE_ALG_AES_ECB_NOPAD;
+
+	res = crypto_ops.cipher.get_ctx_size(algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.cipher.init(ctx, algo, TEE_MODE_ENCRYPT, key,
+				     key_size, NULL, 0, NULL, 0);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.cipher.update(ctx, algo, TEE_MODE_ENCRYPT, true, in,
+				       TEE_AES_BLOCK_SIZE, out);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	crypto_ops.cipher.final(ctx, algo);
+	res = TEE_SUCCESS;
+
+out:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result essiv(uint8_t iv[TEE_AES_BLOCK_SIZE],
+			const uint8_t fek[TEE_FS_KM_FEK_SIZE],
+			uint16_t blk_idx)
+{
+	TEE_Result res;
+	uint8_t sha[TEE_SHA256_HASH_SIZE];
+	uint8_t pad_blkid[TEE_AES_BLOCK_SIZE] = { 0, };
+
+	res = sha256(sha, sizeof(sha), fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	pad_blkid[0] = (blk_idx & 0xFF);
+	pad_blkid[1] = (blk_idx & 0xFF00) >> 8;
+
+	return aes_ecb(iv, pad_blkid, sha, 16);
+}
+
+/*
+ * Encryption/decryption of RPMB FS file data. This is AES CBC with ESSIV.
+ */
+TEE_Result tee_fs_crypt_block(uint8_t *out, const uint8_t *in, size_t size,
+			      uint16_t blk_idx, const uint8_t *encrypted_fek,
+			      TEE_OperationMode mode)
+{
+	TEE_Result res;
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+	uint8_t iv[TEE_AES_BLOCK_SIZE];
+	uint8_t *ctx;
+	size_t ctx_size;
+	uint32_t algo = TEE_ALG_AES_CBC_NOPAD;
+
+	DMSG("%scrypt block #%u", (mode == TEE_MODE_ENCRYPT) ? "En" : "De",
+	     blk_idx);
+
+	/* Decrypt FEK */
+	memcpy(fek, encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	res = fek_crypt(TEE_MODE_DECRYPT, fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Compute initialization vector for this block */
+	res = essiv(iv, fek, blk_idx);
+
+	/* Run AES CBC */
+	res = crypto_ops.cipher.get_ctx_size(algo, &ctx_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	ctx = malloc(ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.cipher.init(ctx, algo, mode, fek, sizeof(fek), NULL,
+				     0, iv, TEE_AES_BLOCK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	res = crypto_ops.cipher.update(ctx, algo, mode, true, in, size, out);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	crypto_ops.cipher.final(ctx, algo);
+
+exit:
+	free(ctx);
+	return res;
+}
+
+service_init_late(tee_fs_init_key_manager);
+
diff --git a/core/tee/tee_fs_rpc.c b/core/tee/tee_fs_rpc.c
new file mode 100644
index 0000000..5e1078a
--- /dev/null
+++ b/core/tee/tee_fs_rpc.c
@@ -0,0 +1,375 @@
+/*
+ * 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 <assert.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <optee_msg_supplicant.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_rpc.h>
+#include <trace.h>
+#include <util.h>
+
+struct tee_fs_dir {
+	int nw_dir;
+	struct tee_fs_dirent d;
+};
+
+static TEE_Result operation_commit(struct tee_fs_rpc_operation *op)
+{
+	return thread_rpc_cmd(op->id, op->num_params, op->params);
+}
+
+static TEE_Result operation_open(uint32_t id, unsigned int cmd,
+				 const char *fname, int *fd)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 3 };
+	TEE_Result res;
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t fname_size = strlen(fname) + 1;
+
+	va = tee_fs_rpc_cache_alloc(fname_size, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = cmd;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = fname_size;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, fname, fname_size);
+
+	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
+
+	res = operation_commit(&op);
+	if (res == TEE_SUCCESS)
+		*fd = op.params[2].u.value.a;
+
+	return res;
+}
+
+TEE_Result tee_fs_rpc_open(uint32_t id, const char *fname, int *fd)
+{
+	return operation_open(id, OPTEE_MRF_OPEN, fname, fd);
+}
+
+TEE_Result tee_fs_rpc_create(uint32_t id, const char *fname, int *fd)
+{
+	return operation_open(id, OPTEE_MRF_CREATE, fname, fd);
+}
+
+TEE_Result tee_fs_rpc_close(uint32_t id, int fd)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_CLOSE;
+	op.params[0].u.value.b = fd;
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_read_init(struct tee_fs_rpc_operation *op,
+				uint32_t id, int fd, tee_fs_off_t offset,
+				size_t data_len, void **out_data)
+{
+	uint8_t *va;
+	paddr_t pa;
+	uint64_t cookie;
+
+	if (offset < 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	va = tee_fs_rpc_cache_alloc(data_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memset(op, 0, sizeof(*op));
+	op->id = id;
+	op->num_params = 2;
+
+	op->params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op->params[0].u.value.a = OPTEE_MRF_READ;
+	op->params[0].u.value.b = fd;
+	op->params[0].u.value.c = offset;
+
+	op->params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	op->params[1].u.tmem.buf_ptr = pa;
+	op->params[1].u.tmem.size = data_len;
+	op->params[1].u.tmem.shm_ref = cookie;
+
+	*out_data = va;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_fs_rpc_read_final(struct tee_fs_rpc_operation *op,
+				 size_t *data_len)
+{
+	TEE_Result res = operation_commit(op);
+
+	if (res == TEE_SUCCESS)
+		*data_len = op->params[1].u.tmem.size;
+	return res;
+}
+
+TEE_Result tee_fs_rpc_write_init(struct tee_fs_rpc_operation *op,
+				 uint32_t id, int fd, tee_fs_off_t offset,
+				 size_t data_len, void **data)
+{
+	uint8_t *va;
+	paddr_t pa;
+	uint64_t cookie;
+
+	if (offset < 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	va = tee_fs_rpc_cache_alloc(data_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	memset(op, 0, sizeof(*op));
+	op->id = id;
+	op->num_params = 2;
+
+
+	op->params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op->params[0].u.value.a = OPTEE_MRF_WRITE;
+	op->params[0].u.value.b = fd;
+	op->params[0].u.value.c = offset;
+
+	op->params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op->params[1].u.tmem.buf_ptr = pa;
+	op->params[1].u.tmem.size = data_len;
+	op->params[1].u.tmem.shm_ref = cookie;
+
+	*data = va;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_fs_rpc_write_final(struct tee_fs_rpc_operation *op)
+{
+	return operation_commit(op);
+}
+
+TEE_Result tee_fs_rpc_truncate(uint32_t id, int fd, size_t len)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_TRUNCATE;
+	op.params[0].u.value.b = fd;
+	op.params[0].u.value.c = len;
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_remove(uint32_t id, const char *fname)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 2 };
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t name_len = strlen(fname) + 1;
+
+	va = tee_fs_rpc_cache_alloc(name_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_REMOVE;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = name_len;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, fname, name_len);
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_rename(uint32_t id, const char *old_fname,
+			     const char *new_fname, bool overwrite)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 3 };
+	char *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t old_fname_size = strlen(old_fname) + 1;
+	size_t new_fname_size = strlen(new_fname) + 1;
+
+	va = tee_fs_rpc_cache_alloc(old_fname_size + new_fname_size,
+				    &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_RENAME;
+	op.params[0].u.value.b = overwrite;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = old_fname_size;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, old_fname, old_fname_size);
+
+	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[2].u.tmem.buf_ptr = pa + old_fname_size;
+	op.params[2].u.tmem.size = new_fname_size;
+	op.params[2].u.tmem.shm_ref = cookie;
+	strlcpy(va + old_fname_size, new_fname, new_fname_size);
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_opendir(uint32_t id, const char *name,
+			      struct tee_fs_dir **d)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 3 };
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	size_t name_len = strlen(name) + 1;
+	struct tee_fs_dir *dir = calloc(1, sizeof(*dir));
+
+	if (!dir)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	va = tee_fs_rpc_cache_alloc(name_len, &pa, &cookie);
+	if (!va) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err_exit;
+	}
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_OPENDIR;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = name_len;
+	op.params[1].u.tmem.shm_ref = cookie;
+	strlcpy(va, name, name_len);
+
+	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
+
+	res = operation_commit(&op);
+
+	if (res != TEE_SUCCESS)
+		goto err_exit;
+
+	dir->nw_dir = op.params[2].u.value.a;
+	*d = dir;
+
+	return TEE_SUCCESS;
+err_exit:
+	free(dir);
+
+	return res;
+}
+
+TEE_Result tee_fs_rpc_closedir(uint32_t id, struct tee_fs_dir *d)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_CLOSEDIR;
+	op.params[0].u.value.b = d->nw_dir;
+
+	if (d)
+		free(d->d.d_name);
+	free(d);
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_readdir(uint32_t id, struct tee_fs_dir *d,
+			      struct tee_fs_dirent **ent)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 2 };
+	void *va;
+	paddr_t pa;
+	uint64_t cookie;
+	const size_t max_name_len = TEE_FS_NAME_MAX + 1;
+
+	if (!d)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	va = tee_fs_rpc_cache_alloc(max_name_len, &pa, &cookie);
+	if (!va)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_READDIR;
+	op.params[0].u.value.b = d->nw_dir;
+
+	op.params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	op.params[1].u.tmem.buf_ptr = pa;
+	op.params[1].u.tmem.size = max_name_len;
+	op.params[1].u.tmem.shm_ref = cookie;
+
+	res = operation_commit(&op);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	free(d->d.d_name);
+	d->d.d_name = strndup(va, max_name_len);
+	if (!d->d.d_name)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	*ent = &d->d;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_fs_rpc_begin_transaction(uint32_t id)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_BEGIN_TRANSACTION;
+
+	return operation_commit(&op);
+}
+
+TEE_Result tee_fs_rpc_end_transaction(uint32_t id, bool rollback)
+{
+	struct tee_fs_rpc_operation op = { .id = id, .num_params = 1 };
+
+	op.params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
+	op.params[0].u.value.a = OPTEE_MRF_END_TRANSACTION;
+	op.params[0].u.value.b = rollback;
+
+	return operation_commit(&op);
+}
diff --git a/core/tee/tee_fs_rpc_cache.c b/core/tee/tee_fs_rpc_cache.c
new file mode 100644
index 0000000..aef2eaa
--- /dev/null
+++ b/core/tee/tee_fs_rpc_cache.c
@@ -0,0 +1,85 @@
+/*
+ * 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 <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <tee/tee_fs_rpc.h>
+
+void tee_fs_rpc_cache_clear(struct thread_specific_data *tsd)
+{
+	if (tsd->rpc_fs_payload) {
+		thread_rpc_free_payload(tsd->rpc_fs_payload_cookie);
+		tsd->rpc_fs_payload = NULL;
+		tsd->rpc_fs_payload_cookie = 0;
+		tsd->rpc_fs_payload_size = 0;
+	}
+}
+
+void *tee_fs_rpc_cache_alloc(size_t size, paddr_t *pa, uint64_t *cookie)
+{
+	struct thread_specific_data *tsd = thread_get_tsd();
+	size_t sz = size;
+	uint64_t c = 0;
+	paddr_t p;
+	void *va;
+
+	if (!size)
+		return NULL;
+
+	/*
+	 * Always allocate in page chunks as normal world allocates payload
+	 * memory as complete pages.
+	 */
+	sz = ROUNDUP(size, SMALL_PAGE_SIZE);
+
+	if (sz > tsd->rpc_fs_payload_size) {
+		tee_fs_rpc_cache_clear(tsd);
+
+		thread_rpc_alloc_payload(sz, &p, &c);
+		if (!p)
+			return NULL;
+		if (!ALIGNMENT_IS_OK(p, uint64_t))
+			goto err;
+
+		va = phys_to_virt(p, MEM_AREA_NSEC_SHM);
+		if (!va)
+			goto err;
+
+		tsd->rpc_fs_payload = va;
+		tsd->rpc_fs_payload_pa = p;
+		tsd->rpc_fs_payload_cookie = c;
+		tsd->rpc_fs_payload_size = sz;
+	}
+
+	*pa = tsd->rpc_fs_payload_pa;
+	*cookie = tsd->rpc_fs_payload_cookie;
+	return tsd->rpc_fs_payload;
+err:
+	thread_rpc_free_payload(c);
+	return NULL;
+}
diff --git a/core/tee/tee_obj.c b/core/tee/tee_obj.c
new file mode 100644
index 0000000..78cf937
--- /dev/null
+++ b/core/tee/tee_obj.c
@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <tee/tee_obj.h>
+
+#include <stdlib.h>
+#include <tee_api_defines.h>
+#include <mm/tee_mmu.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_pobj.h>
+#include <trace.h>
+#include <tee/tee_svc_storage.h>
+#include <tee/tee_svc_cryp.h>
+
+void tee_obj_add(struct user_ta_ctx *utc, struct tee_obj *o)
+{
+	TAILQ_INSERT_TAIL(&utc->objects, o, link);
+}
+
+TEE_Result tee_obj_get(struct user_ta_ctx *utc, uint32_t obj_id,
+		       struct tee_obj **obj)
+{
+	struct tee_obj *o;
+
+	TAILQ_FOREACH(o, &utc->objects, link) {
+		if (obj_id == (vaddr_t)o) {
+			*obj = o;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+void tee_obj_close(struct user_ta_ctx *utc, struct tee_obj *o)
+{
+	TAILQ_REMOVE(&utc->objects, o, link);
+
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		o->pobj->fops->close(&o->fh);
+		tee_pobj_release(o->pobj);
+	}
+
+	tee_obj_free(o);
+}
+
+void tee_obj_close_all(struct user_ta_ctx *utc)
+{
+	struct tee_obj_head *objects = &utc->objects;
+
+	while (!TAILQ_EMPTY(objects))
+		tee_obj_close(utc, TAILQ_FIRST(objects));
+}
+
+TEE_Result tee_obj_verify(struct tee_ta_session *sess, struct tee_obj *o)
+{
+	TEE_Result res;
+	char *file = NULL;
+	const struct tee_file_operations *fops = o->pobj->fops;
+	struct tee_file_handle *fh = NULL;
+
+	if (!fops)
+		return TEE_ERROR_STORAGE_NOT_AVAILABLE;
+
+	file = tee_svc_storage_create_filename(sess,
+					       o->pobj->obj_id,
+					       o->pobj->obj_id_len,
+					       false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	res = fops->open(file, &fh);
+	if (res == TEE_ERROR_CORRUPT_OBJECT) {
+		EMSG("Object corrupt\n");
+		tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+		fops->remove(file);
+	}
+
+	free(file);
+	fops->close(&fh);
+exit:
+	return res;
+}
+
+struct tee_obj *tee_obj_alloc(void)
+{
+	return calloc(1, sizeof(struct tee_obj));
+}
+
+void tee_obj_free(struct tee_obj *o)
+{
+	if (o) {
+		tee_obj_attr_free(o);
+		free(o->attr);
+		free(o);
+	}
+}
diff --git a/core/tee/tee_pobj.c b/core/tee/tee_pobj.c
new file mode 100644
index 0000000..a7aee31
--- /dev/null
+++ b/core/tee/tee_pobj.c
@@ -0,0 +1,190 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <kernel/mutex.h>
+#include <stdlib.h>
+#include <string.h>
+#include <tee/tee_pobj.h>
+#include <trace.h>
+
+static TAILQ_HEAD(tee_pobjs, tee_pobj) tee_pobjs =
+		TAILQ_HEAD_INITIALIZER(tee_pobjs);
+static struct mutex pobjs_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result tee_pobj_check_access(uint32_t oflags, uint32_t nflags)
+{
+	/* meta is exclusive */
+	if ((oflags & TEE_DATA_FLAG_ACCESS_WRITE_META) ||
+	    (nflags & TEE_DATA_FLAG_ACCESS_WRITE_META))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	/*
+	 * Excerpt of TEE Internal Core API Specification v1.1:
+	 * If more than one handle is opened on the same  object, and if any
+	 * of these object handles was opened with the flag
+	 * TEE_DATA_FLAG_ACCESS_READ, then all the object handles MUST have been
+	 * opened with the flag TEE_DATA_FLAG_SHARE_READ
+	 */
+	if (((oflags & TEE_DATA_FLAG_ACCESS_READ) ||
+	     (nflags & TEE_DATA_FLAG_ACCESS_READ)) &&
+	    !((nflags & TEE_DATA_FLAG_SHARE_READ) &&
+	      (oflags & TEE_DATA_FLAG_SHARE_READ)))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	/*
+	 * Excerpt of TEE Internal Core API Specification v1.1:
+	 * An object can be opened with only share flags, which locks the access
+	 * to an object against a given mode.
+	 * An object can be opened with no flag set, which completely locks all
+	 * subsequent attempts to access the object
+	 */
+	if ((nflags & TEE_DATA_FLAG_SHARE_READ) !=
+	    (oflags & TEE_DATA_FLAG_SHARE_READ))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	/* Same on WRITE access */
+	if (((oflags & TEE_DATA_FLAG_ACCESS_WRITE) ||
+	     (nflags & TEE_DATA_FLAG_ACCESS_WRITE)) &&
+	    !((nflags & TEE_DATA_FLAG_SHARE_WRITE) &&
+	      (oflags & TEE_DATA_FLAG_SHARE_WRITE)))
+		return TEE_ERROR_ACCESS_CONFLICT;
+	if ((nflags & TEE_DATA_FLAG_SHARE_WRITE) !=
+	    (oflags & TEE_DATA_FLAG_SHARE_WRITE))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_pobj_get(TEE_UUID *uuid, void *obj_id, uint32_t obj_id_len,
+			uint32_t flags, const struct tee_file_operations *fops,
+			struct tee_pobj **obj)
+{
+	struct tee_pobj *o;
+	TEE_Result res;
+
+	*obj = NULL;
+
+	mutex_lock(&pobjs_mutex);
+	/* Check if file is open */
+	TAILQ_FOREACH(o, &tee_pobjs, link) {
+		if ((obj_id_len == o->obj_id_len) &&
+		    (memcmp(obj_id, o->obj_id, obj_id_len) == 0) &&
+		    (memcmp(uuid, &o->uuid, sizeof(TEE_UUID)) == 0) &&
+		    (fops == o->fops)) {
+			*obj = o;
+		}
+	}
+
+	if (*obj) {
+		res = tee_pobj_check_access((*obj)->flags, flags);
+		if (res != TEE_SUCCESS)
+			*obj = NULL;
+		else
+			(*obj)->refcnt++;
+		goto out;
+	}
+
+	/* new file */
+	o = calloc(sizeof(struct tee_pobj), 1);
+	if (!o) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	o->refcnt = 1;
+	memcpy(&o->uuid, uuid, sizeof(TEE_UUID));
+	o->flags = flags;
+	o->fops = fops;
+
+	o->obj_id = malloc(obj_id_len);
+	if (o->obj_id == NULL) {
+		free(o);
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	memcpy(o->obj_id, obj_id, obj_id_len);
+	o->obj_id_len = obj_id_len;
+
+	TAILQ_INSERT_TAIL(&tee_pobjs, o, link);
+	*obj = o;
+
+	res = TEE_SUCCESS;
+out:
+	mutex_unlock(&pobjs_mutex);
+	return res;
+}
+
+TEE_Result tee_pobj_release(struct tee_pobj *obj)
+{
+	if (obj == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	mutex_lock(&pobjs_mutex);
+	obj->refcnt--;
+	if (obj->refcnt == 0) {
+		TAILQ_REMOVE(&tee_pobjs, obj, link);
+		free(obj->obj_id);
+		free(obj);
+	}
+	mutex_unlock(&pobjs_mutex);
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_pobj_rename(struct tee_pobj *obj, void *obj_id,
+			   uint32_t obj_id_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	void *new_obj_id = NULL;
+
+	if (obj == NULL || obj_id == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	mutex_lock(&pobjs_mutex);
+	if (obj->refcnt != 1) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	new_obj_id = malloc(obj_id_len);
+	if (new_obj_id == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+	memcpy(new_obj_id, obj_id, obj_id_len);
+
+	/* update internal data */
+	free(obj->obj_id);
+	obj->obj_id = new_obj_id;
+	obj->obj_id_len = obj_id_len;
+	new_obj_id = NULL;
+
+exit:
+	mutex_unlock(&pobjs_mutex);
+	free(new_obj_id);
+	return res;
+}
diff --git a/core/tee/tee_ree_fs.c b/core/tee/tee_ree_fs.c
new file mode 100644
index 0000000..7a82acb
--- /dev/null
+++ b/core/tee/tee_ree_fs.c
@@ -0,0 +1,792 @@
+/*
+ * Copyright (c) 2015, 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 <assert.h>
+#include <kernel/thread.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <mm/core_memprot.h>
+#include <optee_msg_supplicant.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <sys/queue.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs_rpc.h>
+#include <tee/tee_fs_key_manager.h>
+#include <trace.h>
+#include <utee_defines.h>
+#include <util.h>
+
+/*
+ * This file implements the tee_file_operations structure for a secure
+ * filesystem based on single file in normal world.
+ *
+ * All fields in the REE file are duplicated with two versions 0 and 1. The
+ * active meta-data block is selected by the lowest bit in the
+ * meta-counter.  The active file block is selected by corresponding bit
+ * number in struct tee_fs_file_info.backup_version_table.
+ *
+ * The atomicity of each operation is ensured by updating meta-counter when
+ * everything in the secondary blocks (both meta-data and file-data blocks)
+ * are successfully written.  The main purpose of the code below is to
+ * perform block encryption and authentication of the file data, and
+ * properly handle seeking through the file. One file (in the sense of
+ * struct tee_file_operations) maps to one file in the REE filesystem, and
+ * has the following structure:
+ *
+ * [ 4 bytes meta-counter]
+ * [ meta-data version 0][ meta-data version 1 ]
+ * [ Block 0 version 0 ][ Block 0 version 1 ]
+ * [ Block 1 version 0 ][ Block 1 version 1 ]
+ * ...
+ * [ Block n version 0 ][ Block n version 1 ]
+ *
+ * One meta-data block is built up as:
+ * [ struct meta_header | struct tee_fs_get_header_size ]
+ *
+ * One data block is built up as:
+ * [ struct block_header | BLOCK_FILE_SIZE bytes ]
+ *
+ * struct meta_header and struct block_header are defined in
+ * tee_fs_key_manager.h.
+ *
+ */
+
+#define BLOCK_SHIFT	12
+
+#define BLOCK_SIZE	(1 << BLOCK_SHIFT)
+
+#define MAX_FILE_SIZE	(BLOCK_SIZE * NUM_BLOCKS_PER_FILE)
+
+struct tee_fs_fd {
+	uint32_t meta_counter;
+	struct tee_fs_file_meta meta;
+	tee_fs_off_t pos;
+	uint32_t flags;
+	bool is_new_file;
+	int fd;
+};
+
+static inline int pos_to_block_num(int position)
+{
+	return position >> BLOCK_SHIFT;
+}
+
+static inline int get_last_block_num(size_t size)
+{
+	return pos_to_block_num(size - 1);
+}
+
+static bool get_backup_version_of_block(struct tee_fs_file_meta *meta,
+					size_t block_num)
+{
+	uint32_t index = (block_num / 32);
+	uint32_t block_mask = 1 << (block_num % 32);
+
+	return !!(meta->info.backup_version_table[index] & block_mask);
+}
+
+static inline void toggle_backup_version_of_block(
+		struct tee_fs_file_meta *meta,
+		size_t block_num)
+{
+	uint32_t index = (block_num / 32);
+	uint32_t block_mask = 1 << (block_num % 32);
+
+	meta->info.backup_version_table[index] ^= block_mask;
+}
+
+struct block_operations {
+
+	/*
+	 * Read a block from REE File System which is corresponding
+	 * to the given block_num.
+	 */
+	struct block *(*read)(struct tee_fs_fd *fdp, int block_num);
+
+	/*
+	 * Write the given block to REE File System
+	 */
+	int (*write)(struct tee_fs_fd *fdp, struct block *b,
+			struct tee_fs_file_meta *new_meta);
+};
+
+static struct mutex ree_fs_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result ree_fs_opendir_rpc(const char *name, struct tee_fs_dir **d)
+
+{
+	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_FS, name, d);
+}
+
+static void ree_fs_closedir_rpc(struct tee_fs_dir *d)
+{
+	if (d)
+		tee_fs_rpc_closedir(OPTEE_MSG_RPC_CMD_FS, d);
+}
+
+static TEE_Result ree_fs_readdir_rpc(struct tee_fs_dir *d,
+				     struct tee_fs_dirent **ent)
+{
+	return tee_fs_rpc_readdir(OPTEE_MSG_RPC_CMD_FS, d, ent);
+}
+
+static size_t meta_size(void)
+{
+	return tee_fs_get_header_size(META_FILE) +
+	       sizeof(struct tee_fs_file_meta);
+}
+
+static size_t meta_pos_raw(struct tee_fs_fd *fdp, bool active)
+{
+	size_t offs = sizeof(uint32_t);
+
+	if ((fdp->meta_counter & 1) == active)
+		offs += meta_size();
+	return offs;
+}
+
+static size_t block_size_raw(void)
+{
+	return tee_fs_get_header_size(BLOCK_FILE) + BLOCK_SIZE;
+}
+
+static size_t block_pos_raw(struct tee_fs_file_meta *meta, size_t block_num,
+			    bool active)
+{
+	size_t n = block_num * 2;
+
+	if (active == get_backup_version_of_block(meta, block_num))
+		n++;
+
+	return sizeof(uint32_t) + meta_size() * 2 + n * block_size_raw();
+}
+
+/*
+ * encrypted_fek: as input for META_FILE and BLOCK_FILE
+ */
+static TEE_Result encrypt_and_write_file(struct tee_fs_fd *fdp,
+		enum tee_fs_file_type file_type, size_t offs,
+		void *data_in, size_t data_in_size,
+		uint8_t *encrypted_fek)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	void *ciphertext;
+	size_t header_size = tee_fs_get_header_size(file_type);
+	size_t ciphertext_size = header_size + data_in_size;
+
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd,
+				    offs, ciphertext_size, &ciphertext);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_encrypt_file(file_type, data_in, data_in_size,
+				  ciphertext, &ciphertext_size, encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+/*
+ * encrypted_fek: as output for META_FILE
+ *                as input for BLOCK_FILE
+ */
+static TEE_Result read_and_decrypt_file(struct tee_fs_fd *fdp,
+		enum tee_fs_file_type file_type, size_t offs,
+		void *data_out, size_t *data_out_size,
+		uint8_t *encrypted_fek)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	size_t bytes;
+	void *ciphertext;
+
+	bytes = *data_out_size + tee_fs_get_header_size(file_type);
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, offs,
+				   bytes, &ciphertext);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!bytes) {
+		*data_out_size = 0;
+		return TEE_SUCCESS;
+	}
+
+	res = tee_fs_decrypt_file(file_type, ciphertext, bytes, data_out,
+				  data_out_size, encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_CORRUPT_OBJECT;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result write_meta_file(struct tee_fs_fd *fdp,
+		struct tee_fs_file_meta *meta)
+{
+	size_t offs = meta_pos_raw(fdp, false);
+
+	return encrypt_and_write_file(fdp, META_FILE, offs,
+			(void *)&meta->info, sizeof(meta->info),
+			meta->encrypted_fek);
+}
+
+static TEE_Result write_meta_counter(struct tee_fs_fd *fdp)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	size_t bytes = sizeof(uint32_t);
+	void *data;
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, 0,
+				    bytes, &data);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(data, &fdp->meta_counter, bytes);
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+static TEE_Result create_meta(struct tee_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+
+	memset(fdp->meta.info.backup_version_table, 0xff,
+		sizeof(fdp->meta.info.backup_version_table));
+	fdp->meta.info.length = 0;
+
+	res = tee_fs_generate_fek(fdp->meta.encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_create(OPTEE_MSG_RPC_CMD_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	fdp->meta.counter = fdp->meta_counter;
+
+	res = write_meta_file(fdp, &fdp->meta);
+	if (res != TEE_SUCCESS)
+		return res;
+	return write_meta_counter(fdp);
+}
+
+static TEE_Result commit_meta_file(struct tee_fs_fd *fdp,
+				   struct tee_fs_file_meta *new_meta)
+{
+	TEE_Result res;
+
+	new_meta->counter = fdp->meta_counter + 1;
+
+	res = write_meta_file(fdp, new_meta);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/*
+	 * From now on the new meta is successfully committed,
+	 * change tee_fs_fd accordingly
+	 */
+	fdp->meta = *new_meta;
+	fdp->meta_counter = fdp->meta.counter;
+
+	return write_meta_counter(fdp);
+}
+
+static TEE_Result read_meta_file(struct tee_fs_fd *fdp,
+		struct tee_fs_file_meta *meta)
+{
+	size_t meta_info_size = sizeof(struct tee_fs_file_info);
+	size_t offs = meta_pos_raw(fdp, true);
+
+	return read_and_decrypt_file(fdp, META_FILE, offs,
+				     &meta->info, &meta_info_size,
+				     meta->encrypted_fek);
+}
+
+static TEE_Result read_meta_counter(struct tee_fs_fd *fdp)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	void *data;
+	size_t bytes = sizeof(uint32_t);
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, 0,
+				   bytes, &data);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (bytes != sizeof(uint32_t))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	memcpy(&fdp->meta_counter, data, bytes);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result read_meta(struct tee_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+
+	res = tee_fs_rpc_open(OPTEE_MSG_RPC_CMD_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = read_meta_counter(fdp);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return read_meta_file(fdp, &fdp->meta);
+}
+
+static TEE_Result read_block(struct tee_fs_fd *fdp, int bnum, uint8_t *data)
+{
+	TEE_Result res;
+	size_t ct_size = block_size_raw();
+	size_t out_size = BLOCK_SIZE;
+	ssize_t pos = block_pos_raw(&fdp->meta, bnum, true);
+	size_t bytes;
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd, pos,
+				   ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!bytes) {
+		memset(data, 0, BLOCK_SIZE);
+		return TEE_SUCCESS; /* Block does not exist */
+	}
+
+	return tee_fs_decrypt_file(BLOCK_FILE, ct, bytes, data,
+				   &out_size, fdp->meta.encrypted_fek);
+}
+
+static TEE_Result write_block(struct tee_fs_fd *fdp, size_t bnum, uint8_t *data,
+			      struct tee_fs_file_meta *new_meta)
+{
+	TEE_Result res;
+	size_t offs = block_pos_raw(new_meta, bnum, false);
+
+	res = encrypt_and_write_file(fdp, BLOCK_FILE, offs, data,
+				     BLOCK_SIZE, new_meta->encrypted_fek);
+	if (res == TEE_SUCCESS)
+		toggle_backup_version_of_block(new_meta, bnum);
+	return res;
+}
+
+static TEE_Result out_of_place_write(struct tee_fs_fd *fdp, const void *buf,
+		size_t len, struct tee_fs_file_meta *new_meta)
+{
+	TEE_Result res;
+	int start_block_num = pos_to_block_num(fdp->pos);
+	int end_block_num = pos_to_block_num(fdp->pos + len - 1);
+	size_t remain_bytes = len;
+	uint8_t *data_ptr = (uint8_t *)buf;
+	uint8_t *block;
+	int orig_pos = fdp->pos;
+
+	block = malloc(BLOCK_SIZE);
+	if (!block)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	while (start_block_num <= end_block_num) {
+		int offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_write = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_write + offset > BLOCK_SIZE)
+			size_to_write = BLOCK_SIZE - offset;
+
+		res = read_block(fdp, start_block_num, block);
+		if (res == TEE_ERROR_ITEM_NOT_FOUND)
+			memset(block, 0, BLOCK_SIZE);
+		else if (res != TEE_SUCCESS)
+			goto exit;
+
+		if (data_ptr)
+			memcpy(block + offset, data_ptr, size_to_write);
+		else
+			memset(block + offset, 0, size_to_write);
+
+		res = write_block(fdp, start_block_num, block, new_meta);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		if (data_ptr)
+			data_ptr += size_to_write;
+		remain_bytes -= size_to_write;
+		start_block_num++;
+		fdp->pos += size_to_write;
+	}
+
+	if (fdp->pos > (tee_fs_off_t)new_meta->info.length)
+		new_meta->info.length = fdp->pos;
+
+exit:
+	free(block);
+	if (res != TEE_SUCCESS)
+		fdp->pos = orig_pos;
+	return res;
+}
+
+static TEE_Result open_internal(const char *file, bool create,
+				struct tee_file_handle **fh)
+{
+	TEE_Result res;
+	size_t len;
+	struct tee_fs_fd *fdp = NULL;
+
+	if (!file)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	len = strlen(file) + 1;
+	if (len > TEE_FS_NAME_MAX)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	fdp = calloc(1, sizeof(struct tee_fs_fd));
+	if (!fdp)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	fdp->fd = -1;
+
+	mutex_lock(&ree_fs_mutex);
+
+	if (create)
+		res = create_meta(fdp, file);
+	else
+		res = read_meta(fdp, file);
+
+	if (res == TEE_SUCCESS) {
+		*fh = (struct tee_file_handle *)fdp;
+	} else {
+		if (fdp->fd != -1)
+			tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_FS, fdp->fd);
+		if (create)
+			tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, file);
+		free(fdp);
+	}
+
+	mutex_unlock(&ree_fs_mutex);
+	return res;
+}
+
+static TEE_Result ree_fs_open(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, false, fh);
+}
+
+static TEE_Result ree_fs_create(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, true, fh);
+}
+
+static void ree_fs_close(struct tee_file_handle **fh)
+{
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)*fh;
+
+	if (fdp) {
+		tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_FS, fdp->fd);
+		free(fdp);
+		*fh = NULL;
+	}
+}
+
+static TEE_Result ree_fs_seek(struct tee_file_handle *fh, int32_t offset,
+			      TEE_Whence whence, int32_t *new_offs)
+{
+	TEE_Result res;
+	tee_fs_off_t new_pos;
+	size_t filelen;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+
+	mutex_lock(&ree_fs_mutex);
+
+	DMSG("offset=%d, whence=%d", (int)offset, whence);
+
+	filelen = fdp->meta.info.length;
+
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		new_pos = offset;
+		break;
+
+	case TEE_DATA_SEEK_CUR:
+		new_pos = fdp->pos + offset;
+		break;
+
+	case TEE_DATA_SEEK_END:
+		new_pos = filelen + offset;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	if (new_pos < 0)
+		new_pos = 0;
+
+	if (new_pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	fdp->pos = new_pos;
+	if (new_offs)
+		*new_offs = new_pos;
+	res = TEE_SUCCESS;
+exit:
+	mutex_unlock(&ree_fs_mutex);
+	return res;
+}
+
+/*
+ * To ensure atomic truncate operation, we can:
+ *
+ *  - update file length to new length
+ *  - commit new meta
+ *
+ * To ensure atomic extend operation, we can:
+ *
+ *  - update file length to new length
+ *  - allocate and fill zero data to new blocks
+ *  - commit new meta
+ *
+ * Any failure before committing new meta is considered as
+ * update failed, and the file content will not be updated
+ */
+static TEE_Result ree_fs_ftruncate_internal(struct tee_fs_fd *fdp,
+					    tee_fs_off_t new_file_len)
+{
+	TEE_Result res;
+	size_t old_file_len = fdp->meta.info.length;
+	struct tee_fs_file_meta new_meta;
+
+	if (new_file_len > MAX_FILE_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	new_meta = fdp->meta;
+	new_meta.info.length = new_file_len;
+
+	if ((size_t)new_file_len > old_file_len) {
+		size_t ext_len = new_file_len - old_file_len;
+		int orig_pos = fdp->pos;
+
+		fdp->pos = old_file_len;
+		res = out_of_place_write(fdp, NULL, ext_len, &new_meta);
+		fdp->pos = orig_pos;
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	return commit_meta_file(fdp, &new_meta);
+}
+
+static TEE_Result ree_fs_read(struct tee_file_handle *fh, void *buf,
+			      size_t *len)
+{
+	TEE_Result res;
+	int start_block_num;
+	int end_block_num;
+	size_t remain_bytes;
+	uint8_t *data_ptr = buf;
+	uint8_t *block = NULL;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+
+	mutex_lock(&ree_fs_mutex);
+
+	remain_bytes = *len;
+	if ((fdp->pos + remain_bytes) < remain_bytes ||
+	    fdp->pos > (tee_fs_off_t)fdp->meta.info.length)
+		remain_bytes = 0;
+	else if (fdp->pos + (tee_fs_off_t)remain_bytes >
+		(tee_fs_off_t)fdp->meta.info.length)
+		remain_bytes = fdp->meta.info.length - fdp->pos;
+
+	*len = remain_bytes;
+
+	if (!remain_bytes) {
+		res = TEE_SUCCESS;
+		goto exit;
+	}
+
+	start_block_num = pos_to_block_num(fdp->pos);
+	end_block_num = pos_to_block_num(fdp->pos + remain_bytes - 1);
+
+	block = malloc(BLOCK_SIZE);
+	if (!block) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	while (start_block_num <= end_block_num) {
+		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_read = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_read + offset > BLOCK_SIZE)
+			size_to_read = BLOCK_SIZE - offset;
+
+		res = read_block(fdp, start_block_num, block);
+		if (res != TEE_SUCCESS) {
+			if (res == TEE_ERROR_MAC_INVALID)
+				res = TEE_ERROR_CORRUPT_OBJECT;
+			goto exit;
+		}
+
+		memcpy(data_ptr, block + offset, size_to_read);
+
+		data_ptr += size_to_read;
+		remain_bytes -= size_to_read;
+		fdp->pos += size_to_read;
+
+		start_block_num++;
+	}
+	res = TEE_SUCCESS;
+exit:
+	mutex_unlock(&ree_fs_mutex);
+	free(block);
+	return res;
+}
+
+/*
+ * To ensure atomicity of write operation, we need to
+ * do the following steps:
+ * (The sequence of operations is very important)
+ *
+ *  - Create a new backup version of meta file as a copy
+ *    of current meta file.
+ *  - For each blocks to write:
+ *    - Create new backup version for current block.
+ *    - Write data to new backup version.
+ *    - Update the new meta file accordingly.
+ *  - Write the new meta file.
+ *
+ * (Any failure in above steps is considered as update failed,
+ *  and the file content will not be updated)
+ */
+static TEE_Result ree_fs_write(struct tee_file_handle *fh, const void *buf,
+			       size_t len)
+{
+	TEE_Result res;
+	struct tee_fs_file_meta new_meta;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+	size_t file_size;
+
+	if (!len)
+		return TEE_SUCCESS;
+
+	mutex_lock(&ree_fs_mutex);
+
+	file_size = fdp->meta.info.length;
+
+	if ((fdp->pos + len) > MAX_FILE_SIZE || (fdp->pos + len) < len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	if (file_size < (size_t)fdp->pos) {
+		res = ree_fs_ftruncate_internal(fdp, fdp->pos);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	new_meta = fdp->meta;
+	res = out_of_place_write(fdp, buf, len, &new_meta);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = commit_meta_file(fdp, &new_meta);
+exit:
+	mutex_unlock(&ree_fs_mutex);
+	return res;
+}
+
+static TEE_Result ree_fs_rename(const char *old, const char *new,
+				bool overwrite)
+{
+	TEE_Result res;
+
+	mutex_lock(&ree_fs_mutex);
+	res = tee_fs_rpc_rename(OPTEE_MSG_RPC_CMD_FS, old, new, overwrite);
+	mutex_unlock(&ree_fs_mutex);
+
+	return res;
+}
+
+static TEE_Result ree_fs_remove(const char *file)
+{
+	TEE_Result res;
+
+	mutex_lock(&ree_fs_mutex);
+	res = tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, file);
+	mutex_unlock(&ree_fs_mutex);
+
+	return res;
+}
+
+static TEE_Result ree_fs_truncate(struct tee_file_handle *fh, size_t len)
+{
+	TEE_Result res;
+	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+
+	mutex_lock(&ree_fs_mutex);
+	res = ree_fs_ftruncate_internal(fdp, len);
+	mutex_unlock(&ree_fs_mutex);
+
+	return res;
+}
+
+const struct tee_file_operations ree_fs_ops = {
+	.open = ree_fs_open,
+	.create = ree_fs_create,
+	.close = ree_fs_close,
+	.read = ree_fs_read,
+	.write = ree_fs_write,
+	.seek = ree_fs_seek,
+	.truncate = ree_fs_truncate,
+	.rename = ree_fs_rename,
+	.remove = ree_fs_remove,
+	.opendir = ree_fs_opendir_rpc,
+	.closedir = ree_fs_closedir_rpc,
+	.readdir = ree_fs_readdir_rpc,
+};
diff --git a/core/tee/tee_rpmb_fs.c b/core/tee/tee_rpmb_fs.c
new file mode 100644
index 0000000..229bce7
--- /dev/null
+++ b/core/tee/tee_rpmb_fs.c
@@ -0,0 +1,2617 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <assert.h>
+#include <kernel/tee_common.h>
+#include <kernel/mutex.h>
+#include <kernel/panic.h>
+#include <kernel/tee_common_otp.h>
+#include <kernel/thread.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_mm.h>
+#include <optee_msg_supplicant.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <sys/queue.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_key_manager.h>
+#include <trace.h>
+#include <util.h>
+
+#define RPMB_STORAGE_START_ADDRESS      0
+#define RPMB_FS_FAT_START_ADDRESS       512
+#define RPMB_BLOCK_SIZE_SHIFT           8
+
+#define RPMB_FS_MAGIC                   0x52504D42
+#define FS_VERSION                      2
+#define N_ENTRIES                       8
+
+#define FILE_IS_ACTIVE                  (1u << 0)
+#define FILE_IS_LAST_ENTRY              (1u << 1)
+
+#define TEE_RPMB_FS_FILENAME_LENGTH 224
+
+/**
+ * FS parameters: Information often used by internal functions.
+ * fat_start_address will be set by rpmb_fs_setup().
+ * rpmb_fs_parameters can be read by any other function.
+ */
+struct rpmb_fs_parameters {
+	uint32_t fat_start_address;
+	uint32_t max_rpmb_address;
+};
+
+/**
+ * File entry for a single file in a RPMB_FS partition.
+ */
+struct rpmb_fat_entry {
+	uint32_t start_address;
+	uint32_t data_size;
+	uint32_t flags;
+	uint32_t write_counter;
+	uint8_t fek[TEE_FS_KM_FEK_SIZE];
+	char filename[TEE_RPMB_FS_FILENAME_LENGTH];
+};
+
+/**
+ * FAT entry context with reference to a FAT entry and its
+ * location in RPMB.
+ */
+struct rpmb_file_handle {
+	struct rpmb_fat_entry fat_entry;
+	char filename[TEE_RPMB_FS_FILENAME_LENGTH];
+	/* Address for current entry in RPMB */
+	uint32_t rpmb_fat_address;
+	/* Current position */
+	uint32_t pos;
+};
+
+/**
+ * RPMB_FS partition data
+ */
+struct rpmb_fs_partition {
+	uint32_t rpmb_fs_magic;
+	uint32_t fs_version;
+	uint32_t write_counter;
+	uint32_t fat_start_address;
+	/* Do not use reserved[] for other purpose than partition data. */
+	uint8_t reserved[112];
+};
+
+/**
+ * A node in a list of directory entries. entry->name is a
+ * pointer to name here.
+ */
+struct tee_rpmb_fs_dirent {
+	struct tee_fs_dirent entry;
+	char name[TEE_RPMB_FS_FILENAME_LENGTH];
+	/* */
+	SIMPLEQ_ENTRY(tee_rpmb_fs_dirent) link;
+};
+
+/**
+ * The RPMB directory representation. It contains a queue of
+ * RPMB directory entries: 'next'.
+ * The current pointer points to the last directory entry
+ * returned by readdir().
+ */
+struct tee_fs_dir {
+	struct tee_rpmb_fs_dirent *current;
+	/* */
+	SIMPLEQ_HEAD(next_head, tee_rpmb_fs_dirent) next;
+};
+
+static struct rpmb_fs_parameters *fs_par;
+
+/*
+ * Lower interface to RPMB device
+ */
+
+#define RPMB_DATA_OFFSET            (RPMB_STUFF_DATA_SIZE + RPMB_KEY_MAC_SIZE)
+#define RPMB_MAC_PROTECT_DATA_SIZE  (RPMB_DATA_FRAME_SIZE - RPMB_DATA_OFFSET)
+
+#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
+
+#define RPMB_STUFF_DATA_SIZE                        196
+#define RPMB_KEY_MAC_SIZE                           32
+#define RPMB_DATA_SIZE                              256
+#define RPMB_NONCE_SIZE                             16
+#define RPMB_DATA_FRAME_SIZE                        512
+
+#define RPMB_RESULT_OK                              0x00
+#define RPMB_RESULT_GENERAL_FAILURE                 0x01
+#define RPMB_RESULT_AUTH_FAILURE                    0x02
+#define RPMB_RESULT_COUNTER_FAILURE                 0x03
+#define RPMB_RESULT_ADDRESS_FAILURE                 0x04
+#define RPMB_RESULT_WRITE_FAILURE                   0x05
+#define RPMB_RESULT_READ_FAILURE                    0x06
+#define RPMB_RESULT_AUTH_KEY_NOT_PROGRAMMED         0x07
+#define RPMB_RESULT_MASK                            0x3F
+#define RPMB_RESULT_WR_CNT_EXPIRED                  0x80
+
+/* RPMB internal commands */
+#define RPMB_CMD_DATA_REQ      0x00
+#define RPMB_CMD_GET_DEV_INFO  0x01
+
+#define RPMB_SIZE_SINGLE (128 * 1024)
+
+/* Error codes for get_dev_info request/response. */
+#define RPMB_CMD_GET_DEV_INFO_RET_OK     0x00
+#define RPMB_CMD_GET_DEV_INFO_RET_ERROR  0x01
+
+struct rpmb_data_frame {
+	uint8_t stuff_bytes[RPMB_STUFF_DATA_SIZE];
+	uint8_t key_mac[RPMB_KEY_MAC_SIZE];
+	uint8_t data[RPMB_DATA_SIZE];
+	uint8_t nonce[RPMB_NONCE_SIZE];
+	uint8_t write_counter[4];
+	uint8_t address[2];
+	uint8_t block_count[2];
+	uint8_t op_result[2];
+	uint8_t msg_type[2];
+};
+
+struct rpmb_req {
+	uint16_t cmd;
+	uint16_t dev_id;
+	uint16_t block_count;
+	/* variable length of data */
+	/* uint8_t data[]; REMOVED! */
+};
+
+#define TEE_RPMB_REQ_DATA(req) \
+		((void *)((struct rpmb_req *)(req) + 1))
+
+struct rpmb_raw_data {
+	uint16_t msg_type;
+	uint16_t *op_result;
+	uint16_t *block_count;
+	uint16_t *blk_idx;
+	uint32_t *write_counter;
+	uint8_t *nonce;
+	uint8_t *key_mac;
+	uint8_t *data;
+	/* data length to read or write */
+	uint32_t len;
+	/* Byte address offset in the first block involved */
+	uint8_t byte_offset;
+};
+
+#define RPMB_EMMC_CID_SIZE 16
+struct rpmb_dev_info {
+	uint8_t cid[RPMB_EMMC_CID_SIZE];
+	/* EXT CSD-slice 168 "RPMB Size" */
+	uint8_t rpmb_size_mult;
+	/* EXT CSD-slice 222 "Reliable Write Sector Count" */
+	uint8_t rel_wr_sec_c;
+	/* Check the ret code and accept the data only if it is OK. */
+	uint8_t ret_code;
+};
+
+/*
+ * Struct for rpmb context data.
+ *
+ * @key              RPMB key.
+ * @cid              eMMC card ID.
+ * @hash_ctx_size    Hash context size
+ * @wr_cnt           Current write counter.
+ * @max_blk_idx      The highest block index supported by current device.
+ * @rel_wr_blkcnt    Max number of data blocks for each reliable write.
+ * @dev_id           Device ID of the eMMC device.
+ * @wr_cnt_synced    Flag indicating if write counter is synced to RPMB.
+ * @key_derived      Flag indicating if key has been generated.
+ * @key_verified     Flag indicating the key generated is verified ok.
+ * @dev_info_synced  Flag indicating if dev info has been retrieved from RPMB.
+ */
+struct tee_rpmb_ctx {
+	uint8_t key[RPMB_KEY_MAC_SIZE];
+	uint8_t cid[RPMB_EMMC_CID_SIZE];
+	size_t hash_ctx_size;
+	uint32_t wr_cnt;
+	uint16_t max_blk_idx;
+	uint16_t rel_wr_blkcnt;
+	uint16_t dev_id;
+	bool wr_cnt_synced;
+	bool key_derived;
+	bool key_verified;
+	bool dev_info_synced;
+};
+
+static struct tee_rpmb_ctx *rpmb_ctx;
+
+/*
+ * Mutex to serialize the operations exported by this file.
+ * It protects rpmb_ctx and prevents overlapping operations on eMMC devices with
+ * different IDs.
+ */
+static struct mutex rpmb_mutex = MUTEX_INITIALIZER;
+
+#ifdef CFG_RPMB_TESTKEY
+
+static const uint8_t rpmb_test_key[RPMB_KEY_MAC_SIZE] = {
+	0xD3, 0xEB, 0x3E, 0xC3, 0x6E, 0x33, 0x4C, 0x9F,
+	0x98, 0x8C, 0xE2, 0xC0, 0xB8, 0x59, 0x54, 0x61,
+	0x0D, 0x2B, 0xCF, 0x86, 0x64, 0x84, 0x4D, 0xF2,
+	0xAB, 0x56, 0xE6, 0xC6, 0x1B, 0xB7, 0x01, 0xE4
+};
+
+static TEE_Result tee_rpmb_key_gen(uint16_t dev_id __unused,
+				   uint8_t *key, uint32_t len)
+{
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!key || RPMB_KEY_MAC_SIZE != len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	DMSG("RPMB: Using test key");
+	memcpy(key, rpmb_test_key, RPMB_KEY_MAC_SIZE);
+
+out:
+	return res;
+}
+
+#else /* !CFG_RPMB_TESTKEY */
+
+/*
+ * NOTE: We need a common API to get hw unique key and it
+ * should return error when the hw unique is not a valid
+ * one as stated below.
+ * We need to make sure the hw unique we get is valid by:
+ * 1. In case of HUK is used, checking if OTP is hidden (in
+ *    which case only zeros will be returned) or not;
+ * 2. In case of SSK is used, checking if SSK in OTP is
+ *    write_locked (which means a valid key is provisioned)
+ *    or not.
+ *
+ * Maybe tee_get_hw_unique_key() should be exposed as
+ * generic API for getting hw unique key!
+ * We should change the API tee_otp_get_hw_unique_key()
+ * to return error code!
+ */
+static TEE_Result tee_get_hw_unique_key(struct tee_hw_unique_key *hwkey)
+{
+	if (!hwkey)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	tee_otp_get_hw_unique_key(hwkey);
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_rpmb_key_gen(uint16_t dev_id __unused,
+				   uint8_t *key, uint32_t len)
+{
+	TEE_Result res;
+	struct tee_hw_unique_key hwkey;
+	uint8_t *ctx = NULL;
+
+	if (!key || RPMB_KEY_MAC_SIZE != len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	IMSG("RPMB: Using generated key");
+	res = tee_get_hw_unique_key(&hwkey);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ctx = malloc(rpmb_ctx->hash_ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, hwkey.data,
+				  HW_UNIQUE_KEY_LENGTH);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
+				    (uint8_t *)rpmb_ctx->cid,
+				    RPMB_EMMC_CID_SIZE);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, key, len);
+
+out:
+	free(ctx);
+	return res;
+}
+
+#endif /* !CFG_RPMB_TESTKEY */
+
+static void u32_to_bytes(uint32_t u32, uint8_t *bytes)
+{
+	*bytes = (uint8_t) (u32 >> 24);
+	*(bytes + 1) = (uint8_t) (u32 >> 16);
+	*(bytes + 2) = (uint8_t) (u32 >> 8);
+	*(bytes + 3) = (uint8_t) u32;
+}
+
+static void bytes_to_u32(uint8_t *bytes, uint32_t *u32)
+{
+	*u32 = (uint32_t) ((*(bytes) << 24) +
+			   (*(bytes + 1) << 16) +
+			   (*(bytes + 2) << 8) + (*(bytes + 3)));
+}
+
+static void u16_to_bytes(uint16_t u16, uint8_t *bytes)
+{
+	*bytes = (uint8_t) (u16 >> 8);
+	*(bytes + 1) = (uint8_t) u16;
+}
+
+static void bytes_to_u16(uint8_t *bytes, uint16_t *u16)
+{
+	*u16 = (uint16_t) ((*bytes << 8) + *(bytes + 1));
+}
+
+static TEE_Result tee_rpmb_mac_calc(uint8_t *mac, uint32_t macsize,
+				    uint8_t *key, uint32_t keysize,
+				    struct rpmb_data_frame *datafrms,
+				    uint16_t blkcnt)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	int i;
+	uint8_t *ctx = NULL;
+
+	if (!mac || !key || !datafrms)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	ctx = malloc(rpmb_ctx->hash_ctx_size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, key, keysize);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	for (i = 0; i < blkcnt; i++) {
+		res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
+					  datafrms[i].data,
+					  RPMB_MAC_PROTECT_DATA_SIZE);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, mac, macsize);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	free(ctx);
+	return res;
+}
+
+struct tee_rpmb_mem {
+	paddr_t phreq;
+	uint64_t phreq_cookie;
+	paddr_t phresp;
+	uint64_t phresp_cookie;
+	size_t req_size;
+	size_t resp_size;
+};
+
+static void tee_rpmb_free(struct tee_rpmb_mem *mem)
+{
+	if (!mem)
+		return;
+
+	if (mem->phreq) {
+		thread_rpc_free_payload(mem->phreq_cookie);
+		mem->phreq_cookie = 0;
+		mem->phreq = 0;
+	}
+	if (mem->phresp) {
+		thread_rpc_free_payload(mem->phresp_cookie);
+		mem->phresp_cookie = 0;
+		mem->phresp = 0;
+	}
+}
+
+
+static TEE_Result tee_rpmb_alloc(size_t req_size, size_t resp_size,
+		struct tee_rpmb_mem *mem, void **req, void **resp)
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t req_s = ROUNDUP(req_size, sizeof(uint32_t));
+	size_t resp_s = ROUNDUP(resp_size, sizeof(uint32_t));
+
+	if (!mem)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	memset(mem, 0, sizeof(*mem));
+	thread_rpc_alloc_payload(req_s, &mem->phreq, &mem->phreq_cookie);
+	thread_rpc_alloc_payload(resp_s, &mem->phresp, &mem->phresp_cookie);
+	if (!mem->phreq || !mem->phresp) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	*req = phys_to_virt(mem->phreq, MEM_AREA_NSEC_SHM);
+	*resp = phys_to_virt(mem->phresp, MEM_AREA_NSEC_SHM);
+	if (!*req || !*resp) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	mem->req_size = req_size;
+	mem->resp_size = resp_size;
+
+out:
+	if (res != TEE_SUCCESS)
+		tee_rpmb_free(mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_invoke(struct tee_rpmb_mem *mem)
+{
+	struct optee_msg_param params[2];
+
+	memset(params, 0, sizeof(params));
+	params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
+	params[1].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
+	params[0].u.tmem.buf_ptr = mem->phreq;
+	params[0].u.tmem.size = mem->req_size;
+	params[0].u.tmem.shm_ref = mem->phreq_cookie;
+	params[1].u.tmem.buf_ptr = mem->phresp;
+	params[1].u.tmem.size = mem->resp_size;
+	params[1].u.tmem.shm_ref = mem->phresp_cookie;
+
+	return thread_rpc_cmd(OPTEE_MSG_RPC_CMD_RPMB, 2, params);
+}
+
+static bool is_zero(const uint8_t *buf, size_t size)
+{
+	size_t i;
+
+	for (i = 0; i < size; i++)
+		if (buf[i])
+			return false;
+	return true;
+}
+
+static TEE_Result encrypt_block(uint8_t *out, const uint8_t *in,
+				uint16_t blk_idx, const uint8_t *fek)
+{
+	return tee_fs_crypt_block(out, in, RPMB_DATA_SIZE, blk_idx, fek,
+				  TEE_MODE_ENCRYPT);
+}
+
+static TEE_Result decrypt_block(uint8_t *out, const uint8_t *in,
+				uint16_t blk_idx, const uint8_t *fek)
+{
+	return tee_fs_crypt_block(out, in, RPMB_DATA_SIZE, blk_idx, fek,
+				  TEE_MODE_DECRYPT);
+}
+
+/* Decrypt/copy at most one block of data */
+static TEE_Result decrypt(uint8_t *out, const struct rpmb_data_frame *frm,
+			  size_t size, size_t offset,
+			  uint16_t blk_idx __maybe_unused, const uint8_t *fek)
+{
+	uint8_t *tmp __maybe_unused;
+
+
+	if ((size + offset < size) || (size + offset > RPMB_DATA_SIZE))
+		panic("invalid size or offset");
+
+	if (!fek) {
+		/* Block is not encrypted (not a file data block) */
+		memcpy(out, frm->data + offset, size);
+	} else if (is_zero(fek, TEE_FS_KM_FEK_SIZE)) {
+		/* The file was created with encryption disabled */
+		return TEE_ERROR_SECURITY;
+	} else {
+		/* Block is encrypted */
+		if (size < RPMB_DATA_SIZE) {
+			/*
+			 * Since output buffer is not large enough to hold one
+			 * block we must allocate a temporary buffer.
+			 */
+			tmp = malloc(RPMB_DATA_SIZE);
+			if (!tmp)
+				return TEE_ERROR_OUT_OF_MEMORY;
+			decrypt_block(tmp, frm->data, blk_idx, fek);
+			memcpy(out, tmp + offset, size);
+			free(tmp);
+		} else {
+			decrypt_block(out, frm->data, blk_idx, fek);
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_rpmb_req_pack(struct rpmb_req *req,
+				    struct rpmb_raw_data *rawdata,
+				    uint16_t nbr_frms, uint16_t dev_id,
+				    const uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	int i;
+	struct rpmb_data_frame *datafrm;
+
+	if (!req || !rawdata || !nbr_frms)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/*
+	 * Check write blockcount is not bigger than reliable write
+	 * blockcount.
+	 */
+	if ((rawdata->msg_type == RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE) &&
+	    (nbr_frms > rpmb_ctx->rel_wr_blkcnt)) {
+		DMSG("wr_blkcnt(%d) > rel_wr_blkcnt(%d)", nbr_frms,
+		     rpmb_ctx->rel_wr_blkcnt);
+		return TEE_ERROR_GENERIC;
+	}
+
+	req->cmd = RPMB_CMD_DATA_REQ;
+	req->dev_id = dev_id;
+
+	/* Allocate memory for construct all data packets and calculate MAC. */
+	datafrm = calloc(nbr_frms, RPMB_DATA_FRAME_SIZE);
+	if (!datafrm)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	for (i = 0; i < nbr_frms; i++) {
+		u16_to_bytes(rawdata->msg_type, datafrm[i].msg_type);
+
+		if (rawdata->block_count)
+			u16_to_bytes(*rawdata->block_count,
+				     datafrm[i].block_count);
+
+		if (rawdata->blk_idx) {
+			/* Check the block index is within range. */
+			if ((*rawdata->blk_idx + nbr_frms) >
+			    rpmb_ctx->max_blk_idx) {
+				res = TEE_ERROR_GENERIC;
+				goto func_exit;
+			}
+			u16_to_bytes(*rawdata->blk_idx, datafrm[i].address);
+		}
+
+		if (rawdata->write_counter)
+			u32_to_bytes(*rawdata->write_counter,
+				     datafrm[i].write_counter);
+
+		if (rawdata->nonce)
+			memcpy(datafrm[i].nonce, rawdata->nonce,
+			       RPMB_NONCE_SIZE);
+
+		if (rawdata->data) {
+			if (fek)
+				encrypt_block(datafrm[i].data,
+					rawdata->data + (i * RPMB_DATA_SIZE),
+					*rawdata->blk_idx + i, fek);
+			else
+				memcpy(datafrm[i].data,
+				       rawdata->data + (i * RPMB_DATA_SIZE),
+				       RPMB_DATA_SIZE);
+		}
+	}
+
+	if (rawdata->key_mac) {
+		if (rawdata->msg_type == RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE) {
+			res =
+			    tee_rpmb_mac_calc(rawdata->key_mac,
+					      RPMB_KEY_MAC_SIZE, rpmb_ctx->key,
+					      RPMB_KEY_MAC_SIZE, datafrm,
+					      nbr_frms);
+			if (res != TEE_SUCCESS)
+				goto func_exit;
+		}
+		memcpy(datafrm[nbr_frms - 1].key_mac,
+		       rawdata->key_mac, RPMB_KEY_MAC_SIZE);
+	}
+
+	memcpy(TEE_RPMB_REQ_DATA(req), datafrm,
+	       nbr_frms * RPMB_DATA_FRAME_SIZE);
+
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+	for (i = 0; i < nbr_frms; i++) {
+		DMSG("Dumping data frame %d:", i);
+		DHEXDUMP((uint8_t *)&datafrm[i] + RPMB_STUFF_DATA_SIZE,
+			 512 - RPMB_STUFF_DATA_SIZE);
+	}
+#endif
+
+	res = TEE_SUCCESS;
+func_exit:
+	free(datafrm);
+	return res;
+}
+
+static TEE_Result data_cpy_mac_calc_1b(struct rpmb_raw_data *rawdata,
+				       struct rpmb_data_frame *frm,
+				       uint8_t *fek)
+{
+	TEE_Result res;
+	uint8_t *data;
+	uint16_t idx;
+
+	if (rawdata->len + rawdata->byte_offset > RPMB_DATA_SIZE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_rpmb_mac_calc(rawdata->key_mac, RPMB_KEY_MAC_SIZE,
+				rpmb_ctx->key, RPMB_KEY_MAC_SIZE, frm, 1);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	data = rawdata->data;
+	bytes_to_u16(frm->address, &idx);
+
+	res = decrypt(data, frm, rawdata->len, rawdata->byte_offset, idx, fek);
+	return res;
+}
+
+static TEE_Result tee_rpmb_data_cpy_mac_calc(struct rpmb_data_frame *datafrm,
+					     struct rpmb_raw_data *rawdata,
+					     uint16_t nbr_frms,
+					     struct rpmb_data_frame *lastfrm,
+					     uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	int i;
+	uint8_t *ctx = NULL;
+	uint16_t offset;
+	uint32_t size;
+	uint8_t *data;
+	uint16_t start_idx;
+	struct rpmb_data_frame localfrm;
+
+	if (!datafrm || !rawdata || !nbr_frms || !lastfrm)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (nbr_frms == 1)
+		return data_cpy_mac_calc_1b(rawdata, lastfrm, fek);
+
+	/* nbr_frms > 1 */
+
+	data = rawdata->data;
+
+	ctx = malloc(rpmb_ctx->hash_ctx_size);
+	if (!ctx) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto func_exit;
+	}
+
+	res = crypto_ops.mac.init(ctx, TEE_ALG_HMAC_SHA256, rpmb_ctx->key,
+				  RPMB_KEY_MAC_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	/*
+	 * Note: JEDEC JESD84-B51: "In every packet the address is the start
+	 * address of the full access (not address of the individual half a
+	 * sector)"
+	 */
+	bytes_to_u16(lastfrm->address, &start_idx);
+
+	for (i = 0; i < (nbr_frms - 1); i++) {
+
+		/*
+		 * By working on a local copy of the RPMB frame, we ensure that
+		 * the data can not be modified after the MAC is computed but
+		 * before the payload is decrypted/copied to the output buffer.
+		 */
+		memcpy(&localfrm, &datafrm[i], RPMB_DATA_FRAME_SIZE);
+
+		res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256,
+					    localfrm.data,
+					    RPMB_MAC_PROTECT_DATA_SIZE);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		if (i == 0) {
+			/* First block */
+			offset = rawdata->byte_offset;
+			size = RPMB_DATA_SIZE - offset;
+		} else {
+			/* Middle blocks */
+			size = RPMB_DATA_SIZE;
+			offset = 0;
+		}
+
+		res = decrypt(data, &localfrm, size, offset, start_idx + i,
+			      fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		data += size;
+	}
+
+	/* Last block */
+	size = (rawdata->len + rawdata->byte_offset) % RPMB_DATA_SIZE;
+	if (size == 0)
+		size = RPMB_DATA_SIZE;
+	res = decrypt(data, lastfrm, size, 0, start_idx + nbr_frms - 1, fek);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	/* Update MAC against the last block */
+	res = crypto_ops.mac.update(ctx, TEE_ALG_HMAC_SHA256, lastfrm->data,
+				    RPMB_MAC_PROTECT_DATA_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = crypto_ops.mac.final(ctx, TEE_ALG_HMAC_SHA256, rawdata->key_mac,
+				   RPMB_KEY_MAC_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	free(ctx);
+	return res;
+}
+
+static TEE_Result tee_rpmb_resp_unpack_verify(struct rpmb_data_frame *datafrm,
+					      struct rpmb_raw_data *rawdata,
+					      uint16_t nbr_frms, uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint16_t msg_type;
+	uint32_t wr_cnt;
+	uint16_t blk_idx;
+	uint16_t op_result;
+	struct rpmb_data_frame lastfrm;
+
+	if (!datafrm || !rawdata || !nbr_frms)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+	for (uint32_t i = 0; i < nbr_frms; i++) {
+		DMSG("Dumping data frame %d:", i);
+		DHEXDUMP((uint8_t *)&datafrm[i] + RPMB_STUFF_DATA_SIZE,
+			 512 - RPMB_STUFF_DATA_SIZE);
+	}
+#endif
+
+	/* Make sure the last data packet can't be modified once verified */
+	memcpy(&lastfrm, &datafrm[nbr_frms - 1], RPMB_DATA_FRAME_SIZE);
+
+	/* Handle operation result and translate to TEEC error code. */
+	bytes_to_u16(lastfrm.op_result, &op_result);
+	if (rawdata->op_result)
+		*rawdata->op_result = op_result;
+	if (op_result != RPMB_RESULT_OK)
+		return TEE_ERROR_GENERIC;
+
+	/* Check the response msg_type. */
+	bytes_to_u16(lastfrm.msg_type, &msg_type);
+	if (msg_type != rawdata->msg_type) {
+		DMSG("Unexpected msg_type (0x%04x != 0x%04x)", msg_type,
+		     rawdata->msg_type);
+		return TEE_ERROR_GENERIC;
+	}
+
+	if (rawdata->blk_idx) {
+		bytes_to_u16(lastfrm.address, &blk_idx);
+		if (blk_idx != *rawdata->blk_idx) {
+			DMSG("Unexpected block index");
+			return TEE_ERROR_GENERIC;
+		}
+	}
+
+	if (rawdata->write_counter) {
+		wr_cnt = *rawdata->write_counter;
+		bytes_to_u32(lastfrm.write_counter, rawdata->write_counter);
+		if (msg_type == RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE) {
+			/* Verify the write counter is incremented by 1 */
+			if (*rawdata->write_counter != wr_cnt + 1) {
+				DMSG("Counter mismatched (0x%04x/0x%04x)",
+				     *rawdata->write_counter, wr_cnt + 1);
+				return TEE_ERROR_SECURITY;
+			}
+			rpmb_ctx->wr_cnt++;
+		}
+	}
+
+	if (rawdata->nonce) {
+		if (buf_compare_ct(rawdata->nonce, lastfrm.nonce,
+				   RPMB_NONCE_SIZE) != 0) {
+			DMSG("Nonce mismatched");
+			return TEE_ERROR_SECURITY;
+		}
+	}
+
+	if (rawdata->key_mac) {
+		if (msg_type == RPMB_MSG_TYPE_RESP_AUTH_DATA_READ) {
+			if (!rawdata->data)
+				return TEE_ERROR_GENERIC;
+
+			res = tee_rpmb_data_cpy_mac_calc(datafrm, rawdata,
+							 nbr_frms, &lastfrm,
+							 fek);
+
+			if (res != TEE_SUCCESS)
+				return res;
+		} else {
+			/*
+			 * There should be only one data frame for
+			 * other msg types.
+			 */
+			if (nbr_frms != 1)
+				return TEE_ERROR_GENERIC;
+
+			res = tee_rpmb_mac_calc(rawdata->key_mac,
+						RPMB_KEY_MAC_SIZE,
+						rpmb_ctx->key,
+						RPMB_KEY_MAC_SIZE,
+						&lastfrm, 1);
+
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+#ifndef CFG_RPMB_FS_NO_MAC
+		if (buf_compare_ct(rawdata->key_mac,
+				   (datafrm + nbr_frms - 1)->key_mac,
+				   RPMB_KEY_MAC_SIZE) != 0) {
+			DMSG("MAC mismatched:");
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+			DHEXDUMP((uint8_t *)rawdata->key_mac, 32);
+#endif
+			return TEE_ERROR_SECURITY;
+		}
+#endif /* !CFG_RPMB_FS_NO_MAC */
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_rpmb_get_dev_info(uint16_t dev_id,
+					struct rpmb_dev_info *dev_info)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem;
+	struct rpmb_dev_info *di;
+	struct rpmb_req *req = NULL;
+	uint8_t *resp = NULL;
+	uint32_t req_size;
+	uint32_t resp_size;
+
+	if (!dev_info)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	req_size = sizeof(struct rpmb_req);
+	resp_size = sizeof(struct rpmb_dev_info);
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	req->cmd = RPMB_CMD_GET_DEV_INFO;
+	req->dev_id = dev_id;
+
+	di = (struct rpmb_dev_info *)resp;
+	di->ret_code = RPMB_CMD_GET_DEV_INFO_RET_ERROR;
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	if (di->ret_code != RPMB_CMD_GET_DEV_INFO_RET_OK) {
+		res = TEE_ERROR_GENERIC;
+		goto func_exit;
+	}
+
+	memcpy((uint8_t *)dev_info, resp, sizeof(struct rpmb_dev_info));
+
+#ifdef CFG_RPMB_FS_DEBUG_DATA
+	DMSG("Dumping dev_info:");
+	DHEXDUMP((uint8_t *)dev_info, sizeof(struct rpmb_dev_info));
+#endif
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_init_read_wr_cnt(uint16_t dev_id,
+					    uint32_t *wr_cnt,
+					    uint16_t *op_result)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem;
+	uint16_t msg_type;
+	uint8_t nonce[RPMB_NONCE_SIZE];
+	uint8_t hmac[RPMB_KEY_MAC_SIZE];
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+
+	if (!wr_cnt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
+	resp_size = RPMB_DATA_FRAME_SIZE;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = crypto_ops.prng.read(nonce, RPMB_NONCE_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_REQ_WRITE_COUNTER_VAL_READ;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.nonce = nonce;
+
+	res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_RESP_WRITE_COUNTER_VAL_READ;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.op_result = op_result;
+	rawdata.write_counter = wr_cnt;
+	rawdata.nonce = nonce;
+	rawdata.key_mac = hmac;
+
+	res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_verify_key_sync_counter(uint16_t dev_id)
+{
+	uint16_t op_result = 0;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	res = tee_rpmb_init_read_wr_cnt(dev_id, &rpmb_ctx->wr_cnt,
+					&op_result);
+
+	if (res == TEE_SUCCESS) {
+		rpmb_ctx->key_verified = true;
+		rpmb_ctx->wr_cnt_synced = true;
+	}
+
+	DMSG("Verify key returning 0x%x\n", res);
+	return res;
+}
+
+#ifdef CFG_RPMB_WRITE_KEY
+static TEE_Result tee_rpmb_write_key(uint16_t dev_id)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem = { 0 };
+	uint16_t msg_type;
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+
+	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
+	resp_size = RPMB_DATA_FRAME_SIZE;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_REQ_AUTH_KEY_PROGRAM;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.key_mac = rpmb_ctx->key;
+
+	res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_RESP_AUTH_KEY_PROGRAM;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+
+	res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_write_and_verify_key(uint16_t dev_id)
+{
+	TEE_Result res;
+
+	DMSG("RPMB INIT: Writing Key");
+	res = tee_rpmb_write_key(dev_id);
+	if (res == TEE_SUCCESS) {
+		DMSG("RPMB INIT: Verifying Key");
+		res = tee_rpmb_verify_key_sync_counter(dev_id);
+	}
+	return res;
+}
+#else
+static TEE_Result tee_rpmb_write_and_verify_key(uint16_t dev_id __unused)
+{
+	return TEE_ERROR_BAD_STATE;
+}
+#endif
+
+/* True when all the required crypto functions are available */
+static bool have_crypto_ops(void)
+{
+	return (crypto_ops.mac.init && crypto_ops.mac.update &&
+		crypto_ops.mac.final && crypto_ops.prng.read);
+}
+
+/* This function must never return TEE_SUCCESS if rpmb_ctx == NULL */
+static TEE_Result tee_rpmb_init(uint16_t dev_id)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct rpmb_dev_info dev_info;
+
+	if (!have_crypto_ops())
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	if (!rpmb_ctx) {
+		rpmb_ctx = calloc(1, sizeof(struct tee_rpmb_ctx));
+		if (!rpmb_ctx)
+			return TEE_ERROR_OUT_OF_MEMORY;
+	} else if (rpmb_ctx->dev_id != dev_id) {
+		memset(rpmb_ctx, 0x00, sizeof(struct tee_rpmb_ctx));
+	}
+
+	rpmb_ctx->dev_id = dev_id;
+
+	if (!rpmb_ctx->dev_info_synced) {
+		DMSG("RPMB: Syncing device information");
+
+		dev_info.rpmb_size_mult = 0;
+		dev_info.rel_wr_sec_c = 0;
+		res = tee_rpmb_get_dev_info(dev_id, &dev_info);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		DMSG("RPMB: RPMB size is %d*128 KB", dev_info.rpmb_size_mult);
+		DMSG("RPMB: Reliable Write Sector Count is %d",
+		     dev_info.rel_wr_sec_c);
+
+		if (dev_info.rpmb_size_mult == 0) {
+			res = TEE_ERROR_GENERIC;
+			goto func_exit;
+		}
+
+		rpmb_ctx->max_blk_idx = (dev_info.rpmb_size_mult *
+					 RPMB_SIZE_SINGLE / RPMB_DATA_SIZE) - 1;
+
+		memcpy(rpmb_ctx->cid, dev_info.cid, RPMB_EMMC_CID_SIZE);
+
+		if ((rpmb_ctx->hash_ctx_size == 0) &&
+		    (crypto_ops.mac.get_ctx_size(
+			    TEE_ALG_HMAC_SHA256,
+			    &rpmb_ctx->hash_ctx_size))) {
+			rpmb_ctx->hash_ctx_size = 0;
+			res = TEE_ERROR_GENERIC;
+			goto func_exit;
+		}
+
+#ifdef RPMB_DRIVER_MULTIPLE_WRITE_FIXED
+		rpmb_ctx->rel_wr_blkcnt = dev_info.rel_wr_sec_c * 2;
+#else
+		rpmb_ctx->rel_wr_blkcnt = 1;
+#endif
+
+		rpmb_ctx->dev_info_synced = true;
+	}
+
+	if (!rpmb_ctx->key_derived) {
+		DMSG("RPMB INIT: Deriving key");
+
+		res = tee_rpmb_key_gen(dev_id, rpmb_ctx->key,
+				       RPMB_KEY_MAC_SIZE);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		rpmb_ctx->key_derived = true;
+	}
+
+	/* Perform a write counter read to verify if the key is ok. */
+	if (!rpmb_ctx->wr_cnt_synced || !rpmb_ctx->key_verified) {
+		DMSG("RPMB INIT: Verifying Key");
+
+		res = tee_rpmb_verify_key_sync_counter(dev_id);
+		if (res != TEE_SUCCESS && !rpmb_ctx->key_verified) {
+			/*
+			 * Need to write the key here and verify it.
+			 */
+			res = tee_rpmb_write_and_verify_key(dev_id);
+		}
+	}
+
+func_exit:
+	return res;
+}
+
+/*
+ * Read RPMB data in bytes.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @addr       Byte address of data.
+ * @data       Pointer to the data.
+ * @len        Size of data in bytes.
+ * @fek        Encrypted File Encryption Key or NULL.
+ */
+static TEE_Result tee_rpmb_read(uint16_t dev_id, uint32_t addr, uint8_t *data,
+				uint32_t len, uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_rpmb_mem mem = { 0 };
+	uint16_t msg_type;
+	uint8_t nonce[RPMB_NONCE_SIZE];
+	uint8_t hmac[RPMB_KEY_MAC_SIZE];
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+	uint16_t blk_idx;
+	uint16_t blkcnt;
+	uint8_t byte_offset;
+
+	if (!data || !len)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	blk_idx = addr / RPMB_DATA_SIZE;
+	byte_offset = addr % RPMB_DATA_SIZE;
+
+	blkcnt =
+	    ROUNDUP(len + byte_offset, RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
+	res = tee_rpmb_init(dev_id);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	req_size = sizeof(struct rpmb_req) + RPMB_DATA_FRAME_SIZE;
+	resp_size = RPMB_DATA_FRAME_SIZE * blkcnt;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_REQ_AUTH_DATA_READ;
+	res = crypto_ops.prng.read(nonce, RPMB_NONCE_SIZE);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.nonce = nonce;
+	rawdata.blk_idx = &blk_idx;
+	res = tee_rpmb_req_pack(req, &rawdata, 1, dev_id, NULL);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	req->block_count = blkcnt;
+
+	DMSG("Read %u block%s at index %u", blkcnt, ((blkcnt > 1) ? "s" : ""),
+	     blk_idx);
+
+	res = tee_rpmb_invoke(&mem);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	msg_type = RPMB_MSG_TYPE_RESP_AUTH_DATA_READ;
+
+	memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+	rawdata.msg_type = msg_type;
+	rawdata.block_count = &blkcnt;
+	rawdata.blk_idx = &blk_idx;
+	rawdata.nonce = nonce;
+	rawdata.key_mac = hmac;
+	rawdata.data = data;
+
+	rawdata.len = len;
+	rawdata.byte_offset = byte_offset;
+
+	res = tee_rpmb_resp_unpack_verify(resp, &rawdata, blkcnt, fek);
+	if (res != TEE_SUCCESS)
+		goto func_exit;
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static TEE_Result tee_rpmb_write_blk(uint16_t dev_id, uint16_t blk_idx,
+				     const uint8_t *data_blks, uint16_t blkcnt,
+				     const uint8_t *fek)
+{
+	TEE_Result res;
+	struct tee_rpmb_mem mem;
+	uint16_t msg_type;
+	uint32_t wr_cnt;
+	uint8_t hmac[RPMB_KEY_MAC_SIZE];
+	struct rpmb_req *req = NULL;
+	struct rpmb_data_frame *resp = NULL;
+	struct rpmb_raw_data rawdata;
+	uint32_t req_size;
+	uint32_t resp_size;
+	uint32_t nbr_writes;
+	uint16_t tmp_blkcnt;
+	uint16_t tmp_blk_idx;
+	uint16_t i;
+
+	DMSG("Write %u block%s at index %u", blkcnt, ((blkcnt > 1) ? "s" : ""),
+	     blk_idx);
+
+	if (!data_blks || !blkcnt)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_rpmb_init(dev_id);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/*
+	 * We need to split data when block count
+	 * is bigger than reliable block write count.
+	 */
+	if (blkcnt < rpmb_ctx->rel_wr_blkcnt)
+		req_size = sizeof(struct rpmb_req) +
+		    RPMB_DATA_FRAME_SIZE * blkcnt;
+	else
+		req_size = sizeof(struct rpmb_req) +
+		    RPMB_DATA_FRAME_SIZE * rpmb_ctx->rel_wr_blkcnt;
+
+	resp_size = RPMB_DATA_FRAME_SIZE;
+	res = tee_rpmb_alloc(req_size, resp_size, &mem,
+			     (void *)&req, (void *)&resp);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	nbr_writes = blkcnt / rpmb_ctx->rel_wr_blkcnt;
+	if (blkcnt % rpmb_ctx->rel_wr_blkcnt > 0)
+		nbr_writes += 1;
+
+	tmp_blkcnt = rpmb_ctx->rel_wr_blkcnt;
+	tmp_blk_idx = blk_idx;
+	for (i = 0; i < nbr_writes; i++) {
+		/*
+		 * To handle the last write of block count which is
+		 * equal or smaller than reliable write block count.
+		 */
+		if (i == nbr_writes - 1)
+			tmp_blkcnt = blkcnt - rpmb_ctx->rel_wr_blkcnt *
+			    (nbr_writes - 1);
+
+		msg_type = RPMB_MSG_TYPE_REQ_AUTH_DATA_WRITE;
+		wr_cnt = rpmb_ctx->wr_cnt;
+
+		memset(req, 0x00, req_size);
+		memset(resp, 0x00, resp_size);
+
+		memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+		rawdata.msg_type = msg_type;
+		rawdata.block_count = &tmp_blkcnt;
+		rawdata.blk_idx = &tmp_blk_idx;
+		rawdata.write_counter = &wr_cnt;
+		rawdata.key_mac = hmac;
+		rawdata.data = (uint8_t *)data_blks +
+				i * rpmb_ctx->rel_wr_blkcnt * RPMB_DATA_SIZE;
+
+		res = tee_rpmb_req_pack(req, &rawdata, tmp_blkcnt, dev_id,
+					fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = tee_rpmb_invoke(&mem);
+		if (res != TEE_SUCCESS) {
+			/*
+			 * To force wr_cnt sync next time, as it might get
+			 * out of sync due to inconsistent operation result!
+			 */
+			rpmb_ctx->wr_cnt_synced = false;
+			goto out;
+		}
+
+		msg_type = RPMB_MSG_TYPE_RESP_AUTH_DATA_WRITE;
+
+		memset(&rawdata, 0x00, sizeof(struct rpmb_raw_data));
+		rawdata.msg_type = msg_type;
+		rawdata.block_count = &tmp_blkcnt;
+		rawdata.blk_idx = &tmp_blk_idx;
+		rawdata.write_counter = &wr_cnt;
+		rawdata.key_mac = hmac;
+
+		res = tee_rpmb_resp_unpack_verify(resp, &rawdata, 1, NULL);
+		if (res != TEE_SUCCESS) {
+			/*
+			 * To force wr_cnt sync next time, as it might get
+			 * out of sync due to inconsistent operation result!
+			 */
+			rpmb_ctx->wr_cnt_synced = false;
+			goto out;
+		}
+
+		tmp_blk_idx += tmp_blkcnt;
+	}
+
+out:
+	tee_rpmb_free(&mem);
+	return res;
+}
+
+static bool tee_rpmb_write_is_atomic(uint16_t dev_id __unused, uint32_t addr,
+				     uint32_t len)
+{
+	uint8_t byte_offset = addr % RPMB_DATA_SIZE;
+	uint16_t blkcnt = ROUNDUP(len + byte_offset,
+				  RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
+
+	return (blkcnt <= rpmb_ctx->rel_wr_blkcnt);
+}
+
+/*
+ * Write RPMB data in bytes.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @addr       Byte address of data.
+ * @data       Pointer to the data.
+ * @len        Size of data in bytes.
+ * @fek        Encrypted File Encryption Key or NULL.
+ */
+static TEE_Result tee_rpmb_write(uint16_t dev_id, uint32_t addr,
+				 const uint8_t *data, uint32_t len,
+				 uint8_t *fek)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint8_t *data_tmp = NULL;
+	uint16_t blk_idx;
+	uint16_t blkcnt;
+	uint8_t byte_offset;
+
+	blk_idx = addr / RPMB_DATA_SIZE;
+	byte_offset = addr % RPMB_DATA_SIZE;
+
+	blkcnt =
+	    ROUNDUP(len + byte_offset, RPMB_DATA_SIZE) / RPMB_DATA_SIZE;
+
+	if (byte_offset == 0 && (len % RPMB_DATA_SIZE) == 0) {
+		res = tee_rpmb_write_blk(dev_id, blk_idx, data, blkcnt, fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	} else {
+		data_tmp = calloc(blkcnt, RPMB_DATA_SIZE);
+		if (!data_tmp) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto func_exit;
+		}
+
+		/* Read the complete blocks */
+		res = tee_rpmb_read(dev_id, blk_idx * RPMB_DATA_SIZE, data_tmp,
+				    blkcnt * RPMB_DATA_SIZE, fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+
+		/* Partial update of the data blocks */
+		memcpy(data_tmp + byte_offset, data, len);
+
+		res = tee_rpmb_write_blk(dev_id, blk_idx, data_tmp, blkcnt,
+					 fek);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	res = TEE_SUCCESS;
+
+func_exit:
+	free(data_tmp);
+	return res;
+}
+
+/*
+ * Read the RPMB write counter.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @counter    Pointer to the counter.
+ */
+static TEE_Result tee_rpmb_get_write_counter(uint16_t dev_id,
+					     uint32_t *counter)
+{
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!counter)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!rpmb_ctx || !rpmb_ctx->wr_cnt_synced) {
+		res = tee_rpmb_init(dev_id);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	*counter = rpmb_ctx->wr_cnt;
+
+func_exit:
+	return res;
+}
+
+/*
+ * Read the RPMB max block.
+ *
+ * @dev_id     Device ID of the eMMC device.
+ * @counter    Pointer to receive the max block.
+ */
+static TEE_Result tee_rpmb_get_max_block(uint16_t dev_id, uint32_t *max_block)
+{
+	TEE_Result res = TEE_SUCCESS;
+
+	if (!max_block)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!rpmb_ctx || !rpmb_ctx->dev_info_synced) {
+		res = tee_rpmb_init(dev_id);
+		if (res != TEE_SUCCESS)
+			goto func_exit;
+	}
+
+	*max_block = rpmb_ctx->max_blk_idx;
+
+func_exit:
+	return res;
+}
+
+/*
+ * End of lower interface to RPMB device
+ */
+
+static TEE_Result get_fat_start_address(uint32_t *addr);
+
+static void dump_fat(void)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_fat_entry *fat_entries = NULL;
+	uint32_t fat_address;
+	size_t size;
+	int i;
+	bool last_entry_found = false;
+
+	res = get_fat_start_address(&fat_address);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
+	fat_entries = malloc(size);
+	if (!fat_entries) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	while (!last_entry_found) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
+				    (uint8_t *)fat_entries, size, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		for (i = 0; i < N_ENTRIES; i++) {
+
+			FMSG("flags 0x%x, size %d, address 0x%x, filename '%s'",
+				fat_entries[i].flags,
+				fat_entries[i].data_size,
+				fat_entries[i].start_address,
+				fat_entries[i].filename);
+
+			if ((fat_entries[i].flags & FILE_IS_LAST_ENTRY) != 0) {
+				last_entry_found = true;
+				break;
+			}
+
+			/* Move to next fat_entry. */
+			fat_address += sizeof(struct rpmb_fat_entry);
+		}
+	}
+
+out:
+	free(fat_entries);
+}
+
+#if (TRACE_LEVEL >= TRACE_DEBUG)
+static void dump_fh(struct rpmb_file_handle *fh)
+{
+	DMSG("fh->filename=%s", fh->filename);
+	DMSG("fh->pos=%u", fh->pos);
+	DMSG("fh->rpmb_fat_address=%u", fh->rpmb_fat_address);
+	DMSG("fh->fat_entry.start_address=%u", fh->fat_entry.start_address);
+	DMSG("fh->fat_entry.data_size=%u", fh->fat_entry.data_size);
+}
+#else
+static void dump_fh(struct rpmb_file_handle *fh __unused)
+{
+}
+#endif
+
+static struct rpmb_file_handle *alloc_file_handle(const char *filename)
+{
+	struct rpmb_file_handle *fh = NULL;
+
+	fh = calloc(1, sizeof(struct rpmb_file_handle));
+	if (!fh)
+		return NULL;
+
+	if (filename)
+		strlcpy(fh->filename, filename, sizeof(fh->filename));
+
+	return fh;
+}
+
+/**
+ * write_fat_entry: Store info in a fat_entry to RPMB.
+ */
+static TEE_Result write_fat_entry(struct rpmb_file_handle *fh,
+				  bool update_write_counter)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	/* Protect partition data. */
+	if (fh->rpmb_fat_address < sizeof(struct rpmb_fs_partition)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto out;
+	}
+
+	if (fh->rpmb_fat_address % sizeof(struct rpmb_fat_entry) != 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (update_write_counter) {
+		res = tee_rpmb_get_write_counter(CFG_RPMB_FS_DEV_ID,
+						 &fh->fat_entry.write_counter);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, fh->rpmb_fat_address,
+			     (uint8_t *)&fh->fat_entry,
+			     sizeof(struct rpmb_fat_entry), NULL);
+
+	dump_fat();
+
+out:
+	return res;
+}
+
+/**
+ * rpmb_fs_setup: Setup rpmb fs.
+ * Set initial partition and FS values and write to RPMB.
+ * Store frequently used data in RAM.
+ */
+static TEE_Result rpmb_fs_setup(void)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_fs_partition *partition_data = NULL;
+	struct rpmb_file_handle *fh = NULL;
+	uint32_t max_rpmb_block = 0;
+
+	if (fs_par) {
+		res = TEE_SUCCESS;
+		goto out;
+	}
+
+	res = tee_rpmb_get_max_block(CFG_RPMB_FS_DEV_ID, &max_rpmb_block);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	partition_data = calloc(1, sizeof(struct rpmb_fs_partition));
+	if (!partition_data) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, RPMB_STORAGE_START_ADDRESS,
+			    (uint8_t *)partition_data,
+			    sizeof(struct rpmb_fs_partition), NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+#ifndef CFG_RPMB_RESET_FAT
+	if (partition_data->rpmb_fs_magic == RPMB_FS_MAGIC) {
+		if (partition_data->fs_version == FS_VERSION) {
+			res = TEE_SUCCESS;
+			goto store_fs_par;
+		} else {
+			/* Wrong software is in use. */
+			res = TEE_ERROR_ACCESS_DENIED;
+			goto out;
+		}
+	}
+#else
+	EMSG("**** Clearing Storage ****");
+#endif
+
+	/* Setup new partition data. */
+	partition_data->rpmb_fs_magic = RPMB_FS_MAGIC;
+	partition_data->fs_version = FS_VERSION;
+	partition_data->fat_start_address = RPMB_FS_FAT_START_ADDRESS;
+
+	/* Initial FAT entry with FILE_IS_LAST_ENTRY flag set. */
+	fh = alloc_file_handle(NULL);
+	if (!fh) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	fh->fat_entry.flags = FILE_IS_LAST_ENTRY;
+	fh->rpmb_fat_address = partition_data->fat_start_address;
+
+	/* Write init FAT entry and partition data to RPMB. */
+	res = write_fat_entry(fh, true);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res =
+	    tee_rpmb_get_write_counter(CFG_RPMB_FS_DEV_ID,
+				       &partition_data->write_counter);
+	if (res != TEE_SUCCESS)
+		goto out;
+	res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, RPMB_STORAGE_START_ADDRESS,
+			     (uint8_t *)partition_data,
+			     sizeof(struct rpmb_fs_partition), NULL);
+
+#ifndef CFG_RPMB_RESET_FAT
+store_fs_par:
+#endif
+
+	/* Store FAT start address. */
+	fs_par = calloc(1, sizeof(struct rpmb_fs_parameters));
+	if (!fs_par) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	fs_par->fat_start_address = partition_data->fat_start_address;
+	fs_par->max_rpmb_address = max_rpmb_block << RPMB_BLOCK_SIZE_SHIFT;
+
+	dump_fat();
+
+out:
+	free(fh);
+	free(partition_data);
+	return res;
+}
+
+/**
+ * get_fat_start_address:
+ * FAT start_address from fs_par.
+ */
+static TEE_Result get_fat_start_address(uint32_t *addr)
+{
+	if (!fs_par)
+		return TEE_ERROR_NO_DATA;
+
+	*addr = fs_par->fat_start_address;
+
+	return TEE_SUCCESS;
+}
+
+/**
+ * read_fat: Read FAT entries
+ * Return matching FAT entry for read, rm rename and stat.
+ * Build up memory pool and return matching entry for write operation.
+ * "Last FAT entry" can be returned during write.
+ */
+static TEE_Result read_fat(struct rpmb_file_handle *fh, tee_mm_pool_t *p)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	tee_mm_entry_t *mm = NULL;
+	struct rpmb_fat_entry *fat_entries = NULL;
+	uint32_t fat_address;
+	size_t size;
+	int i;
+	bool entry_found = false;
+	bool last_entry_found = false;
+	bool expand_fat = false;
+	struct rpmb_file_handle last_fh;
+
+	DMSG("fat_address %d", fh->rpmb_fat_address);
+
+	res = rpmb_fs_setup();
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = get_fat_start_address(&fat_address);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
+	fat_entries = malloc(size);
+	if (!fat_entries) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/*
+	 * The pool is used to represent the current RPMB layout. To find
+	 * a slot for the file tee_mm_alloc is called on the pool. Thus
+	 * if it is not NULL the entire FAT must be traversed to fill in
+	 * the pool.
+	 */
+	while (!last_entry_found && (!entry_found || p)) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
+				    (uint8_t *)fat_entries, size, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		for (i = 0; i < N_ENTRIES; i++) {
+			/*
+			 * Look for an entry, matching filenames. (read, rm,
+			 * rename and stat.). Only store first filename match.
+			 */
+			if (fh->filename &&
+			    (strcmp(fh->filename,
+				    fat_entries[i].filename) == 0) &&
+			    (fat_entries[i].flags & FILE_IS_ACTIVE) &&
+			    (!entry_found)) {
+				entry_found = true;
+				fh->rpmb_fat_address = fat_address;
+				memcpy(&fh->fat_entry, &fat_entries[i],
+				       sizeof(struct rpmb_fat_entry));
+				if (!p)
+					break;
+			}
+
+			/* Add existing files to memory pool. (write) */
+			if (p) {
+				if ((fat_entries[i].flags & FILE_IS_ACTIVE) &&
+				    (fat_entries[i].data_size > 0)) {
+
+					mm = tee_mm_alloc2
+						(p,
+						 fat_entries[i].start_address,
+						 fat_entries[i].data_size);
+					if (!mm) {
+						res = TEE_ERROR_OUT_OF_MEMORY;
+						goto out;
+					}
+				}
+
+				/* Unused FAT entries can be reused (write) */
+				if (((fat_entries[i].flags & FILE_IS_ACTIVE) ==
+				     0) && (fh->rpmb_fat_address == 0)) {
+					fh->rpmb_fat_address = fat_address;
+					memcpy(&fh->fat_entry, &fat_entries[i],
+					       sizeof(struct rpmb_fat_entry));
+				}
+			}
+
+			if ((fat_entries[i].flags & FILE_IS_LAST_ENTRY) != 0) {
+				last_entry_found = true;
+
+				/*
+				 * If the last entry was reached and was chosen
+				 * by the previous check, then the FAT needs to
+				 * be expanded.
+				 * fh->rpmb_fat_address is the address chosen
+				 * to store the files FAT entry and fat_address
+				 * is the current FAT entry address being
+				 * compared.
+				 */
+				if (p && fh->rpmb_fat_address == fat_address)
+					expand_fat = true;
+				break;
+			}
+
+			/* Move to next fat_entry. */
+			fat_address += sizeof(struct rpmb_fat_entry);
+		}
+	}
+
+	/*
+	 * Represent the FAT table in the pool.
+	 */
+	if (p) {
+		/*
+		 * Since fat_address is the start of the last entry it needs to
+		 * be moved up by an entry.
+		 */
+		fat_address += sizeof(struct rpmb_fat_entry);
+
+		/* Make room for yet a FAT entry and add to memory pool. */
+		if (expand_fat)
+			fat_address += sizeof(struct rpmb_fat_entry);
+
+		mm = tee_mm_alloc2(p, RPMB_STORAGE_START_ADDRESS, fat_address);
+		if (!mm) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		if (expand_fat) {
+			/*
+			 * Point fat_address to the beginning of the new
+			 * entry.
+			 */
+			fat_address -= sizeof(struct rpmb_fat_entry);
+			memset(&last_fh, 0, sizeof(last_fh));
+			last_fh.fat_entry.flags = FILE_IS_LAST_ENTRY;
+			last_fh.rpmb_fat_address = fat_address;
+			res = write_fat_entry(&last_fh, true);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+	}
+
+	if (fh->filename && !fh->rpmb_fat_address)
+		res = TEE_ERROR_ITEM_NOT_FOUND;
+
+out:
+	free(fat_entries);
+	return res;
+}
+
+static TEE_Result generate_fek(struct rpmb_fat_entry *fe)
+{
+	TEE_Result res;
+
+again:
+	res = tee_fs_generate_fek(fe->fek, sizeof(fe->fek));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (is_zero(fe->fek, sizeof(fe->fek)))
+		goto again;
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_open_internal(const char *file, bool create,
+					struct tee_file_handle **ret_fh)
+{
+	struct rpmb_file_handle *fh = NULL;
+	size_t filelen;
+	tee_mm_pool_t p;
+	bool pool_result;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	mutex_lock(&rpmb_mutex);
+
+	filelen = strlen(file);
+	if (filelen >= TEE_RPMB_FS_FILENAME_LENGTH - 1 || filelen == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (file[filelen - 1] == '/') {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh = alloc_file_handle(file);
+	if (!fh) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/* We need to do setup in order to make sure fs_par is filled in */
+	res = rpmb_fs_setup();
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (create) {
+		/* Upper memory allocation must be used for RPMB_FS. */
+		pool_result = tee_mm_init(&p,
+					  RPMB_STORAGE_START_ADDRESS,
+					  fs_par->max_rpmb_address,
+					  RPMB_BLOCK_SIZE_SHIFT,
+					  TEE_MM_POOL_HI_ALLOC);
+
+		if (!pool_result) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		res = read_fat(fh, &p);
+		tee_mm_final(&p);
+		if (res != TEE_SUCCESS)
+			goto out;
+	} else {
+		res = read_fat(fh, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	/*
+	 * If this is opened with create and the entry found was not active
+	 * then this is a new file and the FAT entry must be written
+	 */
+	if (create) {
+		if ((fh->fat_entry.flags & FILE_IS_ACTIVE) == 0) {
+			memset(&fh->fat_entry, 0,
+				sizeof(struct rpmb_fat_entry));
+			memcpy(fh->fat_entry.filename, file, strlen(file));
+			/* Start address and size are 0 */
+			fh->fat_entry.flags = FILE_IS_ACTIVE;
+
+			res = generate_fek(&fh->fat_entry);
+			if (res != TEE_SUCCESS)
+				goto out;
+			DMSG("GENERATE FEK key: %p",
+			     (void *)fh->fat_entry.fek);
+			DHEXDUMP(fh->fat_entry.fek, sizeof(fh->fat_entry.fek));
+
+			res = write_fat_entry(fh, true);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+	}
+
+	res = TEE_SUCCESS;
+
+out:
+	if (res == TEE_SUCCESS)
+		*ret_fh = (struct tee_file_handle *)fh;
+	else
+		free(fh);
+
+	mutex_unlock(&rpmb_mutex);
+	return res;
+}
+
+static void rpmb_fs_close(struct tee_file_handle **tfh)
+{
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)*tfh;
+
+	free(fh);
+	*tfh = NULL;
+}
+
+static TEE_Result rpmb_fs_read(struct tee_file_handle *tfh, void *buf,
+			       size_t *len)
+{
+	TEE_Result res;
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	size_t size = *len;
+
+	if (!size)
+		return TEE_SUCCESS;
+
+	mutex_lock(&rpmb_mutex);
+
+	dump_fh(fh);
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (fh->pos >= fh->fat_entry.data_size) {
+		*len = 0;
+		goto out;
+	}
+
+	size = MIN(size, fh->fat_entry.data_size - fh->pos);
+	if (size) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
+				    fh->fat_entry.start_address + fh->pos, buf,
+				    size, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+	*len = size;
+	fh->pos += size;
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	return res;
+}
+
+static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, const void *buf,
+				size_t size)
+{
+	TEE_Result res;
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	tee_mm_pool_t p;
+	bool pool_result = false;
+	tee_mm_entry_t *mm;
+	size_t end;
+	size_t newsize;
+	uint8_t *newbuf = NULL;
+	uintptr_t newaddr;
+	uint32_t start_addr;
+
+	if (!size)
+		return TEE_SUCCESS;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (!fs_par) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	dump_fh(fh);
+
+	/* Upper memory allocation must be used for RPMB_FS. */
+	pool_result = tee_mm_init(&p,
+				  RPMB_STORAGE_START_ADDRESS,
+				  fs_par->max_rpmb_address,
+				  RPMB_BLOCK_SIZE_SHIFT,
+				  TEE_MM_POOL_HI_ALLOC);
+	if (!pool_result) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = read_fat(fh, &p);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (fh->fat_entry.flags & FILE_IS_LAST_ENTRY)
+		panic("invalid last entry flag");
+
+	end = fh->pos + size;
+	start_addr = fh->fat_entry.start_address + fh->pos;
+
+	if (end <= fh->fat_entry.data_size &&
+	    tee_rpmb_write_is_atomic(CFG_RPMB_FS_DEV_ID, start_addr, size)) {
+
+		DMSG("Updating data in-place");
+		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, start_addr, buf,
+				     size, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+	} else {
+		/*
+		 * File must be extended, or update cannot be atomic: allocate,
+		 * read, update, write.
+		 */
+
+		DMSG("Need to re-allocate");
+		newsize = MAX(end, fh->fat_entry.data_size);
+		mm = tee_mm_alloc(&p, newsize);
+		newbuf = calloc(newsize, 1);
+		if (!mm || !newbuf) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		if (fh->fat_entry.data_size) {
+			res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
+					    fh->fat_entry.start_address,
+					    newbuf, fh->fat_entry.data_size,
+					    fh->fat_entry.fek);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+
+		memcpy(newbuf + fh->pos, buf, size);
+
+		newaddr = tee_mm_get_smem(mm);
+		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
+				     newsize, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		fh->fat_entry.data_size = newsize;
+		fh->fat_entry.start_address = newaddr;
+		res = write_fat_entry(fh, true);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	fh->pos += size;
+out:
+	mutex_unlock(&rpmb_mutex);
+	if (pool_result)
+		tee_mm_final(&p);
+	if (newbuf)
+		free(newbuf);
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_seek(struct tee_file_handle *tfh, int32_t offset,
+			       TEE_Whence whence, int32_t *new_offs)
+
+{
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	TEE_Result res;
+	tee_fs_off_t new_pos;
+
+	mutex_lock(&rpmb_mutex);
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		new_pos = offset;
+		break;
+
+	case TEE_DATA_SEEK_CUR:
+		new_pos = fh->pos + offset;
+		break;
+
+	case TEE_DATA_SEEK_END:
+		new_pos = fh->fat_entry.data_size + offset;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (new_pos < 0)
+		new_pos = 0;
+
+	if (new_pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh->pos = new_pos;
+	if (new_offs)
+		*new_offs = new_pos;
+out:
+	mutex_unlock(&rpmb_mutex);
+	return res;
+}
+
+static TEE_Result rpmb_fs_remove(const char *filename)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_file_handle *fh = NULL;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (!filename || strlen(filename) >= TEE_RPMB_FS_FILENAME_LENGTH - 1) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh = alloc_file_handle(filename);
+	if (!fh) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Clear this file entry. */
+	memset(&fh->fat_entry, 0, sizeof(struct rpmb_fat_entry));
+	res = write_fat_entry(fh, false);
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	free(fh);
+	return res;
+}
+
+static  TEE_Result rpmb_fs_rename(const char *old_name, const char *new_name,
+				  bool overwrite)
+{
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct rpmb_file_handle *fh_old = NULL;
+	struct rpmb_file_handle *fh_new = NULL;
+	uint32_t old_len;
+	uint32_t new_len;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (!old_name || !new_name) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	old_len = strlen(old_name);
+	new_len = strlen(new_name);
+
+	if ((old_len >= TEE_RPMB_FS_FILENAME_LENGTH - 1) ||
+	    (new_len >= TEE_RPMB_FS_FILENAME_LENGTH - 1) || (new_len == 0)) {
+
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	fh_old = alloc_file_handle(old_name);
+	if (!fh_old) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	fh_new = alloc_file_handle(new_name);
+	if (!fh_new) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	res = read_fat(fh_old, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = read_fat(fh_new, NULL);
+	if (res == TEE_SUCCESS) {
+		if (!overwrite) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		/* Clear this file entry. */
+		memset(&fh_new->fat_entry, 0, sizeof(struct rpmb_fat_entry));
+		res = write_fat_entry(fh_new, false);
+		if (res != TEE_SUCCESS)
+			goto out;
+	}
+
+	memset(fh_old->fat_entry.filename, 0, TEE_RPMB_FS_FILENAME_LENGTH);
+	memcpy(fh_old->fat_entry.filename, new_name, new_len);
+
+	res = write_fat_entry(fh_old, false);
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	free(fh_old);
+	free(fh_new);
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_truncate(struct tee_file_handle *tfh, size_t length)
+{
+	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
+	tee_mm_pool_t p;
+	bool pool_result = false;
+	tee_mm_entry_t *mm;
+	uint32_t newsize;
+	uint8_t *newbuf = NULL;
+	uintptr_t newaddr;
+	TEE_Result res = TEE_ERROR_GENERIC;
+
+	mutex_lock(&rpmb_mutex);
+
+	if (length > INT32_MAX) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+	newsize = length;
+
+	res = read_fat(fh, NULL);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (newsize > fh->fat_entry.data_size) {
+		/* Extend file */
+
+		pool_result = tee_mm_init(&p,
+					  RPMB_STORAGE_START_ADDRESS,
+					  fs_par->max_rpmb_address,
+					  RPMB_BLOCK_SIZE_SHIFT,
+					  TEE_MM_POOL_HI_ALLOC);
+		if (!pool_result) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+		res = read_fat(fh, &p);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		mm = tee_mm_alloc(&p, newsize);
+		newbuf = calloc(newsize, 1);
+		if (!mm || !newbuf) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto out;
+		}
+
+		if (fh->fat_entry.data_size) {
+			res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
+					    fh->fat_entry.start_address,
+					    newbuf, fh->fat_entry.data_size,
+					    fh->fat_entry.fek);
+			if (res != TEE_SUCCESS)
+				goto out;
+		}
+
+		newaddr = tee_mm_get_smem(mm);
+		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
+				     newsize, fh->fat_entry.fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+	} else {
+		/* Don't change file location */
+		newaddr = fh->fat_entry.start_address;
+	}
+
+	/* fh->pos is unchanged */
+	fh->fat_entry.data_size = newsize;
+	fh->fat_entry.start_address = newaddr;
+	res = write_fat_entry(fh, true);
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	if (pool_result)
+		tee_mm_final(&p);
+	if (newbuf)
+		free(newbuf);
+
+	return res;
+}
+
+static void rpmb_fs_dir_free(struct tee_fs_dir *dir)
+{
+	struct tee_rpmb_fs_dirent *e;
+
+	if (!dir)
+		return;
+
+	free(dir->current);
+
+	while ((e = SIMPLEQ_FIRST(&dir->next))) {
+		SIMPLEQ_REMOVE_HEAD(&dir->next, link);
+		free(e);
+	}
+}
+
+static TEE_Result rpmb_fs_dir_populate(const char *path,
+				       struct tee_fs_dir *dir)
+{
+	struct tee_rpmb_fs_dirent *current = NULL;
+	struct rpmb_fat_entry *fat_entries = NULL;
+	uint32_t fat_address;
+	uint32_t filelen;
+	char *filename;
+	int i;
+	bool last_entry_found = false;
+	bool matched;
+	struct tee_rpmb_fs_dirent *next = NULL;
+	uint32_t pathlen;
+	TEE_Result res = TEE_ERROR_GENERIC;
+	uint32_t size;
+	char temp;
+
+	mutex_lock(&rpmb_mutex);
+
+	res = rpmb_fs_setup();
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = get_fat_start_address(&fat_address);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	size = N_ENTRIES * sizeof(struct rpmb_fat_entry);
+	fat_entries = malloc(size);
+	if (!fat_entries) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	pathlen = strlen(path);
+	while (!last_entry_found) {
+		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID, fat_address,
+				    (uint8_t *)fat_entries, size, NULL);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		for (i = 0; i < N_ENTRIES; i++) {
+			filename = fat_entries[i].filename;
+			if (fat_entries[i].flags & FILE_IS_ACTIVE) {
+				matched = false;
+				filelen = strlen(filename);
+				if (filelen > pathlen) {
+					temp = filename[pathlen];
+					filename[pathlen] = '\0';
+					if (strcmp(filename, path) == 0)
+						matched = true;
+
+					filename[pathlen] = temp;
+				}
+
+				if (matched) {
+					next = malloc(sizeof(*next));
+					if (!next) {
+						res = TEE_ERROR_OUT_OF_MEMORY;
+						goto out;
+					}
+
+					memset(next, 0, sizeof(*next));
+					next->entry.d_name = next->name;
+					memcpy(next->name,
+						&filename[pathlen],
+						filelen - pathlen);
+
+					SIMPLEQ_INSERT_TAIL(&dir->next, next,
+							    link);
+					current = next;
+				}
+			}
+
+			if (fat_entries[i].flags & FILE_IS_LAST_ENTRY) {
+				last_entry_found = true;
+				break;
+			}
+
+			/* Move to next fat_entry. */
+			fat_address += sizeof(struct rpmb_fat_entry);
+		}
+	}
+
+	if (current)
+		res = TEE_SUCCESS;
+	else
+		res = TEE_ERROR_ITEM_NOT_FOUND; /* No directories were found. */
+
+out:
+	mutex_unlock(&rpmb_mutex);
+	if (res != TEE_SUCCESS)
+		rpmb_fs_dir_free(dir);
+	if (fat_entries)
+		free(fat_entries);
+
+	return res;
+}
+
+static TEE_Result rpmb_fs_opendir(const char *path, struct tee_fs_dir **dir)
+{
+	uint32_t len;
+	uint32_t max_size;
+	char path_local[TEE_RPMB_FS_FILENAME_LENGTH];
+	TEE_Result res = TEE_ERROR_GENERIC;
+	struct tee_fs_dir *rpmb_dir = NULL;
+
+	if (!path || !dir) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	/*
+	 * There must be room for at least the NULL char and a char for the
+	 * filename after the path.
+	 */
+	max_size = TEE_RPMB_FS_FILENAME_LENGTH - 2;
+	len = strlen(path);
+	if (len > max_size || len == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	memset(path_local, 0, sizeof(path_local));
+	memcpy(path_local, path, len);
+
+	/* Add a slash to correctly match the full directory name. */
+	if (path_local[len - 1] != '/')
+		path_local[len] = '/';
+
+	rpmb_dir = calloc(1, sizeof(*rpmb_dir));
+	if (!rpmb_dir) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+	SIMPLEQ_INIT(&rpmb_dir->next);
+
+	res = rpmb_fs_dir_populate(path_local, rpmb_dir);
+	if (res != TEE_SUCCESS) {
+		free(rpmb_dir);
+		rpmb_dir = NULL;
+		goto out;
+	}
+
+	*dir = rpmb_dir;
+
+out:
+	return res;
+}
+
+static TEE_Result rpmb_fs_readdir(struct tee_fs_dir *dir,
+				  struct tee_fs_dirent **ent)
+{
+	if (!dir)
+		return TEE_ERROR_GENERIC;
+
+	free(dir->current);
+
+	dir->current = SIMPLEQ_FIRST(&dir->next);
+	if (!dir->current)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	SIMPLEQ_REMOVE_HEAD(&dir->next, link);
+
+	*ent = &dir->current->entry;
+	return TEE_SUCCESS;
+}
+
+static void rpmb_fs_closedir(struct tee_fs_dir *dir)
+{
+	if (dir) {
+		rpmb_fs_dir_free(dir);
+		free(dir);
+	}
+}
+
+static TEE_Result rpmb_fs_open(const char *file, struct tee_file_handle **fh)
+{
+	return rpmb_fs_open_internal(file, false, fh);
+}
+
+static TEE_Result rpmb_fs_create(const char *file, struct tee_file_handle **fh)
+{
+	return rpmb_fs_open_internal(file, true, fh);
+}
+
+const struct tee_file_operations rpmb_fs_ops = {
+	.open = rpmb_fs_open,
+	.create = rpmb_fs_create,
+	.close = rpmb_fs_close,
+	.read = rpmb_fs_read,
+	.write = rpmb_fs_write,
+	.seek = rpmb_fs_seek,
+	.truncate = rpmb_fs_truncate,
+	.rename = rpmb_fs_rename,
+	.remove = rpmb_fs_remove,
+	.opendir = rpmb_fs_opendir,
+	.closedir = rpmb_fs_closedir,
+	.readdir = rpmb_fs_readdir,
+};
diff --git a/core/tee/tee_sql_fs.c b/core/tee/tee_sql_fs.c
new file mode 100644
index 0000000..e38e1bc
--- /dev/null
+++ b/core/tee/tee_sql_fs.c
@@ -0,0 +1,610 @@
+/*
+ * 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.
+ */
+
+/*
+ * This file implements the tee_file_operations structure for a secure
+ * filesystem based on an SQLite database in normal world.
+ * The atomicity of each operation is ensured by using SQL transactions.
+ * The main purpose of the code below is to perform block encryption and
+ * authentication of the file data, and properly handle seeking through the
+ * file. One file (in the sense of struct tee_file_operations) maps to one
+ * file in the SQL filesystem, and has the following structure:
+ *
+ * [       File meta-data       ][      Block #0        ][Block #1]...
+ * [meta_header|sql_fs_file_meta][block_header|user data][        ]...
+ *
+ * meta_header and block_header are defined in tee_fs_key_manager.h.
+ */
+
+#include <assert.h>
+#include <kernel/mutex.h>
+#include <optee_msg_supplicant.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <string_ext.h>
+#include <sys/queue.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs_key_manager.h>
+#include <tee/tee_fs_rpc.h>
+#include <trace.h>
+#include <utee_defines.h>
+#include <util.h>
+
+/* Block size for encryption */
+#define BLOCK_SHIFT 12
+#define BLOCK_SIZE (1 << BLOCK_SHIFT)
+
+struct sql_fs_file_meta {
+	size_t length;
+};
+
+/* File descriptor */
+struct sql_fs_fd {
+	struct sql_fs_file_meta meta;
+	uint8_t encrypted_fek[TEE_FS_KM_FEK_SIZE];
+	tee_fs_off_t pos;
+	int fd; /* returned by normal world */
+	int flags; /* open flags */
+};
+
+struct tee_fs_dir {
+	int nw_dir;
+	struct tee_fs_dirent d;
+};
+
+static struct mutex sql_fs_mutex = MUTEX_INITIALIZER;
+
+/*
+ * Interface with tee-supplicant
+ */
+
+static TEE_Result sql_fs_begin_transaction_rpc(void)
+{
+	return tee_fs_rpc_begin_transaction(OPTEE_MSG_RPC_CMD_SQL_FS);
+}
+
+static TEE_Result sql_fs_end_transaction_rpc(bool rollback)
+{
+	return tee_fs_rpc_end_transaction(OPTEE_MSG_RPC_CMD_SQL_FS,
+					     rollback);
+}
+
+static TEE_Result sql_fs_opendir_rpc(const char *name, struct tee_fs_dir **d)
+{
+	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_SQL_FS, name, d);
+}
+
+static TEE_Result sql_fs_readdir_rpc(struct tee_fs_dir *d,
+				     struct tee_fs_dirent **ent)
+{
+	return tee_fs_rpc_readdir(OPTEE_MSG_RPC_CMD_SQL_FS, d, ent);
+}
+
+static TEE_Result sql_fs_remove_rpc(const char *file)
+{
+	return tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, file);
+}
+
+static TEE_Result sql_fs_rename_rpc(const char *old, const char *nw,
+				    bool overwrite)
+{
+	return tee_fs_rpc_rename(OPTEE_MSG_RPC_CMD_SQL_FS, old, nw, overwrite);
+}
+
+static void sql_fs_closedir_rpc(struct tee_fs_dir *d)
+{
+	if (d)
+		tee_fs_rpc_closedir(OPTEE_MSG_RPC_CMD_SQL_FS, d);
+}
+
+/*
+ * End of interface with tee-supplicant
+ */
+
+static size_t meta_size(void)
+{
+	return tee_fs_get_header_size(META_FILE) +
+	       sizeof(struct sql_fs_file_meta);
+}
+
+static size_t block_header_size(void)
+{
+	return tee_fs_get_header_size(BLOCK_FILE);
+}
+
+static size_t block_size_raw(void)
+{
+	return block_header_size() + BLOCK_SIZE;
+}
+
+/* Return the block number from a position in the user data */
+static ssize_t block_num(tee_fs_off_t pos)
+{
+	return pos / BLOCK_SIZE;
+}
+
+/* Return the position of a block in the DB file */
+static ssize_t block_pos_raw(size_t block_num)
+{
+	return meta_size() + block_num * block_size_raw();
+}
+
+static TEE_Result write_meta(struct sql_fs_fd *fdp)
+{
+	TEE_Result res;
+	size_t ct_size = meta_size();
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, 0,
+				    ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+
+
+	res = tee_fs_encrypt_file(META_FILE,
+				  (const uint8_t *)&fdp->meta,
+				  sizeof(fdp->meta), ct, &ct_size,
+				  fdp->encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+static TEE_Result create_meta(struct sql_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+
+	memset(&fdp->meta, 0, sizeof(fdp->meta));
+
+	res = tee_fs_generate_fek(fdp->encrypted_fek, TEE_FS_KM_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_create(OPTEE_MSG_RPC_CMD_SQL_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return write_meta(fdp);
+}
+
+static TEE_Result read_meta(struct sql_fs_fd *fdp, const char *fname)
+{
+	TEE_Result res;
+	size_t msize = meta_size();
+	size_t out_size = sizeof(fdp->meta);
+	void *meta;
+	size_t bytes;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_open(OPTEE_MSG_RPC_CMD_SQL_FS, fname, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, 0,
+				   msize, &meta);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_decrypt_file(META_FILE, meta, msize,
+				   (uint8_t *)&fdp->meta, &out_size,
+				   fdp->encrypted_fek);
+}
+
+/*
+ * Read one block of user data.
+ * Returns:
+ *  < 0: read error
+ *    0: block does not exist (reading past last block)
+ *  > 0: success
+ */
+static TEE_Result read_block(struct sql_fs_fd *fdp, size_t bnum, uint8_t *data)
+{
+	TEE_Result res;
+	size_t ct_size = block_size_raw();
+	size_t out_size = BLOCK_SIZE;
+	ssize_t pos = block_pos_raw(bnum);
+	size_t bytes;
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_read_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, pos,
+				   ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+	res = tee_fs_rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!bytes)
+		return TEE_SUCCESS; /* Block does not exist */
+
+	return tee_fs_decrypt_file(BLOCK_FILE, ct, bytes, data,
+				   &out_size, fdp->encrypted_fek);
+}
+
+/* Write one block of user data */
+static TEE_Result write_block(struct sql_fs_fd *fdp, size_t bnum, uint8_t *data)
+{
+	TEE_Result res;
+	size_t ct_size = block_size_raw();
+	ssize_t pos = block_pos_raw(bnum);
+	void *ct;
+	struct tee_fs_rpc_operation op;
+
+	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd, pos,
+				    ct_size, &ct);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_fs_encrypt_file(BLOCK_FILE, data, BLOCK_SIZE, ct,
+				  &ct_size, fdp->encrypted_fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_fs_rpc_write_final(&op);
+}
+
+/*
+ * Partial write (< BLOCK_SIZE) into a block: read/update/write
+ * To save memory, passing data == NULL is equivalent to passing a buffer
+ * filled with zeroes.
+ */
+static TEE_Result write_block_partial(struct sql_fs_fd *fdp, size_t bnum,
+				      const uint8_t *data, size_t len,
+				      size_t offset)
+{
+	TEE_Result res;
+	size_t buf_size = BLOCK_SIZE;
+	uint8_t *buf = NULL;
+
+	if ((offset >= buf_size) || (offset + len > buf_size))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	buf = malloc(buf_size);
+	if (!buf)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = read_block(fdp, bnum, buf);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (data)
+		memcpy(buf + offset, data, len);
+	else
+		memset(buf + offset, 0, len);
+
+	res = write_block(fdp, bnum, buf);
+exit:
+	free(buf);
+	return res;
+}
+
+static TEE_Result sql_fs_ftruncate_internal(struct sql_fs_fd *fdp,
+					    tee_fs_off_t new_length)
+{
+	TEE_Result res;
+	tee_fs_off_t old_length;
+
+	old_length = (tee_fs_off_t)fdp->meta.length;
+
+	if (new_length == old_length)
+		return TEE_SUCCESS;
+
+	sql_fs_begin_transaction_rpc();
+
+	if (new_length < old_length) {
+		/* Trim unused blocks */
+		int old_last_block = block_num(old_length);
+		int last_block = block_num(new_length);
+		tee_fs_off_t off;
+
+		if (last_block < old_last_block) {
+			off = block_pos_raw(last_block);
+			res = tee_fs_rpc_truncate(OPTEE_MSG_RPC_CMD_SQL_FS,
+						  fdp->fd, off);
+			if (res != TEE_SUCCESS)
+				goto exit;
+		}
+	} else {
+		/* Extend file with zeroes */
+		tee_fs_off_t off = old_length % BLOCK_SIZE;
+		size_t bnum = block_num(old_length);
+		size_t end_bnum = block_num(new_length);
+
+		while (bnum <= end_bnum) {
+			size_t len = (size_t)BLOCK_SIZE - (size_t)off;
+
+			res = write_block_partial(fdp, bnum, NULL, len, off);
+			if (res != TEE_SUCCESS)
+				goto exit;
+			off = 0;
+			bnum++;
+		}
+	}
+
+	fdp->meta.length = new_length;
+	res = write_meta(fdp);
+exit:
+	sql_fs_end_transaction_rpc(res != TEE_SUCCESS);
+	return res;
+}
+
+static TEE_Result sql_fs_seek(struct tee_file_handle *fh, int32_t offset,
+			      TEE_Whence whence, int32_t *new_offs)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	tee_fs_off_t pos;
+
+	mutex_lock(&sql_fs_mutex);
+
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		pos = offset;
+		break;
+
+	case TEE_DATA_SEEK_CUR:
+		pos = fdp->pos + offset;
+		break;
+
+	case TEE_DATA_SEEK_END:
+		pos = fdp->meta.length + offset;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit_ret;
+	}
+
+	if (pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit_ret;
+	}
+
+	if (pos < 0)
+		pos = 0;
+
+	fdp->pos = pos;
+	if (new_offs)
+		*new_offs = pos;
+	res = TEE_SUCCESS;
+exit_ret:
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static void sql_fs_close(struct tee_file_handle **fh)
+{
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)*fh;
+
+	if (fdp) {
+	tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd);
+		free(fdp);
+		*fh = NULL;
+	}
+}
+
+static TEE_Result open_internal(const char *file, bool create,
+				struct tee_file_handle **fh)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp;
+	bool created = false;
+
+	fdp = calloc(1, sizeof(*fdp));
+	if (!fdp)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	fdp->fd = -1;
+
+	mutex_lock(&sql_fs_mutex);
+
+	if (create)
+		res = create_meta(fdp, file);
+	else
+		res = read_meta(fdp, file);
+
+	if (res == TEE_SUCCESS) {
+		*fh = (struct tee_file_handle *)fdp;
+	} else {
+		if (fdp && fdp->fd != -1)
+			tee_fs_rpc_close(OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd);
+		if (created)
+			tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, file);
+		free(fdp);
+	}
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static TEE_Result sql_fs_open(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, false, fh);
+}
+
+static TEE_Result sql_fs_create(const char *file, struct tee_file_handle **fh)
+{
+	return open_internal(file, true, fh);
+}
+
+
+static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
+			      size_t *len)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	size_t remain_bytes = *len;
+	uint8_t *data_ptr = buf;
+	uint8_t *block = NULL;
+	int start_block_num;
+	int end_block_num;
+
+	mutex_lock(&sql_fs_mutex);
+
+	if ((fdp->pos + remain_bytes) < remain_bytes ||
+	    fdp->pos > (tee_fs_off_t)fdp->meta.length)
+		remain_bytes = 0;
+	else if (fdp->pos + remain_bytes > fdp->meta.length)
+		remain_bytes = fdp->meta.length - fdp->pos;
+
+	*len = remain_bytes;
+
+	if (!remain_bytes) {
+		res = TEE_SUCCESS;
+		goto exit;
+	}
+
+	start_block_num = block_num(fdp->pos);
+	end_block_num = block_num(fdp->pos + remain_bytes - 1);
+
+	block = malloc(BLOCK_SIZE);
+	if (!block) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	while (start_block_num <= end_block_num) {
+		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_read = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_read + offset > BLOCK_SIZE)
+			size_to_read = BLOCK_SIZE - offset;
+
+		/*
+		 * REVISIT: implement read_block_partial() since we have
+		 * write_block_partial()?
+		 */
+		res = read_block(fdp, start_block_num, block);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		memcpy(data_ptr, block + offset, size_to_read);
+
+		data_ptr += size_to_read;
+		remain_bytes -= size_to_read;
+		fdp->pos += size_to_read;
+
+		start_block_num++;
+	}
+	res = TEE_SUCCESS;
+exit:
+	free(block);
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static TEE_Result sql_fs_write(struct tee_file_handle *fh, const void *buf,
+			       size_t len)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	size_t remain_bytes = len;
+	const uint8_t *data_ptr = buf;
+	int start_block_num;
+	int end_block_num;
+
+	if (!len)
+		return TEE_SUCCESS;
+
+	mutex_lock(&sql_fs_mutex);
+
+	sql_fs_begin_transaction_rpc();
+
+	if (fdp->meta.length < (size_t)fdp->pos) {
+		/* Fill hole */
+		res = sql_fs_ftruncate_internal(fdp, fdp->pos);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	start_block_num = block_num(fdp->pos);
+	end_block_num = block_num(fdp->pos + len - 1);
+
+	while (start_block_num <= end_block_num) {
+		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t size_to_write = MIN(remain_bytes, (size_t)BLOCK_SIZE);
+
+		if (size_to_write + offset > BLOCK_SIZE)
+			size_to_write = BLOCK_SIZE - offset;
+
+		res = write_block_partial(fdp, start_block_num, data_ptr,
+					  size_to_write, offset);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		data_ptr += size_to_write;
+		remain_bytes -= size_to_write;
+		fdp->pos += size_to_write;
+
+		start_block_num++;
+	}
+
+	if (fdp->meta.length < (size_t)fdp->pos) {
+		fdp->meta.length = fdp->pos;
+		res = write_meta(fdp);
+	}
+exit:
+	sql_fs_end_transaction_rpc(res != TEE_SUCCESS);
+	mutex_unlock(&sql_fs_mutex);
+	return res;
+}
+
+static TEE_Result sql_fs_truncate(struct tee_file_handle *fh, size_t len)
+{
+	TEE_Result res;
+	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+
+	mutex_lock(&sql_fs_mutex);
+	res = sql_fs_ftruncate_internal(fdp, len);
+	mutex_unlock(&sql_fs_mutex);
+
+	return res;
+}
+
+const struct tee_file_operations sql_fs_ops = {
+	.open = sql_fs_open,
+	.create = sql_fs_create,
+	.close = sql_fs_close,
+	.read = sql_fs_read,
+	.write = sql_fs_write,
+	.seek = sql_fs_seek,
+	.truncate = sql_fs_truncate,
+
+	.opendir = sql_fs_opendir_rpc,
+	.closedir = sql_fs_closedir_rpc,
+	.readdir = sql_fs_readdir_rpc,
+	.rename = sql_fs_rename_rpc,
+	.remove = sql_fs_remove_rpc,
+};
diff --git a/core/tee/tee_svc.c b/core/tee/tee_svc.c
new file mode 100644
index 0000000..0c90443
--- /dev/null
+++ b/core/tee/tee_svc.c
@@ -0,0 +1,1111 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <util.h>
+#include <kernel/tee_common_otp.h>
+#include <kernel/tee_common.h>
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <utee_types.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_cryp_utl.h>
+#include <mm/tee_mmu.h>
+#include <mm/tee_mm.h>
+#include <mm/core_memprot.h>
+#include <kernel/tee_time.h>
+
+#include <user_ta_header.h>
+#include <trace.h>
+#include <kernel/trace_ta.h>
+#include <kernel/chip_services.h>
+#include <kernel/pseudo_ta.h>
+#include <mm/mobj.h>
+
+vaddr_t tee_svc_uref_base;
+
+void syscall_log(const void *buf __maybe_unused, size_t len __maybe_unused)
+{
+#ifdef CFG_TEE_CORE_TA_TRACE
+	char *kbuf;
+
+	if (len == 0)
+		return;
+
+	kbuf = malloc(len + 1);
+	if (kbuf == NULL)
+		return;
+
+	if (tee_svc_copy_from_user(kbuf, buf, len) == TEE_SUCCESS) {
+		kbuf[len] = '\0';
+		trace_ext_puts(kbuf);
+	}
+
+	free(kbuf);
+#endif
+}
+
+TEE_Result syscall_not_supported(void)
+{
+	return TEE_ERROR_NOT_SUPPORTED;
+}
+
+/* Configuration properties */
+/* API implementation version */
+static const char api_vers[] = TO_STR(CFG_TEE_API_VERSION);
+
+/* Implementation description (implementation-dependent) */
+static const char descr[] = TO_STR(CFG_TEE_IMPL_DESCR);
+
+/*
+ * TA persistent time protection level
+ * 100: Persistent time based on an REE-controlled real-time clock
+ * and on the TEE Trusted Storage for the storage of origins (default).
+ * 1000: Persistent time based on a TEE-controlled real-time clock
+ * and the TEE Trusted Storage.
+ * The real-time clock MUST be out of reach of software attacks
+ * from the REE.
+ */
+static const uint32_t ta_time_prot_lvl = 100;
+
+/* Elliptic Curve Cryptographic support */
+#ifdef CFG_CRYPTO_ECC
+static const uint32_t crypto_ecc_en = 1;
+#else
+static const uint32_t crypto_ecc_en;
+#endif
+
+/*
+ * Trusted storage anti rollback protection level
+ * 0 (or missing): No antirollback protection (default)
+ * 100: Antirollback enforced at REE level
+ * 1000: Antirollback TEE-controlled hardware
+ */
+static const uint32_t ts_antiroll_prot_lvl;
+
+/* Trusted OS implementation version */
+static const char trustedos_impl_version[] = TO_STR(TEE_IMPL_VERSION);
+
+/* Trusted OS implementation version (binary value) */
+static const uint32_t trustedos_impl_bin_version; /* 0 by default */
+
+/* Trusted OS implementation manufacturer name */
+static const char trustedos_manufacturer[] = TO_STR(CFG_TEE_MANUFACTURER);
+
+/* Trusted firmware version */
+static const char fw_impl_version[] = TO_STR(CFG_TEE_FW_IMPL_VERSION);
+
+/* Trusted firmware version (binary value) */
+static const uint32_t fw_impl_bin_version; /* 0 by default */
+
+/* Trusted firmware manufacturer name */
+static const char fw_manufacturer[] = TO_STR(CFG_TEE_FW_MANUFACTURER);
+
+static TEE_Result get_prop_tee_dev_id(struct tee_ta_session *sess __unused,
+				      void *buf, size_t *blen)
+{
+	TEE_Result res;
+	TEE_UUID uuid;
+	const size_t nslen = 5;
+	uint8_t data[5 + FVR_DIE_ID_NUM_REGS * sizeof(uint32_t)] = {
+	    'O', 'P', 'T', 'E', 'E' };
+
+	if (*blen < sizeof(uuid)) {
+		*blen = sizeof(uuid);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(uuid);
+
+	if (tee_otp_get_die_id(data + nslen, sizeof(data) - nslen))
+		return TEE_ERROR_BAD_STATE;
+
+	res = tee_hash_createdigest(TEE_ALG_SHA256, data, sizeof(data),
+				    (uint8_t *)&uuid, sizeof(uuid));
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_BAD_STATE;
+
+	/*
+	 * Changes the random value into and UUID as specifiec
+	 * in RFC 4122. The magic values are from the example
+	 * code in the RFC.
+	 *
+	 * TEE_UUID is defined slightly different from the RFC,
+	 * but close enough for our purpose.
+	 */
+
+	uuid.timeHiAndVersion &= 0x0fff;
+	uuid.timeHiAndVersion |= 5 << 12;
+
+	/* uuid.clock_seq_hi_and_reserved in the RFC */
+	uuid.clockSeqAndNode[0] &= 0x3f;
+	uuid.clockSeqAndNode[0] |= 0x80;
+
+	return tee_svc_copy_to_user(buf, &uuid, sizeof(TEE_UUID));
+}
+
+static TEE_Result get_prop_tee_sys_time_prot_level(
+			struct tee_ta_session *sess __unused,
+			void *buf, size_t *blen)
+{
+	uint32_t prot;
+
+	if (*blen < sizeof(prot)) {
+		*blen = sizeof(prot);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(prot);
+	prot = tee_time_get_sys_time_protection_level();
+	return tee_svc_copy_to_user(buf, &prot, sizeof(prot));
+}
+
+static TEE_Result get_prop_client_id(struct tee_ta_session *sess __unused,
+				     void *buf, size_t *blen)
+{
+	if (*blen < sizeof(TEE_Identity)) {
+		*blen = sizeof(TEE_Identity);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(TEE_Identity);
+	return tee_svc_copy_to_user(buf, &sess->clnt_id, sizeof(TEE_Identity));
+}
+
+static TEE_Result get_prop_ta_app_id(struct tee_ta_session *sess,
+				     void *buf, size_t *blen)
+{
+	if (*blen < sizeof(TEE_UUID)) {
+		*blen = sizeof(TEE_UUID);
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+	*blen = sizeof(TEE_UUID);
+	return tee_svc_copy_to_user(buf, &sess->ctx->uuid, sizeof(TEE_UUID));
+}
+
+/* Properties of the set TEE_PROPSET_CURRENT_CLIENT */
+const struct tee_props tee_propset_client[] = {
+	{
+		.name = "gpd.client.identity",
+		.prop_type = USER_TA_PROP_TYPE_IDENTITY,
+		.get_prop_func = get_prop_client_id
+	},
+};
+
+/* Properties of the set TEE_PROPSET_CURRENT_TA */
+const struct tee_props tee_propset_ta[] = {
+	{
+		.name = "gpd.ta.appID",
+		.prop_type = USER_TA_PROP_TYPE_UUID,
+		.get_prop_func = get_prop_ta_app_id
+	},
+
+	/*
+	 * Following properties are processed directly in libutee:
+	 *	TA_PROP_STR_SINGLE_INSTANCE
+	 *	TA_PROP_STR_MULTI_SESSION
+	 *	TA_PROP_STR_KEEP_ALIVE
+	 *	TA_PROP_STR_DATA_SIZE
+	 *	TA_PROP_STR_STACK_SIZE
+	 *	TA_PROP_STR_VERSION
+	 *	TA_PROP_STR_DESCRIPTION
+	 *	USER_TA_PROP_TYPE_STRING,
+	 *	TA_DESCRIPTION
+	 */
+};
+
+/* Properties of the set TEE_PROPSET_TEE_IMPLEMENTATION */
+const struct tee_props tee_propset_tee[] = {
+	{
+		.name = "gpd.tee.apiversion",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = api_vers,
+		.len = sizeof(api_vers),
+	},
+	{
+		.name = "gpd.tee.description",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = descr, .len = sizeof(descr)
+	},
+	{
+		.name = "gpd.tee.deviceID",
+		.prop_type = USER_TA_PROP_TYPE_UUID,
+		.get_prop_func = get_prop_tee_dev_id
+	},
+	{
+		.name = "gpd.tee.systemTime.protectionLevel",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.get_prop_func = get_prop_tee_sys_time_prot_level
+	},
+	{
+		.name = "gpd.tee.TAPersistentTime.protectionLevel",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &ta_time_prot_lvl,
+		.len = sizeof(ta_time_prot_lvl)
+	},
+	{
+		.name = "gpd.tee.cryptography.ecc",
+		.prop_type = USER_TA_PROP_TYPE_BOOL,
+		.data = &crypto_ecc_en,
+		.len = sizeof(crypto_ecc_en)
+	},
+	{
+		.name = "gpd.tee.trustedStorage.antiRollback.protectionLevel",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &ts_antiroll_prot_lvl,
+		.len = sizeof(ts_antiroll_prot_lvl)
+	},
+	{
+		.name = "gpd.tee.trustedos.implementation.version",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = trustedos_impl_version,
+		.len = sizeof(trustedos_impl_version)
+	},
+	{
+		.name = "gpd.tee.trustedos.implementation.binaryversion",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &trustedos_impl_bin_version,
+		.len = sizeof(trustedos_impl_bin_version)
+	},
+	{
+		.name = "gpd.tee.trustedos.manufacturer",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = trustedos_manufacturer,
+		.len = sizeof(trustedos_manufacturer)
+	},
+	{
+		.name = "gpd.tee.firmware.implementation.version",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = fw_impl_version,
+		.len = sizeof(fw_impl_version)
+	},
+	{
+		.name = "gpd.tee.firmware.implementation.binaryversion",
+		.prop_type = USER_TA_PROP_TYPE_U32,
+		.data = &fw_impl_bin_version,
+		.len = sizeof(fw_impl_bin_version)
+	},
+	{
+		.name = "gpd.tee.firmware.manufacturer",
+		.prop_type = USER_TA_PROP_TYPE_STRING,
+		.data = fw_manufacturer,
+		.len = sizeof(fw_manufacturer)
+	},
+
+	/*
+	 * Following properties are processed directly in libutee:
+	 *	gpd.tee.arith.maxBigIntSize
+	 */
+};
+
+__weak const struct tee_vendor_props vendor_props_client;
+__weak const struct tee_vendor_props vendor_props_ta;
+__weak const struct tee_vendor_props vendor_props_tee;
+
+static void get_prop_set(unsigned long prop_set,
+			 const struct tee_props **props,
+			 size_t *size,
+			 const struct tee_props **vendor_props,
+			 size_t *vendor_size)
+{
+	if ((TEE_PropSetHandle)prop_set == TEE_PROPSET_CURRENT_CLIENT) {
+		*props = tee_propset_client;
+		*size = ARRAY_SIZE(tee_propset_client);
+		*vendor_props = vendor_props_client.props;
+		*vendor_size = vendor_props_client.len;
+	} else if ((TEE_PropSetHandle)prop_set == TEE_PROPSET_CURRENT_TA) {
+		*props = tee_propset_ta;
+		*size = ARRAY_SIZE(tee_propset_ta);
+		*vendor_props = vendor_props_ta.props;
+		*vendor_size = vendor_props_ta.len;
+	} else if ((TEE_PropSetHandle)prop_set ==
+		   TEE_PROPSET_TEE_IMPLEMENTATION) {
+		*props = tee_propset_tee;
+		*size = ARRAY_SIZE(tee_propset_tee);
+		*vendor_props = vendor_props_tee.props;
+		*vendor_size = vendor_props_tee.len;
+	} else {
+		*props = NULL;
+		*size = 0;
+		*vendor_props = NULL;
+		*vendor_size = 0;
+	}
+}
+
+static const struct tee_props *get_prop_struct(unsigned long prop_set,
+					       unsigned long index)
+{
+	const struct tee_props *props;
+	const struct tee_props *vendor_props;
+	size_t size;
+	size_t vendor_size;
+
+	get_prop_set(prop_set, &props, &size, &vendor_props, &vendor_size);
+
+	if (index < size)
+		return &(props[index]);
+	index -= size;
+
+	if (index < vendor_size)
+		return &(vendor_props[index]);
+
+	return NULL;
+}
+
+/*
+ * prop_set is part of TEE_PROPSET_xxx
+ * index is the index in the Property Set to retrieve
+ * if name is not NULL, the name of "index" property is returned
+ * if buf is not NULL, the property is returned
+ */
+TEE_Result syscall_get_property(unsigned long prop_set,
+				unsigned long index,
+				void *name, uint32_t *name_len,
+				void *buf, uint32_t *blen,
+				uint32_t *prop_type)
+{
+	struct tee_ta_session *sess;
+	TEE_Result res;
+	TEE_Result res2;
+	const struct tee_props *prop;
+	uint32_t klen;
+	size_t klen_size;
+	uint32_t elen;
+
+	prop = get_prop_struct(prop_set, index);
+	if (!prop)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Get the property type */
+	if (prop_type) {
+		res = tee_svc_copy_to_user(prop_type, &prop->prop_type,
+					   sizeof(*prop_type));
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	/* Get the property */
+	if (buf && blen) {
+		res = tee_svc_copy_from_user(&klen, blen, sizeof(klen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (prop->get_prop_func) {
+			klen_size = klen;
+			res = prop->get_prop_func(sess, buf, &klen_size);
+			klen = klen_size;
+			res2 = tee_svc_copy_to_user(blen, &klen, sizeof(*blen));
+		} else {
+			if (klen < prop->len)
+				res = TEE_ERROR_SHORT_BUFFER;
+			else
+				res = tee_svc_copy_to_user(buf, prop->data,
+							   prop->len);
+			res2 = tee_svc_copy_to_user(blen, &prop->len,
+						    sizeof(*blen));
+		}
+		if (res2 != TEE_SUCCESS)
+			return res2;
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	/* Get the property name */
+	if (name && name_len) {
+		res = tee_svc_copy_from_user(&klen, name_len, sizeof(klen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		elen = strlen(prop->name) + 1;
+
+		if (klen < elen)
+			res = TEE_ERROR_SHORT_BUFFER;
+		else
+			res = tee_svc_copy_to_user(name, prop->name, elen);
+		res2 = tee_svc_copy_to_user(name_len, &elen, sizeof(*name_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	return res;
+}
+
+/*
+ * prop_set is part of TEE_PROPSET_xxx
+ */
+TEE_Result syscall_get_property_name_to_index(unsigned long prop_set,
+					      void *name,
+					      unsigned long name_len,
+					      uint32_t *index)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	const struct tee_props *props;
+	size_t size;
+	const struct tee_props *vendor_props;
+	size_t vendor_size;
+	char *kname = 0;
+	uint32_t i;
+
+	get_prop_set(prop_set, &props, &size, &vendor_props, &vendor_size);
+	if (!props)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (!name || !name_len) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	kname = malloc(name_len);
+	if (!kname)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = tee_svc_copy_from_user(kname, name, name_len);
+	if (res != TEE_SUCCESS)
+		goto out;
+	kname[name_len - 1] = 0;
+
+	res = TEE_ERROR_ITEM_NOT_FOUND;
+	for (i = 0; i < size; i++) {
+		if (!strcmp(kname, props[i].name)) {
+			res = tee_svc_copy_to_user(index, &i, sizeof(*index));
+			goto out;
+		}
+	}
+	for (i = size; i < size + vendor_size; i++) {
+		if (!strcmp(kname, vendor_props[i - size].name)) {
+			res = tee_svc_copy_to_user(index, &i, sizeof(*index));
+			goto out;
+		}
+	}
+
+out:
+	free(kname);
+	return res;
+}
+
+static void utee_param_to_param(struct tee_ta_param *p, struct utee_params *up)
+{
+	size_t n;
+	uint32_t types = up->types;
+
+	p->types = types;
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		uintptr_t a = up->vals[n * 2];
+		size_t b = up->vals[n * 2 + 1];
+
+		switch (TEE_PARAM_TYPE_GET(types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			p->u[n].mem.mobj = &mobj_virt;
+			p->u[n].mem.offs = a;
+			p->u[n].mem.size = b;
+			break;
+		case TEE_PARAM_TYPE_VALUE_INPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			p->u[n].val.a = a;
+			p->u[n].val.b = b;
+			break;
+		default:
+			memset(&p->u[n], 0, sizeof(p->u[n]));
+			break;
+		}
+	}
+}
+
+static TEE_Result alloc_temp_sec_mem(size_t size, struct mobj **mobj,
+				     uint8_t **va)
+{
+	/* Allocate section in secure DDR */
+	mutex_lock(&tee_ta_mutex);
+#ifdef CFG_PAGED_USER_TA
+	*mobj = mobj_seccpy_shm_alloc(size);
+#else
+	*mobj = mobj_mm_alloc(mobj_sec_ddr, size, &tee_mm_sec_ddr);
+#endif
+	mutex_unlock(&tee_ta_mutex);
+	if (!*mobj)
+		return TEE_ERROR_GENERIC;
+
+	*va = mobj_get_va(*mobj, 0);
+	return TEE_SUCCESS;
+}
+
+/*
+ * TA invokes some TA with parameter.
+ * If some parameters are memory references:
+ * - either the memref is inside TA private RAM: TA is not allowed to expose
+ *   its private RAM: use a temporary memory buffer and copy the data.
+ * - or the memref is not in the TA private RAM:
+ *   - if the memref was mapped to the TA, TA is allowed to expose it.
+ *   - if so, converts memref virtual address into a physical address.
+ */
+static TEE_Result tee_svc_copy_param(struct tee_ta_session *sess,
+				     struct tee_ta_session *called_sess,
+				     struct utee_params *callee_params,
+				     struct tee_ta_param *param,
+				     void *tmp_buf_va[TEE_NUM_PARAMS],
+				     struct mobj **mobj_tmp)
+{
+	size_t n;
+	TEE_Result res;
+	size_t req_mem = 0;
+	size_t s;
+	uint8_t *dst = 0;
+	bool ta_private_memref[TEE_NUM_PARAMS];
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+	void *va;
+	size_t dst_offs;
+
+	/* fill 'param' input struct with caller params description buffer */
+	if (!callee_params) {
+		memset(param, 0, sizeof(*param));
+	} else {
+		res = tee_mmu_check_access_rights(utc,
+			TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_ANY_OWNER,
+			(uaddr_t)callee_params, sizeof(struct utee_params));
+		if (res != TEE_SUCCESS)
+			return res;
+		utee_param_to_param(param, callee_params);
+	}
+
+	if (called_sess && is_pseudo_ta_ctx(called_sess->ctx)) {
+		/*
+		 * static TA, borrow the mapping of the calling
+		 * during this call.
+		 */
+		return TEE_SUCCESS;
+	}
+
+	/* All mobj in param are of type MOJB_TYPE_VIRT */
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+
+		ta_private_memref[n] = false;
+
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			va = (void *)param->u[n].mem.offs;
+			s = param->u[n].mem.size;
+			if (!va) {
+				if (!s)
+					return TEE_ERROR_BAD_PARAMETERS;
+				break;
+			}
+			/* uTA cannot expose its private memory */
+			if (tee_mmu_is_vbuf_inside_ta_private(utc, va, s)) {
+
+				s = ROUNDUP(s, sizeof(uint32_t));
+				/* Check overflow */
+				if (req_mem + s < req_mem)
+					return TEE_ERROR_BAD_PARAMETERS;
+				req_mem += s;
+				ta_private_memref[n] = true;
+				break;
+			}
+
+			res = tee_mmu_vbuf_to_mobj_offs(utc, va, s,
+							&param->u[n].mem.mobj,
+							&param->u[n].mem.offs);
+			if (res != TEE_SUCCESS)
+				return res;
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (req_mem == 0)
+		return TEE_SUCCESS;
+
+	res = alloc_temp_sec_mem(req_mem, mobj_tmp, &dst);
+	if (res != TEE_SUCCESS)
+		return res;
+	dst_offs = 0;
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+
+		if (!ta_private_memref[n])
+			continue;
+
+		s = ROUNDUP(param->u[n].mem.size, sizeof(uint32_t));
+
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_INPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			va = (void *)param->u[n].mem.offs;
+			if (va) {
+				res = tee_svc_copy_from_user(dst, va,
+						param->u[n].mem.size);
+				if (res != TEE_SUCCESS)
+					return res;
+				param->u[n].mem.offs = dst_offs;
+				param->u[n].mem.mobj = *mobj_tmp;
+				tmp_buf_va[n] = dst;
+				dst += s;
+				dst_offs += s;
+			}
+			break;
+
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+			va = (void *)param->u[n].mem.offs;
+			if (va) {
+				param->u[n].mem.offs = dst_offs;
+				param->u[n].mem.mobj = *mobj_tmp;
+				tmp_buf_va[n] = dst;
+				dst += s;
+				dst_offs += s;
+			}
+			break;
+
+		default:
+			continue;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+/*
+ * Back from execution of service: update parameters passed from TA:
+ * If some parameters were memory references:
+ * - either the memref was temporary: copy back data and update size
+ * - or it was the original TA memref: update only the size value.
+ */
+static TEE_Result tee_svc_update_out_param(
+		struct tee_ta_session *sess,
+		struct tee_ta_session *called_sess,
+		struct tee_ta_param *param,
+		void *tmp_buf_va[TEE_NUM_PARAMS],
+		struct utee_params *usr_param)
+{
+	size_t n;
+	void *p;
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+	bool have_private_mem_map = is_user_ta_ctx(called_sess->ctx);
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		switch (TEE_PARAM_TYPE_GET(param->types, n)) {
+		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
+		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			p = (void *)(uintptr_t)usr_param->vals[n * 2];
+
+			/* outside TA private => memref is valid, update size */
+			if (!tee_mmu_is_vbuf_inside_ta_private(utc, p,
+					param->u[n].mem.size)) {
+				usr_param->vals[n * 2 + 1] =
+					param->u[n].mem.size;
+				break;
+			}
+
+			/*
+			 * If we called a kernel TA the parameters are in shared
+			 * memory and no copy is needed.
+			 */
+			if (have_private_mem_map &&
+			    param->u[n].mem.size <=
+			    usr_param->vals[n * 2 + 1]) {
+				uint8_t *src = tmp_buf_va[n];
+				TEE_Result res;
+
+				res = tee_svc_copy_to_user(p, src,
+						 param->u[n].mem.size);
+				if (res != TEE_SUCCESS)
+					return res;
+
+			}
+			usr_param->vals[n * 2 + 1] = param->u[n].mem.size;
+			break;
+
+		case TEE_PARAM_TYPE_VALUE_OUTPUT:
+		case TEE_PARAM_TYPE_VALUE_INOUT:
+			usr_param->vals[n * 2] = param->u[n].val.a;
+			usr_param->vals[n * 2 + 1] = param->u[n].val.b;
+			break;
+
+		default:
+			continue;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+/* Called when a TA calls an OpenSession on another TA */
+TEE_Result syscall_open_ta_session(const TEE_UUID *dest,
+			unsigned long cancel_req_to,
+			struct utee_params *usr_param, uint32_t *ta_sess,
+			uint32_t *ret_orig)
+{
+	TEE_Result res;
+	uint32_t ret_o = TEE_ORIGIN_TEE;
+	struct tee_ta_session *s = NULL;
+	struct tee_ta_session *sess;
+	struct mobj *mobj_param = NULL;
+	TEE_UUID *uuid = malloc(sizeof(TEE_UUID));
+	struct tee_ta_param *param = malloc(sizeof(struct tee_ta_param));
+	TEE_Identity *clnt_id = malloc(sizeof(TEE_Identity));
+	void *tmp_buf_va[TEE_NUM_PARAMS];
+	struct user_ta_ctx *utc;
+
+	if (uuid == NULL || param == NULL || clnt_id == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out_free_only;
+	}
+
+	memset(param, 0, sizeof(struct tee_ta_param));
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto out_free_only;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_copy_from_user(uuid, dest, sizeof(TEE_UUID));
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	clnt_id->login = TEE_LOGIN_TRUSTED_APP;
+	memcpy(&clnt_id->uuid, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	res = tee_svc_copy_param(sess, NULL, usr_param, param, tmp_buf_va,
+				 &mobj_param);
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	/*
+	 * Find session of a multi session TA or a static TA
+	 * In such a case, there is no need to ask the supplicant for the TA
+	 * code
+	 */
+	res = tee_ta_open_session(&ret_o, &s, &utc->open_sessions, uuid,
+				  clnt_id, cancel_req_to, param);
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	res = tee_svc_update_out_param(sess, s, param, tmp_buf_va, usr_param);
+
+function_exit:
+	if (mobj_param) {
+		mutex_lock(&tee_ta_mutex);
+		mobj_free(mobj_param);
+		mutex_unlock(&tee_ta_mutex);
+	}
+	if (res == TEE_SUCCESS)
+		tee_svc_copy_kaddr_to_uref(ta_sess, s);
+	tee_svc_copy_to_user(ret_orig, &ret_o, sizeof(ret_o));
+
+out_free_only:
+	free(param);
+	free(uuid);
+	free(clnt_id);
+	return res;
+}
+
+TEE_Result syscall_close_ta_session(unsigned long ta_sess)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	TEE_Identity clnt_id;
+	struct tee_ta_session *s = tee_svc_uref_to_kaddr(ta_sess);
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	clnt_id.login = TEE_LOGIN_TRUSTED_APP;
+	memcpy(&clnt_id.uuid, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	return tee_ta_close_session(s, &utc->open_sessions, &clnt_id);
+}
+
+TEE_Result syscall_invoke_ta_command(unsigned long ta_sess,
+			unsigned long cancel_req_to, unsigned long cmd_id,
+			struct utee_params *usr_param, uint32_t *ret_orig)
+{
+	TEE_Result res;
+	TEE_Result res2;
+	uint32_t ret_o = TEE_ORIGIN_TEE;
+	struct tee_ta_param param = { 0 };
+	TEE_Identity clnt_id;
+	struct tee_ta_session *sess;
+	struct tee_ta_session *called_sess;
+	struct mobj *mobj_param = NULL;
+	void *tmp_buf_va[TEE_NUM_PARAMS];
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	called_sess = tee_ta_get_session(
+				(vaddr_t)tee_svc_uref_to_kaddr(ta_sess), true,
+				&utc->open_sessions);
+	if (!called_sess)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	clnt_id.login = TEE_LOGIN_TRUSTED_APP;
+	memcpy(&clnt_id.uuid, &sess->ctx->uuid, sizeof(TEE_UUID));
+
+	res = tee_svc_copy_param(sess, called_sess, usr_param, &param,
+				 tmp_buf_va, &mobj_param);
+	if (res != TEE_SUCCESS)
+		goto function_exit;
+
+	res = tee_ta_invoke_command(&ret_o, called_sess, &clnt_id,
+				    cancel_req_to, cmd_id, &param);
+
+	res2 = tee_svc_update_out_param(sess, called_sess, &param, tmp_buf_va,
+					usr_param);
+	if (res2 != TEE_SUCCESS) {
+		/*
+		 * Spec for TEE_InvokeTACommand() says:
+		 * "If the return origin is different from
+		 * TEE_ORIGIN_TRUSTED_APP, then the function has failed
+		 * before it could reach the destination Trusted
+		 * Application."
+		 *
+		 * But if we can't update params to the caller we have no
+		 * choice we need to return some error to indicate that
+		 * parameters aren't updated as expected.
+		 */
+		ret_o = TEE_ORIGIN_TEE;
+		res = res2;
+	}
+
+function_exit:
+	tee_ta_put_session(called_sess);
+	if (mobj_param) {
+		mutex_lock(&tee_ta_mutex);
+		mobj_free(mobj_param);
+		mutex_unlock(&tee_ta_mutex);
+	}
+	if (ret_orig)
+		tee_svc_copy_to_user(ret_orig, &ret_o, sizeof(ret_o));
+	return res;
+}
+
+TEE_Result syscall_check_access_rights(unsigned long flags, const void *buf,
+				       size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx), flags,
+					   (uaddr_t)buf, len);
+}
+
+TEE_Result tee_svc_copy_from_user(void *kaddr, const void *uaddr, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx),
+					TEE_MEMORY_ACCESS_READ |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t)uaddr, len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(kaddr, uaddr, len);
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_svc_copy_to_user(void *uaddr, const void *kaddr, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *s;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(s->ctx),
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t)uaddr, len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(uaddr, kaddr, len);
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_svc_copy_kaddr_to_uref(uint32_t *uref, void *kaddr)
+{
+	uint32_t ref = tee_svc_kaddr_to_uref(kaddr);
+
+	return tee_svc_copy_to_user(uref, &ref, sizeof(ref));
+}
+
+TEE_Result syscall_get_cancellation_flag(uint32_t *cancel)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	uint32_t c;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	c = tee_ta_session_is_cancelled(s, NULL);
+
+	return tee_svc_copy_to_user(cancel, &c, sizeof(c));
+}
+
+TEE_Result syscall_unmask_cancellation(uint32_t *old_mask)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	uint32_t m;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	m = s->cancel_mask;
+	s->cancel_mask = false;
+	return tee_svc_copy_to_user(old_mask, &m, sizeof(m));
+}
+
+TEE_Result syscall_mask_cancellation(uint32_t *old_mask)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	uint32_t m;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	m = s->cancel_mask;
+	s->cancel_mask = true;
+	return tee_svc_copy_to_user(old_mask, &m, sizeof(m));
+}
+
+TEE_Result syscall_wait(unsigned long timeout)
+{
+	TEE_Result res = TEE_SUCCESS;
+	uint32_t mytime = 0;
+	struct tee_ta_session *s;
+	TEE_Time base_time;
+	TEE_Time current_time;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_time_get_sys_time(&base_time);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	while (true) {
+		res = tee_time_get_sys_time(&current_time);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		if (tee_ta_session_is_cancelled(s, &current_time))
+			return TEE_ERROR_CANCEL;
+
+		mytime = (current_time.seconds - base_time.seconds) * 1000 +
+		    (int)current_time.millis - (int)base_time.millis;
+		if (mytime >= timeout)
+			return TEE_SUCCESS;
+
+		tee_time_wait(timeout - mytime);
+	}
+
+	return res;
+}
+
+TEE_Result syscall_get_time(unsigned long cat, TEE_Time *mytime)
+{
+	TEE_Result res, res2;
+	struct tee_ta_session *s = NULL;
+	TEE_Time t;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (cat) {
+	case UTEE_TIME_CAT_SYSTEM:
+		res = tee_time_get_sys_time(&t);
+		break;
+	case UTEE_TIME_CAT_TA_PERSISTENT:
+		res = tee_time_get_ta_time((const void *)&s->ctx->uuid, &t);
+		break;
+	case UTEE_TIME_CAT_REE:
+		res = tee_time_get_ree_time(&t);
+		break;
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	}
+
+	if (res == TEE_SUCCESS || res == TEE_ERROR_OVERFLOW) {
+		res2 = tee_svc_copy_to_user(mytime, &t, sizeof(t));
+		if (res2 != TEE_SUCCESS)
+			res = res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_set_ta_time(const TEE_Time *mytime)
+{
+	TEE_Result res;
+	struct tee_ta_session *s = NULL;
+	TEE_Time t;
+
+	res = tee_ta_get_current_session(&s);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&t, mytime, sizeof(t));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_time_set_ta_time((const void *)&s->ctx->uuid, &t);
+}
diff --git a/core/tee/tee_svc_cryp.c b/core/tee/tee_svc_cryp.c
new file mode 100644
index 0000000..c668a3e
--- /dev/null
+++ b/core/tee/tee_svc_cryp.c
@@ -0,0 +1,3554 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <assert.h>
+#include <tee_api_types.h>
+#include <kernel/tee_ta_manager.h>
+#include <utee_defines.h>
+#include <mm/tee_mmu.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_cryp_utl.h>
+#include <sys/queue.h>
+#include <tee/tee_obj.h>
+#include <tee/tee_cryp_provider.h>
+#include <trace.h>
+#include <string_ext.h>
+#include <string.h>
+#include <util.h>
+#if defined(CFG_CRYPTO_HKDF) || defined(CFG_CRYPTO_CONCAT_KDF) || \
+	defined(CFG_CRYPTO_PBKDF2)
+#include <tee_api_defines_extensions.h>
+#endif
+#if defined(CFG_CRYPTO_HKDF)
+#include <tee/tee_cryp_hkdf.h>
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+#include <tee/tee_cryp_concat_kdf.h>
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+#include <tee/tee_cryp_pbkdf2.h>
+#endif
+
+typedef void (*tee_cryp_ctx_finalize_func_t) (void *ctx, uint32_t algo);
+struct tee_cryp_state {
+	TAILQ_ENTRY(tee_cryp_state) link;
+	uint32_t algo;
+	uint32_t mode;
+	vaddr_t key1;
+	vaddr_t key2;
+	size_t ctx_size;
+	void *ctx;
+	tee_cryp_ctx_finalize_func_t ctx_finalize;
+};
+
+struct tee_cryp_obj_secret {
+	uint32_t key_size;
+	uint32_t alloc_size;
+
+	/*
+	 * Pseudo code visualize layout of structure
+	 * Next follows data, such as:
+	 *	uint8_t data[alloc_size]
+	 * key_size must never exceed alloc_size
+	 */
+};
+
+#define TEE_TYPE_ATTR_OPTIONAL       0x0
+#define TEE_TYPE_ATTR_REQUIRED       0x1
+#define TEE_TYPE_ATTR_OPTIONAL_GROUP 0x2
+#define TEE_TYPE_ATTR_SIZE_INDICATOR 0x4
+#define TEE_TYPE_ATTR_GEN_KEY_OPT    0x8
+#define TEE_TYPE_ATTR_GEN_KEY_REQ    0x10
+
+    /* Handle storing of generic secret keys of varying lengths */
+#define ATTR_OPS_INDEX_SECRET     0
+    /* Convert to/from big-endian byte array and provider-specific bignum */
+#define ATTR_OPS_INDEX_BIGNUM     1
+    /* Convert to/from value attribute depending on direction */
+#define ATTR_OPS_INDEX_VALUE      2
+
+struct tee_cryp_obj_type_attrs {
+	uint32_t attr_id;
+	uint16_t flags;
+	uint16_t ops_index;
+	uint16_t raw_offs;
+	uint16_t raw_size;
+};
+
+#define RAW_DATA(_x, _y)	\
+	.raw_offs = offsetof(_x, _y), .raw_size = MEMBER_SIZE(_x, _y)
+
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_secret_value_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_SECRET_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_rsa_pub_key_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_RSA_MODULUS,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_public_key, n)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PUBLIC_EXPONENT,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_public_key, e)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_rsa_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_RSA_MODULUS,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, n)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PUBLIC_EXPONENT,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, e)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PRIVATE_EXPONENT,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, d)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PRIME1,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_PRIME2,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_EXPONENT1,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, dp)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_EXPONENT2,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, dq)
+	},
+
+	{
+	.attr_id = TEE_ATTR_RSA_COEFFICIENT,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct rsa_keypair, qp)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_dsa_pub_key_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_DSA_PRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_SUBPRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_BASE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, g)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_PUBLIC_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_public_key, y)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_dsa_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_DSA_PRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_SUBPRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR |
+		 TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_BASE,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, g)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_PRIVATE_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DSA_PUBLIC_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dsa_keypair, y)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_dh_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_DH_PRIME,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR |
+		 TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, p)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_BASE,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_GEN_KEY_REQ,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, g)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_PUBLIC_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, y)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_PRIVATE_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_SUBPRIME,
+	.flags = TEE_TYPE_ATTR_OPTIONAL_GROUP |	 TEE_TYPE_ATTR_GEN_KEY_OPT,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct dh_keypair, q)
+	},
+
+	{
+	.attr_id = TEE_ATTR_DH_X_BITS,
+	.flags = TEE_TYPE_ATTR_GEN_KEY_OPT,
+	.ops_index = ATTR_OPS_INDEX_VALUE,
+	RAW_DATA(struct dh_keypair, xbits)
+	},
+};
+
+#if defined(CFG_CRYPTO_HKDF)
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_hkdf_ikm_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_HKDF_IKM,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+#endif
+
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_concat_kdf_z_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_CONCAT_KDF_Z,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+#endif
+
+#if defined(CFG_CRYPTO_PBKDF2)
+static const struct tee_cryp_obj_type_attrs
+	tee_cryp_obj_pbkdf2_passwd_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_PBKDF2_PASSWORD,
+	.flags = TEE_TYPE_ATTR_REQUIRED | TEE_TYPE_ATTR_SIZE_INDICATOR,
+	.ops_index = ATTR_OPS_INDEX_SECRET,
+	.raw_offs = 0,
+	.raw_size = 0
+	},
+};
+#endif
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_ecc_pub_key_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_X,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_public_key, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_Y,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_public_key, y)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_CURVE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_VALUE,
+	RAW_DATA(struct ecc_public_key, curve)
+	},
+};
+
+static const struct tee_cryp_obj_type_attrs tee_cryp_obj_ecc_keypair_attrs[] = {
+	{
+	.attr_id = TEE_ATTR_ECC_PRIVATE_VALUE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_keypair, d)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_X,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_keypair, x)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_PUBLIC_VALUE_Y,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_BIGNUM,
+	RAW_DATA(struct ecc_keypair, y)
+	},
+
+	{
+	.attr_id = TEE_ATTR_ECC_CURVE,
+	.flags = TEE_TYPE_ATTR_REQUIRED,
+	.ops_index = ATTR_OPS_INDEX_VALUE,
+	RAW_DATA(struct ecc_keypair, curve)
+	},
+};
+
+struct tee_cryp_obj_type_props {
+	TEE_ObjectType obj_type;
+	uint16_t min_size;	/* may not be smaller than this */
+	uint16_t max_size;	/* may not be larger than this */
+	uint16_t alloc_size;	/* this many bytes are allocated to hold data */
+	uint8_t quanta;		/* may only be an multiple of this */
+
+	uint8_t num_type_attrs;
+	const struct tee_cryp_obj_type_attrs *type_attrs;
+};
+
+#define PROP(obj_type, quanta, min_size, max_size, alloc_size, type_attrs) \
+		{ (obj_type), (min_size), (max_size), (alloc_size), (quanta), \
+		  ARRAY_SIZE(type_attrs), (type_attrs) }
+
+static const struct tee_cryp_obj_type_props tee_cryp_obj_props[] = {
+	PROP(TEE_TYPE_AES, 64, 128, 256,	/* valid sizes 128, 192, 256 */
+		256 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_DES, 56, 56, 56,
+		/*
+		* Valid size 56 without parity, note that we still allocate
+		* for 64 bits since the key is supplied with parity.
+		*/
+		64 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_DES3, 56, 112, 168,
+		/*
+		* Valid sizes 112, 168 without parity, note that we still
+		* allocate for with space for the parity since the key is
+		* supplied with parity.
+		*/
+		192 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_MD5, 8, 64, 512,
+		512 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA1, 8, 80, 512,
+		512 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA224, 8, 112, 512,
+		512 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA256, 8, 192, 1024,
+		1024 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA384, 8, 256, 1024,
+		1024 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_HMAC_SHA512, 8, 256, 1024,
+		1024 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+	PROP(TEE_TYPE_GENERIC_SECRET, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_secret_value_attrs),
+#if defined(CFG_CRYPTO_HKDF)
+	PROP(TEE_TYPE_HKDF_IKM, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_hkdf_ikm_attrs),
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+	PROP(TEE_TYPE_CONCAT_KDF_Z, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_concat_kdf_z_attrs),
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+	PROP(TEE_TYPE_PBKDF2_PASSWORD, 8, 0, 4096,
+		4096 / 8 + sizeof(struct tee_cryp_obj_secret),
+		tee_cryp_obj_pbkdf2_passwd_attrs),
+#endif
+	PROP(TEE_TYPE_RSA_PUBLIC_KEY, 1, 256, 2048,
+		sizeof(struct rsa_public_key),
+		tee_cryp_obj_rsa_pub_key_attrs),
+
+	PROP(TEE_TYPE_RSA_KEYPAIR, 1, 256, 2048,
+		sizeof(struct rsa_keypair),
+		tee_cryp_obj_rsa_keypair_attrs),
+
+	PROP(TEE_TYPE_DSA_PUBLIC_KEY, 64, 512, 3072,
+		sizeof(struct dsa_public_key),
+		tee_cryp_obj_dsa_pub_key_attrs),
+
+	PROP(TEE_TYPE_DSA_KEYPAIR, 64, 512, 3072,
+		sizeof(struct dsa_keypair),
+		tee_cryp_obj_dsa_keypair_attrs),
+
+	PROP(TEE_TYPE_DH_KEYPAIR, 1, 256, 2048,
+		sizeof(struct dh_keypair),
+		tee_cryp_obj_dh_keypair_attrs),
+
+	PROP(TEE_TYPE_ECDSA_PUBLIC_KEY, 1, 192, 521,
+		sizeof(struct ecc_public_key),
+		tee_cryp_obj_ecc_pub_key_attrs),
+
+	PROP(TEE_TYPE_ECDSA_KEYPAIR, 1, 192, 521,
+		sizeof(struct ecc_keypair),
+		tee_cryp_obj_ecc_keypair_attrs),
+
+	PROP(TEE_TYPE_ECDH_PUBLIC_KEY, 1, 192, 521,
+		sizeof(struct ecc_public_key),
+		tee_cryp_obj_ecc_pub_key_attrs),
+
+	PROP(TEE_TYPE_ECDH_KEYPAIR, 1, 192, 521,
+		sizeof(struct ecc_keypair),
+		tee_cryp_obj_ecc_keypair_attrs),
+};
+
+struct attr_ops {
+	TEE_Result (*from_user)(void *attr, const void *buffer, size_t size);
+	TEE_Result (*to_user)(void *attr, struct tee_ta_session *sess,
+			      void *buffer, uint64_t *size);
+	void (*to_binary)(void *attr, void *data, size_t data_len,
+			  size_t *offs);
+	bool (*from_binary)(void *attr, const void *data, size_t data_len,
+			    size_t *offs);
+	TEE_Result (*from_obj)(void *attr, void *src_attr);
+	void (*free)(void *attr);
+	void (*clear)(void *attr);
+};
+
+static void op_u32_to_binary_helper(uint32_t v, uint8_t *data,
+				    size_t data_len, size_t *offs)
+{
+	uint32_t field;
+
+	if (data && (*offs + sizeof(field)) <= data_len) {
+		field = TEE_U32_TO_BIG_ENDIAN(v);
+		memcpy(data + *offs, &field, sizeof(field));
+	}
+	(*offs) += sizeof(field);
+}
+
+static bool op_u32_from_binary_helper(uint32_t *v, const uint8_t *data,
+				      size_t data_len, size_t *offs)
+{
+	uint32_t field;
+
+	if (!data || (*offs + sizeof(field)) > data_len)
+		return false;
+
+	memcpy(&field, data + *offs, sizeof(field));
+	*v = TEE_U32_FROM_BIG_ENDIAN(field);
+	(*offs) += sizeof(field);
+	return true;
+}
+
+static TEE_Result op_attr_secret_value_from_user(void *attr, const void *buffer,
+						 size_t size)
+{
+	struct tee_cryp_obj_secret *key = attr;
+
+	/* Data size has to fit in allocated buffer */
+	if (size > key->alloc_size)
+		return TEE_ERROR_SECURITY;
+	memcpy(key + 1, buffer, size);
+	key->key_size = size;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result op_attr_secret_value_to_user(void *attr,
+			struct tee_ta_session *sess __unused,
+			void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	struct tee_cryp_obj_secret *key = attr;
+	uint64_t s;
+	uint64_t key_size;
+
+	res = tee_svc_copy_from_user(&s, size, sizeof(s));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	key_size = key->key_size;
+	res = tee_svc_copy_to_user(size, &key_size, sizeof(key_size));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (s < key->key_size)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	return tee_svc_copy_to_user(buffer, key + 1, key->key_size);
+}
+
+static void op_attr_secret_value_to_binary(void *attr, void *data,
+					   size_t data_len, size_t *offs)
+{
+	struct tee_cryp_obj_secret *key = attr;
+
+	op_u32_to_binary_helper(key->key_size, data, data_len, offs);
+	if (data && (*offs + key->key_size) <= data_len)
+		memcpy((uint8_t *)data + *offs, key + 1, key->key_size);
+	(*offs) += key->key_size;
+}
+
+static bool op_attr_secret_value_from_binary(void *attr, const void *data,
+					     size_t data_len, size_t *offs)
+{
+	struct tee_cryp_obj_secret *key = attr;
+	uint32_t s;
+
+	if (!op_u32_from_binary_helper(&s, data, data_len, offs))
+		return false;
+
+	if ((*offs + s) > data_len)
+		return false;
+
+	/* Data size has to fit in allocated buffer */
+	if (s > key->alloc_size)
+		return false;
+	key->key_size = s;
+	memcpy(key + 1, (const uint8_t *)data + *offs, s);
+	(*offs) += s;
+	return true;
+}
+
+
+static TEE_Result op_attr_secret_value_from_obj(void *attr, void *src_attr)
+{
+	struct tee_cryp_obj_secret *key = attr;
+	struct tee_cryp_obj_secret *src_key = src_attr;
+
+	if (src_key->key_size > key->alloc_size)
+		return TEE_ERROR_BAD_STATE;
+	memcpy(key + 1, src_key + 1, src_key->key_size);
+	key->key_size = src_key->key_size;
+	return TEE_SUCCESS;
+}
+
+static void op_attr_secret_value_clear(void *attr)
+{
+	struct tee_cryp_obj_secret *key = attr;
+
+	key->key_size = 0;
+	memset(key + 1, 0, key->alloc_size);
+}
+
+static TEE_Result op_attr_bignum_from_user(void *attr, const void *buffer,
+					   size_t size)
+{
+	struct bignum **bn = attr;
+
+	if (!crypto_ops.bignum.bin2bn)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	return crypto_ops.bignum.bin2bn(buffer, size, *bn);
+}
+
+static TEE_Result op_attr_bignum_to_user(void *attr,
+					 struct tee_ta_session *sess,
+					 void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	struct bignum **bn = attr;
+	uint64_t req_size;
+	uint64_t s;
+
+	res = tee_svc_copy_from_user(&s, size, sizeof(s));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	req_size = crypto_ops.bignum.num_bytes(*bn);
+	res = tee_svc_copy_to_user(size, &req_size, sizeof(req_size));
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!req_size)
+		return TEE_SUCCESS;
+	if (s < req_size)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	/* Check we can access data using supplied user mode pointer */
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)buffer, req_size);
+	if (res != TEE_SUCCESS)
+		return res;
+	/*
+	* Write the bignum (wich raw data points to) into an array of
+	* bytes (stored in buffer)
+	*/
+	crypto_ops.bignum.bn2bin(*bn, buffer);
+	return TEE_SUCCESS;
+}
+
+static void op_attr_bignum_to_binary(void *attr, void *data, size_t data_len,
+				     size_t *offs)
+{
+	struct bignum **bn = attr;
+	uint32_t n = crypto_ops.bignum.num_bytes(*bn);
+
+	op_u32_to_binary_helper(n, data, data_len, offs);
+
+	if (data && (*offs + n) <= data_len)
+		crypto_ops.bignum.bn2bin(*bn, (uint8_t *)data + *offs);
+	(*offs) += n;
+}
+
+static bool op_attr_bignum_from_binary(void *attr, const void *data,
+				       size_t data_len, size_t *offs)
+{
+	struct bignum **bn = attr;
+	uint32_t n;
+
+	if (!op_u32_from_binary_helper(&n, data, data_len, offs))
+		return false;
+
+	if ((*offs + n) > data_len)
+		return false;
+	if (crypto_ops.bignum.bin2bn((const uint8_t *)data + *offs,
+				     n, *bn) != TEE_SUCCESS)
+		return false;
+	(*offs) += n;
+	return true;
+}
+
+static TEE_Result op_attr_bignum_from_obj(void *attr, void *src_attr)
+{
+	struct bignum **bn = attr;
+	struct bignum **src_bn = src_attr;
+
+	crypto_ops.bignum.copy(*bn, *src_bn);
+	return TEE_SUCCESS;
+}
+
+static void op_attr_bignum_clear(void *attr)
+{
+	struct bignum **bn = attr;
+
+	crypto_ops.bignum.clear(*bn);
+}
+
+static void op_attr_bignum_free(void *attr)
+{
+	struct bignum **bn = attr;
+
+	crypto_ops.bignum.free(*bn);
+	*bn = NULL;
+}
+
+static TEE_Result op_attr_value_from_user(void *attr, const void *buffer,
+					  size_t size)
+{
+	uint32_t *v = attr;
+
+	if (size != sizeof(uint32_t) * 2)
+		return TEE_ERROR_GENERIC; /* "can't happen */
+
+	/* Note that only the first value is copied */
+	memcpy(v, buffer, sizeof(uint32_t));
+	return TEE_SUCCESS;
+}
+
+static TEE_Result op_attr_value_to_user(void *attr,
+					struct tee_ta_session *sess __unused,
+					void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	uint32_t *v = attr;
+	uint64_t s;
+	uint32_t value[2] = { *v };
+	uint64_t req_size = sizeof(value);
+
+	res = tee_svc_copy_from_user(&s, size, sizeof(s));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (s < req_size)
+		return TEE_ERROR_SHORT_BUFFER;
+
+	return tee_svc_copy_to_user(buffer, value, req_size);
+}
+
+static void op_attr_value_to_binary(void *attr, void *data, size_t data_len,
+				    size_t *offs)
+{
+	uint32_t *v = attr;
+
+	op_u32_to_binary_helper(*v, data, data_len, offs);
+}
+
+static bool op_attr_value_from_binary(void *attr, const void *data,
+				      size_t data_len, size_t *offs)
+{
+	uint32_t *v = attr;
+
+	return op_u32_from_binary_helper(v, data, data_len, offs);
+}
+
+static TEE_Result op_attr_value_from_obj(void *attr, void *src_attr)
+{
+	uint32_t *v = attr;
+	uint32_t *src_v = src_attr;
+
+	*v = *src_v;
+	return TEE_SUCCESS;
+}
+
+static void op_attr_value_clear(void *attr)
+{
+	uint32_t *v = attr;
+
+	*v = 0;
+}
+
+static const struct attr_ops attr_ops[] = {
+	[ATTR_OPS_INDEX_SECRET] = {
+		.from_user = op_attr_secret_value_from_user,
+		.to_user = op_attr_secret_value_to_user,
+		.to_binary = op_attr_secret_value_to_binary,
+		.from_binary = op_attr_secret_value_from_binary,
+		.from_obj = op_attr_secret_value_from_obj,
+		.free = op_attr_secret_value_clear, /* not a typo */
+		.clear = op_attr_secret_value_clear,
+	},
+	[ATTR_OPS_INDEX_BIGNUM] = {
+		.from_user = op_attr_bignum_from_user,
+		.to_user = op_attr_bignum_to_user,
+		.to_binary = op_attr_bignum_to_binary,
+		.from_binary = op_attr_bignum_from_binary,
+		.from_obj = op_attr_bignum_from_obj,
+		.free = op_attr_bignum_free,
+		.clear = op_attr_bignum_clear,
+	},
+	[ATTR_OPS_INDEX_VALUE] = {
+		.from_user = op_attr_value_from_user,
+		.to_user = op_attr_value_to_user,
+		.to_binary = op_attr_value_to_binary,
+		.from_binary = op_attr_value_from_binary,
+		.from_obj = op_attr_value_from_obj,
+		.free = op_attr_value_clear, /* not a typo */
+		.clear = op_attr_value_clear,
+	},
+};
+
+TEE_Result syscall_cryp_obj_get_info(unsigned long obj, TEE_ObjectInfo *info)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_svc_copy_to_user(info, &o->info, sizeof(o->info));
+
+exit:
+	return res;
+}
+
+TEE_Result syscall_cryp_obj_restrict_usage(unsigned long obj,
+			unsigned long usage)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o->info.objectUsage &= usage;
+
+exit:
+	return res;
+}
+
+static int tee_svc_cryp_obj_find_type_attr_idx(
+		uint32_t attr_id,
+		const struct tee_cryp_obj_type_props *type_props)
+{
+	size_t n;
+
+	for (n = 0; n < type_props->num_type_attrs; n++) {
+		if (attr_id == type_props->type_attrs[n].attr_id)
+			return n;
+	}
+	return -1;
+}
+
+static const struct tee_cryp_obj_type_props *tee_svc_find_type_props(
+		TEE_ObjectType obj_type)
+{
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(tee_cryp_obj_props); n++) {
+		if (tee_cryp_obj_props[n].obj_type == obj_type)
+			return tee_cryp_obj_props + n;
+	}
+
+	return NULL;
+}
+
+/* Set an attribute on an object */
+static void set_attribute(struct tee_obj *o,
+			  const struct tee_cryp_obj_type_props *props,
+			  uint32_t attr)
+{
+	int idx = tee_svc_cryp_obj_find_type_attr_idx(attr, props);
+
+	if (idx < 0)
+		return;
+	o->have_attrs |= BIT(idx);
+}
+
+/* Get an attribute on an object */
+static uint32_t get_attribute(const struct tee_obj *o,
+			      const struct tee_cryp_obj_type_props *props,
+			      uint32_t attr)
+{
+	int idx = tee_svc_cryp_obj_find_type_attr_idx(attr, props);
+
+	if (idx < 0)
+		return 0;
+	return o->have_attrs & BIT(idx);
+}
+
+TEE_Result syscall_cryp_obj_get_attr(unsigned long obj, unsigned long attr_id,
+			void *buffer, uint64_t *size)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	const struct tee_cryp_obj_type_props *type_props;
+	int idx;
+	const struct attr_ops *ops;
+	void *attr;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/* Check that the object is initialized */
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Check that getting the attribute is allowed */
+	if (!(attr_id & TEE_ATTR_BIT_PROTECTED) &&
+	    !(o->info.objectUsage & TEE_USAGE_EXTRACTABLE))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	type_props = tee_svc_find_type_props(o->info.objectType);
+	if (!type_props) {
+		/* Unknown object type, "can't happen" */
+		return TEE_ERROR_BAD_STATE;
+	}
+
+	idx = tee_svc_cryp_obj_find_type_attr_idx(attr_id, type_props);
+	if ((idx < 0) || ((o->have_attrs & (1 << idx)) == 0))
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	ops = attr_ops + type_props->type_attrs[idx].ops_index;
+	attr = (uint8_t *)o->attr + type_props->type_attrs[idx].raw_offs;
+	return ops->to_user(attr, sess, buffer, size);
+}
+
+void tee_obj_attr_free(struct tee_obj *o)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+
+	if (!o->attr)
+		return;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+
+		attr_ops[ta->ops_index].free((uint8_t *)o->attr + ta->raw_offs);
+	}
+}
+
+void tee_obj_attr_clear(struct tee_obj *o)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+
+	if (!o->attr)
+		return;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+
+		attr_ops[ta->ops_index].clear((uint8_t *)o->attr +
+					      ta->raw_offs);
+	}
+}
+
+TEE_Result tee_obj_attr_to_binary(struct tee_obj *o, void *data,
+				  size_t *data_len)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+	size_t offs = 0;
+	size_t len = data ? *data_len : 0;
+
+	if (o->info.objectType == TEE_TYPE_DATA) {
+		*data_len = 0;
+		return TEE_SUCCESS; /* pure data object */
+	}
+	if (!o->attr)
+		return TEE_ERROR_BAD_STATE;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return TEE_ERROR_BAD_STATE;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+		void *attr = (uint8_t *)o->attr + ta->raw_offs;
+
+		attr_ops[ta->ops_index].to_binary(attr, data, len, &offs);
+	}
+
+	*data_len = offs;
+	if (data && offs > len)
+		return TEE_ERROR_SHORT_BUFFER;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_obj_attr_from_binary(struct tee_obj *o, const void *data,
+				    size_t data_len)
+{
+	const struct tee_cryp_obj_type_props *tp;
+	size_t n;
+	size_t offs = 0;
+
+	if (o->info.objectType == TEE_TYPE_DATA)
+		return TEE_SUCCESS; /* pure data object */
+	if (!o->attr)
+		return TEE_ERROR_BAD_STATE;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return TEE_ERROR_BAD_STATE;
+
+	for (n = 0; n < tp->num_type_attrs; n++) {
+		const struct tee_cryp_obj_type_attrs *ta = tp->type_attrs + n;
+		void *attr = (uint8_t *)o->attr + ta->raw_offs;
+
+		if (!attr_ops[ta->ops_index].from_binary(attr, data, data_len,
+							 &offs))
+			return TEE_ERROR_CORRUPT_OBJECT;
+	}
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_obj_attr_copy_from(struct tee_obj *o, const struct tee_obj *src)
+{
+	TEE_Result res;
+	const struct tee_cryp_obj_type_props *tp;
+	const struct tee_cryp_obj_type_attrs *ta;
+	size_t n;
+	uint32_t have_attrs = 0;
+	void *attr;
+	void *src_attr;
+
+	if (o->info.objectType == TEE_TYPE_DATA)
+		return TEE_SUCCESS; /* pure data object */
+	if (!o->attr)
+		return TEE_ERROR_BAD_STATE;
+	tp = tee_svc_find_type_props(o->info.objectType);
+	if (!tp)
+		return TEE_ERROR_BAD_STATE;
+
+	if (o->info.objectType == src->info.objectType) {
+		have_attrs = src->have_attrs;
+		for (n = 0; n < tp->num_type_attrs; n++) {
+			ta = tp->type_attrs + n;
+			attr = (uint8_t *)o->attr + ta->raw_offs;
+			src_attr = (uint8_t *)src->attr + ta->raw_offs;
+			res = attr_ops[ta->ops_index].from_obj(attr, src_attr);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+	} else {
+		const struct tee_cryp_obj_type_props *tp_src;
+		int idx;
+
+		if (o->info.objectType == TEE_TYPE_RSA_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_RSA_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else if (o->info.objectType == TEE_TYPE_DSA_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_DSA_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else if (o->info.objectType == TEE_TYPE_ECDSA_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_ECDSA_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else if (o->info.objectType == TEE_TYPE_ECDH_PUBLIC_KEY) {
+			if (src->info.objectType != TEE_TYPE_ECDH_KEYPAIR)
+				return TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+
+		tp_src = tee_svc_find_type_props(src->info.objectType);
+		if (!tp_src)
+			return TEE_ERROR_BAD_STATE;
+
+		have_attrs = BIT32(tp->num_type_attrs) - 1;
+		for (n = 0; n < tp->num_type_attrs; n++) {
+			ta = tp->type_attrs + n;
+
+			idx = tee_svc_cryp_obj_find_type_attr_idx(ta->attr_id,
+								  tp_src);
+			if (idx < 0)
+				return TEE_ERROR_BAD_STATE;
+
+			attr = (uint8_t *)o->attr + ta->raw_offs;
+			src_attr = (uint8_t *)src->attr +
+				   tp_src->type_attrs[idx].raw_offs;
+			res = attr_ops[ta->ops_index].from_obj(attr, src_attr);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+	}
+
+	o->have_attrs = have_attrs;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_obj_set_type(struct tee_obj *o, uint32_t obj_type,
+			    size_t max_key_size)
+{
+	TEE_Result res = TEE_SUCCESS;
+	const struct tee_cryp_obj_type_props *type_props;
+
+	/* Can only set type for newly allocated objs */
+	if (o->attr)
+		return TEE_ERROR_BAD_STATE;
+
+	/*
+	 * Verify that maxKeySize is supported and find out how
+	 * much should be allocated.
+	 */
+
+	if (obj_type == TEE_TYPE_DATA) {
+		if (max_key_size)
+			return TEE_ERROR_NOT_SUPPORTED;
+	} else {
+		/* Find description of object */
+		type_props = tee_svc_find_type_props(obj_type);
+		if (!type_props)
+			return TEE_ERROR_NOT_SUPPORTED;
+
+		/* Check that maxKeySize follows restrictions */
+		if (max_key_size % type_props->quanta != 0)
+			return TEE_ERROR_NOT_SUPPORTED;
+		if (max_key_size < type_props->min_size)
+			return TEE_ERROR_NOT_SUPPORTED;
+		if (max_key_size > type_props->max_size)
+			return TEE_ERROR_NOT_SUPPORTED;
+
+		o->attr = calloc(1, type_props->alloc_size);
+		if (!o->attr)
+			return TEE_ERROR_OUT_OF_MEMORY;
+	}
+
+	/* If we have a key structure, pre-allocate the bignums inside */
+	switch (obj_type) {
+	case TEE_TYPE_RSA_PUBLIC_KEY:
+		if (!crypto_ops.acipher.alloc_rsa_public_key)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_rsa_public_key(o->attr,
+							      max_key_size);
+		break;
+	case TEE_TYPE_RSA_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_rsa_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_rsa_keypair(o->attr,
+							   max_key_size);
+		break;
+	case TEE_TYPE_DSA_PUBLIC_KEY:
+		if (!crypto_ops.acipher.alloc_dsa_public_key)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_dsa_public_key(o->attr,
+							      max_key_size);
+		break;
+	case TEE_TYPE_DSA_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_dsa_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_dsa_keypair(o->attr,
+							   max_key_size);
+		break;
+	case TEE_TYPE_DH_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_dh_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_dh_keypair(o->attr,
+							  max_key_size);
+		break;
+	case TEE_TYPE_ECDSA_PUBLIC_KEY:
+	case TEE_TYPE_ECDH_PUBLIC_KEY:
+		if (!crypto_ops.acipher.alloc_ecc_public_key)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_ecc_public_key(o->attr,
+							      max_key_size);
+		break;
+	case TEE_TYPE_ECDSA_KEYPAIR:
+	case TEE_TYPE_ECDH_KEYPAIR:
+		if (!crypto_ops.acipher.alloc_ecc_keypair)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.acipher.alloc_ecc_keypair(o->attr,
+							   max_key_size);
+		break;
+	default:
+		if (obj_type != TEE_TYPE_DATA) {
+			struct tee_cryp_obj_secret *key = o->attr;
+
+			key->alloc_size = type_props->alloc_size -
+					  sizeof(*key);
+		}
+		break;
+	}
+
+	if (res != TEE_SUCCESS)
+		return res;
+
+	o->info.objectType = obj_type;
+	o->info.maxKeySize = max_key_size;
+	o->info.objectUsage = TEE_USAGE_DEFAULT;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_obj_alloc(unsigned long obj_type,
+			unsigned long max_key_size, uint32_t *obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	if (obj_type == TEE_TYPE_DATA)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	o = tee_obj_alloc();
+	if (!o)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = tee_obj_set_type(o, obj_type, max_key_size);
+	if (res != TEE_SUCCESS) {
+		tee_obj_free(o);
+		return res;
+	}
+
+	tee_obj_add(to_user_ta_ctx(sess->ctx), o);
+
+	res = tee_svc_copy_kaddr_to_uref(obj, o);
+	if (res != TEE_SUCCESS)
+		tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+	return res;
+}
+
+TEE_Result syscall_cryp_obj_close(unsigned long obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/*
+	 * If it's busy it's used by an operation, a client should never have
+	 * this handle.
+	 */
+	if (o->busy)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_obj_reset(unsigned long obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) == 0) {
+		tee_obj_attr_clear(o);
+		o->info.keySize = 0;
+		o->info.objectUsage = TEE_USAGE_DEFAULT;
+	} else {
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	/* the object is no more initialized */
+	o->info.handleFlags &= ~TEE_HANDLE_FLAG_INITIALIZED;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result copy_in_attrs(struct user_ta_ctx *utc,
+			const struct utee_attribute *usr_attrs,
+			uint32_t attr_count, TEE_Attribute *attrs)
+{
+	TEE_Result res;
+	uint32_t n;
+
+	res = tee_mmu_check_access_rights(utc,
+			TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_ANY_OWNER,
+			(uaddr_t)usr_attrs,
+			attr_count * sizeof(struct utee_attribute));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	for (n = 0; n < attr_count; n++) {
+		attrs[n].attributeID = usr_attrs[n].attribute_id;
+		if (attrs[n].attributeID & TEE_ATTR_BIT_VALUE) {
+			attrs[n].content.value.a = usr_attrs[n].a;
+			attrs[n].content.value.b = usr_attrs[n].b;
+		} else {
+			uintptr_t buf = usr_attrs[n].a;
+			size_t len = usr_attrs[n].b;
+
+			res = tee_mmu_check_access_rights(utc,
+				TEE_MEMORY_ACCESS_READ |
+				TEE_MEMORY_ACCESS_ANY_OWNER, buf, len);
+			if (res != TEE_SUCCESS)
+				return res;
+			attrs[n].content.ref.buffer = (void *)buf;
+			attrs[n].content.ref.length = len;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+enum attr_usage {
+	ATTR_USAGE_POPULATE,
+	ATTR_USAGE_GENERATE_KEY
+};
+
+static TEE_Result tee_svc_cryp_check_attr(enum attr_usage usage,
+					  const struct tee_cryp_obj_type_props
+						*type_props,
+					  const TEE_Attribute *attrs,
+					  uint32_t attr_count)
+{
+	uint32_t required_flag;
+	uint32_t opt_flag;
+	bool all_opt_needed;
+	uint32_t req_attrs = 0;
+	uint32_t opt_grp_attrs = 0;
+	uint32_t attrs_found = 0;
+	size_t n;
+	uint32_t bit;
+	uint32_t flags;
+	int idx;
+
+	if (usage == ATTR_USAGE_POPULATE) {
+		required_flag = TEE_TYPE_ATTR_REQUIRED;
+		opt_flag = TEE_TYPE_ATTR_OPTIONAL_GROUP;
+		all_opt_needed = true;
+	} else {
+		required_flag = TEE_TYPE_ATTR_GEN_KEY_REQ;
+		opt_flag = TEE_TYPE_ATTR_GEN_KEY_OPT;
+		all_opt_needed = false;
+	}
+
+	/*
+	 * First find out which attributes are required and which belong to
+	 * the optional group
+	 */
+	for (n = 0; n < type_props->num_type_attrs; n++) {
+		bit = 1 << n;
+		flags = type_props->type_attrs[n].flags;
+
+		if (flags & required_flag)
+			req_attrs |= bit;
+		else if (flags & opt_flag)
+			opt_grp_attrs |= bit;
+	}
+
+	/*
+	 * Verify that all required attributes are in place and
+	 * that the same attribute isn't repeated.
+	 */
+	for (n = 0; n < attr_count; n++) {
+		idx = tee_svc_cryp_obj_find_type_attr_idx(
+							attrs[n].attributeID,
+							type_props);
+
+		/* attribute not defined in current object type */
+		if (idx < 0)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+
+		bit = 1 << idx;
+
+		/* attribute not repeated */
+		if ((attrs_found & bit) != 0)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+
+		attrs_found |= bit;
+	}
+	/* Required attribute missing */
+	if ((attrs_found & req_attrs) != req_attrs)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	/*
+	 * If the flag says that "if one of the optional attributes are included
+	 * all of them has to be included" this must be checked.
+	 */
+	if (all_opt_needed && (attrs_found & opt_grp_attrs) != 0 &&
+	    (attrs_found & opt_grp_attrs) != opt_grp_attrs)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_cryp_obj_populate_type(
+		struct tee_obj *o,
+		const struct tee_cryp_obj_type_props *type_props,
+		const TEE_Attribute *attrs,
+		uint32_t attr_count)
+{
+	TEE_Result res;
+	uint32_t have_attrs = 0;
+	size_t obj_size = 0;
+	size_t n;
+	int idx;
+	const struct attr_ops *ops;
+	void *attr;
+
+	for (n = 0; n < attr_count; n++) {
+		idx = tee_svc_cryp_obj_find_type_attr_idx(
+							attrs[n].attributeID,
+							type_props);
+		/* attribute not defined in current object type */
+		if (idx < 0)
+			return TEE_ERROR_ITEM_NOT_FOUND;
+
+		have_attrs |= BIT32(idx);
+		ops = attr_ops + type_props->type_attrs[idx].ops_index;
+		attr = (uint8_t *)o->attr +
+		       type_props->type_attrs[idx].raw_offs;
+		if (attrs[n].attributeID & TEE_ATTR_BIT_VALUE)
+			res = ops->from_user(attr, &attrs[n].content.value,
+					     sizeof(attrs[n].content.value));
+		else
+			res = ops->from_user(attr, attrs[n].content.ref.buffer,
+					     attrs[n].content.ref.length);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		/*
+		 * First attr_idx signifies the attribute that gives the size
+		 * of the object
+		 */
+		if (type_props->type_attrs[idx].flags &
+		    TEE_TYPE_ATTR_SIZE_INDICATOR)
+			obj_size += attrs[n].content.ref.length * 8;
+	}
+
+	/*
+	 * We have to do it like this because the parity bits aren't counted
+	 * when telling the size of the key in bits.
+	 */
+	if (o->info.objectType == TEE_TYPE_DES ||
+	    o->info.objectType == TEE_TYPE_DES3)
+		obj_size -= obj_size / 8; /* Exclude parity in size of key */
+
+	o->have_attrs = have_attrs;
+	o->info.keySize = obj_size;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_obj_populate(unsigned long obj,
+			struct utee_attribute *usr_attrs,
+			unsigned long attr_count)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	const struct tee_cryp_obj_type_props *type_props;
+	TEE_Attribute *attrs = NULL;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Must be a transient object */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Must not be initialized already */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	type_props = tee_svc_find_type_props(o->info.objectType);
+	if (!type_props)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	attrs = malloc(sizeof(TEE_Attribute) * attr_count);
+	if (!attrs)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(to_user_ta_ctx(sess->ctx), usr_attrs, attr_count,
+			    attrs);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_cryp_check_attr(ATTR_USAGE_POPULATE, type_props,
+				      attrs, attr_count);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_cryp_obj_populate_type(o, type_props, attrs, attr_count);
+	if (res == TEE_SUCCESS)
+		o->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+
+out:
+	free(attrs);
+	return res;
+}
+
+TEE_Result syscall_cryp_obj_copy(unsigned long dst, unsigned long src)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *dst_o;
+	struct tee_obj *src_o;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(dst), &dst_o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(src), &src_o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if ((src_o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+	if ((dst_o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+	if ((dst_o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) != 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_obj_attr_copy_from(dst_o, src_o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	dst_o->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+	dst_o->info.keySize = src_o->info.keySize;
+	dst_o->info.objectUsage = src_o->info.objectUsage;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_rsa(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size,
+	const TEE_Attribute *params, uint32_t param_count)
+{
+	TEE_Result res;
+	struct rsa_keypair *key = o->attr;
+	uint32_t e = TEE_U32_TO_BIG_ENDIAN(65537);
+
+	if (!crypto_ops.acipher.gen_rsa_key || !crypto_ops.bignum.bin2bn)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	/* Copy the present attributes into the obj before starting */
+	res = tee_svc_cryp_obj_populate_type(o, type_props, params,
+					     param_count);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (!get_attribute(o, type_props, TEE_ATTR_RSA_PUBLIC_EXPONENT))
+		crypto_ops.bignum.bin2bn((const uint8_t *)&e, sizeof(e),
+					 key->e);
+	res = crypto_ops.acipher.gen_rsa_key(key, key_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for all known attributes for this object type */
+	o->have_attrs = (1 << type_props->num_type_attrs) - 1;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_dsa(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size)
+{
+	TEE_Result res;
+
+	if (!crypto_ops.acipher.gen_dsa_key)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.acipher.gen_dsa_key(o->attr, key_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for all known attributes for this object type */
+	o->have_attrs = (1 << type_props->num_type_attrs) - 1;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_dh(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size __unused,
+	const TEE_Attribute *params, uint32_t param_count)
+{
+	TEE_Result res;
+	struct dh_keypair *tee_dh_key;
+	struct bignum *dh_q = NULL;
+	uint32_t dh_xbits = 0;
+
+	/* Copy the present attributes into the obj before starting */
+	res = tee_svc_cryp_obj_populate_type(o, type_props, params,
+					     param_count);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	tee_dh_key = (struct dh_keypair *)o->attr;
+
+	if (get_attribute(o, type_props, TEE_ATTR_DH_SUBPRIME))
+		dh_q = tee_dh_key->q;
+	if (get_attribute(o, type_props, TEE_ATTR_DH_X_BITS))
+		dh_xbits = tee_dh_key->xbits;
+	if (!crypto_ops.acipher.gen_dh_key)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.acipher.gen_dh_key(tee_dh_key, dh_q, dh_xbits);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for the generated public and private key */
+	set_attribute(o, type_props, TEE_ATTR_DH_PUBLIC_VALUE);
+	set_attribute(o, type_props, TEE_ATTR_DH_PRIVATE_VALUE);
+	set_attribute(o, type_props, TEE_ATTR_DH_X_BITS);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_obj_generate_key_ecc(
+	struct tee_obj *o, const struct tee_cryp_obj_type_props *type_props,
+	uint32_t key_size __unused,
+	const TEE_Attribute *params, uint32_t param_count)
+{
+	TEE_Result res;
+	struct ecc_keypair *tee_ecc_key;
+
+	/* Copy the present attributes into the obj before starting */
+	res = tee_svc_cryp_obj_populate_type(o, type_props, params,
+					     param_count);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	tee_ecc_key = (struct ecc_keypair *)o->attr;
+
+	if (!crypto_ops.acipher.gen_ecc_key)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.acipher.gen_ecc_key(tee_ecc_key);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Set bits for the generated public and private key */
+	set_attribute(o, type_props, TEE_ATTR_ECC_PRIVATE_VALUE);
+	set_attribute(o, type_props, TEE_ATTR_ECC_PUBLIC_VALUE_X);
+	set_attribute(o, type_props, TEE_ATTR_ECC_PUBLIC_VALUE_Y);
+	set_attribute(o, type_props, TEE_ATTR_ECC_CURVE);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_obj_generate_key(unsigned long obj, unsigned long key_size,
+			const struct utee_attribute *usr_params,
+			unsigned long param_count)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	const struct tee_cryp_obj_type_props *type_props;
+	struct tee_obj *o;
+	struct tee_cryp_obj_secret *key;
+	size_t byte_size;
+	TEE_Attribute *params = NULL;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* Must be a transient object */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT) != 0)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Must not be initialized already */
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) != 0)
+		return TEE_ERROR_BAD_STATE;
+
+	/* Find description of object */
+	type_props = tee_svc_find_type_props(o->info.objectType);
+	if (!type_props)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	/* Check that maxKeySize follows restrictions */
+	if (key_size % type_props->quanta != 0)
+		return TEE_ERROR_NOT_SUPPORTED;
+	if (key_size < type_props->min_size)
+		return TEE_ERROR_NOT_SUPPORTED;
+	if (key_size > type_props->max_size)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	params = malloc(sizeof(TEE_Attribute) * param_count);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(to_user_ta_ctx(sess->ctx), usr_params, param_count,
+			    params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_cryp_check_attr(ATTR_USAGE_GENERATE_KEY, type_props,
+				      params, param_count);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	switch (o->info.objectType) {
+	case TEE_TYPE_AES:
+	case TEE_TYPE_DES:
+	case TEE_TYPE_DES3:
+	case TEE_TYPE_HMAC_MD5:
+	case TEE_TYPE_HMAC_SHA1:
+	case TEE_TYPE_HMAC_SHA224:
+	case TEE_TYPE_HMAC_SHA256:
+	case TEE_TYPE_HMAC_SHA384:
+	case TEE_TYPE_HMAC_SHA512:
+	case TEE_TYPE_GENERIC_SECRET:
+		byte_size = key_size / 8;
+
+		/*
+		 * We have to do it like this because the parity bits aren't
+		 * counted when telling the size of the key in bits.
+		 */
+		if (o->info.objectType == TEE_TYPE_DES ||
+		    o->info.objectType == TEE_TYPE_DES3) {
+			byte_size = (key_size + key_size / 7) / 8;
+		}
+
+		key = (struct tee_cryp_obj_secret *)o->attr;
+		if (byte_size > key->alloc_size) {
+			res = TEE_ERROR_EXCESS_DATA;
+			goto out;
+		}
+
+		res = crypto_ops.prng.read((void *)(key + 1), byte_size);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		key->key_size = byte_size;
+
+		/* Set bits for all known attributes for this object type */
+		o->have_attrs = (1 << type_props->num_type_attrs) - 1;
+
+		break;
+
+	case TEE_TYPE_RSA_KEYPAIR:
+		res = tee_svc_obj_generate_key_rsa(o, type_props, key_size,
+						   params, param_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	case TEE_TYPE_DSA_KEYPAIR:
+		res = tee_svc_obj_generate_key_dsa(o, type_props, key_size);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	case TEE_TYPE_DH_KEYPAIR:
+		res = tee_svc_obj_generate_key_dh(o, type_props, key_size,
+						  params, param_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	case TEE_TYPE_ECDSA_KEYPAIR:
+	case TEE_TYPE_ECDH_KEYPAIR:
+		res = tee_svc_obj_generate_key_ecc(o, type_props, key_size,
+						  params, param_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_FORMAT;
+	}
+
+out:
+	free(params);
+	if (res == TEE_SUCCESS) {
+		o->info.keySize = key_size;
+		o->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+	}
+	return res;
+}
+
+static TEE_Result tee_svc_cryp_get_state(struct tee_ta_session *sess,
+					 uint32_t state_id,
+					 struct tee_cryp_state **state)
+{
+	struct tee_cryp_state *s;
+	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+
+	TAILQ_FOREACH(s, &utc->cryp_states, link) {
+		if (state_id == (vaddr_t)s) {
+			*state = s;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+static void cryp_state_free(struct user_ta_ctx *utc, struct tee_cryp_state *cs)
+{
+	struct tee_obj *o;
+
+	if (tee_obj_get(utc, cs->key1, &o) == TEE_SUCCESS)
+		tee_obj_close(utc, o);
+	if (tee_obj_get(utc, cs->key2, &o) == TEE_SUCCESS)
+		tee_obj_close(utc, o);
+
+	TAILQ_REMOVE(&utc->cryp_states, cs, link);
+	if (cs->ctx_finalize != NULL)
+		cs->ctx_finalize(cs->ctx, cs->algo);
+	free(cs->ctx);
+	free(cs);
+}
+
+static TEE_Result tee_svc_cryp_check_key_type(const struct tee_obj *o,
+					      uint32_t algo,
+					      TEE_OperationMode mode)
+{
+	uint32_t req_key_type;
+	uint32_t req_key_type2 = 0;
+
+	switch (TEE_ALG_GET_MAIN_ALG(algo)) {
+	case TEE_MAIN_ALGO_MD5:
+		req_key_type = TEE_TYPE_HMAC_MD5;
+		break;
+	case TEE_MAIN_ALGO_SHA1:
+		req_key_type = TEE_TYPE_HMAC_SHA1;
+		break;
+	case TEE_MAIN_ALGO_SHA224:
+		req_key_type = TEE_TYPE_HMAC_SHA224;
+		break;
+	case TEE_MAIN_ALGO_SHA256:
+		req_key_type = TEE_TYPE_HMAC_SHA256;
+		break;
+	case TEE_MAIN_ALGO_SHA384:
+		req_key_type = TEE_TYPE_HMAC_SHA384;
+		break;
+	case TEE_MAIN_ALGO_SHA512:
+		req_key_type = TEE_TYPE_HMAC_SHA512;
+		break;
+	case TEE_MAIN_ALGO_AES:
+		req_key_type = TEE_TYPE_AES;
+		break;
+	case TEE_MAIN_ALGO_DES:
+		req_key_type = TEE_TYPE_DES;
+		break;
+	case TEE_MAIN_ALGO_DES3:
+		req_key_type = TEE_TYPE_DES3;
+		break;
+	case TEE_MAIN_ALGO_RSA:
+		req_key_type = TEE_TYPE_RSA_KEYPAIR;
+		if (mode == TEE_MODE_ENCRYPT || mode == TEE_MODE_VERIFY)
+			req_key_type2 = TEE_TYPE_RSA_PUBLIC_KEY;
+		break;
+	case TEE_MAIN_ALGO_DSA:
+		req_key_type = TEE_TYPE_DSA_KEYPAIR;
+		if (mode == TEE_MODE_ENCRYPT || mode == TEE_MODE_VERIFY)
+			req_key_type2 = TEE_TYPE_DSA_PUBLIC_KEY;
+		break;
+	case TEE_MAIN_ALGO_DH:
+		req_key_type = TEE_TYPE_DH_KEYPAIR;
+		break;
+	case TEE_MAIN_ALGO_ECDSA:
+		req_key_type = TEE_TYPE_ECDSA_KEYPAIR;
+		if (mode == TEE_MODE_VERIFY)
+			req_key_type2 = TEE_TYPE_ECDSA_PUBLIC_KEY;
+		break;
+	case TEE_MAIN_ALGO_ECDH:
+		req_key_type = TEE_TYPE_ECDH_KEYPAIR;
+		break;
+#if defined(CFG_CRYPTO_HKDF)
+	case TEE_MAIN_ALGO_HKDF:
+		req_key_type = TEE_TYPE_HKDF_IKM;
+		break;
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+	case TEE_MAIN_ALGO_CONCAT_KDF:
+		req_key_type = TEE_TYPE_CONCAT_KDF_Z;
+		break;
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+	case TEE_MAIN_ALGO_PBKDF2:
+		req_key_type = TEE_TYPE_PBKDF2_PASSWORD;
+		break;
+#endif
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (req_key_type != o->info.objectType &&
+	    req_key_type2 != o->info.objectType)
+		return TEE_ERROR_BAD_PARAMETERS;
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_cryp_state_alloc(unsigned long algo, unsigned long mode,
+			unsigned long key1, unsigned long key2,
+			uint32_t *state)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o1 = NULL;
+	struct tee_obj *o2 = NULL;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	if (key1 != 0) {
+		res = tee_obj_get(utc, tee_svc_uref_to_vaddr(key1), &o1);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (o1->busy)
+			return TEE_ERROR_BAD_PARAMETERS;
+		res = tee_svc_cryp_check_key_type(o1, algo, mode);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+	if (key2 != 0) {
+		res = tee_obj_get(utc, tee_svc_uref_to_vaddr(key2), &o2);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (o2->busy)
+			return TEE_ERROR_BAD_PARAMETERS;
+		res = tee_svc_cryp_check_key_type(o2, algo, mode);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	cs = calloc(1, sizeof(struct tee_cryp_state));
+	if (!cs)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	TAILQ_INSERT_TAIL(&utc->cryp_states, cs, link);
+	cs->algo = algo;
+	cs->mode = mode;
+
+	switch (TEE_ALG_GET_CLASS(algo)) {
+	case TEE_OPERATION_CIPHER:
+		if ((algo == TEE_ALG_AES_XTS && (key1 == 0 || key2 == 0)) ||
+		    (algo != TEE_ALG_AES_XTS && (key1 == 0 || key2 != 0))) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.cipher.get_ctx_size)
+				res = crypto_ops.cipher.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_AE:
+		if (key1 == 0 || key2 != 0) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.authenc.get_ctx_size)
+				res = crypto_ops.authenc.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_MAC:
+		if (key1 == 0 || key2 != 0) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.mac.get_ctx_size)
+				res = crypto_ops.mac.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_DIGEST:
+		if (key1 != 0 || key2 != 0) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		} else {
+			if (crypto_ops.hash.get_ctx_size)
+				res = crypto_ops.hash.get_ctx_size(algo,
+								&cs->ctx_size);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+			if (res != TEE_SUCCESS)
+				break;
+			cs->ctx = calloc(1, cs->ctx_size);
+			if (!cs->ctx)
+				res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		break;
+	case TEE_OPERATION_ASYMMETRIC_CIPHER:
+	case TEE_OPERATION_ASYMMETRIC_SIGNATURE:
+		if (key1 == 0 || key2 != 0)
+			res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	case TEE_OPERATION_KEY_DERIVATION:
+		if (key1 == 0 || key2 != 0)
+			res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	default:
+		res = TEE_ERROR_NOT_SUPPORTED;
+		break;
+	}
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_svc_copy_kaddr_to_uref(state, cs);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Register keys */
+	if (o1 != NULL) {
+		o1->busy = true;
+		cs->key1 = (vaddr_t)o1;
+	}
+	if (o2 != NULL) {
+		o2->busy = true;
+		cs->key2 = (vaddr_t)o2;
+	}
+
+out:
+	if (res != TEE_SUCCESS)
+		cryp_state_free(utc, cs);
+	return res;
+}
+
+TEE_Result syscall_cryp_state_copy(unsigned long dst, unsigned long src)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs_dst;
+	struct tee_cryp_state *cs_src;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(dst), &cs_dst);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(src), &cs_src);
+	if (res != TEE_SUCCESS)
+		return res;
+	if (cs_dst->algo != cs_src->algo || cs_dst->mode != cs_src->mode)
+		return TEE_ERROR_BAD_PARAMETERS;
+	/* "Can't happen" */
+	if (cs_dst->ctx_size != cs_src->ctx_size)
+		return TEE_ERROR_BAD_STATE;
+
+	memcpy(cs_dst->ctx, cs_src->ctx, cs_src->ctx_size);
+	return TEE_SUCCESS;
+}
+
+void tee_svc_cryp_free_states(struct user_ta_ctx *utc)
+{
+	struct tee_cryp_state_head *states = &utc->cryp_states;
+
+	while (!TAILQ_EMPTY(states))
+		cryp_state_free(utc, TAILQ_FIRST(states));
+}
+
+TEE_Result syscall_cryp_state_free(unsigned long state)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+	cryp_state_free(to_user_ta_ctx(sess->ctx), cs);
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_hash_init(unsigned long state,
+			     const void *iv __maybe_unused,
+			     size_t iv_len __maybe_unused)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (TEE_ALG_GET_CLASS(cs->algo)) {
+	case TEE_OPERATION_DIGEST:
+		if (!crypto_ops.hash.init)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.hash.init(cs->ctx, cs->algo);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+	case TEE_OPERATION_MAC:
+		{
+			struct tee_obj *o;
+			struct tee_cryp_obj_secret *key;
+
+			res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+					  cs->key1, &o);
+			if (res != TEE_SUCCESS)
+				return res;
+			if ((o->info.handleFlags &
+			     TEE_HANDLE_FLAG_INITIALIZED) == 0)
+				return TEE_ERROR_BAD_PARAMETERS;
+
+			key = (struct tee_cryp_obj_secret *)o->attr;
+			if (!crypto_ops.mac.init)
+				return TEE_ERROR_NOT_IMPLEMENTED;
+			res = crypto_ops.mac.init(cs->ctx, cs->algo,
+						  (void *)(key + 1),
+						  key->key_size);
+			if (res != TEE_SUCCESS)
+				return res;
+			break;
+		}
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_hash_update(unsigned long state, const void *chunk,
+			size_t chunk_size)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	/* No data, but size provided isn't valid parameters. */
+	if (!chunk && chunk_size)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	/* Zero length hash is valid, but nothing we need to do. */
+	if (!chunk_size)
+		return TEE_SUCCESS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)chunk, chunk_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (TEE_ALG_GET_CLASS(cs->algo)) {
+	case TEE_OPERATION_DIGEST:
+		if (!crypto_ops.hash.update)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.hash.update(cs->ctx, cs->algo, chunk,
+					     chunk_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+	case TEE_OPERATION_MAC:
+		if (!crypto_ops.mac.update)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = crypto_ops.mac.update(cs->ctx, cs->algo, chunk,
+					    chunk_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_hash_final(unsigned long state, const void *chunk,
+			size_t chunk_size, void *hash, uint64_t *hash_len)
+{
+	TEE_Result res, res2;
+	size_t hash_size;
+	uint64_t hlen;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	/* No data, but size provided isn't valid parameters. */
+	if (!chunk && chunk_size)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)chunk, chunk_size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&hlen, hash_len, sizeof(hlen));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)hash, hlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	switch (TEE_ALG_GET_CLASS(cs->algo)) {
+	case TEE_OPERATION_DIGEST:
+		if (!crypto_ops.hash.update || !crypto_ops.hash.final)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = tee_hash_get_digest_size(cs->algo, &hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (*hash_len < hash_size) {
+			res = TEE_ERROR_SHORT_BUFFER;
+			goto out;
+		}
+
+		if (chunk_size) {
+			res = crypto_ops.hash.update(cs->ctx, cs->algo, chunk,
+						     chunk_size);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = crypto_ops.hash.final(cs->ctx, cs->algo, hash,
+					    hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+
+	case TEE_OPERATION_MAC:
+		if (!crypto_ops.mac.update || !crypto_ops.mac.final)
+			return TEE_ERROR_NOT_IMPLEMENTED;
+		res = tee_mac_get_digest_size(cs->algo, &hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		if (*hash_len < hash_size) {
+			res = TEE_ERROR_SHORT_BUFFER;
+			goto out;
+		}
+
+		if (chunk_size) {
+			res = crypto_ops.mac.update(cs->ctx, cs->algo, chunk,
+						    chunk_size);
+			if (res != TEE_SUCCESS)
+				return res;
+		}
+
+		res = crypto_ops.mac.final(cs->ctx, cs->algo, hash, hash_size);
+		if (res != TEE_SUCCESS)
+			return res;
+		break;
+
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+out:
+	hlen = hash_size;
+	res2 = tee_svc_copy_to_user(hash_len, &hlen, sizeof(*hash_len));
+	if (res2 != TEE_SUCCESS)
+		return res2;
+	return res;
+}
+
+TEE_Result syscall_cipher_init(unsigned long state, const void *iv,
+			size_t iv_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct tee_cryp_obj_secret *key1;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) iv, iv_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(utc, cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		return res;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	key1 = o->attr;
+
+	if (!crypto_ops.cipher.init)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+
+	if (tee_obj_get(utc, cs->key2, &o) == TEE_SUCCESS) {
+		struct tee_cryp_obj_secret *key2 = o->attr;
+
+		if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+			return TEE_ERROR_BAD_PARAMETERS;
+
+		res = crypto_ops.cipher.init(cs->ctx, cs->algo, cs->mode,
+					     (uint8_t *)(key1 + 1),
+					     key1->key_size,
+					     (uint8_t *)(key2 + 1),
+					     key2->key_size,
+					     iv, iv_len);
+	} else {
+		res = crypto_ops.cipher.init(cs->ctx, cs->algo, cs->mode,
+					     (uint8_t *)(key1 + 1),
+					     key1->key_size,
+					     NULL,
+					     0,
+					     iv, iv_len);
+	}
+	if (res != TEE_SUCCESS)
+		return res;
+
+	cs->ctx_finalize = crypto_ops.cipher.final;
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_cipher_update_helper(unsigned long state,
+			bool last_block, const void *src, size_t src_len,
+			void *dst, uint64_t *dst_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)src, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!dst_len) {
+		dlen = 0;
+	} else {
+		res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_WRITE |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t)dst, dlen);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	if (src_len > 0) {
+		/* Permit src_len == 0 to finalize the operation */
+		res = tee_do_cipher_update(cs->ctx, cs->algo, cs->mode,
+					   last_block, src, src_len, dst);
+	}
+
+	if (last_block && cs->ctx_finalize != NULL) {
+		cs->ctx_finalize(cs->ctx, cs->algo);
+		cs->ctx_finalize = NULL;
+	}
+
+out:
+	if ((res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) &&
+	    dst_len != NULL) {
+		TEE_Result res2;
+
+		dlen = src_len;
+		res2 = tee_svc_copy_to_user(dst_len, &dlen, sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			res = res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_cipher_update(unsigned long state, const void *src,
+			size_t src_len, void *dst, uint64_t *dst_len)
+{
+	return tee_svc_cipher_update_helper(state, false /* last_block */,
+					    src, src_len, dst, dst_len);
+}
+
+TEE_Result syscall_cipher_final(unsigned long state, const void *src,
+			size_t src_len, void *dst, uint64_t *dst_len)
+{
+	return tee_svc_cipher_update_helper(state, true /* last_block */,
+					    src, src_len, dst, dst_len);
+}
+
+#if defined(CFG_CRYPTO_HKDF)
+static TEE_Result get_hkdf_params(const TEE_Attribute *params,
+				  uint32_t param_count,
+				  void **salt, size_t *salt_len, void **info,
+				  size_t *info_len, size_t *okm_len)
+{
+	size_t n;
+	enum { SALT = 0x1, LENGTH = 0x2, INFO = 0x4 };
+	uint8_t found = 0;
+
+	*salt = *info = NULL;
+	*salt_len = *info_len = *okm_len = 0;
+
+	for (n = 0; n < param_count; n++) {
+		switch (params[n].attributeID) {
+		case TEE_ATTR_HKDF_SALT:
+			if (!(found & SALT)) {
+				*salt = params[n].content.ref.buffer;
+				*salt_len = params[n].content.ref.length;
+				found |= SALT;
+			}
+			break;
+		case TEE_ATTR_HKDF_OKM_LENGTH:
+			if (!(found & LENGTH)) {
+				*okm_len = params[n].content.value.a;
+				found |= LENGTH;
+			}
+			break;
+		case TEE_ATTR_HKDF_INFO:
+			if (!(found & INFO)) {
+				*info = params[n].content.ref.buffer;
+				*info_len = params[n].content.ref.length;
+				found |= INFO;
+			}
+			break;
+		default:
+			/* Unexpected attribute */
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+
+	}
+
+	if (!(found & LENGTH))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_SUCCESS;
+}
+#endif
+
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+static TEE_Result get_concat_kdf_params(const TEE_Attribute *params,
+					uint32_t param_count,
+					void **other_info,
+					size_t *other_info_len,
+					size_t *derived_key_len)
+{
+	size_t n;
+	enum { LENGTH = 0x1, INFO = 0x2 };
+	uint8_t found = 0;
+
+	*other_info = NULL;
+	*other_info_len = *derived_key_len = 0;
+
+	for (n = 0; n < param_count; n++) {
+		switch (params[n].attributeID) {
+		case TEE_ATTR_CONCAT_KDF_OTHER_INFO:
+			if (!(found & INFO)) {
+				*other_info = params[n].content.ref.buffer;
+				*other_info_len = params[n].content.ref.length;
+				found |= INFO;
+			}
+			break;
+		case TEE_ATTR_CONCAT_KDF_DKM_LENGTH:
+			if (!(found & LENGTH)) {
+				*derived_key_len = params[n].content.value.a;
+				found |= LENGTH;
+			}
+			break;
+		default:
+			/* Unexpected attribute */
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	}
+
+	if (!(found & LENGTH))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_SUCCESS;
+}
+#endif
+
+#if defined(CFG_CRYPTO_PBKDF2)
+static TEE_Result get_pbkdf2_params(const TEE_Attribute *params,
+				   uint32_t param_count, void **salt,
+				   size_t *salt_len, size_t *derived_key_len,
+				   size_t *iteration_count)
+{
+	size_t n;
+	enum { SALT = 0x1, LENGTH = 0x2, COUNT = 0x4 };
+	uint8_t found = 0;
+
+	*salt = NULL;
+	*salt_len = *derived_key_len = *iteration_count = 0;
+
+	for (n = 0; n < param_count; n++) {
+		switch (params[n].attributeID) {
+		case TEE_ATTR_PBKDF2_SALT:
+			if (!(found & SALT)) {
+				*salt = params[n].content.ref.buffer;
+				*salt_len = params[n].content.ref.length;
+				found |= SALT;
+			}
+			break;
+		case TEE_ATTR_PBKDF2_DKM_LENGTH:
+			if (!(found & LENGTH)) {
+				*derived_key_len = params[n].content.value.a;
+				found |= LENGTH;
+			}
+			break;
+		case TEE_ATTR_PBKDF2_ITERATION_COUNT:
+			if (!(found & COUNT)) {
+				*iteration_count = params[n].content.value.a;
+				found |= COUNT;
+			}
+			break;
+		default:
+			/* Unexpected attribute */
+			return TEE_ERROR_BAD_PARAMETERS;
+		}
+	}
+
+	if ((found & (LENGTH|COUNT)) != (LENGTH|COUNT))
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	return TEE_SUCCESS;
+}
+#endif
+
+TEE_Result syscall_cryp_derive_key(unsigned long state,
+			const struct utee_attribute *usr_params,
+			unsigned long param_count, unsigned long derived_key)
+{
+	TEE_Result res = TEE_ERROR_NOT_SUPPORTED;
+	struct tee_ta_session *sess;
+	struct tee_obj *ko;
+	struct tee_obj *so;
+	struct tee_cryp_state *cs;
+	struct tee_cryp_obj_secret *sk;
+	const struct tee_cryp_obj_type_props *type_props;
+	TEE_Attribute *params = NULL;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	params = malloc(sizeof(TEE_Attribute) * param_count);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(utc, usr_params, param_count, params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Get key set in operation */
+	res = tee_obj_get(utc, cs->key1, &ko);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(derived_key), &so);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* Find information needed about the object to initialize */
+	sk = so->attr;
+
+	/* Find description of object */
+	type_props = tee_svc_find_type_props(so->info.objectType);
+	if (!type_props) {
+		res = TEE_ERROR_NOT_SUPPORTED;
+		goto out;
+	}
+
+	if (cs->algo == TEE_ALG_DH_DERIVE_SHARED_SECRET) {
+		size_t alloc_size;
+		struct bignum *pub;
+		struct bignum *ss;
+
+		if (!crypto_ops.bignum.allocate ||
+		    !crypto_ops.bignum.free ||
+		    !crypto_ops.bignum.bin2bn ||
+		    !crypto_ops.bignum.bn2bin ||
+		    !crypto_ops.bignum.num_bytes ||
+		    !crypto_ops.acipher.dh_shared_secret) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			goto out;
+		}
+		if (param_count != 1 ||
+		    params[0].attributeID != TEE_ATTR_DH_PUBLIC_VALUE) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		alloc_size = params[0].content.ref.length * 8;
+		pub = crypto_ops.bignum.allocate(alloc_size);
+		ss = crypto_ops.bignum.allocate(alloc_size);
+		if (pub && ss) {
+			crypto_ops.bignum.bin2bn(params[0].content.ref.buffer,
+					params[0].content.ref.length, pub);
+			res = crypto_ops.acipher.dh_shared_secret(ko->attr,
+								  pub, ss);
+			if (res == TEE_SUCCESS) {
+				sk->key_size = crypto_ops.bignum.num_bytes(ss);
+				crypto_ops.bignum.bn2bin(ss,
+							 (uint8_t *)(sk + 1));
+				so->info.handleFlags |=
+						TEE_HANDLE_FLAG_INITIALIZED;
+				set_attribute(so, type_props,
+					      TEE_ATTR_SECRET_VALUE);
+			}
+		} else {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+		}
+		crypto_ops.bignum.free(pub);
+		crypto_ops.bignum.free(ss);
+	} else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_ECDH) {
+		size_t alloc_size;
+		struct ecc_public_key key_public;
+		uint8_t *pt_secret;
+		unsigned long pt_secret_len;
+
+		if (!crypto_ops.bignum.bin2bn ||
+		    !crypto_ops.acipher.alloc_ecc_public_key ||
+		    !crypto_ops.acipher.free_ecc_public_key ||
+		    !crypto_ops.acipher.ecc_shared_secret) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			goto out;
+		}
+		if (param_count != 2 ||
+		    params[0].attributeID != TEE_ATTR_ECC_PUBLIC_VALUE_X ||
+		    params[1].attributeID != TEE_ATTR_ECC_PUBLIC_VALUE_Y) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		switch (cs->algo) {
+		case TEE_ALG_ECDH_P192:
+			alloc_size = 192;
+			break;
+		case TEE_ALG_ECDH_P224:
+			alloc_size = 224;
+			break;
+		case TEE_ALG_ECDH_P256:
+			alloc_size = 256;
+			break;
+		case TEE_ALG_ECDH_P384:
+			alloc_size = 384;
+			break;
+		case TEE_ALG_ECDH_P521:
+			alloc_size = 521;
+			break;
+		default:
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			goto out;
+		}
+
+		/* Create the public key */
+		res = crypto_ops.acipher.alloc_ecc_public_key(&key_public,
+							      alloc_size);
+		if (res != TEE_SUCCESS)
+			goto out;
+		key_public.curve = ((struct ecc_keypair *)ko->attr)->curve;
+		crypto_ops.bignum.bin2bn(params[0].content.ref.buffer,
+					 params[0].content.ref.length,
+					 key_public.x);
+		crypto_ops.bignum.bin2bn(params[1].content.ref.buffer,
+					 params[1].content.ref.length,
+					 key_public.y);
+
+		pt_secret = (uint8_t *)(sk + 1);
+		pt_secret_len = sk->alloc_size;
+		res = crypto_ops.acipher.ecc_shared_secret(ko->attr,
+				&key_public, pt_secret, &pt_secret_len);
+
+		if (res == TEE_SUCCESS) {
+			sk->key_size = pt_secret_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+
+		/* free the public key */
+		crypto_ops.acipher.free_ecc_public_key(&key_public);
+	}
+#if defined(CFG_CRYPTO_HKDF)
+	else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_HKDF) {
+		void *salt, *info;
+		size_t salt_len, info_len, okm_len;
+		uint32_t hash_id = TEE_ALG_GET_DIGEST_HASH(cs->algo);
+		struct tee_cryp_obj_secret *ik = ko->attr;
+		const uint8_t *ikm = (const uint8_t *)(ik + 1);
+
+		res = get_hkdf_params(params, param_count, &salt, &salt_len,
+				      &info, &info_len, &okm_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		/* Requested size must fit into the output object's buffer */
+		if (okm_len > ik->alloc_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		res = tee_cryp_hkdf(hash_id, ikm, ik->key_size, salt, salt_len,
+				    info, info_len, (uint8_t *)(sk + 1),
+				    okm_len);
+		if (res == TEE_SUCCESS) {
+			sk->key_size = okm_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+	}
+#endif
+#if defined(CFG_CRYPTO_CONCAT_KDF)
+	else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_CONCAT_KDF) {
+		void *info;
+		size_t info_len, derived_key_len;
+		uint32_t hash_id = TEE_ALG_GET_DIGEST_HASH(cs->algo);
+		struct tee_cryp_obj_secret *ss = ko->attr;
+		const uint8_t *shared_secret = (const uint8_t *)(ss + 1);
+
+		res = get_concat_kdf_params(params, param_count, &info,
+					    &info_len, &derived_key_len);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		/* Requested size must fit into the output object's buffer */
+		if (derived_key_len > ss->alloc_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		res = tee_cryp_concat_kdf(hash_id, shared_secret, ss->key_size,
+					  info, info_len, (uint8_t *)(sk + 1),
+					  derived_key_len);
+		if (res == TEE_SUCCESS) {
+			sk->key_size = derived_key_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+	}
+#endif
+#if defined(CFG_CRYPTO_PBKDF2)
+	else if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_PBKDF2) {
+		void *salt;
+		size_t salt_len, iteration_count, derived_key_len;
+		uint32_t hash_id = TEE_ALG_GET_DIGEST_HASH(cs->algo);
+		struct tee_cryp_obj_secret *ss = ko->attr;
+		const uint8_t *password = (const uint8_t *)(ss + 1);
+
+		res = get_pbkdf2_params(params, param_count, &salt, &salt_len,
+					&derived_key_len, &iteration_count);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		/* Requested size must fit into the output object's buffer */
+		if (derived_key_len > ss->alloc_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+
+		res = tee_cryp_pbkdf2(hash_id, password, ss->key_size, salt,
+				      salt_len, iteration_count,
+				      (uint8_t *)(sk + 1), derived_key_len);
+		if (res == TEE_SUCCESS) {
+			sk->key_size = derived_key_len;
+			so->info.handleFlags |= TEE_HANDLE_FLAG_INITIALIZED;
+			set_attribute(so, type_props, TEE_ATTR_SECRET_VALUE);
+		}
+	}
+#endif
+	else
+		res = TEE_ERROR_NOT_SUPPORTED;
+
+out:
+	free(params);
+	return res;
+}
+
+TEE_Result syscall_cryp_random_number_generate(void *buf, size_t blen)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)buf, blen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = crypto_ops.prng.read(buf, blen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return res;
+}
+
+TEE_Result syscall_authenc_init(unsigned long state, const void *nonce,
+			size_t nonce_len, size_t tag_len,
+			size_t aad_len, size_t payload_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct tee_cryp_obj_secret *key;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx), cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		return res;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	if (!crypto_ops.authenc.init)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	key = o->attr;
+	res = crypto_ops.authenc.init(cs->ctx, cs->algo, cs->mode,
+				      (uint8_t *)(key + 1), key->key_size,
+				      nonce, nonce_len, tag_len, aad_len,
+				      payload_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	cs->ctx_finalize = (tee_cryp_ctx_finalize_func_t)
+				crypto_ops.authenc.final;
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_authenc_update_aad(unsigned long state,
+			const void *aad_data, size_t aad_data_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) aad_data,
+					  aad_data_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!crypto_ops.authenc.update_aad)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	res = crypto_ops.authenc.update_aad(cs->ctx, cs->algo, cs->mode,
+					    aad_data, aad_data_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return TEE_SUCCESS;
+}
+
+TEE_Result syscall_authenc_update_payload(unsigned long state,
+			const void *src_data, size_t src_len, void *dst_data,
+			uint64_t *dst_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+	size_t tmp_dlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)dst_data, dlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	if (!crypto_ops.authenc.update_payload)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	tmp_dlen = dlen;
+	res = crypto_ops.authenc.update_payload(cs->ctx, cs->algo, cs->mode,
+						src_data, src_len, dst_data,
+						&tmp_dlen);
+	dlen = tmp_dlen;
+
+out:
+	if (res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) {
+		TEE_Result res2 = tee_svc_copy_to_user(dst_len, &dlen,
+						       sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			res = res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_authenc_enc_final(unsigned long state,
+			const void *src_data, size_t src_len, void *dst_data,
+			uint64_t *dst_len, void *tag, uint64_t *tag_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+	uint64_t tlen;
+	size_t tmp_dlen;
+	size_t tmp_tlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (cs->mode != TEE_MODE_ENCRYPT)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!dst_len) {
+		dlen = 0;
+	} else {
+		res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_WRITE |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t)dst_data, dlen);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = tee_svc_copy_from_user(&tlen, tag_len, sizeof(tlen));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_WRITE |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)tag, tlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!crypto_ops.authenc.enc_final)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	tmp_dlen = dlen;
+	tmp_tlen = tlen;
+	res = crypto_ops.authenc.enc_final(cs->ctx, cs->algo, src_data,
+					   src_len, dst_data, &tmp_dlen, tag,
+					   &tmp_tlen);
+	dlen = tmp_dlen;
+	tlen = tmp_tlen;
+
+out:
+	if (res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) {
+		TEE_Result res2;
+
+		if (dst_len != NULL) {
+			res2 = tee_svc_copy_to_user(dst_len, &dlen,
+						    sizeof(*dst_len));
+			if (res2 != TEE_SUCCESS)
+				return res2;
+		}
+
+		res2 = tee_svc_copy_to_user(tag_len, &tlen, sizeof(*tag_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_authenc_dec_final(unsigned long state,
+			const void *src_data, size_t src_len, void *dst_data,
+			uint64_t *dst_len, const void *tag, size_t tag_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen;
+	size_t tmp_dlen;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (cs->mode != TEE_MODE_DECRYPT)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!dst_len) {
+		dlen = 0;
+	} else {
+		res = tee_svc_copy_from_user(&dlen, dst_len, sizeof(dlen));
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_WRITE |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t)dst_data, dlen);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (dlen < src_len) {
+		res = TEE_ERROR_SHORT_BUFFER;
+		goto out;
+	}
+
+	res = tee_mmu_check_access_rights(to_user_ta_ctx(sess->ctx),
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)tag, tag_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!crypto_ops.authenc.dec_final)
+		return TEE_ERROR_NOT_IMPLEMENTED;
+	tmp_dlen = dlen;
+	res = crypto_ops.authenc.dec_final(cs->ctx, cs->algo, src_data,
+					   src_len, dst_data, &tmp_dlen, tag,
+					   tag_len);
+	dlen = tmp_dlen;
+
+out:
+	if ((res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) &&
+	    dst_len != NULL) {
+		TEE_Result res2;
+
+		res2 = tee_svc_copy_to_user(dst_len, &dlen, sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+	}
+
+	return res;
+}
+
+static int pkcs1_get_salt_len(const TEE_Attribute *params, uint32_t num_params,
+			      size_t default_len)
+{
+	size_t n;
+
+	assert(default_len < INT_MAX);
+
+	for (n = 0; n < num_params; n++) {
+		if (params[n].attributeID == TEE_ATTR_RSA_PSS_SALT_LENGTH) {
+			if (params[n].content.value.a < INT_MAX)
+				return params[n].content.value.a;
+			break;
+		}
+	}
+	/*
+	 * If salt length isn't provided use the default value which is
+	 * the length of the digest.
+	 */
+	return default_len;
+}
+
+TEE_Result syscall_asymm_operate(unsigned long state,
+			const struct utee_attribute *usr_params,
+			size_t num_params, const void *src_data, size_t src_len,
+			void *dst_data, uint64_t *dst_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	uint64_t dlen64;
+	size_t dlen;
+	struct tee_obj *o;
+	void *label = NULL;
+	size_t label_len = 0;
+	size_t n;
+	int salt_len;
+	TEE_Attribute *params = NULL;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(
+		utc,
+		TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_ANY_OWNER,
+		(uaddr_t) src_data, src_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_copy_from_user(&dlen64, dst_len, sizeof(dlen64));
+	if (res != TEE_SUCCESS)
+		return res;
+	dlen = dlen64;
+
+	res = tee_mmu_check_access_rights(
+		utc,
+		TEE_MEMORY_ACCESS_READ | TEE_MEMORY_ACCESS_WRITE |
+			TEE_MEMORY_ACCESS_ANY_OWNER,
+		(uaddr_t) dst_data, dlen);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	params = malloc(sizeof(TEE_Attribute) * num_params);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(utc, usr_params, num_params, params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_obj_get(utc, cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		goto out;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0) {
+		res = TEE_ERROR_GENERIC;
+		goto out;
+	}
+
+	switch (cs->algo) {
+	case TEE_ALG_RSA_NOPAD:
+		if (cs->mode == TEE_MODE_ENCRYPT) {
+			if (crypto_ops.acipher.rsanopad_encrypt)
+				res = crypto_ops.acipher.rsanopad_encrypt(
+					o->attr, src_data, src_len,
+					dst_data, &dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else if (cs->mode == TEE_MODE_DECRYPT) {
+			if (crypto_ops.acipher.rsanopad_decrypt)
+				res = crypto_ops.acipher.rsanopad_decrypt(
+					o->attr, src_data, src_len, dst_data,
+					&dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else {
+			/*
+			 * We will panic because "the mode is not compatible
+			 * with the function"
+			 */
+			res = TEE_ERROR_GENERIC;
+		}
+		break;
+
+	case TEE_ALG_RSAES_PKCS1_V1_5:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA1:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA224:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA256:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA384:
+	case TEE_ALG_RSAES_PKCS1_OAEP_MGF1_SHA512:
+		for (n = 0; n < num_params; n++) {
+			if (params[n].attributeID == TEE_ATTR_RSA_OAEP_LABEL) {
+				label = params[n].content.ref.buffer;
+				label_len = params[n].content.ref.length;
+				break;
+			}
+		}
+
+		if (cs->mode == TEE_MODE_ENCRYPT) {
+			if (crypto_ops.acipher.rsaes_encrypt)
+				res = crypto_ops.acipher.rsaes_encrypt(
+					cs->algo, o->attr, label, label_len,
+					src_data, src_len, dst_data, &dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else if (cs->mode == TEE_MODE_DECRYPT) {
+			if (crypto_ops.acipher.rsaes_decrypt)
+				res = crypto_ops.acipher.rsaes_decrypt(
+					cs->algo, o->attr,
+					label, label_len,
+					src_data, src_len, dst_data, &dlen);
+			else
+				res = TEE_ERROR_NOT_IMPLEMENTED;
+		} else {
+			res = TEE_ERROR_BAD_PARAMETERS;
+		}
+		break;
+
+	case TEE_ALG_RSASSA_PKCS1_V1_5_MD5:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_V1_5_SHA512:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA1:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA224:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA256:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA384:
+	case TEE_ALG_RSASSA_PKCS1_PSS_MGF1_SHA512:
+		if (cs->mode != TEE_MODE_SIGN) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			break;
+		}
+		salt_len = pkcs1_get_salt_len(params, num_params, src_len);
+		if (!crypto_ops.acipher.rsassa_sign) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.rsassa_sign(cs->algo, o->attr,
+						     salt_len, src_data,
+						     src_len, dst_data, &dlen);
+		break;
+
+	case TEE_ALG_DSA_SHA1:
+	case TEE_ALG_DSA_SHA224:
+	case TEE_ALG_DSA_SHA256:
+		if (!crypto_ops.acipher.dsa_sign) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.dsa_sign(cs->algo, o->attr, src_data,
+						  src_len, dst_data, &dlen);
+		break;
+	case TEE_ALG_ECDSA_P192:
+	case TEE_ALG_ECDSA_P224:
+	case TEE_ALG_ECDSA_P256:
+	case TEE_ALG_ECDSA_P384:
+	case TEE_ALG_ECDSA_P521:
+		if (!crypto_ops.acipher.ecc_sign) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.ecc_sign(cs->algo, o->attr, src_data,
+						  src_len, dst_data, &dlen);
+		break;
+
+	default:
+		res = TEE_ERROR_BAD_PARAMETERS;
+		break;
+	}
+
+out:
+	free(params);
+
+	if (res == TEE_SUCCESS || res == TEE_ERROR_SHORT_BUFFER) {
+		TEE_Result res2;
+
+		dlen64 = dlen;
+		res2 = tee_svc_copy_to_user(dst_len, &dlen64, sizeof(*dst_len));
+		if (res2 != TEE_SUCCESS)
+			return res2;
+	}
+
+	return res;
+}
+
+TEE_Result syscall_asymm_verify(unsigned long state,
+			const struct utee_attribute *usr_params,
+			size_t num_params, const void *data, size_t data_len,
+			const void *sig, size_t sig_len)
+{
+	TEE_Result res;
+	struct tee_cryp_state *cs;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	size_t hash_size;
+	int salt_len;
+	TEE_Attribute *params = NULL;
+	uint32_t hash_algo;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_cryp_get_state(sess, tee_svc_uref_to_vaddr(state), &cs);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (cs->mode != TEE_MODE_VERIFY)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)data, data_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t)sig, sig_len);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	params = malloc(sizeof(TEE_Attribute) * num_params);
+	if (!params)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	res = copy_in_attrs(utc, usr_params, num_params, params);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = tee_obj_get(utc, cs->key1, &o);
+	if (res != TEE_SUCCESS)
+		goto out;
+	if ((o->info.handleFlags & TEE_HANDLE_FLAG_INITIALIZED) == 0) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto out;
+	}
+
+	if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_ECDSA)
+		hash_algo = TEE_ALG_SHA1;
+	else
+		hash_algo = TEE_DIGEST_HASH_TO_ALGO(cs->algo);
+
+	res = tee_hash_get_digest_size(hash_algo, &hash_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (TEE_ALG_GET_MAIN_ALG(cs->algo) == TEE_MAIN_ALGO_DSA) {
+		/*
+		 * Depending on the DSA algorithm (NIST), the digital signature
+		 * output size may be truncated to the size of a key pair
+		 * (Q prime size). Q prime size must be less or equal than the
+		 * hash output length of the hash algorithm involved.
+		 */
+		if (data_len > hash_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+	} else {
+		if (data_len != hash_size) {
+			res = TEE_ERROR_BAD_PARAMETERS;
+			goto out;
+		}
+	}
+
+	switch (TEE_ALG_GET_MAIN_ALG(cs->algo)) {
+	case TEE_MAIN_ALGO_RSA:
+		salt_len = pkcs1_get_salt_len(params, num_params, hash_size);
+		if (!crypto_ops.acipher.rsassa_verify) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.rsassa_verify(cs->algo, o->attr,
+						       salt_len, data,
+						       data_len, sig, sig_len);
+		break;
+
+	case TEE_MAIN_ALGO_DSA:
+		if (!crypto_ops.acipher.dsa_verify) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.dsa_verify(cs->algo, o->attr, data,
+						    data_len, sig, sig_len);
+		break;
+
+	case TEE_MAIN_ALGO_ECDSA:
+		if (!crypto_ops.acipher.ecc_verify) {
+			res = TEE_ERROR_NOT_IMPLEMENTED;
+			break;
+		}
+		res = crypto_ops.acipher.ecc_verify(cs->algo, o->attr, data,
+						    data_len, sig, sig_len);
+		break;
+
+	default:
+		res = TEE_ERROR_NOT_SUPPORTED;
+	}
+
+out:
+	free(params);
+	return res;
+}
diff --git a/core/tee/tee_svc_storage.c b/core/tee/tee_svc_storage.c
new file mode 100644
index 0000000..916ddca
--- /dev/null
+++ b/core/tee/tee_svc_storage.c
@@ -0,0 +1,1208 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <kernel/mutex.h>
+#include <kernel/tee_misc.h>
+#include <kernel/tee_ta_manager.h>
+#include <mm/tee_mmu.h>
+#include <string.h>
+#include <tee_api_defines_extensions.h>
+#include <tee_api_defines.h>
+#include <tee/tee_fs_defs.h>
+#include <tee/tee_fs.h>
+#include <tee/tee_obj.h>
+#include <tee/tee_pobj.h>
+#include <tee/tee_svc_cryp.h>
+#include <tee/tee_svc.h>
+#include <tee/tee_svc_storage.h>
+#include <trace.h>
+
+/*
+ * Returns the appropriate tee_file_operations for the specified storage ID.
+ * The value TEE_STORAGE_PRIVATE will select the REE FS if available, otherwise
+ * RPMB.
+ */
+static const struct tee_file_operations *file_ops(uint32_t storage_id)
+{
+
+	switch (storage_id) {
+	case TEE_STORAGE_PRIVATE:
+#if defined(CFG_REE_FS)
+		return &ree_fs_ops;
+#elif defined(CFG_RPMB_FS)
+		return &rpmb_fs_ops;
+#elif defined(CFG_SQL_FS)
+		return &sql_fs_ops;
+#else
+#error At least one filesystem must be enabled.
+#endif
+#ifdef CFG_REE_FS
+	case TEE_STORAGE_PRIVATE_REE:
+		return &ree_fs_ops;
+#endif
+#ifdef CFG_RPMB_FS
+	case TEE_STORAGE_PRIVATE_RPMB:
+		return &rpmb_fs_ops;
+#endif
+#ifdef CFG_SQL_FS
+	case TEE_STORAGE_PRIVATE_SQL:
+		return &sql_fs_ops;
+#endif
+	default:
+		return NULL;
+	}
+}
+
+/* SSF (Secure Storage File version 00 */
+#define TEE_SVC_STORAGE_MAGIC 0x53534600
+
+/* Header of GP formated secure storage files */
+struct tee_svc_storage_head {
+	uint32_t magic;
+	uint32_t head_size;
+	uint32_t meta_size;
+	uint32_t ds_size;
+	uint32_t keySize;
+	uint32_t maxKeySize;
+	uint32_t objectUsage;
+	uint32_t objectType;
+	uint32_t have_attrs;
+};
+
+struct tee_storage_enum {
+	TAILQ_ENTRY(tee_storage_enum) link;
+	struct tee_fs_dir *dir;
+	const struct tee_file_operations *fops;
+};
+
+/*
+ * Protect TA storage directory: avoid race conditions between (create
+ * directory + create file) and (remove directory)
+ */
+static struct mutex ta_dir_mutex = MUTEX_INITIALIZER;
+
+static TEE_Result tee_svc_storage_get_enum(struct user_ta_ctx *utc,
+					   uint32_t enum_id,
+					   struct tee_storage_enum **e_out)
+{
+	struct tee_storage_enum *e;
+
+	TAILQ_FOREACH(e, &utc->storage_enums, link) {
+		if (enum_id == (vaddr_t)e) {
+			*e_out = e;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_BAD_PARAMETERS;
+}
+
+static TEE_Result tee_svc_close_enum(struct user_ta_ctx *utc,
+				     struct tee_storage_enum *e)
+{
+	if (e == NULL || utc == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	TAILQ_REMOVE(&utc->storage_enums, e, link);
+
+	if (e->fops)
+		e->fops->closedir(e->dir);
+
+	e->dir = NULL;
+	e->fops = NULL;
+
+	free(e);
+
+	return TEE_SUCCESS;
+}
+
+/* "/TA_uuid/object_id" or "/TA_uuid/.object_id" */
+char *tee_svc_storage_create_filename(struct tee_ta_session *sess,
+				      void *object_id,
+				      uint32_t object_id_len,
+				      bool transient)
+{
+	uint8_t *file;
+	uint32_t pos = 0;
+	uint32_t hslen = 1 /* Leading slash */
+			+ TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID) + object_id_len)
+			+ 1; /* Intermediate slash */
+
+	/* +1 for the '.' (temporary persistent object) */
+	if (transient)
+		hslen++;
+
+	file = malloc(hslen);
+	if (!file)
+		return NULL;
+
+	file[pos++] = '/';
+	pos += tee_b2hs((uint8_t *)&sess->ctx->uuid, &file[pos],
+			sizeof(TEE_UUID), hslen);
+	file[pos++] = '/';
+
+	if (transient)
+		file[pos++] = '.';
+
+	tee_b2hs(object_id, file + pos, object_id_len, hslen - pos);
+
+	return (char *)file;
+}
+
+/* "/TA_uuid" */
+char *tee_svc_storage_create_dirname(struct tee_ta_session *sess)
+{
+	uint8_t *dir;
+	uint32_t hslen = TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID)) + 1;
+
+	dir = malloc(hslen);
+	if (!dir)
+		return NULL;
+
+	dir[0] = '/';
+	tee_b2hs((uint8_t *)&sess->ctx->uuid, dir + 1, sizeof(TEE_UUID),
+		 hslen);
+
+	return (char *)dir;
+}
+
+static TEE_Result tee_svc_storage_remove_corrupt_obj(
+					struct tee_ta_session *sess,
+					struct tee_obj *o)
+{
+	TEE_Result res;
+	char *file = NULL;
+	const struct tee_file_operations *fops = o->pobj->fops;
+
+	file = tee_svc_storage_create_filename(sess,
+					       o->pobj->obj_id,
+					       o->pobj->obj_id_len,
+					       false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	tee_obj_close(to_user_ta_ctx(sess->ctx), o);
+	fops->remove(file);
+	free(file);
+
+	res = TEE_SUCCESS;
+
+exit:
+	return res;
+}
+
+static TEE_Result tee_svc_storage_read_head(struct tee_ta_session *sess,
+				     struct tee_obj *o)
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t bytes;
+	struct tee_svc_storage_head head;
+	char *file = NULL;
+	const struct tee_file_operations *fops;
+	void *attr = NULL;
+
+	if (o == NULL || o->pobj == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	fops = o->pobj->fops;
+
+	file = tee_svc_storage_create_filename(sess,
+					       o->pobj->obj_id,
+					       o->pobj->obj_id_len,
+					       false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	assert(!o->fh);
+	res = fops->open(file, &o->fh);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* read head */
+	bytes = sizeof(struct tee_svc_storage_head);
+	res = fops->read(o->fh, &head, &bytes);
+	if (res != TEE_SUCCESS) {
+		if (res == TEE_ERROR_CORRUPT_OBJECT)
+			EMSG("Head corrupt\n");
+		goto exit;
+	}
+
+	if (bytes != sizeof(struct tee_svc_storage_head)) {
+		res = TEE_ERROR_BAD_FORMAT;
+		goto exit;
+	}
+
+	res = tee_obj_set_type(o, head.objectType, head.maxKeySize);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (head.meta_size) {
+		attr = malloc(head.meta_size);
+		if (!attr) {
+			res = TEE_ERROR_OUT_OF_MEMORY;
+			goto exit;
+		}
+
+		/* read meta */
+		bytes = head.meta_size;
+		res = fops->read(o->fh, attr, &bytes);
+		if (res != TEE_SUCCESS || bytes != head.meta_size) {
+			res = TEE_ERROR_CORRUPT_OBJECT;
+			goto exit;
+		}
+	}
+
+	res = tee_obj_attr_from_binary(o, attr, head.meta_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o->info.dataSize = head.ds_size;
+	o->info.keySize = head.keySize;
+	o->info.objectUsage = head.objectUsage;
+	o->info.objectType = head.objectType;
+	o->have_attrs = head.have_attrs;
+
+exit:
+	free(attr);
+	free(file);
+
+	return res;
+}
+
+static TEE_Result tee_svc_storage_update_head(struct tee_obj *o,
+					uint32_t ds_size)
+{
+	TEE_Result res;
+	const struct tee_file_operations *fops;
+	int32_t old_off;
+
+	fops = o->pobj->fops;
+
+	/* save original offset */
+	res = fops->seek(o->fh, 0, TEE_DATA_SEEK_CUR, &old_off);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* update head.ds_size */
+	res = fops->seek(o->fh, offsetof(struct tee_svc_storage_head,
+			ds_size), TEE_DATA_SEEK_SET, NULL);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = fops->write(o->fh, &ds_size, sizeof(uint32_t));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	/* restore original offset */
+	res = fops->seek(o->fh, old_off, TEE_DATA_SEEK_SET, NULL);
+	return res;
+}
+
+static TEE_Result tee_svc_storage_init_file(struct tee_ta_session *sess,
+					    struct tee_obj *o,
+					    struct tee_obj *attr_o, void *data,
+					    uint32_t len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	struct tee_svc_storage_head head;
+	char *tmpfile = NULL;
+	const struct tee_file_operations *fops;
+	void *attr = NULL;
+	size_t attr_size = 0;
+
+	if (o == NULL || o->pobj == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	fops = o->pobj->fops;
+
+	/* create temporary persistent object filename */
+	tmpfile = tee_svc_storage_create_filename(sess,
+						   o->pobj->obj_id,
+						   o->pobj->obj_id_len,
+						   true);
+
+	if (tmpfile == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	mutex_lock(&ta_dir_mutex);
+	res = fops->create(tmpfile, &o->fh);
+	mutex_unlock(&ta_dir_mutex);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (attr_o) {
+		res = tee_obj_set_type(o, attr_o->info.objectType,
+				       attr_o->info.maxKeySize);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		res = tee_obj_attr_copy_from(o, attr_o);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		o->have_attrs = attr_o->have_attrs;
+		o->info.objectUsage = attr_o->info.objectUsage;
+		o->info.keySize = attr_o->info.keySize;
+		res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		if (attr_size) {
+			attr = malloc(attr_size);
+			if (!attr) {
+				res = TEE_ERROR_OUT_OF_MEMORY;
+				goto exit;
+			}
+			res = tee_obj_attr_to_binary(o, attr, &attr_size);
+			if (res != TEE_SUCCESS)
+				goto exit;
+		}
+	} else {
+		res = tee_obj_set_type(o, TEE_TYPE_DATA, 0);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	/* write head */
+	head.magic = TEE_SVC_STORAGE_MAGIC;
+	head.head_size = sizeof(struct tee_svc_storage_head);
+	head.meta_size = attr_size;
+	head.ds_size = len;
+	head.keySize = o->info.keySize;
+	head.maxKeySize = o->info.maxKeySize;
+	head.objectUsage = o->info.objectUsage;
+	head.objectType = o->info.objectType;
+	head.have_attrs = o->have_attrs;
+
+	/* write head */
+	res = fops->write(o->fh, &head, sizeof(struct tee_svc_storage_head));
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* write meta */
+	res = fops->write(o->fh, attr, attr_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* write init data */
+	o->info.dataSize = len;
+
+	/* write data to fs if needed */
+	if (data && len)
+		res = fops->write(o->fh, data, len);
+
+exit:
+	free(attr);
+	free(tmpfile);
+	fops->close(&o->fh);
+
+	return res;
+}
+
+TEE_Result syscall_storage_obj_open(unsigned long storage_id, void *object_id,
+			size_t object_id_len, unsigned long flags,
+			uint32_t *obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o = NULL;
+	char *file = NULL;
+	struct tee_pobj *po = NULL;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops = file_ops(storage_id);
+	size_t attr_size;
+
+	if (!fops) {
+		res = TEE_ERROR_ITEM_NOT_FOUND;
+		goto exit;
+	}
+
+	if (object_id_len > TEE_OBJECT_ID_MAX_LEN) {
+		res = TEE_ERROR_BAD_PARAMETERS;
+		goto exit;
+	}
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto err;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) object_id,
+					  object_id_len);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
+			   object_id_len, flags, fops, &po);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	o = tee_obj_alloc();
+	if (o == NULL) {
+		tee_pobj_release(po);
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	o->info.handleFlags =
+	    TEE_HANDLE_FLAG_PERSISTENT | TEE_HANDLE_FLAG_INITIALIZED;
+	o->flags = flags;
+	o->pobj = po;
+	tee_obj_add(utc, o);
+
+	res = tee_svc_storage_read_head(sess, o);
+	if (res != TEE_SUCCESS) {
+		if (res == TEE_ERROR_CORRUPT_OBJECT) {
+			EMSG("Object corrupt");
+			goto err;
+		}
+		goto oclose;
+	}
+
+	res = tee_svc_copy_kaddr_to_uref(obj, o);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = fops->seek(o->fh, sizeof(struct tee_svc_storage_head) + attr_size,
+			 TEE_DATA_SEEK_SET, NULL);
+	if (res  != TEE_SUCCESS)
+		goto err;
+
+	goto exit;
+
+oclose:
+	tee_obj_close(utc, o);
+	o = NULL;
+
+err:
+	if (res == TEE_ERROR_NO_DATA || res == TEE_ERROR_BAD_FORMAT)
+		res = TEE_ERROR_CORRUPT_OBJECT;
+	if (res == TEE_ERROR_CORRUPT_OBJECT && o)
+		tee_svc_storage_remove_corrupt_obj(sess, o);
+
+exit:
+	free(file);
+	file = NULL;
+	return res;
+}
+
+TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
+			size_t object_id_len, unsigned long flags,
+			unsigned long attr, void *data, size_t len,
+			uint32_t *obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o = NULL;
+	struct tee_obj *attr_o = NULL;
+	char *file = NULL;
+	struct tee_pobj *po = NULL;
+	char *tmpfile = NULL;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops = file_ops(storage_id);
+	size_t attr_size;
+
+	if (!fops)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	if (object_id_len > TEE_OBJECT_ID_MAX_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_mmu_check_access_rights(utc,
+					  TEE_MEMORY_ACCESS_READ |
+					  TEE_MEMORY_ACCESS_ANY_OWNER,
+					  (uaddr_t) object_id,
+					  object_id_len);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
+			   object_id_len, flags, fops, &po);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	/* check rights of the provided buffer */
+	if (data && len) {
+		res = tee_mmu_check_access_rights(utc,
+						  TEE_MEMORY_ACCESS_READ |
+						  TEE_MEMORY_ACCESS_ANY_OWNER,
+						  (uaddr_t) data, len);
+
+		if (res != TEE_SUCCESS)
+			goto err;
+	}
+
+	o = tee_obj_alloc();
+	if (o == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	o->info.handleFlags =
+	    TEE_HANDLE_FLAG_PERSISTENT | TEE_HANDLE_FLAG_INITIALIZED;
+	o->flags = flags;
+	o->pobj = po;
+
+	if (attr != TEE_HANDLE_NULL) {
+		res = tee_obj_get(utc, tee_svc_uref_to_vaddr(attr),
+				  &attr_o);
+		if (res != TEE_SUCCESS)
+			goto err;
+	}
+
+	res = tee_svc_storage_init_file(sess, o, attr_o, data, len);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	/* create persistent object filename */
+	file = tee_svc_storage_create_filename(sess, object_id,
+					       object_id_len, false);
+	if (file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	/* create temporary persistent object filename */
+	tmpfile = tee_svc_storage_create_filename(sess, object_id,
+						  object_id_len,
+						  true);
+	if (tmpfile == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto err;
+	}
+
+	/* rename temporary persistent object filename */
+	res = fops->rename(tmpfile, file, !!(flags & TEE_DATA_FLAG_OVERWRITE));
+	if (res != TEE_SUCCESS)
+		goto rmfile;
+
+	res = fops->open(file, &o->fh);
+	if (res != TEE_SUCCESS)
+		goto err;
+
+	tee_obj_add(utc, o);
+
+	res = tee_svc_copy_kaddr_to_uref(obj, o);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	res = fops->seek(o->fh, sizeof(struct tee_svc_storage_head) + attr_size,
+			 TEE_DATA_SEEK_SET, NULL);
+	if (res != TEE_SUCCESS)
+		goto oclose;
+
+	goto exit;
+
+oclose:
+	tee_obj_close(utc, o);
+	goto exit;
+
+rmfile:
+	fops->remove(tmpfile);
+
+err:
+	if (res == TEE_ERROR_NO_DATA || res == TEE_ERROR_BAD_FORMAT)
+		res = TEE_ERROR_CORRUPT_OBJECT;
+	if (res == TEE_ERROR_CORRUPT_OBJECT && file)
+		fops->remove(file);
+	if (o)
+		fops->close(&o->fh);
+	if (po)
+		tee_pobj_release(po);
+	free(o);
+
+exit:
+	free(file);
+	free(tmpfile);
+
+	return res;
+}
+
+TEE_Result syscall_storage_obj_del(unsigned long obj)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	char *file;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE_META))
+		return TEE_ERROR_ACCESS_CONFLICT;
+
+	if (o->pobj == NULL || o->pobj->obj_id == NULL)
+		return TEE_ERROR_BAD_STATE;
+
+	file = tee_svc_storage_create_filename(sess, o->pobj->obj_id,
+						o->pobj->obj_id_len, false);
+	if (file == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	fops = o->pobj->fops;
+	tee_obj_close(utc, o);
+
+	res = fops->remove(file);
+	free(file);
+	return res;
+}
+
+TEE_Result syscall_storage_obj_rename(unsigned long obj, void *object_id,
+			size_t object_id_len)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct tee_pobj *po = NULL;
+	char *new_file = NULL;
+	char *old_file = NULL;
+	struct user_ta_ctx *utc;
+	const struct tee_file_operations *fops;
+
+	if (object_id_len > TEE_OBJECT_ID_MAX_LEN)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE_META)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (o->pobj == NULL || o->pobj->obj_id == NULL) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_READ |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) object_id, object_id_len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* get new ds name */
+	new_file = tee_svc_storage_create_filename(sess, object_id,
+						   object_id_len, false);
+	if (new_file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	old_file = tee_svc_storage_create_filename(sess, o->pobj->obj_id,
+						   o->pobj->obj_id_len, false);
+	if (old_file == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	/* reserve dest name */
+	fops = o->pobj->fops;
+	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
+			   object_id_len, TEE_DATA_FLAG_ACCESS_WRITE_META,
+			   fops, &po);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* move */
+	res = fops->rename(old_file, new_file, false /* no overwrite */);
+	if (res == TEE_ERROR_GENERIC)
+		goto exit;
+
+	res = tee_pobj_rename(o->pobj, object_id, object_id_len);
+
+exit:
+	tee_pobj_release(po);
+
+	free(new_file);
+	free(old_file);
+
+	return res;
+}
+
+TEE_Result syscall_storage_alloc_enum(uint32_t *obj_enum)
+{
+	struct tee_storage_enum *e;
+	struct tee_ta_session *sess;
+	TEE_Result res;
+	struct user_ta_ctx *utc;
+
+	if (obj_enum == NULL)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	e = malloc(sizeof(struct tee_storage_enum));
+	if (e == NULL)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	e->dir = NULL;
+	e->fops = NULL;
+	TAILQ_INSERT_TAIL(&utc->storage_enums, e, link);
+
+	return tee_svc_copy_kaddr_to_uref(obj_enum, e);
+}
+
+TEE_Result syscall_storage_free_enum(unsigned long obj_enum)
+{
+	struct tee_storage_enum *e;
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_storage_get_enum(utc,
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return tee_svc_close_enum(utc, e);
+}
+
+TEE_Result syscall_storage_reset_enum(unsigned long obj_enum)
+{
+	struct tee_storage_enum *e;
+	TEE_Result res;
+	struct tee_ta_session *sess;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_storage_get_enum(to_user_ta_ctx(sess->ctx),
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	e->fops->closedir(e->dir);
+	e->fops = NULL;
+	e->dir = NULL;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result tee_svc_storage_set_enum(char *d_name,
+			const struct tee_file_operations *fops,
+			struct tee_obj *o)
+{
+	TEE_Result res;
+	uint32_t blen;
+	uint32_t hslen;
+
+	o->info.handleFlags =
+	    TEE_HANDLE_FLAG_PERSISTENT | TEE_HANDLE_FLAG_INITIALIZED;
+	o->info.objectUsage = TEE_USAGE_DEFAULT;
+
+	hslen = strlen(d_name);
+	blen = TEE_HS2B_BBUF_SIZE(hslen);
+	o->pobj->obj_id = malloc(blen);
+	if (!o->pobj->obj_id) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+	tee_hs2b((uint8_t *)d_name, o->pobj->obj_id, hslen, blen);
+	o->pobj->obj_id_len = blen;
+	o->pobj->fops = fops;
+
+	res = TEE_SUCCESS;
+
+exit:
+	return res;
+
+}
+
+TEE_Result syscall_storage_start_enum(unsigned long obj_enum,
+				      unsigned long storage_id)
+{
+	struct tee_storage_enum *e;
+	char *dir;
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	const struct tee_file_operations *fops = file_ops(storage_id);
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_svc_storage_get_enum(to_user_ta_ctx(sess->ctx),
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (!fops)
+		return TEE_ERROR_ITEM_NOT_FOUND;
+
+	e->fops = fops;
+	dir = tee_svc_storage_create_dirname(sess);
+	if (dir == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	assert(!e->dir);
+	res = fops->opendir(dir, &e->dir);
+	free(dir);
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_next_enum(unsigned long obj_enum,
+			TEE_ObjectInfo *info, void *obj_id, uint64_t *len)
+{
+	struct tee_storage_enum *e;
+	struct tee_fs_dirent *d;
+	TEE_Result res = TEE_SUCCESS;
+	struct tee_ta_session *sess;
+	struct tee_obj *o = NULL;
+	uint64_t l;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_svc_storage_get_enum(utc,
+			tee_svc_uref_to_vaddr(obj_enum), &e);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	/* check rights of the provided buffers */
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) info,
+					sizeof(TEE_ObjectInfo));
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) obj_id,
+					TEE_OBJECT_ID_MAX_LEN);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!e->fops) {
+		res = TEE_ERROR_ITEM_NOT_FOUND;
+		goto exit;
+	}
+
+	res = e->fops->readdir(e->dir, &d);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o = tee_obj_alloc();
+	if (o == NULL) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+	o->flags = TEE_DATA_FLAG_SHARE_READ;
+
+	o->pobj = calloc(1, sizeof(struct tee_pobj));
+	if (!o->pobj) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto exit;
+	}
+
+	res = tee_svc_storage_set_enum(d->d_name, e->fops, o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_svc_storage_read_head(sess, o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	memcpy(info, &o->info, sizeof(TEE_ObjectInfo));
+	memcpy(obj_id, o->pobj->obj_id, o->pobj->obj_id_len);
+
+	l = o->pobj->obj_id_len;
+	res = tee_svc_copy_to_user(len, &l, sizeof(*len));
+
+exit:
+	if (o) {
+		if (o->pobj) {
+			o->pobj->fops->close(&o->fh);
+			free(o->pobj->obj_id);
+		}
+		free(o->pobj);
+		tee_obj_free(o);
+	}
+
+	return res;
+}
+
+TEE_Result syscall_storage_obj_read(unsigned long obj, void *data, size_t len,
+			uint64_t *count)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	uint64_t u_count;
+	struct user_ta_ctx *utc;
+	size_t bytes;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_READ)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto exit;
+	}
+
+	/* check rights of the provided buffer */
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_WRITE |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) data, len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	bytes = len;
+	res = o->pobj->fops->read(o->fh, data, &bytes);
+	if (res != TEE_SUCCESS) {
+		EMSG("Error code=%x\n", (uint32_t)res);
+		if (res == TEE_ERROR_CORRUPT_OBJECT) {
+			EMSG("Object corrupt\n");
+			tee_svc_storage_remove_corrupt_obj(sess, o);
+		}
+		goto exit;
+	}
+
+	o->info.dataPosition += bytes;
+
+	u_count = bytes;
+	res = tee_svc_copy_to_user(count, &u_count, sizeof(*count));
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_obj_write(unsigned long obj, void *data, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	struct user_ta_ctx *utc;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	utc = to_user_ta_ctx(sess->ctx);
+
+	res = tee_obj_get(utc, tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto exit;
+	}
+
+	/* check rights of the provided buffer */
+	res = tee_mmu_check_access_rights(utc,
+					TEE_MEMORY_ACCESS_READ |
+					TEE_MEMORY_ACCESS_ANY_OWNER,
+					(uaddr_t) data, len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = o->pobj->fops->write(o->fh, data, len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	o->info.dataPosition += len;
+	if (o->info.dataPosition > o->info.dataSize) {
+		res = tee_svc_storage_update_head(o, o->info.dataPosition);
+		if (res != TEE_SUCCESS)
+			goto exit;
+		o->info.dataSize = o->info.dataPosition;
+	}
+
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_obj_trunc(unsigned long obj, size_t len)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	size_t off;
+	size_t attr_size;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	if (!(o->flags & TEE_DATA_FLAG_ACCESS_WRITE)) {
+		res = TEE_ERROR_ACCESS_CONFLICT;
+		goto exit;
+	}
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	off = sizeof(struct tee_svc_storage_head) + attr_size;
+	res = o->pobj->fops->truncate(o->fh, len + off);
+	if (res != TEE_SUCCESS) {
+		if (res == TEE_ERROR_CORRUPT_OBJECT) {
+			EMSG("Object corrupt\n");
+			res = tee_svc_storage_remove_corrupt_obj(sess, o);
+			if (res != TEE_SUCCESS)
+				goto exit;
+			res = TEE_ERROR_CORRUPT_OBJECT;
+			goto exit;
+		} else
+			res = TEE_ERROR_GENERIC;
+	}
+
+exit:
+	return res;
+}
+
+TEE_Result syscall_storage_obj_seek(unsigned long obj, int32_t offset,
+				    unsigned long whence)
+{
+	TEE_Result res;
+	struct tee_ta_session *sess;
+	struct tee_obj *o;
+	int32_t off;
+	size_t attr_size;
+
+	res = tee_ta_get_current_session(&sess);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
+			  tee_svc_uref_to_vaddr(obj), &o);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
+		res = TEE_ERROR_BAD_STATE;
+		goto exit;
+	}
+
+	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	off = offset;
+	if (whence == TEE_DATA_SEEK_SET)
+		off += sizeof(struct tee_svc_storage_head) + attr_size;
+
+	res = o->pobj->fops->seek(o->fh, off, whence, &off);
+	if (res != TEE_SUCCESS)
+		goto exit;
+	o->info.dataPosition = off - (sizeof(struct tee_svc_storage_head) +
+				      attr_size);
+
+exit:
+	return res;
+}
+
+void tee_svc_storage_close_all_enum(struct user_ta_ctx *utc)
+{
+	struct tee_storage_enum_head *eh = &utc->storage_enums;
+
+	/* disregard return value */
+	while (!TAILQ_EMPTY(eh))
+		tee_svc_close_enum(utc, TAILQ_FIRST(eh));
+}
diff --git a/core/tee/tee_time_generic.c b/core/tee/tee_time_generic.c
new file mode 100644
index 0000000..0b983b0
--- /dev/null
+++ b/core/tee/tee_time_generic.c
@@ -0,0 +1,159 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <kernel/panic.h>
+#include <kernel/tee_time.h>
+#include <string.h>
+#include <stdlib.h>
+#include <trace.h>
+#include <utee_defines.h>
+
+struct tee_ta_time_offs {
+	TEE_UUID uuid;
+	TEE_Time offs;
+	bool positive;
+};
+
+static struct tee_ta_time_offs *tee_time_offs;
+static size_t tee_time_num_offs;
+
+static TEE_Result tee_time_ta_get_offs(const TEE_UUID *uuid,
+				       const TEE_Time **offs, bool *positive)
+{
+	size_t n;
+
+	for (n = 0; n < tee_time_num_offs; n++) {
+		if (memcmp(uuid, &tee_time_offs[n].uuid, sizeof(TEE_UUID))
+				== 0) {
+			*offs = &tee_time_offs[n].offs;
+			*positive = tee_time_offs[n].positive;
+			return TEE_SUCCESS;
+		}
+	}
+	return TEE_ERROR_TIME_NOT_SET;
+}
+
+static TEE_Result tee_time_ta_set_offs(const TEE_UUID *uuid,
+				       const TEE_Time *offs, bool positive)
+{
+	size_t n;
+	struct tee_ta_time_offs *o;
+
+	for (n = 0; n < tee_time_num_offs; n++) {
+		if (memcmp(uuid, &tee_time_offs[n].uuid, sizeof(TEE_UUID))
+				== 0) {
+			tee_time_offs[n].offs = *offs;
+			tee_time_offs[n].positive = positive;
+			return TEE_SUCCESS;
+		}
+	}
+
+	n = tee_time_num_offs + 1;
+	o = realloc(tee_time_offs, n * sizeof(struct tee_ta_time_offs));
+	if (!o)
+		return TEE_ERROR_OUT_OF_MEMORY;
+	tee_time_offs = o;
+	tee_time_offs[tee_time_num_offs].uuid = *uuid;
+	tee_time_offs[tee_time_num_offs].offs = *offs;
+	tee_time_offs[tee_time_num_offs].positive = positive;
+	tee_time_num_offs = n;
+	return TEE_SUCCESS;
+}
+
+TEE_Result tee_time_get_ta_time(const TEE_UUID *uuid, TEE_Time *time)
+{
+	TEE_Result res;
+	const TEE_Time *offs;
+	bool positive;
+	TEE_Time t;
+	TEE_Time t2;
+
+	res = tee_time_ta_get_offs(uuid, &offs, &positive);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = tee_time_get_sys_time(&t);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (positive) {
+		TEE_TIME_ADD(t, *offs, t2);
+
+		/* Detect wrapping, the wrapped time should be returned. */
+		if (TEE_TIME_LT(t2, t))
+			res = TEE_ERROR_OVERFLOW;
+	} else {
+		TEE_TIME_SUB(t, *offs, t2);
+
+		/* Detect wrapping, the wrapped time should be returned. */
+		if (TEE_TIME_LE(t, t2))
+			res = TEE_ERROR_OVERFLOW;
+	}
+	*time = t2;
+
+	return res;
+}
+
+TEE_Result tee_time_set_ta_time(const TEE_UUID *uuid, const TEE_Time *time)
+{
+	TEE_Result res;
+	TEE_Time offs;
+	TEE_Time t;
+
+	/* Check that time is normalized. */
+	if (time->millis >= TEE_TIME_MILLIS_BASE)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = tee_time_get_sys_time(&t);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (TEE_TIME_LT(t, *time)) {
+		TEE_TIME_SUB(*time, t, offs);
+		return tee_time_ta_set_offs(uuid, &offs, true);
+	} else {
+		TEE_TIME_SUB(t, *time, offs);
+		return tee_time_ta_set_offs(uuid, &offs, false);
+	}
+}
+
+void tee_time_busy_wait(uint32_t milliseconds_delay)
+{
+	TEE_Time curr;
+	TEE_Time delta;
+	TEE_Time end;
+
+	if (tee_time_get_sys_time(&curr) != TEE_SUCCESS)
+		panic();
+	delta.seconds = milliseconds_delay / 1000;
+	delta.millis = milliseconds_delay % 1000;
+	TEE_TIME_ADD(curr, delta, end);
+
+	while (TEE_TIME_LT(curr, end))
+		if (tee_time_get_sys_time(&curr) != TEE_SUCCESS)
+			panic();
+}
diff --git a/core/tee/uuid.c b/core/tee/uuid.c
new file mode 100644
index 0000000..3642b1c
--- /dev/null
+++ b/core/tee/uuid.c
@@ -0,0 +1,52 @@
+/*
+ * 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 <string.h>
+#include <tee/uuid.h>
+#include <util.h>
+
+void tee_uuid_to_octets(uint8_t *d, const TEE_UUID *s)
+{
+	d[0] = s->timeLow >> 24;
+	d[1] = s->timeLow >> 16;
+	d[2] = s->timeLow >> 8;
+	d[3] = s->timeLow;
+	d[4] = s->timeMid >> 8;
+	d[5] = s->timeMid;
+	d[6] = s->timeHiAndVersion >> 8;
+	d[7] = s->timeHiAndVersion;
+	memcpy(d + 8, s->clockSeqAndNode, sizeof(s->clockSeqAndNode));
+}
+
+void tee_uuid_from_octets(TEE_UUID *d, const uint8_t *s)
+{
+	d->timeLow = SHIFT_U32(s[0], 24) | SHIFT_U32(s[1], 16) |
+		     SHIFT_U32(s[2], 8) | s[3];
+	d->timeMid = SHIFT_U32(s[4], 8) | s[5];
+	d->timeHiAndVersion = SHIFT_U32(s[6], 8) | s[7];
+	memcpy(d->clockSeqAndNode, s + 8, sizeof(d->clockSeqAndNode));
+}