Update from upstream to 2.4.0 versionupstream/2.4.0upstream

Change-Id: I2b3a30f20684d6629fe379d9cd7895aff759c301

156 files changed, 4799 insertions, 3171 deletions

diff --git a/.travis.yml b/.travis.yml
index 91a4295..4922f1a 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -166,6 +166,7 @@ script:
   - $make CFG_WITH_PAGER=y CFG_WITH_LPAE=y CFG_RPMB_FS=y CFG_SQL_FS=y CFG_DT=y CFG_PS2MOUSE=y CFG_PL050=y CFG_PL111=y CFG_TEE_CORE_LOG_LEVEL=0 CFG_TEE_CORE_DEBUG=n DEBUG=0
   - $make CFG_BUILT_IN_ARGS=y CFG_PAGEABLE_ADDR=0 CFG_NS_ENTRY_ADDR=0 CFG_DT_ADDR=0 CFG_DT=y
   - $make CFG_TA_GPROF_SUPPORT=y CFG_ULIBS_GPROF=y
+  - $make CFG_SECURE_DATA_PATH=y
 
   # QEMU-ARMv8A
   - $make PLATFORM=vexpress-qemu_armv8a CFG_ARM64_core=y
@@ -190,6 +191,7 @@ script:
   # i.MX6Quad SABRE
   - $make PLATFORM=imx-mx6qsabrelite
   - $make PLATFORM=imx-mx6qsabresd
+  - $make PLATFORM=imx-mx6dlsabresd
 
   # Texas Instruments dra7xx
   - $make PLATFORM=ti-dra7xx
diff --git a/CHANGELOG.md b/CHANGELOG.md
index 0f29b68..8a051b8 100644
--- a/CHANGELOG.md
+++ b/CHANGELOG.md
@@ -1,3 +1,114 @@
+# OP-TEE - version 2.4.0
+
+[Link][github_commits_2_4_0] to a list of all commits between this release and
+the previous one (2.3.0).
+
+Please note: this release is API-compatible with the previous one, but the
+Secure Storage internal format for the REE and SQL FS is not compatible due to
+commits [a238b74][commit_a238b74] ("core: REE FS: use the new hash tree
+interface") and [44e900e][commit_44e900e] ("core: SQL FS: use the new hash tree
+interface").
+
+## New features
+
+* Add porting guidelines
+
+* Add support for Secure Data Path which allows Client and Trusted Applications
+  to share references to secure memory
+
+* New supported platform: Texas Instruments AM57xx (`PLATFORM=ti-am57xx`)
+
+* ARMv7-A: add support for platform services in secure monitor and add these
+  services for the DRA7xx platform
+
+* SPI framework and PL022 driver cleanup and improvements
+
+* Use CNTPCT (when available) to add entropy to the software PRNG
+
+* Add GlobalPlatform Socket API for UDP and TCP (IPv4 and IPv6)
+
+* DRA7: add TRNG driver, enable GICv2 driver
+
+* Support load address larger than 4G
+
+* libutee: preserve error code when calling TEE_Panic() for easier
+  troubleshooting
+
+* Support TA profiling with gprof (-pg compiler switch)
+
+* Optimize the ELF loader for TAs when pager is enabled
+
+* Update documentation
+
+* Add paged secure shared memory that can be transferred between TAs as
+  needed
+
+* Introduce MOBJ abstraction
+
+* i.MX6: add PSCI "on" function
+
+* arm32: introduce PSCI framework
+
+## Bug fixes
+
+* Secure storage: improve integrity checking of the REE and SQL filesystems by
+  adding a hash tree on the internal data structures. Any external modification
+  is detected, except full rollback. Fixes [#1188][issue1188].
+
+* The linux driver will set the 'privileged' flag (TEE_GEN_CAP_PRIVILEGED) on
+  the device intended for use by tee-supplicant. Fixes [#1199][issue1199].
+
+* RPMB: don't try to program the RPMB key by default
+
+* Fix "make clean" error cases
+
+* Fix issue when resetting persistent storage enumerator [#1332][issue1332]
+
+* Fix TA panic when doing AES CTS with specific buffer sizes
+  [#1203][issue1203].
+
+## Known issues
+
+* On RPi3 xtest sometimes stall (rcu_sched self-detected stall on CPU) [#1353][issue1353]
+* For multi-core PSCI support is to be added for ls1021atwr in OP-TEE.
+* USB keyboard cannot be used to stop the u-boot timeout ([build issue131]).
+* Travis service (build.git) seems unstable from time to time.
+
+## Tested on
+
+In the list below, _standard_ means that the `xtest` program passed with
+its default configuration, while _extended_ means it was run successfully
+with the additional GlobalPlatform™ TEE Initial Configuration Test Suite
+v1.1.0.4.
+
+If a platform is not listed, it means the release was not tested on this
+platform.
+
+<!-- ${PLATFORM}-${PLATFORM_FLAVOR}, ordered alphabetically -->
+* d02: extended
+* hikey: extended
+* imx-mx6ulevk: standard
+* ls-ls1021atwr: standard (single core)
+* mediatek-mt8173: standard
+* rcar-h3: standard
+* rpi3: standard
+* stm-b2260: extended
+* ti-dra7xx: standard
+* vexpress-fvp: standard
+* vexpress-juno: standard
+* vexpress-qemu_armv8a: standard
+* vexpress-qemu_virt: standard
+* zynqmp-zc1751_dc1: standard
+* zynqmp-zc1751_dc2: standard
+* zynqmp-zcu102: standard
+
+[github_commits_2_4_0]: https://github.com/OP-TEE/optee_os/compare/2.3.0...2.4.0
+[issue1332]: https://github.com/OP-TEE/optee_os/issues/1332
+[issue1353]: https://github.com/OP-TEE/optee_os/issues/1353
+[build issue131]: https://github.com/OP-TEE/build/issues/131
+[commit_a238b74]: https://github.com/OP-TEE/optee_os/commit/a238b744b1b3
+[commit_44e900e]: https://github.com/OP-TEE/optee_os/commit/44e900eabfc1
+
 # OP-TEE - version 2.3.0
 
 [Link][github_commits_2_3_0] to a list of all commits between this release and
diff --git a/MAINTAINERS.md b/MAINTAINERS.md
index 59e8c82..e010b02 100644
--- a/MAINTAINERS.md
+++ b/MAINTAINERS.md
@@ -10,8 +10,8 @@ for these platforms.
 | Allwinner A80 Board			|`Sun Yangbang <sunny@allwinnertech.com>`|
 | ARM Juno Board			|`Linaro <op-tee@linaro.org>`|
 | FSL ls1021a				|`Sumit Garg <sumit.garg@freescale.com>`|
-| FSL i.MX6 Quad SABRE Lite Board	|`Yan Yan <yan.yan@windriver.com>`|
-| FSL i.MX6 Quad SABRE SD Board		|`Yan Yan <yan.yan@windriver.com>`|
+| FSL i.MX6 Quad SABRE Lite Board	|`Yan Yan <yan.yan@windriver.com>`,`Feng Yu <Yu.Feng@windriver.com>`|
+| FSL i.MX6 Quad SABRE SD Board		|`Yan Yan <yan.yan@windriver.com>`,`Feng Yu <Yu.Feng@windriver.com>`|
 | FSL i.MX6 UltraLite EVK Board		|`Peng Fan <peng.fan@nxp.com>`|
 | ARM Foundation FVP			|`Linaro <op-tee@linaro.org>`|
 | HiKey Board (HiSilicon Kirin 620)	|`Linaro <op-tee@linaro.org>`|
@@ -23,6 +23,6 @@ for these platforms.
 | STMicroelectronics b2260 - h410	|`Linaro <op-tee@linaro.org>`|
 | STMicroelectronics b2120 - h310 / h410|`Linaro <op-tee@linaro.org>`|
 | Texas Instruments DRA7xx		|`Harinarayan Bhatta <harinarayan@ti.com>`|
-| Xilinx Zynq 7000 ZC702 Board		|`Yan Yan <yan.yan@windriver.com>`|
+| Xilinx Zynq 7000 ZC702 Board		|`Yan Yan <yan.yan@windriver.com>`,`Feng Yu <Yu.Feng@windriver.com>`|
 | Xilinx Zynq UltraScale+ MPSOC		|`Sören Brinkmann <soren.brinkmann@xilinx.com`|
 | Spreadtrum SC9860			|`Aijun Sun <aijun.sun@spreadtrum.com>`|
diff --git a/core/arch/arm/include/kernel/mutex.h b/core/arch/arm/include/kernel/mutex.h
index 1698b35..893313e 100644
--- a/core/arch/arm/include/kernel/mutex.h
+++ b/core/arch/arm/include/kernel/mutex.h
@@ -36,6 +36,15 @@ enum mutex_value {
 	MUTEX_VALUE_LOCKED,
 };
 
+/*
+ * Positive owner ids signifies actual threads, negative ids has special
+ * meanings according to the defines below. Note that only the first of the
+ * defines is allowed in struct mutex::owener_id.
+ */
+#define MUTEX_OWNER_ID_NONE		-1
+#define MUTEX_OWNER_ID_CONDVAR_SLEEP	-2
+#define MUTEX_OWNER_ID_MUTEX_UNLOCK	-3
+
 struct mutex {
 	enum mutex_value value;
 	unsigned spin_lock;	/* used when operating on this struct */
@@ -44,7 +53,7 @@ struct mutex {
 	TAILQ_ENTRY(mutex) link;
 };
 #define MUTEX_INITIALIZER \
-	{ .value = MUTEX_VALUE_UNLOCKED, .owner_id = -1, \
+	{ .value = MUTEX_VALUE_UNLOCKED, .owner_id = MUTEX_OWNER_ID_NONE, \
 	  .wq = WAIT_QUEUE_INITIALIZER, }
 
 TAILQ_HEAD(mutex_head, mutex);
diff --git a/core/arch/arm/include/kernel/pseudo_ta.h b/core/arch/arm/include/kernel/pseudo_ta.h
index 98316bd..55d5e2b 100644
--- a/core/arch/arm/include/kernel/pseudo_ta.h
+++ b/core/arch/arm/include/kernel/pseudo_ta.h
@@ -38,7 +38,9 @@
 				TA_FLAG_MULTI_SESSION | \
 				TA_FLAG_INSTANCE_KEEP_ALIVE)
 
-#define PTA_ALLOWED_FLAGS	PTA_MANDATORY_FLAGS
+#define PTA_ALLOWED_FLAGS	(PTA_MANDATORY_FLAGS | \
+				TA_FLAG_SECURE_DATA_PATH)
+
 #define PTA_DEFAULT_FLAGS	PTA_MANDATORY_FLAGS
 
 struct pseudo_ta_head {
diff --git a/core/arch/arm/include/kernel/spinlock.h b/core/arch/arm/include/kernel/spinlock.h
index c248673..a19b764 100644
--- a/core/arch/arm/include/kernel/spinlock.h
+++ b/core/arch/arm/include/kernel/spinlock.h
@@ -59,7 +59,7 @@ unsigned int __cpu_spin_trylock(unsigned int *lock);
 
 static inline void cpu_spin_lock(unsigned int *lock)
 {
-	assert(thread_irq_disabled());
+	assert(thread_foreign_intr_disabled());
 	__cpu_spin_lock(lock);
 	spinlock_count_incr();
 }
@@ -68,7 +68,7 @@ static inline bool cpu_spin_trylock(unsigned int *lock)
 {
 	unsigned int rc;
 
-	assert(thread_irq_disabled());
+	assert(thread_foreign_intr_disabled());
 	rc = __cpu_spin_trylock(lock);
 	if (!rc)
 		spinlock_count_incr();
@@ -77,7 +77,7 @@ static inline bool cpu_spin_trylock(unsigned int *lock)
 
 static inline void cpu_spin_unlock(unsigned int *lock)
 {
-	assert(thread_irq_disabled());
+	assert(thread_foreign_intr_disabled());
 	__cpu_spin_unlock(lock);
 	spinlock_count_decr();
 }
diff --git a/core/arch/arm/include/kernel/thread.h b/core/arch/arm/include/kernel/thread.h
index 175ba77..831b5d6 100644
--- a/core/arch/arm/include/kernel/thread.h
+++ b/core/arch/arm/include/kernel/thread.h
@@ -30,6 +30,7 @@
 #define KERNEL_THREAD_H
 
 #ifndef ASM
+#include <arm.h>
 #include <types_ext.h>
 #include <compiler.h>
 #include <optee_msg.h>
@@ -203,7 +204,7 @@ struct thread_svc_regs {
 
 #ifndef ASM
 typedef void (*thread_smc_handler_t)(struct thread_smc_args *args);
-typedef void (*thread_fiq_handler_t)(void);
+typedef void (*thread_nintr_handler_t)(void);
 typedef unsigned long (*thread_pm_handler_t)(unsigned long a0,
 					     unsigned long a1);
 struct thread_handlers {
@@ -218,11 +219,12 @@ struct thread_handlers {
 	 *
 	 * fastcall handles fast calls which can't be preemted. This
 	 * handler is executed with a limited stack. This handler must not
-	 * cause any aborts or reenenable FIQs which are temporarily masked
-	 * while executing this handler.
+	 * cause any aborts or reenenable native interrupts which are
+	 * temporarily masked while executing this handler.
 	 *
-	 * TODO investigate if we should execute fastcalls and FIQs on
-	 * different stacks allowing FIQs to be enabled during a fastcall.
+	 * TODO investigate if we should execute fastcalls and native interrupts
+	 * on different stacks allowing native interrupts to be enabled during
+	 * a fastcall.
 	 */
 	thread_smc_handler_t std_smc;
 	thread_smc_handler_t fast_smc;
@@ -231,12 +233,12 @@ struct thread_handlers {
 	 * fiq is called as a regular function and normal ARM Calling
 	 * Convention applies.
 	 *
-	 * This handler handles FIQs which can't be preemted. This handler
-	 * is executed with a limited stack. This handler must not cause
-	 * any aborts or reenenable FIQs which are temporarily masked while
-	 * executing this handler.
+	 * This handler handles native interrupts which can't be preemted. This
+	 * handler is executed with a limited stack. This handler must not cause
+	 * any aborts or reenenable native interrupts which are temporarily
+	 * masked while executing this handler.
 	 */
-	thread_fiq_handler_t fiq;
+	thread_nintr_handler_t nintr;
 
 	/*
 	 * Power management handlers triggered from ARM Trusted Firmware.
@@ -285,28 +287,30 @@ int thread_get_id_may_fail(void);
 struct thread_specific_data *thread_get_tsd(void);
 
 /*
- * Sets IRQ status for current thread, must only be called from an
- * active thread context.
+ * Sets foreign interrupts status for current thread, must only be called
+ * from an active thread context.
  *
- * enable == true  -> enable IRQ
- * enable == false -> disable IRQ
+ * enable == true  -> enable foreign interrupts
+ * enable == false -> disable foreign interrupts
  */
-void thread_set_irq(bool enable);
+void thread_set_foreign_intr(bool enable);
 
 /*
- * Restores the IRQ status (in CPSR) for current thread, must only be called
- * from an active thread context.
+ * Restores the foreign interrupts status (in CPSR) for current thread, must
+ * only be called from an active thread context.
  */
-void thread_restore_irq(void);
+void thread_restore_foreign_intr(void);
 
 /*
  * Defines the bits for the exception mask used the the
  * thread_*_exceptions() functions below.
+ * These definitions are compatible with both ARM32 and ARM64.
  */
-#define THREAD_EXCP_FIQ	(1 << 0)
-#define THREAD_EXCP_IRQ	(1 << 1)
-#define THREAD_EXCP_ABT	(1 << 2)
-#define THREAD_EXCP_ALL	(THREAD_EXCP_FIQ | THREAD_EXCP_IRQ | THREAD_EXCP_ABT)
+#define THREAD_EXCP_FOREIGN_INTR	(ARM32_CPSR_I >> ARM32_CPSR_F_SHIFT)
+#define THREAD_EXCP_NATIVE_INTR		(ARM32_CPSR_F >> ARM32_CPSR_F_SHIFT)
+#define THREAD_EXCP_ALL			(THREAD_EXCP_FOREIGN_INTR	\
+					| THREAD_EXCP_NATIVE_INTR	\
+					| (ARM32_CPSR_A >> ARM32_CPSR_F_SHIFT))
 
 /*
  * thread_get_exceptions() - return current exception mask
@@ -337,18 +341,18 @@ uint32_t thread_mask_exceptions(uint32_t exceptions);
 void thread_unmask_exceptions(uint32_t state);
 
 
-static inline bool thread_irq_disabled(void)
+static inline bool thread_foreign_intr_disabled(void)
 {
-	return !!(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	return !!(thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR);
 }
 
 #ifdef CFG_WITH_VFP
 /*
  * thread_kernel_enable_vfp() - Temporarily enables usage of VFP
  *
- * IRQ is masked while VFP is enabled. User space must not be entered before
- * thread_kernel_disable_vfp() has been called to disable VFP and restore the
- * IRQ status.
+ * Foreign interrupts are masked while VFP is enabled. User space must not be
+ * entered before thread_kernel_disable_vfp() has been called to disable VFP
+ * and restore the foreign interrupt status.
  *
  * This function may only be called from an active thread context and may
  * not be called again before thread_kernel_disable_vfp() has been called.
@@ -364,7 +368,7 @@ uint32_t thread_kernel_enable_vfp(void);
  * thread_kernel_disable_vfp() - Disables usage of VFP
  * @state:	state variable returned by thread_kernel_enable_vfp()
  *
- * Disables usage of VFP and restores IRQ status after a call to
+ * Disables usage of VFP and restores foreign interrupt status after a call to
  * thread_kernel_enable_vfp().
  *
  * This function may only be called after a call to
@@ -484,13 +488,13 @@ bool thread_addr_is_in_stack(vaddr_t va);
 
 /*
  * Adds a mutex to the list of held mutexes for current thread
- * Requires IRQs to be disabled.
+ * Requires foreign interrupts to be disabled.
  */
 void thread_add_mutex(struct mutex *m);
 
 /*
  * Removes a mutex from the list of held mutexes for current thread
- * Requires IRQs to be disabled.
+ * Requires foreign interrupts to be disabled.
  */
 void thread_rem_mutex(struct mutex *m);
 
diff --git a/core/arch/arm/include/kernel/thread_defs.h b/core/arch/arm/include/kernel/thread_defs.h
index 0f54569..e081895 100644
--- a/core/arch/arm/include/kernel/thread_defs.h
+++ b/core/arch/arm/include/kernel/thread_defs.h
@@ -29,7 +29,7 @@
 #define KERNEL_THREAD_DEFS_H
 
 #define THREAD_FLAGS_COPY_ARGS_ON_RETURN	(1 << 0)
-#define THREAD_FLAGS_IRQ_ENABLE			(1 << 1)
-#define THREAD_FLAGS_EXIT_ON_IRQ		(1 << 2)
+#define THREAD_FLAGS_FOREIGN_INTR_ENABLE	(1 << 1)
+#define THREAD_FLAGS_EXIT_ON_FOREIGN_INTR	(1 << 2)
 
 #endif /*KERNEL_THREAD_DEFS_H*/
diff --git a/core/arch/arm/include/kernel/wait_queue.h b/core/arch/arm/include/kernel/wait_queue.h
index eb8f881..bb53cb6 100644
--- a/core/arch/arm/include/kernel/wait_queue.h
+++ b/core/arch/arm/include/kernel/wait_queue.h
@@ -67,7 +67,8 @@ static inline void wq_wait_init(struct wait_queue *wq,
 
 /* Waits for the wait queue element to the awakened. */
 void wq_wait_final(struct wait_queue *wq, struct wait_queue_elem *wqe,
-			const void *sync_obj, const char *fname, int lineno);
+		   const void *sync_obj, int owner, const char *fname,
+		   int lineno);
 
 /* Wakes up the first wait queue element in the wait queue, if there is one */
 void wq_wake_one(struct wait_queue *wq, const void *sync_obj,
diff --git a/core/arch/arm/include/mm/core_memprot.h b/core/arch/arm/include/mm/core_memprot.h
index b7ccd21..99514fd 100644
--- a/core/arch/arm/include/mm/core_memprot.h
+++ b/core/arch/arm/include/mm/core_memprot.h
@@ -45,14 +45,14 @@
 
 /* memory atttributes */
 enum buf_is_attr {
-	CORE_MEM_SEC,
+	CORE_MEM_CACHED,
+	CORE_MEM_EXTRAM,
+	CORE_MEM_NSEC_SHM,
 	CORE_MEM_NON_SEC,
+	CORE_MEM_SEC,
 	CORE_MEM_TEE_RAM,
 	CORE_MEM_TA_RAM,
-	CORE_MEM_NSEC_SHM,
-	CORE_MEM_EXTRAM,
-	CORE_MEM_INTRAM,
-	CORE_MEM_CACHED,
+	CORE_MEM_SDP_MEM,
 };
 
 /* redirect legacy tee_vbuf_is() and tee_pbuf_is() to our routines */
@@ -95,6 +95,13 @@ bool core_vbuf_is(uint32_t flags, const void *vbuf, size_t len);
 void *phys_to_virt(paddr_t pa, enum teecore_memtypes m);
 
 /*
+ * Translate physical address to virtual address trying MEM_AREA_IO_SEC
+ * first then MEM_AREA_IO_NSEC if not found.
+ * Returns NULL on failure or a valid virtual address on success.
+ */
+void *phys_to_virt_io(paddr_t pa);
+
+/*
  * Translate virtual address to physical address
  * Returns 0 on failure or a valid physical address on success.
  */
diff --git a/core/arch/arm/include/mm/core_mmu.h b/core/arch/arm/include/mm/core_mmu.h
index 03ad93d..70be5ab 100644
--- a/core/arch/arm/include/mm/core_mmu.h
+++ b/core/arch/arm/include/mm/core_mmu.h
@@ -100,6 +100,7 @@ enum teecore_memtypes {
 	MEM_AREA_IO_SEC,
 	MEM_AREA_RES_VASPACE,
 	MEM_AREA_TA_VASPACE,
+	MEM_AREA_SDP_MEM,
 	MEM_AREA_MAXTYPE
 };
 
@@ -115,6 +116,13 @@ struct core_mmu_phys_mem {
 		__used __section("phys_mem_map_section") = \
 		{ #addr, (type), (addr), (size) }
 
+#define __register_sdp_mem2(pa, sz, id) \
+	static const struct core_mmu_phys_mem __phys_sdp_mem_ ## id \
+		__used __section("phys_sdp_mem_section") = \
+		{ .type = MEM_AREA_SDP_MEM, .addr = (pa), .size = (sz), }
+
+#define __register_sdp_mem1(pa, sz, id)	__register_sdp_mem2(pa, sz, id)
+#define register_sdp_mem(pa, sz)	__register_sdp_mem1(pa, sz, __COUNTER__)
 
 /* Default NSec shared memory allocated from NSec world */
 extern unsigned long default_nsec_shm_paddr;
@@ -350,20 +358,6 @@ bool core_mmu_is_shm_cached(void);
 
 bool core_mmu_add_mapping(enum teecore_memtypes type, paddr_t addr, size_t len);
 
-/* L1/L2 cache maintenance (op: refer to ???) */
-unsigned int cache_maintenance_l1(int op, void *va, size_t len);
-#ifdef CFG_PL310
-unsigned int cache_maintenance_l2(int op, paddr_t pa, size_t len);
-#else
-static inline unsigned int cache_maintenance_l2(int op __unused,
-						paddr_t pa __unused,
-						size_t len __unused)
-{
-	/* Nothing to do about L2 Cache Maintenance when no PL310 */
-	return TEE_SUCCESS;
-}
-#endif
-
 /* various invalidate secure TLB */
 enum teecore_tlb_op {
 	TLBINV_UNIFIEDTLB,	/* invalidate unified tlb */
@@ -375,25 +369,37 @@ enum teecore_tlb_op {
 int core_tlb_maintenance(int op, unsigned int a);
 
 /* Cache maintenance operation type */
-typedef enum {
-	DCACHE_CLEAN = 0x1,
-	DCACHE_AREA_CLEAN = 0x2,
-	DCACHE_INVALIDATE = 0x3,
-	DCACHE_AREA_INVALIDATE = 0x4,
-	ICACHE_INVALIDATE = 0x5,
-	ICACHE_AREA_INVALIDATE = 0x6,
-	WRITE_BUFFER_DRAIN = 0x7,
-	DCACHE_CLEAN_INV = 0x8,
-	DCACHE_AREA_CLEAN_INV = 0x9,
-	L2CACHE_INVALIDATE = 0xA,
-	L2CACHE_AREA_INVALIDATE = 0xB,
-	L2CACHE_CLEAN = 0xC,
-	L2CACHE_AREA_CLEAN = 0xD,
-	L2CACHE_CLEAN_INV = 0xE,
-	L2CACHE_AREA_CLEAN_INV = 0xF
-} t_cache_operation_id;
+enum cache_op {
+	DCACHE_CLEAN,
+	DCACHE_AREA_CLEAN,
+	DCACHE_INVALIDATE,
+	DCACHE_AREA_INVALIDATE,
+	ICACHE_INVALIDATE,
+	ICACHE_AREA_INVALIDATE,
+	DCACHE_CLEAN_INV,
+	DCACHE_AREA_CLEAN_INV,
+};
+
+/* L1/L2 cache maintenance */
+TEE_Result cache_op_inner(enum cache_op op, void *va, size_t len);
+#ifdef CFG_PL310
+TEE_Result cache_op_outer(enum cache_op op, paddr_t pa, size_t len);
+#else
+static inline TEE_Result cache_op_outer(enum cache_op op __unused,
+						paddr_t pa __unused,
+						size_t len __unused)
+{
+	/* Nothing to do about L2 Cache Maintenance when no PL310 */
+	return TEE_SUCCESS;
+}
+#endif
 
 /* Check cpu mmu enabled or not */
 bool cpu_mmu_enabled(void);
 
+#ifdef CFG_SECURE_DATA_PATH
+/* Alloc and fill SDP memory objects table - table is NULL terminated */
+struct mobj **core_sdp_mem_create_mobjs(void);
+#endif
+
 #endif /* CORE_MMU_H */
diff --git a/core/arch/arm/include/mm/mobj.h b/core/arch/arm/include/mm/mobj.h
index d5eeb69..1a76149 100644
--- a/core/arch/arm/include/mm/mobj.h
+++ b/core/arch/arm/include/mm/mobj.h
@@ -108,6 +108,11 @@ static inline bool mobj_is_secure(struct mobj *mobj)
 	return mobj_matches(mobj, CORE_MEM_SEC);
 }
 
+static inline bool mobj_is_sdp_mem(struct mobj *mobj)
+{
+	return mobj_matches(mobj, CORE_MEM_SDP_MEM);
+}
+
 struct mobj *mobj_mm_alloc(struct mobj *mobj_parent, size_t size,
 			   tee_mm_pool_t *pool);
 
diff --git a/core/arch/arm/include/sm/optee_smc.h b/core/arch/arm/include/sm/optee_smc.h
index b6fcd65..c369708 100644
--- a/core/arch/arm/include/sm/optee_smc.h
+++ b/core/arch/arm/include/sm/optee_smc.h
@@ -385,7 +385,7 @@
 	OPTEE_SMC_FAST_CALL_VAL(OPTEE_SMC_FUNCID_BOOT_SECONDARY)
 
 /*
- * Resume from RPC (for example after processing an IRQ)
+ * Resume from RPC (for example after processing a foreign interrupt)
  *
  * Call register usage:
  * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC
@@ -470,19 +470,19 @@
 	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_FREE)
 
 /*
- * Deliver an IRQ in normal world.
+ * Deliver a foreign interrupt in normal world.
  *
  * "Call" register usage:
- * a0	OPTEE_SMC_RETURN_RPC_IRQ
+ * a0	OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
  * a1-7	Resume information, must be preserved
  *
  * "Return" register usage:
  * a0	SMC Function ID, OPTEE_SMC_CALL_RETURN_FROM_RPC.
  * a1-7	Preserved
  */
-#define OPTEE_SMC_RPC_FUNC_IRQ		4
-#define OPTEE_SMC_RETURN_RPC_IRQ \
-	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_IRQ)
+#define OPTEE_SMC_RPC_FUNC_FOREIGN_INTR	4
+#define OPTEE_SMC_RETURN_RPC_FOREIGN_INTR \
+	OPTEE_SMC_RPC_VAL(OPTEE_SMC_RPC_FUNC_FOREIGN_INTR)
 
 /*
  * Do an RPC request. The supplied struct optee_msg_arg tells which
diff --git a/core/arch/arm/include/sm/sm.h b/core/arch/arm/include/sm/sm.h
index 6368359..3446506 100644
--- a/core/arch/arm/include/sm/sm.h
+++ b/core/arch/arm/include/sm/sm.h
@@ -29,6 +29,7 @@
 #ifndef SM_SM_H
 #define SM_SM_H
 
+#include <compiler.h>
 #include <types_ext.h>
 
 struct sm_mode_regs {
@@ -120,4 +121,17 @@ void *sm_get_sp(void);
  */
 void sm_init(vaddr_t stack_pointer);
 
+#ifndef CFG_SM_PLATFORM_HANDLER
+/*
+ * Returns false if we handled the monitor service and should now return
+ * back to the non-secure state
+ */
+static inline bool sm_platform_handler(__unused struct sm_ctx *ctx)
+{
+	return true;
+}
+#else
+bool sm_platform_handler(struct sm_ctx *ctx);
+#endif
+
 #endif /*SM_SM_H*/
diff --git a/core/arch/arm/kernel/elf_common.h b/core/arch/arm/kernel/elf_common.h
index dd8cd50..497a902 100644
--- a/core/arch/arm/kernel/elf_common.h
+++ b/core/arch/arm/kernel/elf_common.h
@@ -645,6 +645,7 @@ typedef struct {
 #define	R_386_TLS_TPOFF32	37	/* GOT entry of -ve static TLS offset */
 #define	R_386_IRELATIVE		42	/* PLT entry resolved indirectly at runtime */
 
+#define	R_AARCH64_ABS64		257
 #define	R_AARCH64_RELATIVE 	1027
 
 #define	R_ARM_NONE		0	/* No relocation. */
diff --git a/core/arch/arm/kernel/elf_load.c b/core/arch/arm/kernel/elf_load.c
index 420ba59..b1d6102 100644
--- a/core/arch/arm/kernel/elf_load.c
+++ b/core/arch/arm/kernel/elf_load.c
@@ -499,9 +499,13 @@ static TEE_Result e32_process_rel(struct elf_load_state *state, size_t rel_sidx,
 static TEE_Result e64_process_rel(struct elf_load_state *state,
 			size_t rel_sidx, vaddr_t vabase)
 {
+	Elf64_Ehdr *ehdr = state->ehdr;
 	Elf64_Shdr *shdr = state->shdr;
 	Elf64_Rela *rela;
 	Elf64_Rela *rela_end;
+	size_t sym_tab_idx;
+	Elf64_Sym *sym_tab = NULL;
+	size_t num_syms = 0;
 
 	if (shdr[rel_sidx].sh_type != SHT_RELA)
 		return TEE_ERROR_NOT_IMPLEMENTED;
@@ -509,6 +513,27 @@ static TEE_Result e64_process_rel(struct elf_load_state *state,
 	if (shdr[rel_sidx].sh_entsize != sizeof(Elf64_Rela))
 		return TEE_ERROR_BAD_FORMAT;
 
+	sym_tab_idx = shdr[rel_sidx].sh_link;
+	if (sym_tab_idx) {
+		if (sym_tab_idx >= ehdr->e_shnum)
+			return TEE_ERROR_BAD_FORMAT;
+
+		if (shdr[sym_tab_idx].sh_entsize != sizeof(Elf64_Sym))
+			return TEE_ERROR_BAD_FORMAT;
+
+		/* Check the address is inside TA memory */
+		if (shdr[sym_tab_idx].sh_addr > state->vasize ||
+		    (shdr[sym_tab_idx].sh_addr +
+				shdr[sym_tab_idx].sh_size) > state->vasize)
+			return TEE_ERROR_BAD_FORMAT;
+
+		sym_tab = (Elf64_Sym *)(vabase + shdr[sym_tab_idx].sh_addr);
+		if (!ALIGNMENT_IS_OK(sym_tab, Elf64_Sym))
+			return TEE_ERROR_BAD_FORMAT;
+
+		num_syms = shdr[sym_tab_idx].sh_size / sizeof(Elf64_Sym);
+	}
+
 	/* Check the address is inside TA memory */
 	if (shdr[rel_sidx].sh_addr >= state->vasize)
 		return TEE_ERROR_BAD_FORMAT;
@@ -522,6 +547,7 @@ static TEE_Result e64_process_rel(struct elf_load_state *state,
 	rela_end = rela + shdr[rel_sidx].sh_size / sizeof(Elf64_Rela);
 	for (; rela < rela_end; rela++) {
 		Elf64_Addr *where;
+		size_t sym_idx;
 
 		/* Check the address is inside TA memory */
 		if (rela->r_offset >= state->vasize)
@@ -532,6 +558,13 @@ static TEE_Result e64_process_rel(struct elf_load_state *state,
 			return TEE_ERROR_BAD_FORMAT;
 
 		switch (ELF64_R_TYPE(rela->r_info)) {
+		case R_AARCH64_ABS64:
+			sym_idx = ELF64_R_SYM(rela->r_info);
+			if (sym_idx > num_syms)
+				return TEE_ERROR_BAD_FORMAT;
+			*where = rela->r_addend + sym_tab[sym_idx].st_value +
+				 vabase;
+			break;
 		case R_AARCH64_RELATIVE:
 			*where = rela->r_addend + vabase;
 			break;
diff --git a/core/arch/arm/kernel/generic_boot.c b/core/arch/arm/kernel/generic_boot.c
index 8f13c36..0d78d40 100644
--- a/core/arch/arm/kernel/generic_boot.c
+++ b/core/arch/arm/kernel/generic_boot.c
@@ -93,12 +93,6 @@ __weak void plat_cpu_reset_late(void)
 KEEP_PAGER(plat_cpu_reset_late);
 
 /* May be overridden in plat-$(PLATFORM)/main.c */
-__weak void plat_cpu_reset_early(void)
-{
-}
-KEEP_PAGER(plat_cpu_reset_early);
-
-/* May be overridden in plat-$(PLATFORM)/main.c */
 __weak void main_init_gic(void)
 {
 }
@@ -289,9 +283,8 @@ static void init_runtime(unsigned long pageable_part)
 		p = (uint8_t *)(((vaddr_t)__init_start + init_size) &
 				~SMALL_PAGE_MASK);
 
-		cache_maintenance_l1(DCACHE_AREA_CLEAN, p, SMALL_PAGE_SIZE);
-		cache_maintenance_l1(ICACHE_AREA_INVALIDATE, p,
-				     SMALL_PAGE_SIZE);
+		cache_op_inner(DCACHE_AREA_CLEAN, p, SMALL_PAGE_SIZE);
+		cache_op_inner(ICACHE_AREA_INVALIDATE, p, SMALL_PAGE_SIZE);
 	}
 
 	/*
@@ -607,7 +600,8 @@ static void init_primary_helper(unsigned long pageable_part,
 	 * Mask asynchronous exceptions before switch to the thread vector
 	 * as the thread handler requires those to be masked while
 	 * executing with the temporary stack. The thread subsystem also
-	 * asserts that IRQ is blocked when using most if its functions.
+	 * asserts that the foreign interrupts are blocked when using most of
+	 * its functions.
 	 */
 	thread_set_exceptions(THREAD_EXCP_ALL);
 	init_vfp_sec();
@@ -634,7 +628,8 @@ static void init_secondary_helper(unsigned long nsec_entry)
 	 * Mask asynchronous exceptions before switch to the thread vector
 	 * as the thread handler requires those to be masked while
 	 * executing with the temporary stack. The thread subsystem also
-	 * asserts that IRQ is blocked when using most if its functions.
+	 * asserts that the foreign interrupts are blocked when using most of
+	 * its functions.
 	 */
 	thread_set_exceptions(THREAD_EXCP_ALL);
 
diff --git a/core/arch/arm/kernel/generic_entry_a32.S b/core/arch/arm/kernel/generic_entry_a32.S
index 27717d5..9c2ef41 100644
--- a/core/arch/arm/kernel/generic_entry_a32.S
+++ b/core/arch/arm/kernel/generic_entry_a32.S
@@ -25,16 +25,16 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <platform_config.h>
-
-#include <asm.S>
 #include <arm.h>
 #include <arm32_macros.S>
+#include <asm.S>
+#include <platform_config.h>
+#include <keep.h>
+#include <kernel/asan.h>
+#include <kernel/unwind.h>
 #include <sm/optee_smc.h>
 #include <sm/teesmc_opteed_macros.h>
 #include <sm/teesmc_opteed.h>
-#include <kernel/unwind.h>
-#include <kernel/asan.h>
 
 .section .data
 .balign 4
@@ -87,6 +87,14 @@ END_FUNC __assert_flat_mapped_range
 	.endm
 #endif /* CFG_PL310 */
 
+.weak plat_cpu_reset_early
+FUNC plat_cpu_reset_early , :
+UNWIND(	.fnstart)
+	bx	lr
+UNWIND(	.fnend)
+END_FUNC plat_cpu_reset_early
+KEEP_PAGER plat_cpu_reset_early
+
 .section .text.boot
 FUNC _start , :
 	b	reset
diff --git a/core/arch/arm/kernel/kern.ld.S b/core/arch/arm/kernel/kern.ld.S
index 10dac6e..b761aea 100644
--- a/core/arch/arm/kernel/kern.ld.S
+++ b/core/arch/arm/kernel/kern.ld.S
@@ -107,6 +107,10 @@ SECTIONS
 		__start_phys_mem_map_section = . ;
 		KEEP(*(phys_mem_map_section))
 		__end_phys_mem_map_section = . ;
+		. = ALIGN(8);
+		__start_phys_sdp_mem_section = . ;
+		KEEP(*(phys_sdp_mem_section))
+		__end_phys_sdp_mem_section = . ;
 #endif
 		. = ALIGN(8);
 		__rodata_end = .;
@@ -254,6 +258,10 @@ SECTIONS
 		KEEP(*(phys_mem_map_section))
 		__end_phys_mem_map_section = . ;
 		. = ALIGN(8);
+		__start_phys_sdp_mem_section = . ;
+		KEEP(*(phys_sdp_mem_section))
+		__end_phys_sdp_mem_section = . ;
+		. = ALIGN(8);
 		__rodata_init_end = .;
 	}
 	__init_start = __text_init_start;
diff --git a/core/arch/arm/kernel/mutex.c b/core/arch/arm/kernel/mutex.c
index 0e1b836..a25ca12 100644
--- a/core/arch/arm/kernel/mutex.c
+++ b/core/arch/arm/kernel/mutex.c
@@ -45,6 +45,7 @@ static void __mutex_lock(struct mutex *m, const char *fname, int lineno)
 		uint32_t old_itr_status;
 		enum mutex_value old_value;
 		struct wait_queue_elem wqe;
+		int owner = MUTEX_OWNER_ID_NONE;
 
 		/*
 		 * If the mutex is locked we need to initialize the wqe
@@ -61,6 +62,7 @@ static void __mutex_lock(struct mutex *m, const char *fname, int lineno)
 		old_value = m->value;
 		if (old_value == MUTEX_VALUE_LOCKED) {
 			wq_wait_init(&m->wq, &wqe);
+			owner = m->owner_id;
 		} else {
 			m->value = MUTEX_VALUE_LOCKED;
 			thread_add_mutex(m);
@@ -74,7 +76,7 @@ static void __mutex_lock(struct mutex *m, const char *fname, int lineno)
 			 * Someone else is holding the lock, wait in normal
 			 * world for the lock to become available.
 			 */
-			wq_wait_final(&m->wq, &wqe, m, fname, lineno);
+			wq_wait_final(&m->wq, &wqe, m, owner, fname, lineno);
 		} else
 			return;
 	}
@@ -260,7 +262,8 @@ static void __condvar_wait(struct condvar *cv, struct mutex *m,
 	/* Wake eventual waiters */
 	wq_wake_one(&m->wq, m, fname, lineno);
 
-	wq_wait_final(&m->wq, &wqe, m, fname, lineno);
+	wq_wait_final(&m->wq, &wqe,
+		      m, MUTEX_OWNER_ID_CONDVAR_SLEEP, fname, lineno);
 
 	mutex_lock(m);
 }
diff --git a/core/arch/arm/kernel/pseudo_ta.c b/core/arch/arm/kernel/pseudo_ta.c
index 6352a28..78b2bfd 100644
--- a/core/arch/arm/kernel/pseudo_ta.c
+++ b/core/arch/arm/kernel/pseudo_ta.c
@@ -37,9 +37,41 @@
 #include <trace.h>
 #include <types_ext.h>
 
+#ifdef CFG_SECURE_DATA_PATH
+static bool client_is_secure(struct tee_ta_session *s)
+{
+	/* rely on core entry to have constrained client IDs */
+	if (s->clnt_id.login == TEE_LOGIN_TRUSTED_APP)
+		return true;
+
+	return false;
+}
+
+static bool validate_in_param(struct tee_ta_session *s, struct mobj *mobj)
+{
+	/* for secure clients, core entry always holds valid memref objects */
+	if (client_is_secure(s))
+		return true;
+
+	/* all non-secure memory references are hanlded by pTAs */
+	if (mobj_is_nonsec(mobj))
+		return true;
+
+	return false;
+}
+#else
+static bool validate_in_param(struct tee_ta_session *s __unused,
+				struct mobj *mobj __unused)
+{
+	/* At this point, core has filled only valid accessible memref mobj */
+	return true;
+}
+#endif
+
 /* Maps static TA params */
-static TEE_Result copy_in_param(struct tee_ta_param *param,
-				     TEE_Param tee_param[TEE_NUM_PARAMS])
+static TEE_Result copy_in_param(struct tee_ta_session *s __maybe_unused,
+				struct tee_ta_param *param,
+				TEE_Param tee_param[TEE_NUM_PARAMS])
 {
 	size_t n;
 	void *va;
@@ -55,6 +87,9 @@ static TEE_Result copy_in_param(struct tee_ta_param *param,
 		case TEE_PARAM_TYPE_MEMREF_INPUT:
 		case TEE_PARAM_TYPE_MEMREF_OUTPUT:
 		case TEE_PARAM_TYPE_MEMREF_INOUT:
+			if (!validate_in_param(s, param->u[n].mem.mobj))
+				return TEE_ERROR_BAD_PARAMETERS;
+
 			va = mobj_get_va(param->u[n].mem.mobj,
 					 param->u[n].mem.offs);
 			if (!va)
@@ -110,7 +145,7 @@ static TEE_Result pseudo_ta_enter_open_session(struct tee_ta_session *s,
 	}
 
 	if (stc->pseudo_ta->open_session_entry_point) {
-		res = copy_in_param(param, tee_param);
+		res = copy_in_param(s, param, tee_param);
 		if (res != TEE_SUCCESS) {
 			*eo = TEE_ORIGIN_TEE;
 			goto out;
@@ -136,7 +171,7 @@ static TEE_Result pseudo_ta_enter_invoke_cmd(struct tee_ta_session *s,
 	TEE_Param tee_param[TEE_NUM_PARAMS];
 
 	tee_ta_push_current_session(s);
-	res = copy_in_param(param, tee_param);
+	res = copy_in_param(s, param, tee_param);
 	if (res != TEE_SUCCESS) {
 		*eo = TEE_ORIGIN_TEE;
 		goto out;
@@ -224,7 +259,7 @@ TEE_Result tee_ta_init_pseudo_ta_session(const TEE_UUID *uuid,
 	struct tee_ta_ctx *ctx;
 	const struct pseudo_ta_head *ta;
 
-	DMSG("   Lookup for Static TA %pUl", (void *)uuid);
+	DMSG("   Lookup for pseudo TA %pUl", (void *)uuid);
 
 	ta = &__start_ta_head_section;
 	while (true) {
diff --git a/core/arch/arm/kernel/spin_lock_debug.c b/core/arch/arm/kernel/spin_lock_debug.c
index 2a450a5..00a2a00 100644
--- a/core/arch/arm/kernel/spin_lock_debug.c
+++ b/core/arch/arm/kernel/spin_lock_debug.c
@@ -49,10 +49,11 @@ bool have_spinlock(void)
 {
 	struct thread_core_local *l;
 
-	if (!thread_irq_disabled()) {
+	if (!thread_foreign_intr_disabled()) {
 		/*
 		 * Normally we can't be holding a spinlock since doing so would
-		 * imply IRQ are disabled (or the spinlock logic is flawed).
+		 * imply foreign interrupts are disabled (or the spinlock
+		 * logic is flawed).
 		 */
 		return false;
 	}
diff --git a/core/arch/arm/kernel/tee_time_arm_cntpct.c b/core/arch/arm/kernel/tee_time_arm_cntpct.c
index 90e7f20..59d6ea4 100644
--- a/core/arch/arm/kernel/tee_time_arm_cntpct.c
+++ b/core/arch/arm/kernel/tee_time_arm_cntpct.c
@@ -93,7 +93,7 @@ void plat_prng_add_jitter_entropy(void)
 		}
 	}
 	if (bytes) {
-		DMSG("%s: 0x%02X\n", __func__,
+		FMSG("%s: 0x%02X\n", __func__,
 		     (int)acc & ((1 << (bytes * 8)) - 1));
 		tee_prng_add_entropy((uint8_t *)&acc, bytes);
 	}
diff --git a/core/arch/arm/kernel/thread.c b/core/arch/arm/kernel/thread.c
index c988b65..2aaa0e6 100644
--- a/core/arch/arm/kernel/thread.c
+++ b/core/arch/arm/kernel/thread.c
@@ -66,15 +66,11 @@
 #endif
 #define STACK_THREAD_SIZE	8192
 
-#if TRACE_LEVEL > 0
 #ifdef CFG_CORE_SANITIZE_KADDRESS
 #define STACK_ABT_SIZE		3072
 #else
 #define STACK_ABT_SIZE		2048
 #endif
-#else
-#define STACK_ABT_SIZE		1024
-#endif
 
 #endif /*ARM32*/
 
@@ -140,7 +136,7 @@ KEEP_PAGER(stack_tmp_offset);
 
 thread_smc_handler_t thread_std_smc_handler_ptr;
 static thread_smc_handler_t thread_fast_smc_handler_ptr;
-thread_fiq_handler_t thread_fiq_handler_ptr;
+thread_nintr_handler_t thread_nintr_handler_ptr;
 thread_pm_handler_t thread_cpu_on_handler_ptr;
 thread_pm_handler_t thread_cpu_off_handler_ptr;
 thread_pm_handler_t thread_cpu_suspend_handler_ptr;
@@ -234,8 +230,8 @@ void thread_set_exceptions(uint32_t exceptions)
 {
 	uint32_t cpsr = read_cpsr();
 
-	/* IRQ must not be unmasked while holding a spinlock */
-	if (!(exceptions & THREAD_EXCP_IRQ))
+	/* Foreign interrupts must not be unmasked while holding a spinlock */
+	if (!(exceptions & THREAD_EXCP_FOREIGN_INTR))
 		assert_have_no_spinlock();
 
 	cpsr &= ~(THREAD_EXCP_ALL << CPSR_F_SHIFT);
@@ -256,8 +252,8 @@ void thread_set_exceptions(uint32_t exceptions)
 {
 	uint32_t daif = read_daif();
 
-	/* IRQ must not be unmasked while holding a spinlock */
-	if (!(exceptions & THREAD_EXCP_IRQ))
+	/* Foreign interrupts must not be unmasked while holding a spinlock */
+	if (!(exceptions & THREAD_EXCP_FOREIGN_INTR))
 		assert_have_no_spinlock();
 
 	daif &= ~(THREAD_EXCP_ALL << DAIF_F_SHIFT);
@@ -285,11 +281,11 @@ struct thread_core_local *thread_get_core_local(void)
 	uint32_t cpu_id = get_core_pos();
 
 	/*
-	 * IRQs must be disabled before playing with core_local since
-	 * we otherwise may be rescheduled to a different core in the
+	 * Foreign interrupts must be disabled before playing with core_local
+	 * since we otherwise may be rescheduled to a different core in the
 	 * middle of this function.
 	 */
-	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	assert(thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR);
 
 	assert(cpu_id < CFG_TEE_CORE_NB_CORE);
 	return &thread_core_local[cpu_id];
@@ -338,11 +334,12 @@ static void init_regs(struct thread_ctx *thread,
 	thread->regs.pc = (uint32_t)thread_std_smc_entry;
 
 	/*
-	 * Stdcalls starts in SVC mode with masked IRQ, masked Asynchronous
-	 * abort and unmasked FIQ.
-	  */
+	 * Stdcalls starts in SVC mode with masked foreign interrupts, masked
+	 * Asynchronous abort and unmasked native interrupts.
+	 */
 	thread->regs.cpsr = read_cpsr() & ARM32_CPSR_E;
-	thread->regs.cpsr |= CPSR_MODE_SVC | CPSR_I | CPSR_A;
+	thread->regs.cpsr |= CPSR_MODE_SVC | CPSR_A |
+			(THREAD_EXCP_FOREIGN_INTR << ARM32_CPSR_F_SHIFT);
 	/* Enable thumb mode if it's a thumb instruction */
 	if (thread->regs.pc & 1)
 		thread->regs.cpsr |= CPSR_T;
@@ -371,11 +368,11 @@ static void init_regs(struct thread_ctx *thread,
 	thread->regs.pc = (uint64_t)thread_std_smc_entry;
 
 	/*
-	 * Stdcalls starts in SVC mode with masked IRQ, masked Asynchronous
-	 * abort and unmasked FIQ.
-	  */
+	 * Stdcalls starts in SVC mode with masked foreign interrupts, masked
+	 * Asynchronous abort and unmasked native interrupts.
+	 */
 	thread->regs.cpsr = SPSR_64(SPSR_64_MODE_EL1, SPSR_64_MODE_SP_EL0,
-				    DAIFBIT_IRQ | DAIFBIT_ABT);
+				THREAD_EXCP_FOREIGN_INTR | DAIFBIT_ABT);
 	/* Reinitialize stack pointer */
 	thread->regs.sp = thread->stack_va_end;
 
@@ -556,7 +553,7 @@ static void thread_resume_from_rpc(struct thread_smc_args *args)
 		core_mmu_set_user_map(&threads[n].user_map);
 
 	/*
-	 * Return from RPC to request service of an IRQ must not
+	 * Return from RPC to request service of a foreign interrupt must not
 	 * get parameters from non-secure world.
 	 */
 	if (threads[n].flags & THREAD_FLAGS_COPY_ARGS_ON_RETURN) {
@@ -769,8 +766,10 @@ bool thread_init_stack(uint32_t thread_id, vaddr_t sp)
 
 int thread_get_id_may_fail(void)
 {
-	/* thread_get_core_local() requires IRQs to be disabled */
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	/*
+	 * thread_get_core_local() requires foreign interrupts to be disabled
+	 */
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 	struct thread_core_local *l = thread_get_core_local();
 	int ct = l->curr_thread;
 
@@ -790,7 +789,7 @@ static void init_handlers(const struct thread_handlers *handlers)
 {
 	thread_std_smc_handler_ptr = handlers->std_smc;
 	thread_fast_smc_handler_ptr = handlers->fast_smc;
-	thread_fiq_handler_ptr = handlers->fiq;
+	thread_nintr_handler_ptr = handlers->nintr;
 	thread_cpu_on_handler_ptr = handlers->cpu_on;
 	thread_cpu_off_handler_ptr = handlers->cpu_off;
 	thread_cpu_suspend_handler_ptr = handlers->cpu_suspend;
@@ -890,10 +889,10 @@ struct thread_ctx_regs *thread_get_ctx_regs(void)
 	return &threads[l->curr_thread].regs;
 }
 
-void thread_set_irq(bool enable)
+void thread_set_foreign_intr(bool enable)
 {
-	/* thread_get_core_local() requires IRQs to be disabled */
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	/* thread_get_core_local() requires foreign interrupts to be disabled */
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 	struct thread_core_local *l;
 
 	l = thread_get_core_local();
@@ -901,35 +900,37 @@ void thread_set_irq(bool enable)
 	assert(l->curr_thread != -1);
 
 	if (enable) {
-		threads[l->curr_thread].flags |= THREAD_FLAGS_IRQ_ENABLE;
-		thread_set_exceptions(exceptions & ~THREAD_EXCP_IRQ);
+		threads[l->curr_thread].flags |=
+					THREAD_FLAGS_FOREIGN_INTR_ENABLE;
+		thread_set_exceptions(exceptions & ~THREAD_EXCP_FOREIGN_INTR);
 	} else {
 		/*
-		 * No need to disable IRQ here since it's already disabled
-		 * above.
+		 * No need to disable foreign interrupts here since they're
+		 * already disabled above.
 		 */
-		threads[l->curr_thread].flags &= ~THREAD_FLAGS_IRQ_ENABLE;
+		threads[l->curr_thread].flags &=
+					~THREAD_FLAGS_FOREIGN_INTR_ENABLE;
 	}
 }
 
-void thread_restore_irq(void)
+void thread_restore_foreign_intr(void)
 {
-	/* thread_get_core_local() requires IRQs to be disabled */
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	/* thread_get_core_local() requires foreign interrupts to be disabled */
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 	struct thread_core_local *l;
 
 	l = thread_get_core_local();
 
 	assert(l->curr_thread != -1);
 
-	if (threads[l->curr_thread].flags & THREAD_FLAGS_IRQ_ENABLE)
-		thread_set_exceptions(exceptions & ~THREAD_EXCP_IRQ);
+	if (threads[l->curr_thread].flags & THREAD_FLAGS_FOREIGN_INTR_ENABLE)
+		thread_set_exceptions(exceptions & ~THREAD_EXCP_FOREIGN_INTR);
 }
 
 #ifdef CFG_WITH_VFP
 uint32_t thread_kernel_enable_vfp(void)
 {
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 	struct thread_ctx *thr = threads + thread_get_id();
 	struct thread_user_vfp_state *tuv = thr->vfp_state.uvfp;
 
@@ -967,9 +968,9 @@ void thread_kernel_disable_vfp(uint32_t state)
 
 	vfp_disable();
 	exceptions = thread_get_exceptions();
-	assert(exceptions & THREAD_EXCP_IRQ);
-	exceptions &= ~THREAD_EXCP_IRQ;
-	exceptions |= state & THREAD_EXCP_IRQ;
+	assert(exceptions & THREAD_EXCP_FOREIGN_INTR);
+	exceptions &= ~THREAD_EXCP_FOREIGN_INTR;
+	exceptions |= state & THREAD_EXCP_FOREIGN_INTR;
 	thread_set_exceptions(exceptions);
 }
 
@@ -977,7 +978,7 @@ void thread_kernel_save_vfp(void)
 {
 	struct thread_ctx *thr = threads + thread_get_id();
 
-	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	assert(thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR);
 	if (vfp_is_enabled()) {
 		vfp_lazy_save_state_init(&thr->vfp_state.sec);
 		thr->vfp_state.sec_lazy_saved = true;
@@ -988,7 +989,7 @@ void thread_kernel_restore_vfp(void)
 {
 	struct thread_ctx *thr = threads + thread_get_id();
 
-	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	assert(thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR);
 	assert(!vfp_is_enabled());
 	if (thr->vfp_state.sec_lazy_saved) {
 		vfp_lazy_restore_state(&thr->vfp_state.sec,
@@ -1003,7 +1004,7 @@ void thread_user_enable_vfp(struct thread_user_vfp_state *uvfp)
 	struct thread_ctx *thr = threads + thread_get_id();
 	struct thread_user_vfp_state *tuv = thr->vfp_state.uvfp;
 
-	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	assert(thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR);
 	assert(!vfp_is_enabled());
 
 	if (!thr->vfp_state.ns_saved) {
@@ -1030,7 +1031,7 @@ void thread_user_save_vfp(void)
 	struct thread_ctx *thr = threads + thread_get_id();
 	struct thread_user_vfp_state *tuv = thr->vfp_state.uvfp;
 
-	assert(thread_get_exceptions() & THREAD_EXCP_IRQ);
+	assert(thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR);
 	if (!vfp_is_enabled())
 		return;
 
@@ -1110,7 +1111,7 @@ void thread_add_mutex(struct mutex *m)
 	int ct = l->curr_thread;
 
 	assert(ct != -1 && threads[ct].state == THREAD_STATE_ACTIVE);
-	assert(m->owner_id == -1);
+	assert(m->owner_id == MUTEX_OWNER_ID_NONE);
 	m->owner_id = ct;
 	TAILQ_INSERT_TAIL(&threads[ct].mutexes, m, link);
 }
@@ -1122,7 +1123,7 @@ void thread_rem_mutex(struct mutex *m)
 
 	assert(ct != -1 && threads[ct].state == THREAD_STATE_ACTIVE);
 	assert(m->owner_id == ct);
-	m->owner_id = -1;
+	m->owner_id = MUTEX_OWNER_ID_NONE;
 	TAILQ_REMOVE(&threads[ct].mutexes, m, link);
 }
 
@@ -1130,7 +1131,7 @@ bool thread_disable_prealloc_rpc_cache(uint64_t *cookie)
 {
 	bool rv;
 	size_t n;
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 
 	lock_global();
 
@@ -1163,7 +1164,7 @@ bool thread_enable_prealloc_rpc_cache(void)
 {
 	bool rv;
 	size_t n;
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 
 	lock_global();
 
@@ -1194,7 +1195,14 @@ static uint32_t rpc_cmd_nolock(uint32_t cmd, size_t num_params,
 
 	assert(arg && carg && num_params <= THREAD_RPC_MAX_NUM_PARAMS);
 
-	plat_prng_add_jitter_entropy();
+
+	/*
+	 * Break recursion in case plat_prng_add_jitter_entropy_norpc()
+	 * sleeps on a mutex or unlocks a mutex with a sleeper (contended
+	 * mutex).
+	 */
+	if (cmd != OPTEE_MSG_RPC_CMD_WAIT_QUEUE)
+		plat_prng_add_jitter_entropy_norpc();
 
 	memset(arg, 0, OPTEE_MSG_GET_ARG_SIZE(THREAD_RPC_MAX_NUM_PARAMS));
 	arg->cmd = cmd;
diff --git a/core/arch/arm/kernel/thread_a32.S b/core/arch/arm/kernel/thread_a32.S
index 6d3ac35..1a1c696 100644
--- a/core/arch/arm/kernel/thread_a32.S
+++ b/core/arch/arm/kernel/thread_a32.S
@@ -75,7 +75,7 @@ UNWIND(	.fnstart)
 UNWIND(	.cantunwind)
  	/* Secure Monitor received a FIQ and passed control to us. */
 	bl	thread_check_canaries
- 	ldr	lr, =thread_fiq_handler_ptr
+	ldr	lr, =thread_nintr_handler_ptr
  	ldr	lr, [lr]
  	blx	lr
 	mov	r1, r0
@@ -392,7 +392,7 @@ UNWIND(	.cantunwind)
 	 */
 	push	{r0-r3, r8-r12, lr}
 	bl	thread_check_canaries
-	ldr	lr, =thread_fiq_handler_ptr
+	ldr	lr, =thread_nintr_handler_ptr
 	ldr	lr, [lr]
 	blx	lr
 	pop	{r0-r3, r8-r12, lr}
@@ -416,7 +416,7 @@ UNWIND(	.cantunwind)
 
 	bl	thread_save_state
 
-	mov	r0, #THREAD_FLAGS_EXIT_ON_IRQ
+	mov	r0, #THREAD_FLAGS_EXIT_ON_FOREIGN_INTR
 	mrs	r1, spsr
 	pop	{r12}
 	pop	{r2}
@@ -432,7 +432,7 @@ UNWIND(	.cantunwind)
 	mov	sp, r0
 
 	ldr	r0, =TEESMC_OPTEED_RETURN_CALL_DONE
-	ldr	r1, =OPTEE_SMC_RETURN_RPC_IRQ
+	ldr	r1, =OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
 	mov	r2, #0
 	mov	r3, #0
 	/* r4 is already filled in above */
diff --git a/core/arch/arm/kernel/thread_a64.S b/core/arch/arm/kernel/thread_a64.S
index abd482b..241868a 100644
--- a/core/arch/arm/kernel/thread_a64.S
+++ b/core/arch/arm/kernel/thread_a64.S
@@ -77,7 +77,7 @@ END_FUNC vector_fast_smc_entry
 LOCAL_FUNC vector_fiq_entry , :
 	/* Secure Monitor received a FIQ and passed control to us. */
 	bl	thread_check_canaries
-	adr	x16, thread_fiq_handler_ptr
+	adr	x16, thread_nintr_handler_ptr
 	ldr	x16, [x16]
 	blr	x16
 	ldr	x0, =TEESMC_OPTEED_RETURN_FIQ_DONE
@@ -487,9 +487,9 @@ LOCAL_FUNC el0_svc , :
 	mov	x0, sp
 
 	/*
-	 * Unmask FIQ, Serror, and debug exceptions since we have nothing
-	 * left in sp_el1. Note that the SVC handler is excepted to
-	 * re-enable IRQs by itself.
+	 * Unmask native interrupts, Serror, and debug exceptions since we have
+	 * nothing left in sp_el1. Note that the SVC handler is excepted to
+	 * re-enable foreign interrupts by itself.
 	 */
 	msr	daifclr, #(DAIFBIT_FIQ | DAIFBIT_ABT | DAIFBIT_DBG)
 
@@ -713,7 +713,7 @@ LOCAL_FUNC elx_irq , :
 	/*
 	 * Mark current thread as suspended
 	 */
-	mov	w0, #THREAD_FLAGS_EXIT_ON_IRQ
+	mov	w0, #THREAD_FLAGS_EXIT_ON_FOREIGN_INTR
 	mrs	x1, spsr_el1
 	mrs	x2, elr_el1
 	bl	thread_state_suspend
@@ -734,7 +734,7 @@ LOCAL_FUNC elx_irq , :
 	 */
 
 	ldr	w0, =TEESMC_OPTEED_RETURN_CALL_DONE
-	ldr	w1, =OPTEE_SMC_RETURN_RPC_IRQ
+	ldr	w1, =OPTEE_SMC_RETURN_RPC_FOREIGN_INTR
 	mov	w2, #0
 	mov	w3, #0
 	/* w4 is already filled in above */
@@ -787,7 +787,7 @@ LOCAL_FUNC elx_fiq , :
 	stp	x30, x2, [sp, #ELX_FIQ_REC_LR]
 
 	bl	thread_check_canaries
-	adr	x16, thread_fiq_handler_ptr
+	adr	x16, thread_nintr_handler_ptr
 	ldr	x16, [x16]
 	blr	x16
 
diff --git a/core/arch/arm/kernel/trace_ext.c b/core/arch/arm/kernel/trace_ext.c
index 8b8454c..6cedba3 100644
--- a/core/arch/arm/kernel/trace_ext.c
+++ b/core/arch/arm/kernel/trace_ext.c
@@ -27,21 +27,40 @@
 #include <stdbool.h>
 #include <trace.h>
 #include <console.h>
+#include <kernel/spinlock.h>
 #include <kernel/thread.h>
+#include <mm/core_mmu.h>
 
 const char trace_ext_prefix[] = "TEE-CORE";
 int trace_level = TRACE_LEVEL;
+static unsigned int puts_lock = SPINLOCK_UNLOCK;
 
 void trace_ext_puts(const char *str)
 {
+	uint32_t itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
+	bool mmu_enabled = cpu_mmu_enabled();
+	bool was_contended = false;
 	const char *p;
 
+	if (mmu_enabled && !cpu_spin_trylock(&puts_lock)) {
+		was_contended = true;
+		cpu_spin_lock(&puts_lock);
+	}
+
 	console_flush();
 
+	if (was_contended)
+		console_putc('*');
+
 	for (p = str; *p; p++)
 		console_putc(*p);
 
 	console_flush();
+
+	if (mmu_enabled)
+		cpu_spin_unlock(&puts_lock);
+
+	thread_unmask_exceptions(itr_status);
 }
 
 int trace_ext_get_thread_id(void)
diff --git a/core/arch/arm/kernel/user_ta.c b/core/arch/arm/kernel/user_ta.c
index a63fb22..5c9aae8 100644
--- a/core/arch/arm/kernel/user_ta.c
+++ b/core/arch/arm/kernel/user_ta.c
@@ -194,8 +194,8 @@ static TEE_Result config_final_paging(struct user_ta_ctx *utc)
 	size_t vasize = utc->mmu->ta_private_vmem_end -
 			utc->mmu->ta_private_vmem_start;
 
-	cache_maintenance_l1(DCACHE_AREA_CLEAN, va, vasize);
-	cache_maintenance_l1(ICACHE_AREA_INVALIDATE, va, vasize);
+	cache_op_inner(DCACHE_AREA_CLEAN, va, vasize);
+	cache_op_inner(ICACHE_AREA_INVALIDATE, va, vasize);
 	return TEE_SUCCESS;
 }
 #endif /*!CFG_PAGED_USER_TA*/
@@ -386,7 +386,7 @@ static TEE_Result ta_load(const TEE_UUID *uuid, const struct shdr *signed_ta,
 	uint32_t man_flags = TA_FLAG_USER_MODE | TA_FLAG_EXEC_DDR;
 	/* opt_flags: optional flags */
 	uint32_t opt_flags = man_flags | TA_FLAG_SINGLE_INSTANCE |
-	    TA_FLAG_MULTI_SESSION | TA_FLAG_UNSAFE_NW_PARAMS |
+	    TA_FLAG_MULTI_SESSION | TA_FLAG_SECURE_DATA_PATH |
 	    TA_FLAG_INSTANCE_KEEP_ALIVE | TA_FLAG_CACHE_MAINTENANCE;
 	struct user_ta_ctx *utc = NULL;
 	struct shdr *sec_shdr = NULL;
@@ -748,8 +748,8 @@ static void user_ta_ctx_destroy(struct tee_ta_ctx *ctx)
 			va = mobj_get_va(utc->mobj_code, 0);
 			if (va) {
 				memset(va, 0, utc->mobj_code->size);
-				cache_maintenance_l1(DCACHE_AREA_CLEAN, va,
-						     utc->mobj_code->size);
+				cache_op_inner(DCACHE_AREA_CLEAN, va,
+						utc->mobj_code->size);
 			}
 		}
 
@@ -757,8 +757,8 @@ static void user_ta_ctx_destroy(struct tee_ta_ctx *ctx)
 			va = mobj_get_va(utc->mobj_stack, 0);
 			if (va) {
 				memset(va, 0, utc->mobj_stack->size);
-				cache_maintenance_l1(DCACHE_AREA_CLEAN, va,
-						     utc->mobj_stack->size);
+				cache_op_inner(DCACHE_AREA_CLEAN, va,
+						utc->mobj_stack->size);
 			}
 		}
 	}
diff --git a/core/arch/arm/kernel/wait_queue.c b/core/arch/arm/kernel/wait_queue.c
index a96e0fe..6fb4456 100644
--- a/core/arch/arm/kernel/wait_queue.c
+++ b/core/arch/arm/kernel/wait_queue.c
@@ -43,7 +43,8 @@ void wq_init(struct wait_queue *wq)
 }
 
 static void wq_rpc(uint32_t func, int id, const void *sync_obj __maybe_unused,
-			const char *fname, int lineno __maybe_unused)
+		   int owner __maybe_unused, const char *fname,
+		   int lineno __maybe_unused)
 {
 	uint32_t ret;
 	struct optee_msg_param params;
@@ -51,10 +52,10 @@ static void wq_rpc(uint32_t func, int id, const void *sync_obj __maybe_unused,
 	     func == OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP ? "sleep" : "wake ";
 
 	if (fname)
-		DMSG("%s thread %u %p %s:%d", cmd_str, id,
-		     sync_obj, fname, lineno);
+		DMSG("%s thread %u %p %d %s:%d", cmd_str, id,
+		     sync_obj, owner, fname, lineno);
 	else
-		DMSG("%s thread %u %p", cmd_str, id, sync_obj);
+		DMSG("%s thread %u %p %d", cmd_str, id, sync_obj, owner);
 
 	memset(&params, 0, sizeof(params));
 	params.attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
@@ -99,14 +100,15 @@ void wq_wait_init_condvar(struct wait_queue *wq, struct wait_queue_elem *wqe,
 }
 
 void wq_wait_final(struct wait_queue *wq, struct wait_queue_elem *wqe,
-			const void *sync_obj, const char *fname, int lineno)
+		   const void *sync_obj, int owner, const char *fname,
+		   int lineno)
 {
 	uint32_t old_itr_status;
 	unsigned done;
 
 	do {
 		wq_rpc(OPTEE_MSG_RPC_WAIT_QUEUE_SLEEP, wqe->handle,
-		       sync_obj, fname, lineno);
+		       sync_obj, owner, fname, lineno);
 
 		old_itr_status = thread_mask_exceptions(THREAD_EXCP_ALL);
 		cpu_spin_lock(&wq_spin_lock);
@@ -145,7 +147,7 @@ void wq_wake_one(struct wait_queue *wq, const void *sync_obj,
 
 	if (do_wakeup)
 		wq_rpc(OPTEE_MSG_RPC_WAIT_QUEUE_WAKEUP, handle,
-		       sync_obj, fname, lineno);
+		       sync_obj, MUTEX_OWNER_ID_MUTEX_UNLOCK, fname, lineno);
 }
 
 void wq_promote_condvar(struct wait_queue *wq, struct condvar *cv,
diff --git a/core/arch/arm/mm/core_mmu.c b/core/arch/arm/mm/core_mmu.c
index 62dda73..f85e496 100644
--- a/core/arch/arm/mm/core_mmu.c
+++ b/core/arch/arm/mm/core_mmu.c
@@ -101,6 +101,10 @@ static struct memaccess_area nsec_shared[] = {
 	MEMACCESS_AREA(CFG_SHMEM_START, CFG_SHMEM_SIZE),
 };
 
+#ifdef CFG_TEE_SDP_MEM_BASE
+register_sdp_mem(CFG_TEE_SDP_MEM_BASE, CFG_TEE_SDP_MEM_SIZE);
+#endif
+
 register_phys_mem(MEM_AREA_TEE_RAM, CFG_TEE_RAM_START, CFG_TEE_RAM_PH_SIZE);
 register_phys_mem(MEM_AREA_TA_RAM, CFG_TA_RAM_START, CFG_TA_RAM_SIZE);
 register_phys_mem(MEM_AREA_NSEC_SHM, CFG_SHMEM_START, CFG_SHMEM_SIZE);
@@ -222,6 +226,107 @@ static struct tee_mmap_region *find_map_by_pa(unsigned long pa)
 	return NULL;
 }
 
+#ifdef CFG_SECURE_DATA_PATH
+extern const struct core_mmu_phys_mem __start_phys_sdp_mem_section;
+extern const struct core_mmu_phys_mem __end_phys_sdp_mem_section;
+
+static bool pbuf_is_sdp_mem(paddr_t pbuf, size_t len)
+{
+	const struct core_mmu_phys_mem *mem;
+
+	for (mem = &__start_phys_sdp_mem_section;
+	     mem < &__end_phys_sdp_mem_section; mem++)
+		if (core_is_buffer_inside(pbuf, len, mem->addr, mem->size))
+			return true;
+
+	return false;
+}
+
+#define MSG_SDP_INSTERSECT(pa1, sz1, pa2, sz2) \
+	EMSG("[%" PRIxPA " %" PRIxPA "] intersecs [%" PRIxPA " %" PRIxPA "]", \
+			pa1, pa1 + sz1, pa2, pa2 + sz2)
+
+/* Check SDP memories comply with registered memories */
+static void verify_sdp_mem_areas(struct tee_mmap_region *mem_map, size_t len)
+{
+	const struct core_mmu_phys_mem *mem;
+	const struct core_mmu_phys_mem *mem2;
+	const struct core_mmu_phys_mem *start = &__start_phys_sdp_mem_section;
+	const struct core_mmu_phys_mem *end = &__end_phys_sdp_mem_section;
+	struct tee_mmap_region *mmap;
+	size_t n;
+
+	if (start == end) {
+		IMSG("Secure data path enabled without any SDP memory area");
+		return;
+	}
+
+	for (mem = start; mem < end; mem++)
+		DMSG("SDP memory [%" PRIxPA " %" PRIxPA "]",
+			mem->addr, mem->addr + mem->size);
+
+	/* Check SDP memories do not intersect each other */
+	for (mem = start; mem < end - 1; mem++) {
+		for (mem2 = mem + 1; mem2 < end; mem2++) {
+			if (core_is_buffer_intersect(mem2->addr, mem2->size,
+						     mem->addr, mem->size)) {
+				MSG_SDP_INSTERSECT(mem2->addr, mem2->size,
+						   mem->addr, mem->size);
+				panic("SDP memory intersection");
+			}
+		}
+	}
+
+	/*
+	 * Check SDP memories do not intersect any mapped memory.
+	 * This is called before reserved VA space is loaded in mem_map.
+	 */
+	for (mem = start; mem < end; mem++) {
+		for (mmap = mem_map, n = 0; n < len; mmap++, n++) {
+			if (core_is_buffer_intersect(mem->addr, mem->size,
+						     mmap->pa, mmap->size)) {
+				MSG_SDP_INSTERSECT(mem->addr, mem->size,
+						   mmap->pa, mmap->size);
+				panic("SDP memory intersection");
+			}
+		}
+	}
+}
+
+struct mobj **core_sdp_mem_create_mobjs(void)
+{
+	const struct core_mmu_phys_mem *mem;
+	struct mobj **mobj_base;
+	struct mobj **mobj;
+	int cnt = &__end_phys_sdp_mem_section - &__start_phys_sdp_mem_section;
+
+	/* SDP mobjs table must end with a NULL entry */
+	mobj_base = calloc(cnt + 1, sizeof(struct mobj *));
+	if (!mobj_base)
+		panic("Out of memory");
+
+	for (mem = &__start_phys_sdp_mem_section, mobj = mobj_base;
+	     mem < &__end_phys_sdp_mem_section; mem++, mobj++) {
+		 *mobj = mobj_phys_alloc(mem->addr, mem->size,
+					 TEE_MATTR_CACHE_CACHED,
+					 CORE_MEM_SDP_MEM);
+		if (!*mobj)
+			panic("can't create SDP physical memory object");
+	}
+	return mobj_base;
+}
+#else /* CFG_SECURE_DATA_PATH */
+static bool pbuf_is_sdp_mem(paddr_t pbuf __unused, size_t len __unused)
+{
+	return false;
+}
+
+static void verify_sdp_mem_areas(struct tee_mmap_region *mem_map __unused,
+				 size_t len __unused)
+{
+}
+#endif /* CFG_SECURE_DATA_PATH */
+
 extern const struct core_mmu_phys_mem __start_phys_mem_map_section;
 extern const struct core_mmu_phys_mem __end_phys_mem_map_section;
 
@@ -353,6 +458,8 @@ static void init_mem_map(struct tee_mmap_region *memory_map, size_t num_elems)
 		add_phys_mem(memory_map, num_elems, &m, &last);
 	}
 
+	verify_sdp_mem_areas(memory_map, num_elems);
+
 	add_va_space(memory_map, num_elems, MEM_AREA_RES_VASPACE,
 		     RES_VASPACE_SIZE, &last);
 
@@ -382,7 +489,7 @@ static void init_mem_map(struct tee_mmap_region *memory_map, size_t num_elems)
 	assert(map->type == MEM_AREA_TEE_RAM);
 	map->va = map->pa;
 #ifdef CFG_WITH_PAGER
-	map->region_size = SMALL_PAGE_SIZE,
+	map->region_size = SMALL_PAGE_SIZE;
 #endif
 	map->attr = core_mmu_type_to_attr(map->type);
 
@@ -393,6 +500,7 @@ static void init_mem_map(struct tee_mmap_region *memory_map, size_t num_elems)
 		while (map->type != MEM_AREA_NOTYPE) {
 			map->attr = core_mmu_type_to_attr(map->type);
 			va -= map->size;
+			va = ROUNDDOWN(va, map->region_size);
 			map->va = va;
 			map++;
 		}
@@ -413,6 +521,7 @@ static void init_mem_map(struct tee_mmap_region *memory_map, size_t num_elems)
 		map++;
 		while (map->type != MEM_AREA_NOTYPE) {
 			map->attr = core_mmu_type_to_attr(map->type);
+			va = ROUNDUP(va, map->region_size);
 			map->va = va;
 			va += map->size;
 			map++;
@@ -541,6 +650,8 @@ bool core_pbuf_is(uint32_t attr, paddr_t pbuf, size_t len)
 		return pbuf_inside_map_area(pbuf, len, map_ta_ram);
 	case CORE_MEM_NSEC_SHM:
 		return pbuf_inside_map_area(pbuf, len, map_nsec_shm);
+	case CORE_MEM_SDP_MEM:
+		return pbuf_is_sdp_mem(pbuf, len);
 	case CORE_MEM_EXTRAM:
 		return pbuf_is_inside(ddr, pbuf, len);
 	case CORE_MEM_CACHED:
@@ -648,7 +759,7 @@ int core_tlb_maintenance(int op, unsigned int a)
 	return 0;
 }
 
-unsigned int cache_maintenance_l1(int op, void *va, size_t len)
+TEE_Result cache_op_inner(enum cache_op op, void *va, size_t len)
 {
 	switch (op) {
 	case DCACHE_CLEAN:
@@ -672,10 +783,6 @@ unsigned int cache_maintenance_l1(int op, void *va, size_t len)
 		if (len)
 			arm_cl1_i_inv(va, (char *)va + len - 1);
 		break;
-	case WRITE_BUFFER_DRAIN:
-		DMSG("unsupported operation 0x%X (WRITE_BUFFER_DRAIN)",
-		     (unsigned int)op);
-		return -1;
 	case DCACHE_CLEAN_INV:
 		arm_cl1_d_cleaninvbysetway();
 		break;
@@ -690,31 +797,31 @@ unsigned int cache_maintenance_l1(int op, void *va, size_t len)
 }
 
 #ifdef CFG_PL310
-unsigned int cache_maintenance_l2(int op, paddr_t pa, size_t len)
+TEE_Result cache_op_outer(enum cache_op op, paddr_t pa, size_t len)
 {
-	unsigned int ret = TEE_SUCCESS;
-	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	TEE_Result ret = TEE_SUCCESS;
+	uint32_t exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 
 	tee_l2cc_mutex_lock();
 	switch (op) {
-	case L2CACHE_INVALIDATE:
+	case DCACHE_INVALIDATE:
 		arm_cl2_invbyway(pl310_base());
 		break;
-	case L2CACHE_AREA_INVALIDATE:
+	case DCACHE_AREA_INVALIDATE:
 		if (len)
 			arm_cl2_invbypa(pl310_base(), pa, pa + len - 1);
 		break;
-	case L2CACHE_CLEAN:
+	case DCACHE_CLEAN:
 		arm_cl2_cleanbyway(pl310_base());
 		break;
-	case L2CACHE_AREA_CLEAN:
+	case DCACHE_AREA_CLEAN:
 		if (len)
 			arm_cl2_cleanbypa(pl310_base(), pa, pa + len - 1);
 		break;
-	case L2CACHE_CLEAN_INV:
+	case DCACHE_CLEAN_INV:
 		arm_cl2_cleaninvbyway(pl310_base());
 		break;
-	case L2CACHE_AREA_CLEAN_INV:
+	case DCACHE_AREA_CLEAN_INV:
 		if (len)
 			arm_cl2_cleaninvbypa(pl310_base(), pa, pa + len - 1);
 		break;
@@ -1163,6 +1270,21 @@ void *phys_to_virt(paddr_t pa, enum teecore_memtypes m)
 	return va;
 }
 
+void *phys_to_virt_io(paddr_t pa)
+{
+	struct tee_mmap_region *map;
+	void *va;
+
+	map = find_map_by_type_and_pa(MEM_AREA_IO_SEC, pa);
+	if (!map)
+		map = find_map_by_type_and_pa(MEM_AREA_IO_NSEC, pa);
+	if (!map)
+		return NULL;
+	va = map_pa2va(map, pa);
+	check_va_matches_pa(pa, va);
+	return va;
+}
+
 bool cpu_mmu_enabled(void)
 {
 	uint32_t sctlr;
diff --git a/core/arch/arm/mm/core_mmu_lpae.c b/core/arch/arm/mm/core_mmu_lpae.c
index eb96c70..7a5d74a 100644
--- a/core/arch/arm/mm/core_mmu_lpae.c
+++ b/core/arch/arm/mm/core_mmu_lpae.c
@@ -640,7 +640,7 @@ bool core_mmu_find_table(vaddr_t va, unsigned max_level,
 		if (!tbl)
 			return false;
 
-		va_base += n << level_size_shift;
+		va_base += (vaddr_t)n << level_size_shift;
 		level++;
 		num_entries = XLAT_TABLE_ENTRIES;
 	}
diff --git a/core/arch/arm/mm/mobj.c b/core/arch/arm/mm/mobj.c
index 5458638..6daef50 100644
--- a/core/arch/arm/mm/mobj.c
+++ b/core/arch/arm/mm/mobj.c
@@ -114,12 +114,13 @@ static bool mobj_phys_matches(struct mobj *mobj, enum buf_is_attr attr)
 	switch (attr) {
 	case CORE_MEM_SEC:
 		return a == CORE_MEM_SEC || a == CORE_MEM_TEE_RAM ||
-		       a == CORE_MEM_TA_RAM;
+		       a == CORE_MEM_TA_RAM || a == CORE_MEM_SDP_MEM;
 	case CORE_MEM_NON_SEC:
 		return a == CORE_MEM_NSEC_SHM;
 	case CORE_MEM_TEE_RAM:
 	case CORE_MEM_TA_RAM:
 	case CORE_MEM_NSEC_SHM:
+	case CORE_MEM_SDP_MEM:
 		return attr == a;
 	default:
 		return false;
@@ -151,6 +152,7 @@ struct mobj *mobj_phys_alloc(paddr_t pa, size_t size, uint32_t cattr,
 			     enum buf_is_attr battr)
 {
 	struct mobj_phys *moph;
+	enum teecore_memtypes area_type;
 	void *va;
 
 	if ((pa & CORE_MMU_USER_PARAM_MASK) ||
@@ -159,8 +161,27 @@ struct mobj *mobj_phys_alloc(paddr_t pa, size_t size, uint32_t cattr,
 		return NULL;
 	}
 
-	va = phys_to_virt(pa, battr);
-	if (!va)
+	switch (battr) {
+	case CORE_MEM_TEE_RAM:
+		area_type = MEM_AREA_TEE_RAM;
+		break;
+	case CORE_MEM_TA_RAM:
+		area_type = MEM_AREA_TA_RAM;
+		break;
+	case CORE_MEM_NSEC_SHM:
+		area_type = MEM_AREA_NSEC_SHM;
+		break;
+	case CORE_MEM_SDP_MEM:
+		area_type = MEM_AREA_SDP_MEM;
+		break;
+	default:
+		DMSG("can't allocate with specified attribute");
+		return NULL;
+	}
+
+	/* Only SDP memory may not have a virtual address */
+	va = phys_to_virt(pa, area_type);
+	if (!va && battr != CORE_MEM_SDP_MEM)
 		return NULL;
 
 	moph = calloc(1, sizeof(*moph));
diff --git a/core/arch/arm/mm/tee_pager.c b/core/arch/arm/mm/tee_pager.c
index c7238fe..c75eee8 100644
--- a/core/arch/arm/mm/tee_pager.c
+++ b/core/arch/arm/mm/tee_pager.c
@@ -891,10 +891,10 @@ bool tee_pager_set_uta_area_attr(struct user_ta_ctx *utc, vaddr_t base,
 				void *va = (void *)area_idx2va(pmem->area,
 							       pmem->pgidx);
 
-				cache_maintenance_l1(DCACHE_AREA_CLEAN, va,
-						     SMALL_PAGE_SIZE);
-				cache_maintenance_l1(ICACHE_AREA_INVALIDATE, va,
-						     SMALL_PAGE_SIZE);
+				cache_op_inner(DCACHE_AREA_CLEAN, va,
+						SMALL_PAGE_SIZE);
+				cache_op_inner(ICACHE_AREA_INVALIDATE, va,
+						SMALL_PAGE_SIZE);
 			}
 		}
 
@@ -1269,10 +1269,9 @@ bool tee_pager_handle_fault(struct abort_info *ai)
 			 * Doing these operations to LoUIS (Level of
 			 * unification, Inner Shareable) would be enough
 			 */
-			cache_maintenance_l1(DCACHE_AREA_CLEAN,
-				pmem->va_alias, SMALL_PAGE_SIZE);
-
-			cache_maintenance_l1(ICACHE_INVALIDATE, NULL, 0);
+			cache_op_inner(DCACHE_AREA_CLEAN, pmem->va_alias,
+					SMALL_PAGE_SIZE);
+			cache_op_inner(ICACHE_INVALIDATE, NULL, 0);
 		}
 
 		pmem->area = area;
diff --git a/core/arch/arm/plat-d02/main.c b/core/arch/arm/plat-d02/main.c
index 95161d1..2cd339b 100644
--- a/core/arch/arm/plat-d02/main.c
+++ b/core/arch/arm/plat-d02/main.c
@@ -42,7 +42,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
 	.cpu_suspend = pm_do_nothing,
@@ -51,6 +51,8 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct hi16xx_uart_data console_data __early_bss;
+
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, HI16XX_UART_REG_SIZE);
 
 const struct thread_handlers *generic_boot_get_handlers(void)
@@ -63,34 +65,9 @@ static void main_fiq(void)
 	panic();
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	hi16xx_uart_init(console_base(), CONSOLE_UART_CLK_IN_HZ,
-			 CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		hi16xx_uart_putc('\r', base);
-	hi16xx_uart_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	hi16xx_uart_flush(console_base());
+	hi16xx_uart_init(&console_data, CONSOLE_UART_BASE,
+			 CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
 }
diff --git a/core/arch/arm/plat-hikey/main.c b/core/arch/arm/plat-hikey/main.c
index 36789ce..8d72f38 100644
--- a/core/arch/arm/plat-hikey/main.c
+++ b/core/arch/arm/plat-hikey/main.c
@@ -49,7 +49,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
 	.cpu_suspend = pm_do_nothing,
@@ -58,6 +58,8 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct pl011_data console_data __early_bss;
+
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
 register_phys_mem(MEM_AREA_IO_NSEC, PMUSSI_BASE, PMUSSI_REG_SIZE);
 #ifdef CFG_SPI
@@ -78,35 +80,11 @@ static void main_fiq(void)
 	panic();
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	pl011_init(console_base(), CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		pl011_putc('\r', base);
-	pl011_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	pl011_flush(console_base());
+	pl011_init(&console_data, CONSOLE_UART_BASE, CONSOLE_UART_CLK_IN_HZ,
+		   CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
 }
 
 vaddr_t nsec_periph_base(paddr_t pa)
diff --git a/core/arch/arm/plat-imx/a9_plat_init.S b/core/arch/arm/plat-imx/a9_plat_init.S
index 64d03f5..db13018 100644
--- a/core/arch/arm/plat-imx/a9_plat_init.S
+++ b/core/arch/arm/plat-imx/a9_plat_init.S
@@ -85,7 +85,7 @@ UNWIND(	.fnstart)
 	 * - NSec cannot access PLE (PLE bit16=0)
 	 * - NSec can use SIMD/VFP (CP10/CP11) (bit15:14=2b00, bit11:10=2b11)
 	 *
-	 * PCR = 0x00000001
+	 * PCR
 	 * - no change latency, enable clk gating
 	 */
 	movw r0, #0x4000
@@ -100,8 +100,8 @@ UNWIND(	.fnstart)
 	movt r0, #0x0002
 	write_nsacr r0
 
-	movw r0, #0x0000
-	movt r0, #0x0001
+	read_pcr r0
+	orr r0, r0, #0x1
 	write_pcr r0
 
 	mov pc, lr
diff --git a/core/arch/arm/plat-imx/conf.mk b/core/arch/arm/plat-imx/conf.mk
index 785736a..d780228 100644
--- a/core/arch/arm/plat-imx/conf.mk
+++ b/core/arch/arm/plat-imx/conf.mk
@@ -3,7 +3,7 @@ PLATFORM_FLAVOR ?= mx6ulevk
 ifeq ($(PLATFORM_FLAVOR),mx6ulevk)
 arm32-platform-cpuarch		:= cortex-a7
 endif
-ifeq ($(PLATFORM_FLAVOR),$(filter $(PLATFORM_FLAVOR),mx6qsabrelite mx6qsabresd))
+ifeq ($(PLATFORM_FLAVOR),$(filter $(PLATFORM_FLAVOR),mx6qsabrelite mx6qsabresd mx6dlsabresd))
 arm32-platform-cpuarch		:= cortex-a9
 endif
 arm32-platform-cflags		+= -mcpu=$(arm32-platform-cpuarch)
@@ -19,7 +19,7 @@ $(call force,CFG_WITH_SOFTWARE_PRNG,y)
 ifeq ($(PLATFORM_FLAVOR),mx6ulevk)
 $(call force,CFG_SECURE_TIME_SOURCE_CNTPCT,y)
 endif
-ifeq ($(PLATFORM_FLAVOR),$(filter $(PLATFORM_FLAVOR),mx6qsabrelite mx6qsabresd))
+ifeq ($(PLATFORM_FLAVOR),$(filter $(PLATFORM_FLAVOR),mx6qsabrelite mx6qsabresd mx6dlsabresd))
 $(call force,CFG_PL310,y)
 $(call force,CFG_PL310_LOCKED,y)
 $(call force,CFG_SECURE_TIME_SOURCE_REE,y)
diff --git a/core/arch/arm/plat-imx/main.c b/core/arch/arm/plat-imx/main.c
index edfbc37..36750a2 100644
--- a/core/arch/arm/plat-imx/main.c
+++ b/core/arch/arm/plat-imx/main.c
@@ -45,7 +45,8 @@
 #include <tee/entry_std.h>
 
 #if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
-	defined(PLATFORM_FLAVOR_mx6qsabresd)
+	defined(PLATFORM_FLAVOR_mx6qsabresd) || \
+	defined(PLATFORM_FLAVOR_mx6dlsabresd)
 #include <kernel/tz_ssvce_pl310.h>
 #endif
 
@@ -55,7 +56,7 @@ static struct gic_data gic_data;
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -64,11 +65,14 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_panic,
 };
 
+static struct imx_uart_data console_data __early_bss;
+
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, GIC_BASE, CORE_MMU_DEVICE_SIZE);
 
 #if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
-	defined(PLATFORM_FLAVOR_mx6qsabresd)
+	defined(PLATFORM_FLAVOR_mx6qsabresd) || \
+	defined(PLATFORM_FLAVOR_mx6dlsabresd)
 register_phys_mem(MEM_AREA_IO_SEC, PL310_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, SRC_BASE, CORE_MMU_DEVICE_SIZE);
 #endif
@@ -84,7 +88,8 @@ static void main_fiq(void)
 }
 
 #if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
-	defined(PLATFORM_FLAVOR_mx6qsabresd)
+	defined(PLATFORM_FLAVOR_mx6qsabresd) || \
+	defined(PLATFORM_FLAVOR_mx6dlsabresd)
 void plat_cpu_reset_late(void)
 {
 	uintptr_t addr;
@@ -126,41 +131,10 @@ void plat_cpu_reset_late(void)
 }
 #endif
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE,
-					  MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	vaddr_t base = console_base();
-
-	imx_uart_init(base);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	/* If \n, also do \r */
-	if (ch == '\n')
-		imx_uart_putc('\r', base);
-	imx_uart_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	vaddr_t base = console_base();
-
-	imx_uart_flush_tx_fifo(base);
+	imx_uart_init(&console_data, CONSOLE_UART_BASE);
+	register_serial_console(&console_data.chip);
 }
 
 void main_init_gic(void)
@@ -182,7 +156,8 @@ void main_init_gic(void)
 }
 
 #if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
-	defined(PLATFORM_FLAVOR_mx6qsabresd)
+	defined(PLATFORM_FLAVOR_mx6qsabresd) || \
+	defined(PLATFORM_FLAVOR_mx6dlsabresd)
 vaddr_t pl310_base(void)
 {
 	static void *va __early_bss;
diff --git a/core/arch/arm/plat-imx/platform_config.h b/core/arch/arm/plat-imx/platform_config.h
index 8e55ee8..b92322e 100644
--- a/core/arch/arm/plat-imx/platform_config.h
+++ b/core/arch/arm/plat-imx/platform_config.h
@@ -113,7 +113,8 @@
 /* For i.MX6 Quad SABRE Lite and Smart Device board */
 
 #elif defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
-	defined(PLATFORM_FLAVOR_mx6qsabresd)
+	defined(PLATFORM_FLAVOR_mx6qsabresd) || \
+	defined(PLATFORM_FLAVOR_mx6dlsabresd)
 
 #define SCU_BASE			0x00A00000
 #define PL310_BASE			0x00A02000
@@ -128,8 +129,16 @@
 #define GICD_OFFSET			0x1000
 #define GIC_CPU_BASE			(GIC_BASE + GICC_OFFSET)
 #define GIC_DIST_BASE			(GIC_BASE + GICD_OFFSET)
+
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
 #define UART1_BASE			0x02020000
 #define UART2_BASE			0x021E8000
+#else
+#define UART1_BASE			0x02020000
+#define UART3_BASE			0x021EC000
+#define UART5_BASE			0x021F4000
+#endif
 
 /* Central Security Unit register values */
 #define CSU_BASE			0x021C0000
@@ -146,12 +155,20 @@
 #if defined(PLATFORM_FLAVOR_mx6qsabresd)
 #define CONSOLE_UART_BASE		UART1_BASE
 #endif
+#if defined(PLATFORM_FLAVOR_mx6dlsabresd)
+#define CONSOLE_UART_BASE		UART1_BASE
+#endif
 #define DRAM0_BASE			0x10000000
 #define DRAM0_SIZE			0x40000000
 
 #define CFG_TEE_RAM_VA_SIZE		(1024 * 1024)
 
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
 #define CFG_TEE_CORE_NB_CORE		4
+#else
+#define CFG_TEE_CORE_NB_CORE		2
+#endif
 
 #define DDR_PHYS_START			DRAM0_BASE
 #define DDR_SIZE			DRAM0_SIZE
@@ -200,7 +217,12 @@
  * Full Line Zero (FLZ) disabled (bit0=0)
  */
 #ifndef PL310_AUX_CTRL_INIT
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
 #define PL310_AUX_CTRL_INIT		0x3C470800
+#else
+#define PL310_AUX_CTRL_INIT		0x3C440800
+#endif
 #endif
 
 /*
@@ -384,7 +406,12 @@
  * Cacheable accesses have high prio (bit10=0)
  * Full Line Zero (FLZ) disabled (bit0=0)
  */
+#if defined(PLATFORM_FLAVOR_mx6qsabrelite) || \
+	defined(PLATFORM_FLAVOR_mx6qsabresd)
 #define PL310_AUX_CTRL_INIT		0x3C470800
+#else
+#define PL310_AUX_CTRL_INIT		0x3C440800
+#endif
 
 /*
  * Prefetch Control Register
diff --git a/core/arch/arm/plat-imx/psci.c b/core/arch/arm/plat-imx/psci.c
index 065555b..12b60fb 100644
--- a/core/arch/arm/plat-imx/psci.c
+++ b/core/arch/arm/plat-imx/psci.c
@@ -43,7 +43,8 @@
 
 static vaddr_t src_base(void)
 {
-	static void *va __data; /* in case it's used before .bss is cleared */
+	/* in case it's used before .bss is cleared */
+	static void *va __early_bss;
 
 	if (cpu_mmu_enabled()) {
 		if (!va)
diff --git a/core/arch/arm/plat-imx/sub.mk b/core/arch/arm/plat-imx/sub.mk
index d0a2f51..75a2fbc 100644
--- a/core/arch/arm/plat-imx/sub.mk
+++ b/core/arch/arm/plat-imx/sub.mk
@@ -6,4 +6,5 @@ srcs-$(CFG_PSCI_ARM32) += psci.c
 
 srcs-$(PLATFORM_FLAVOR_mx6qsabrelite) += a9_plat_init.S
 srcs-$(PLATFORM_FLAVOR_mx6qsabresd) += a9_plat_init.S
+srcs-$(PLATFORM_FLAVOR_mx6dlsabresd) += a9_plat_init.S
 srcs-$(PLATFORM_FLAVOR_mx6ulevk) += imx6ul.c
diff --git a/core/arch/arm/plat-ls/main.c b/core/arch/arm/plat-ls/main.c
index 23ac0c6..7f8d523 100644
--- a/core/arch/arm/plat-ls/main.c
+++ b/core/arch/arm/plat-ls/main.c
@@ -48,7 +48,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -58,6 +58,7 @@ static const struct thread_handlers handlers = {
 };
 
 static struct gic_data gic_data;
+static struct ns16550_data console_data __early_bss;
 
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, GIC_BASE, CORE_MMU_DEVICE_SIZE);
@@ -120,38 +121,10 @@ void plat_cpu_reset_late(void)
 	}
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va __early_bss;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	/*
-	 * Do nothing, uart driver shared with normal world,
-	 * everything for uart driver intialization is done in bootloader.
-	 */
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		ns16550_putc('\r', base);
-	ns16550_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	ns16550_flush(console_base());
+	ns16550_init(&console_data, CONSOLE_UART_BASE);
+	register_serial_console(&console_data.chip);
 }
 
 void main_init_gic(void)
diff --git a/core/arch/arm/plat-ls/plat_init.S b/core/arch/arm/plat-ls/plat_init.S
index 81ba7d7..8813480 100644
--- a/core/arch/arm/plat-ls/plat_init.S
+++ b/core/arch/arm/plat-ls/plat_init.S
@@ -69,23 +69,20 @@ UNWIND(	.fnstart)
 	 *
 	 * SCTLR = 0x00000000
 	 *
-	 * ACTRL = 0x00000041
-	 * - core always in full SMP (FW bit0=1)
+	 * ACTRL = 0x00000040
+	 * - core NOT booted in full SMP (FW bit0=0)
 	 *
-	 * NSACR = 0x00020C00
+	 * NSACR = 0x00000C00
 	 * - NSec cannot change ACTRL.SMP (NS_SMP bit18=0)
 	 * - NSec can use SIMD/VFP (CP10/CP11) (bit15:14=2b00, bit11:10=2b11)
 	 */
-	movw r0, #0x0000
-	movt r0, #0x0000
+	mov_imm r0, 0x00000000
 	write_sctlr r0
 
-	movw r0, #0x0040
-	movt r0, #0x0000
+	mov_imm r0, 0x00000040
 	write_actlr r0
 
-	movw r0, #0x0C00
-	movt r0, #0x0000
+	mov_imm r0, 0x00000C00
 	write_nsacr r0
 
 	mov pc, lr
diff --git a/core/arch/arm/plat-mediatek/main.c b/core/arch/arm/plat-mediatek/main.c
index 7780591..62180fc 100644
--- a/core/arch/arm/plat-mediatek/main.c
+++ b/core/arch/arm/plat-mediatek/main.c
@@ -41,7 +41,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
 	.cpu_suspend = pm_do_nothing,
@@ -50,6 +50,8 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct serial8250_uart_data console_data __early_bss;
+
 const struct thread_handlers *generic_boot_get_handlers(void)
 {
 	return &handlers;
@@ -60,34 +62,9 @@ static void main_fiq(void)
 	panic();
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	serial8250_uart_init(console_base(), CONSOLE_UART_CLK_IN_HZ,
-			     CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		serial8250_uart_putc('\r', base);
-	serial8250_uart_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	serial8250_uart_flush_tx_fifo(console_base());
+	serial8250_uart_init(&console_data, CONSOLE_UART_BASE,
+			     CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
 }
diff --git a/core/arch/arm/plat-rcar/main.c b/core/arch/arm/plat-rcar/main.c
index 8f9482e..6a7e332 100644
--- a/core/arch/arm/plat-rcar/main.c
+++ b/core/arch/arm/plat-rcar/main.c
@@ -46,7 +46,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
 	.cpu_suspend = pm_do_nothing,
@@ -55,6 +55,8 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct scif_uart_data console_data __early_bss;
+
 const struct thread_handlers *generic_boot_get_handlers(void)
 {
 	return &handlers;
@@ -65,31 +67,8 @@ static void main_fiq(void)
 	panic();
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_SEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	scif_uart_init(console_base());
-}
-
-void console_putc(int ch)
-{
-	if (ch == '\n')
-		scif_uart_putc('\r', console_base());
-	scif_uart_putc(ch, console_base());
-}
-
-void console_flush(void)
-{
-	scif_uart_flush(console_base());
+	scif_uart_init(&console_data, CONSOLE_UART_BASE);
+	register_serial_console(&console_data.chip);
 }
diff --git a/core/arch/arm/plat-rpi3/conf.mk b/core/arch/arm/plat-rpi3/conf.mk
index 567680a..49fa817 100644
--- a/core/arch/arm/plat-rpi3/conf.mk
+++ b/core/arch/arm/plat-rpi3/conf.mk
@@ -25,7 +25,6 @@ CFG_CRYPTO_WITH_CE ?= n
 CFG_WITH_STACK_CANARIES ?= y
 
 CFG_TEE_CORE_EMBED_INTERNAL_TESTS ?= y
-CFG_TEE_FS_KEY_MANAGER_TEST ?= y
 CFG_WITH_STACK_CANARIES ?= y
 CFG_WITH_STATS ?= y
 
diff --git a/core/arch/arm/plat-rpi3/main.c b/core/arch/arm/plat-rpi3/main.c
index 9270e19..8a714cb 100644
--- a/core/arch/arm/plat-rpi3/main.c
+++ b/core/arch/arm/plat-rpi3/main.c
@@ -45,7 +45,7 @@ static void main_fiq(void)
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
 	.cpu_suspend = pm_do_nothing,
@@ -54,41 +54,16 @@ static const struct thread_handlers handlers = {
 	.system_reset = pm_do_nothing,
 };
 
+static struct serial8250_uart_data console_data __early_bss;
+
 const struct thread_handlers *generic_boot_get_handlers(void)
 {
 	return &handlers;
 }
 
-static vaddr_t console_base(void)
-{
-	static vaddr_t va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = (vaddr_t)phys_to_virt(CONSOLE_UART_BASE,
-						   MEM_AREA_IO_NSEC);
-		return va;
-	}
-
-	return CONSOLE_UART_BASE;
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		serial8250_uart_putc('\r', base);
-	serial8250_uart_putc(ch, base);
-}
-
 void console_init(void)
 {
-	serial8250_uart_init(console_base(), CONSOLE_UART_CLK_IN_HZ,
-			     CONSOLE_BAUDRATE);
-}
-
-void console_flush(void)
-{
-	serial8250_uart_flush_tx_fifo(console_base());
+	serial8250_uart_init(&console_data, CONSOLE_UART_BASE,
+			     CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
 }
diff --git a/core/arch/arm/plat-sprd/console.c b/core/arch/arm/plat-sprd/console.c
index 3263d69..ae56d95 100644
--- a/core/arch/arm/plat-sprd/console.c
+++ b/core/arch/arm/plat-sprd/console.c
@@ -29,30 +29,18 @@
 #include <mm/core_memprot.h>
 #include <platform_config.h>
 
-static vaddr_t console_base(void)
-{
-	static vaddr_t base;
-
-	if (cpu_mmu_enabled())
-		base = (vaddr_t)phys_to_virt(CONSOLE_UART_BASE,
-					     MEM_AREA_IO_SEC);
-	else
-		base = CONSOLE_UART_BASE;
-
-	return base;
-}
+static struct sprd_uart_data console_data __early_bss;
 
-/* Do nothing in this function */
 void console_init(void)
 {
+	sprd_uart_init(&console_data, CONSOLE_UART_BASE);
+	register_serial_console(&console_data.chip);
 }
 
 void console_putc(int ch)
 {
-	sprd_uart_putc(console_base(), (unsigned char)(ch & 0xff));
-}
+	struct serial_chip *cons = &console_data.chip;
 
-void console_flush(void)
-{
-	sprd_uart_flush(console_base());
+	cons->ops->putc(cons, ch & 0xff);
 }
+
diff --git a/core/arch/arm/plat-sprd/main.c b/core/arch/arm/plat-sprd/main.c
index bf3a62d..ea2c984 100644
--- a/core/arch/arm/plat-sprd/main.c
+++ b/core/arch/arm/plat-sprd/main.c
@@ -40,7 +40,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
 	.cpu_suspend = pm_do_nothing,
diff --git a/core/arch/arm/plat-stm/asc.S b/core/arch/arm/plat-stm/asc.S
deleted file mode 100644
index 3f2b6aa..0000000
--- a/core/arch/arm/plat-stm/asc.S
+++ /dev/null
@@ -1,108 +0,0 @@
-/*
- * 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 <platform_config.h>
-#include <asm.S>
-#include <kernel/unwind.h>
-
-#define ASC_BAUDRATE	0x00
-#define ASC_TXBUFFER	0x04
-#define ASC_RXBUFFER	0x08
-#define ASC_CONTROL	0x0c
-#define ASC_INTENABLE	0x10
-#define ASC_STATUS	0x14
-#define ASC_GUARDTIME	0x18
-#define ASC_TIMEOUT	0x1c
-#define ASC_TXRESET	0x20
-#define ASC_RXRESET	0x24
-#define ASC_RETRIES	0x28
-
-.section .text.asc
-
-
-/*
- * void __asc_flush(vaddr_t base)
- *
- *    Clobbers r0-r3
- */
-FUNC __asc_flush , :
-UNWIND(	.fnstart)
-
-    ADD r3, r0, #ASC_STATUS
-
-flush_wait:
-    LDR r1, [r3]
-    ANDS r1, r1, #0x02   /* AND TX FIFO EMPTY flag */
-    BEQ flush_wait          /* ANDS should have set Z bit if zero */
-
-    LDR r0, =0
-    BX lr
-UNWIND(	.fnend)
-END_FUNC __asc_flush
-
-/*
- * int __asc_xmit_char(char p, vaddr_t base) - Transmit a single character.
- *
- *    R0 is the 1-byte character to be transmited
- *    R1 is the base address of the uart
- *    Clobbers r0-r3
- */
-FUNC __asc_xmit_char , :
-UNWIND(	.fnstart)
-
-    ADD r2, r1, #ASC_TXBUFFER
-    ADD r3, r1, #ASC_STATUS
-
-    /* Output byte */
-
-    /* Spin until TX FIFO ready */
-__asc_char_crwait:
-    LDR r1, [r3]
-    ANDS r1, r1, #0x04         /* AND TX FIFO HALF EMPTY flag */
-    BEQ __asc_char_crwait      /* ANDS should have set Z bit if zero */
-
-    MOVS r1, r0
-    LDR r0, =0xFF
-    AND r1, r1, r0
-    BEQ __asc_char_exit
-    CMP r1, #0xa               /* r1 == \n (line feed) ? */
-    BNE __asc_char_notlf
-
-    /* Transmit character extra carriage return for each line feed */
-    LDR r1, =0x0d
-    STR r1, [r2]
-
-    LDR r1, =0x0a              /* replace line feed */
-
-__asc_char_notlf:
-    /* Transmit character */
-    STR r1, [r2]
-
-__asc_char_exit:
-    LDR r0, =0
-    BX lr
-UNWIND(	.fnend)
-END_FUNC __asc_xmit_char
diff --git a/core/arch/arm/plat-stm/conf.mk b/core/arch/arm/plat-stm/conf.mk
index 4afe256..e34491f 100644
--- a/core/arch/arm/plat-stm/conf.mk
+++ b/core/arch/arm/plat-stm/conf.mk
@@ -22,6 +22,7 @@ CFG_TEE_CORE_EMBED_INTERNAL_TESTS ?= y
 CFG_WITH_STACK_CANARIES ?= y
 CFG_WITH_STATS ?= y
 CFG_WITH_SOFTWARE_PRNG ?= n
+CFG_STIH_UART ?= y
 
 ifeq ($(PLATFORM_FLAVOR),b2260)
 CFG_PL310_LOCKED ?= y
diff --git a/core/arch/arm/plat-stm/main.c b/core/arch/arm/plat-stm/main.c
index e569e07..db92680 100644
--- a/core/arch/arm/plat-stm/main.c
+++ b/core/arch/arm/plat-stm/main.c
@@ -26,10 +26,9 @@
  */
 
 #include <arm32.h>
-#include <asc.h>
 #include <console.h>
 #include <drivers/gic.h>
-#include <drivers/pl011.h>
+#include <drivers/stih_asc.h>
 #include <io.h>
 #include <kernel/generic_boot.h>
 #include <kernel/misc.h>
@@ -50,6 +49,8 @@ register_phys_mem(MEM_AREA_IO_SEC, RNG_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_NSEC, UART_CONSOLE_BASE, CORE_MMU_DEVICE_SIZE);
 
 static struct gic_data gic_data;
+static struct stih_asc_pd console_data __early_bss;
+
 static void main_fiq(void);
 
 #if defined(PLATFORM_FLAVOR_b2260)
@@ -75,7 +76,7 @@ static void stm_tee_entry_std(struct thread_smc_args *smc_args)
 static const struct thread_handlers handlers = {
 	.std_smc = stm_tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -89,35 +90,30 @@ const struct thread_handlers *generic_boot_get_handlers(void)
 	return &handlers;
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va __early_bss;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(UART_CONSOLE_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return UART_CONSOLE_BASE;
-}
-
 void console_init(void)
 {
+	stih_asc_init(&console_data, UART_CONSOLE_BASE);
 }
 
 void console_putc(int ch)
 {
+
 	if (ns_resources_ready()) {
+		struct serial_chip *cons = &console_data.chip;
+
 		if (ch == '\n')
-			__asc_xmit_char('\r', console_base());
-		__asc_xmit_char((char)ch, console_base());
+			cons->ops->putc(cons, '\r');
+		cons->ops->putc(cons, ch);
 	}
 }
 
 void console_flush(void)
 {
-	if (ns_resources_ready())
-		__asc_flush(console_base());
+	if (ns_resources_ready()) {
+		struct serial_chip *cons = &console_data.chip;
+
+		cons->ops->flush(cons);
+	}
 }
 
 vaddr_t pl310_base(void)
diff --git a/core/arch/arm/plat-stm/platform_config.h b/core/arch/arm/plat-stm/platform_config.h
index 407a412..a9afb90 100644
--- a/core/arch/arm/plat-stm/platform_config.h
+++ b/core/arch/arm/plat-stm/platform_config.h
@@ -242,13 +242,16 @@
  *  |   external memory  +------------------+   |
  *  |                    |  TA_RAM          |   |
  *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  | Secure Data Path test memory (opt.)   |   |
+ *  +---------------------------------------+   |
  *  |     Non secure     |  SHM             |   |
  *  |   shared memory    |                  |   |
  *  +---------------------------------------+   v
  *
  *  TEE_RAM : default 1MByte
- *  PUB_RAM : default 2MByte
  *  TA_RAM  : all what is left
+ *  SDP_RAM : optional default SDP test memory 8MByte
+ *  PUB_RAM : default 2MByte
  *
  * ----------------------------------------------------------------------------
  * TEE RAM layout with CFG_WITH_PAGER=y:
@@ -262,12 +265,15 @@
  *  | TEE private secure |  TA_RAM          |   ^
  *  |   external memory  |                  |   |
  *  +---------------------------------------+   | CFG_DDR_TEETZ_RESERVED_SIZE
+ *  | Secure Data Path test memory (opt.)   |   |
+ *  +---------------------------------------+   |
  *  |     Non secure     |  SHM             |   |
  *  |   shared memory    |                  |   |
  *  +---------------------------------------+   v
  *
  *  TEE_RAM : default 256kByte
  *  TA_RAM  : all what is left in DDR TEE reserved area
+ *  SDP_RAM : optional default SDP test memory 8MByte
  *  PUB_RAM : default 2MByte
  */
 
@@ -282,13 +288,30 @@
 				CFG_SHMEM_SIZE)
 #endif
 
+#if defined(CFG_SECURE_DATA_PATH) && !defined(CFG_TEE_SDP_MEM_BASE)
+/* default locate SDP memory right before the shared memory in DDR */
+#define CFG_TEE_SDP_TEST_MEM_SIZE	0x00300000
+
+#define CFG_TEE_SDP_MEM_SIZE	CFG_TEE_SDP_TEST_MEM_SIZE
+#define CFG_TEE_SDP_MEM_BASE	(CFG_DDR_TEETZ_RESERVED_START + \
+				CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_SHMEM_SIZE - \
+				CFG_TEE_SDP_MEM_SIZE)
+#endif
+
+#ifndef CFG_TEE_SDP_TEST_MEM_SIZE
+#define CFG_TEE_SDP_TEST_MEM_SIZE	0
+#endif
+
 #if defined(CFG_WITH_PAGER)
 
 #define TZSRAM_BASE		CFG_CORE_TZSRAM_EMUL_START
 #define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
 
 #define TZDRAM_BASE		CFG_DDR_TEETZ_RESERVED_START
-#define TZDRAM_SIZE		(CFG_DDR_TEETZ_RESERVED_SIZE - CFG_SHMEM_SIZE)
+#define TZDRAM_SIZE		(CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_SHMEM_SIZE - \
+				CFG_TEE_SDP_TEST_MEM_SIZE)
 
 #define CFG_TEE_RAM_START	TZSRAM_BASE
 #define CFG_TEE_RAM_PH_SIZE	TZSRAM_SIZE
@@ -299,7 +322,9 @@
 #else  /* CFG_WITH_PAGER */
 
 #define TZDRAM_BASE		CFG_DDR_TEETZ_RESERVED_START
-#define TZDRAM_SIZE		(CFG_DDR_TEETZ_RESERVED_SIZE - CFG_SHMEM_SIZE)
+#define TZDRAM_SIZE		(CFG_DDR_TEETZ_RESERVED_SIZE - \
+				CFG_SHMEM_SIZE - \
+				CFG_TEE_SDP_TEST_MEM_SIZE)
 
 #define CFG_TEE_RAM_START	TZDRAM_BASE
 #ifndef CFG_TEE_RAM_PH_SIZE
diff --git a/core/arch/arm/plat-stm/sub.mk b/core/arch/arm/plat-stm/sub.mk
index d16bb72..4793bfb 100644
--- a/core/arch/arm/plat-stm/sub.mk
+++ b/core/arch/arm/plat-stm/sub.mk
@@ -1,6 +1,5 @@
 global-incdirs-y += .
 
 srcs-y += rng_support.c
-srcs-y += asc.S
 srcs-y += tz_a9init.S
 srcs-y += main.c
diff --git a/core/arch/arm/plat-stm/tz_a9init.S b/core/arch/arm/plat-stm/tz_a9init.S
index aee7dbe..2f78e04 100644
--- a/core/arch/arm/plat-stm/tz_a9init.S
+++ b/core/arch/arm/plat-stm/tz_a9init.S
@@ -78,20 +78,16 @@ END_FUNC arm_cl2_enable
 FUNC plat_cpu_reset_early , :
 UNWIND(	.fnstart)
 
-	movw r0, #(CPU_SCTLR_INIT & 0xFFFF)
-	movt r0, #((CPU_SCTLR_INIT >> 16) & 0xFFFF)
+	mov_imm r0, CPU_SCTLR_INIT
 	write_sctlr r0
 
-	movw r0, #(CPU_ACTLR_INIT & 0xFFFF)
-	movt r0, #((CPU_ACTLR_INIT >> 16) & 0xFFFF)
+	mov_imm r0, CPU_ACTLR_INIT
 	write_actlr r0
 
-	movw r0, #(CPU_NSACR_INIT & 0xFFFF)
-	movt r0, #((CPU_NSACR_INIT >> 16) & 0xFFFF)
+	mov_imm r0, CPU_NSACR_INIT
 	write_nsacr r0
 
-	movw r0, #(CPU_PCR_INIT & 0xFFFF)
-	movt r0, #((CPU_PCR_INIT >> 16) & 0xFFFF)
+	mov_imm r0, CPU_PCR_INIT
 	write_pcr r0
 
 	mov pc, lr
diff --git a/core/arch/arm/plat-sunxi/main.c b/core/arch/arm/plat-sunxi/main.c
index 3954d9d..5546c67 100644
--- a/core/arch/arm/plat-sunxi/main.c
+++ b/core/arch/arm/plat-sunxi/main.c
@@ -25,31 +25,30 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <platform_config.h>
-
-#include <stdint.h>
-#include <string.h>
-#include <assert.h>
-
-#include <sm/sm.h>
-#include <sm/tee_mon.h>
-#include <sm/optee_smc.h>
-#include <optee_msg.h>
-
 #include <arm.h>
-#include <kernel/thread.h>
-#include <kernel/time_source.h>
+#include <assert.h>
+#include <console.h>
+#include <drivers/sunxi_uart.h>
+#include <kernel/misc.h>
 #include <kernel/panic.h>
 #include <kernel/pm_stubs.h>
-#include <kernel/misc.h>
-#include <mm/tee_mmu.h>
+#include <kernel/thread.h>
+#include <kernel/time_source.h>
+#include <malloc.h>
 #include <mm/core_mmu.h>
-#include <tee/entry_std.h>
-#include <tee/entry_fast.h>
+#include <mm/tee_mmu.h>
+#include <optee_msg.h>
+#include <platform_config.h>
 #include <platform.h>
-#include <util.h>
+#include <sm/optee_smc.h>
+#include <sm/sm.h>
+#include <sm/tee_mon.h>
+#include <stdint.h>
+#include <string.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
 #include <trace.h>
-#include <malloc.h>
+#include <util.h>
 
 /* teecore heap address/size is defined in scatter file */
 extern unsigned char teecore_heap_start;
@@ -62,7 +61,7 @@ static void main_tee_entry_fast(struct thread_smc_args *args);
 static const struct thread_handlers handlers = {
 	.std_smc = main_tee_entry_std,
 	.fast_smc = main_tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -78,12 +77,13 @@ void main_init(uint32_t nsec_entry)
 	size_t pos = get_core_pos();
 
 	/*
-	 * Mask IRQ and FIQ before switch to the thread vector as the
-	 * thread handler requires IRQ and FIQ to be masked while executing
+	 * Mask the interrupts before switch to the thread vector as the
+	 * thread handler requires the interrupts to be masked while executing
 	 * with the temporary stack. The thread subsystem also asserts that
-	 * IRQ is blocked when using most if its functions.
+	 * foreign interrupts are blocked when using most if its functions.
 	 */
-	thread_mask_exceptions(THREAD_EXCP_FIQ | THREAD_EXCP_IRQ);
+	thread_mask_exceptions(
+			THREAD_EXCP_NATIVE_INTR | THREAD_EXCP_FOREIGN_INTR);
 
 	if (pos == 0) {
 		thread_init_primary(&handlers);
@@ -175,3 +175,11 @@ void tee_entry_get_api_call_count(struct thread_smc_args *args)
 {
 	args->a0 = tee_entry_generic_get_api_call_count() + 3;
 }
+
+static struct sunxi_uart_data console_data __early_bss;
+
+void console_init(void)
+{
+	sunxi_uart_init(&console_data, CONSOLE_UART_BASE);
+	register_serial_console(&console_data.chip);
+}
diff --git a/core/arch/arm/plat-sunxi/platform.c b/core/arch/arm/plat-sunxi/platform.c
index e46541a..34331be 100644
--- a/core/arch/arm/plat-sunxi/platform.c
+++ b/core/arch/arm/plat-sunxi/platform.c
@@ -109,11 +109,11 @@ uint32_t platform_smc_handle(struct thread_smc_args *smc_args)
 	switch (smc_args->a1) {
 	case OPTEE_SMC_SIP_SUNXI_SET_SMP_BOOTENTRY:
 		sunxi_secondary_ns_entry = smc_args->a2;
-		
+
 		/* in order to sync with secondary up cpu */
-		cache_maintenance_l1(DCACHE_AREA_CLEAN, 
-		                       (void *)(&sunxi_secondary_ns_entry), 
-		                       sizeof(uint32_t));
+		cache_op_inner(DCACHE_AREA_CLEAN,
+				(void *)(&sunxi_secondary_ns_entry),
+				sizeof(uint32_t));
 		break;
 	default:
 		ret = OPTEE_SMC_RETURN_EBADCMD;
diff --git a/core/arch/arm/plat-sunxi/sub.mk b/core/arch/arm/plat-sunxi/sub.mk
index 7c98a65..f3fda2c 100644
--- a/core/arch/arm/plat-sunxi/sub.mk
+++ b/core/arch/arm/plat-sunxi/sub.mk
@@ -6,4 +6,3 @@ srcs-y += platform.c
 srcs-y += smp_boot.S
 srcs-y += smp_fixup.S
 srcs-y += head.c
-srcs-y += console.c
diff --git a/core/arch/arm/plat-ti/api_monitor_index.h b/core/arch/arm/plat-ti/api_monitor_index.h
new file mode 100644
index 0000000..2f8e089
--- /dev/null
+++ b/core/arch/arm/plat-ti/api_monitor_index.h
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2015, Texas Instruments
+ * 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 API_MONITOR_INDEX_H
+#define API_MONITOR_INDEX_H
+
+/* Number of APIs */
+#define NB_MAX_API_MONITOR 10
+
+/* Base Index of APIs */
+#define API_MONITOR_BASE_INDEX 0x00000100
+
+/* HyperVisor Start */
+#define API_MONITOR_HYP_STARTHYPERVISOR_INDEX   (API_MONITOR_BASE_INDEX + 0x00000002)
+/* Caches cleaning */
+#define API_MONITOR_CACHES_CLEAN_INDEX          (API_MONITOR_BASE_INDEX + 0x00000003)
+/* Write the L2 Cache Controller Auxiliary Control */
+#define API_MONITOR_L2ACTLR_SETREGISTER_INDEX   (API_MONITOR_BASE_INDEX + 0x00000004)
+/* Set the Data and Tag RAM Latency */
+#define API_MONITOR_L2CACHE_SETLATENCY_INDEX    (API_MONITOR_BASE_INDEX + 0x00000005)
+/* L2 Cache Prefetch Control Register */
+#define API_MONITOR_L2PFR_SETREGISTER_INDEX     (API_MONITOR_BASE_INDEX + 0x00000006)
+/* Set Auxiliary Control Register */
+#define API_MONITOR_ACTLR_SETREGISTER_INDEX     (API_MONITOR_BASE_INDEX + 0x00000007)
+/* AMBA IF mode */
+#define API_MONITOR_WUGEN_MPU_SETAMBAIF_INDEX   (API_MONITOR_BASE_INDEX + 0x00000008)
+/* Timer CNTFRQ register set */
+#define API_MONITOR_TIMER_SETCNTFRQ_INDEX       (API_MONITOR_BASE_INDEX + 0x00000009)
+
+#endif /* API_MONITOR_INDEX_H */
diff --git a/core/arch/arm/plat-ti/conf.mk b/core/arch/arm/plat-ti/conf.mk
index 64e499f..231a0b1 100644
--- a/core/arch/arm/plat-ti/conf.mk
+++ b/core/arch/arm/plat-ti/conf.mk
@@ -4,6 +4,7 @@ CFG_WITH_STACK_CANARIES ?= y
 CFG_WITH_STATS ?= y
 CFG_WITH_SOFTWARE_PRNG ?= n
 
+$(call force,CFG_SM_PLATFORM_HANDLER,y)
 $(call force,CFG_8250_UART,y)
 $(call force,CFG_ARM32_core,y)
 $(call force,CFG_GENERIC_BOOT,y)
diff --git a/core/arch/arm/plat-ti/main.c b/core/arch/arm/plat-ti/main.c
index c811862..be3bf3a 100644
--- a/core/arch/arm/plat-ti/main.c
+++ b/core/arch/arm/plat-ti/main.c
@@ -26,11 +26,12 @@
  */
 
 #include <platform_config.h>
-
+#include <console.h>
 #include <stdint.h>
 #include <string.h>
 #include <assert.h>
 #include <drivers/gic.h>
+#include <drivers/serial8250_uart.h>
 #include <arm.h>
 #include <kernel/generic_boot.h>
 #include <kernel/panic.h>
@@ -47,9 +48,12 @@
 #include <sm/sm.h>
 
 static struct gic_data gic_data;
+static struct serial8250_uart_data console_data __early_bss;
 
 register_phys_mem(MEM_AREA_IO_SEC, GICC_BASE, GICC_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, GICD_BASE, GICD_SIZE);
+register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE,
+		  SERIAL8250_UART_REG_SIZE);
 
 void main_init_gic(void)
 {
@@ -79,7 +83,7 @@ static void main_fiq(void)
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -126,8 +130,8 @@ void init_sec_mon(unsigned long nsec_entry)
 		panic();
 
 	/* Invalidate cache to fetch data from external memory */
-	cache_maintenance_l1(DCACHE_AREA_INVALIDATE,
-			     plat_ctx, sizeof(*plat_ctx));
+	cache_op_inner(DCACHE_AREA_INVALIDATE,
+			plat_ctx, sizeof(*plat_ctx));
 
 	/* Initialize secure monitor */
 	nsec_ctx = sm_get_nsec_ctx();
@@ -149,3 +153,10 @@ void init_sec_mon(unsigned long nsec_entry)
 	nsec_ctx->mon_lr = plat_ctx->mon_lr;
 	nsec_ctx->mon_spsr = plat_ctx->mon_spsr;
 }
+
+void console_init(void)
+{
+	serial8250_uart_init(&console_data, CONSOLE_UART_BASE,
+			     CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
+}
diff --git a/core/arch/arm/plat-ti/platform_config.h b/core/arch/arm/plat-ti/platform_config.h
index c1aaee9..48505a6 100644
--- a/core/arch/arm/plat-ti/platform_config.h
+++ b/core/arch/arm/plat-ti/platform_config.h
@@ -28,7 +28,7 @@
 #ifndef PLATFORM_CONFIG_H
 #define PLATFORM_CONFIG_H
 
-#if defined(PLATFORM_FLAVOR_dra7xx)
+#if defined(PLATFORM_FLAVOR_dra7xx) || defined(PLATFORM_FLAVOR_am57xx)
 
 #define DRAM0_BASE		0xbe000000
 #define DRAM0_SIZE		0x02000000
@@ -43,10 +43,14 @@
 
 #define CFG_TEE_CORE_NB_CORE	2
 
+#define UART1_BASE      0x4806A000
+#define UART2_BASE      0x4806C000
+#define UART3_BASE      0x48020000
+
 /* UART1 */
-#define CONSOLE_UART_BASE		0x4806A000
+#define CONSOLE_UART_BASE       UART1_BASE
+#define CONSOLE_BAUDRATE        115200
 #define CONSOLE_UART_CLK_IN_HZ	48000000
-#define UART_BAUDRATE			115200
 
 #define GIC_BASE        0x48210000
 #define GICC_OFFSET     0x2000
@@ -65,6 +69,14 @@
 #error "Unknown platform flavor"
 #endif
 
+#if defined(PLATFORM_FLAVOR_am57xx)
+
+/* UART3 */
+#undef CONSOLE_UART_BASE
+#define CONSOLE_UART_BASE       UART3_BASE
+
+#endif
+
 /* Make stacks aligned to data cache line length */
 #define STACK_ALIGNMENT		64
 
@@ -99,11 +111,4 @@
 #define DEVICE2_SIZE		CORE_MMU_DEVICE_SIZE
 #define DEVICE2_TYPE		MEM_AREA_IO_SEC
 
-#ifndef UART_BAUDRATE
-#define UART_BAUDRATE		115200
-#endif
-#ifndef CONSOLE_BAUDRATE
-#define CONSOLE_BAUDRATE	UART_BAUDRATE
-#endif
-
 #endif /*PLATFORM_CONFIG_H*/
diff --git a/core/arch/arm/plat-ti/sm_platform_handler.c b/core/arch/arm/plat-ti/sm_platform_handler.c
new file mode 100644
index 0000000..23bb48d
--- /dev/null
+++ b/core/arch/arm/plat-ti/sm_platform_handler.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2017, Texas Instruments
+ * 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 <arm32.h>
+#include <sm/sm.h>
+#include "api_monitor_index.h"
+
+#define API_HAL_RET_VALUE_OK 0x00000000
+#define API_HAL_RET_VALUE_SERVICE_UNKNWON 0xFFFFFFFF
+
+bool sm_platform_handler(struct sm_ctx *ctx)
+{
+	if (ctx->nsec.r12 == 0x200)
+		return true;
+
+	switch (ctx->nsec.r12) {
+	case API_MONITOR_ACTLR_SETREGISTER_INDEX:
+		write_actlr(ctx->nsec.r0);
+		isb();
+		ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
+		break;
+	case API_MONITOR_TIMER_SETCNTFRQ_INDEX:
+		write_cntfrq(ctx->nsec.r0);
+		isb();
+		ctx->nsec.r0 = API_HAL_RET_VALUE_OK;
+		break;
+	default:
+		ctx->nsec.r0 = API_HAL_RET_VALUE_SERVICE_UNKNWON;
+		break;
+	}
+
+	return false;
+}
diff --git a/core/arch/arm/plat-ti/sub.mk b/core/arch/arm/plat-ti/sub.mk
index 3a8214b..9e195b7 100644
--- a/core/arch/arm/plat-ti/sub.mk
+++ b/core/arch/arm/plat-ti/sub.mk
@@ -1,3 +1,3 @@
 global-incdirs-y += .
 srcs-y += main.c
-srcs-y += console.c
+srcs-y += sm_platform_handler.c
diff --git a/core/arch/arm/plat-vexpress/conf.mk b/core/arch/arm/plat-vexpress/conf.mk
index 5d7d8c1..5ba0840 100644
--- a/core/arch/arm/plat-vexpress/conf.mk
+++ b/core/arch/arm/plat-vexpress/conf.mk
@@ -46,7 +46,6 @@ else
 $(call force,CFG_ARM32_core,y)
 endif
 
-CFG_TEE_FS_KEY_MANAGER_TEST ?= y
 CFG_WITH_STACK_CANARIES ?= y
 CFG_WITH_STATS ?= y
 
diff --git a/core/arch/arm/plat-vexpress/main.c b/core/arch/arm/plat-vexpress/main.c
index 85ed9ee..44eef24 100644
--- a/core/arch/arm/plat-vexpress/main.c
+++ b/core/arch/arm/plat-vexpress/main.c
@@ -55,7 +55,7 @@ static void main_fiq(void);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 #if defined(CFG_WITH_ARM_TRUSTED_FW)
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
@@ -74,6 +74,7 @@ static const struct thread_handlers handlers = {
 };
 
 static struct gic_data gic_data;
+static struct pl011_data console_data __early_bss;
 
 register_phys_mem(MEM_AREA_IO_SEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
 
@@ -116,44 +117,20 @@ static void main_fiq(void)
 	gic_it_handle(&gic_data);
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_SEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	pl011_init(console_base(), CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	vaddr_t base = console_base();
-
-	if (ch == '\n')
-		pl011_putc('\r', base);
-	pl011_putc(ch, base);
-}
-
-void console_flush(void)
-{
-	pl011_flush(console_base());
+	pl011_init(&console_data, CONSOLE_UART_BASE, CONSOLE_UART_CLK_IN_HZ,
+		   CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
 }
 
 #ifdef IT_CONSOLE_UART
 static enum itr_return console_itr_cb(struct itr_handler *h __unused)
 {
-	paddr_t uart_base = console_base();
+	struct serial_chip *cons = &console_data.chip;
 
-	while (pl011_have_rx_data(uart_base)) {
-		int ch __maybe_unused = pl011_getchar(uart_base);
+	while (cons->ops->have_rx_data(cons)) {
+		int ch __maybe_unused = cons->ops->getchar(cons);
 
 		DMSG("cpu %zu: got 0x%x", get_core_pos(), ch);
 	}
diff --git a/core/arch/arm/plat-vexpress/platform_config.h b/core/arch/arm/plat-vexpress/platform_config.h
index bd006ca..45d9993 100644
--- a/core/arch/arm/plat-vexpress/platform_config.h
+++ b/core/arch/arm/plat-vexpress/platform_config.h
@@ -39,6 +39,13 @@
 #endif
 #endif /*ARM64*/
 
+/* SDP enable but no pool defined: reserve 4MB for SDP tests */
+#if defined(CFG_SECURE_DATA_PATH) && !defined(CFG_TEE_SDP_MEM_BASE)
+#define CFG_TEE_SDP_MEM_TEST_SIZE	0x00400000
+#else
+#define CFG_TEE_SDP_MEM_TEST_SIZE	0
+#endif
+
 #if defined(PLATFORM_FLAVOR_fvp)
 
 #define GIC_BASE		0x2c000000
@@ -187,23 +194,25 @@
 #define DRAM0_TEERES_BASE	(DRAM0_BASE + DRAM0_SIZE)
 #define DRAM0_TEERES_SIZE	CFG_SHMEM_SIZE
 
+#define SECRAM_BASE		0x0e000000
+#define SECRAM_SIZE		0x01000000
+
 #ifdef CFG_WITH_PAGER
 
 /* Emulated SRAM */
-#define TZSRAM_BASE		0x0e000000
+#define TZSRAM_BASE		SECRAM_BASE
 #define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
 
 #define TZDRAM_BASE		(TZSRAM_BASE + TZSRAM_SIZE)
-#define TZDRAM_SIZE		(0x01000000 - TZSRAM_SIZE)
+#define TZDRAM_SIZE		(SECRAM_SIZE - TZSRAM_SIZE)
 
 #else /* CFG_WITH_PAGER */
 
-#define TZDRAM_BASE		0x0e000000
-#define TZDRAM_SIZE		0x01000000
+#define TZDRAM_BASE		SECRAM_BASE
+#define TZDRAM_SIZE		SECRAM_SIZE
 
 #endif /* CFG_WITH_PAGER */
 
-
 #define CFG_TEE_CORE_NB_CORE	2
 
 #define CFG_SHMEM_START		(DRAM0_TEERES_BASE + \
@@ -226,8 +235,12 @@
 #define DRAM0_TEERES_BASE	(DRAM0_BASE + DRAM0_SIZE)
 #define DRAM0_TEERES_SIZE	CFG_SHMEM_SIZE
 
-#define TZDRAM_BASE		0x0e100000
-#define TZDRAM_SIZE		0x00f00000
+#define SECRAM_BASE		0x0e000000
+#define SECRAM_SIZE		0x01000000
+
+/* First 1MByte of the secure RAM is reserved to ARM-TF runtime services */
+#define TZDRAM_BASE		(SECRAM_BASE + 0x00100000)
+#define TZDRAM_SIZE		(SECRAM_SIZE - 0x00100000)
 
 #define CFG_TEE_CORE_NB_CORE	2
 
@@ -254,28 +267,42 @@
  * | TZSRAM | TEE_RAM |
  * +--------+---------+
  * | TZDRAM | TA_RAM  |
+ * |        +---------+
+ * |        | SDP RAM | (SDP test pool, optional)
  * +--------+---------+
  */
 #define CFG_TEE_RAM_PH_SIZE	TZSRAM_SIZE
 #define CFG_TEE_RAM_START	TZSRAM_BASE
 #define CFG_TA_RAM_START	ROUNDUP(TZDRAM_BASE, CORE_MMU_DEVICE_SIZE)
-#define CFG_TA_RAM_SIZE		ROUNDDOWN(TZDRAM_SIZE, CORE_MMU_DEVICE_SIZE)
+
 #else
 /*
  * Assumes that either TZSRAM isn't large enough or TZSRAM doesn't exist,
  * everything is in TZDRAM.
  * +------------------+
  * |        | TEE_RAM |
- * + TZDRAM +---------+
+ * | TZDRAM +---------+
  * |        | TA_RAM  |
+ * |        +---------+
+ * |        | SDP RAM | (test pool, optional)
  * +--------+---------+
  */
 #define CFG_TEE_RAM_PH_SIZE	CFG_TEE_RAM_VA_SIZE
 #define CFG_TEE_RAM_START	TZDRAM_BASE
-#define CFG_TA_RAM_START	ROUNDUP((TZDRAM_BASE + CFG_TEE_RAM_VA_SIZE), \
+#define CFG_TA_RAM_START	ROUNDUP(TZDRAM_BASE + CFG_TEE_RAM_VA_SIZE, \
 					CORE_MMU_DEVICE_SIZE)
-#define CFG_TA_RAM_SIZE		ROUNDDOWN((TZDRAM_SIZE - CFG_TEE_RAM_VA_SIZE), \
+#endif
+
+#define CFG_TA_RAM_SIZE		ROUNDDOWN(TZDRAM_SIZE - \
+					  (CFG_TA_RAM_START - TZDRAM_BASE) - \
+					  CFG_TEE_SDP_MEM_TEST_SIZE, \
 					  CORE_MMU_DEVICE_SIZE)
+
+/* Secure data path test memory pool: located at end of TA RAM */
+#if CFG_TEE_SDP_MEM_TEST_SIZE
+#define CFG_TEE_SDP_MEM_SIZE		CFG_TEE_SDP_MEM_TEST_SIZE
+#define CFG_TEE_SDP_MEM_BASE		(TZDRAM_BASE + TZDRAM_SIZE - \
+						CFG_TEE_SDP_MEM_SIZE)
 #endif
 
 #ifdef GIC_BASE
diff --git a/core/arch/arm/plat-zynq7k/main.c b/core/arch/arm/plat-zynq7k/main.c
index 74bc1ce..2991b94 100644
--- a/core/arch/arm/plat-zynq7k/main.c
+++ b/core/arch/arm/plat-zynq7k/main.c
@@ -51,7 +51,7 @@ static void platform_tee_entry_fast(struct thread_smc_args *args);
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = platform_tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 	.cpu_on = pm_panic,
 	.cpu_off = pm_panic,
 	.cpu_suspend = pm_panic,
@@ -61,6 +61,7 @@ static const struct thread_handlers handlers = {
 };
 
 static struct gic_data gic_data;
+static struct cdns_uart_data console_data __early_bss;
 
 register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, CORE_MMU_DEVICE_SIZE);
 register_phys_mem(MEM_AREA_IO_SEC, GIC_BASE, CORE_MMU_DEVICE_SIZE);
@@ -117,33 +118,10 @@ void plat_cpu_reset_late(void)
 	}
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va __early_bss;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE,
-					  MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-}
-
-void console_putc(int ch)
-{
-	if (ch == '\n')
-		cdns_uart_putc('\r', console_base());
-	cdns_uart_putc(ch, console_base());
-}
-
-void console_flush(void)
-{
-	cdns_uart_flush(console_base());
+	cdns_uart_init(&console_data, CONSOLE_UART_BASE, 0, 0);
+	register_serial_console(&console_data.chip);
 }
 
 vaddr_t pl310_base(void)
diff --git a/core/arch/arm/plat-zynq7k/plat_init.S b/core/arch/arm/plat-zynq7k/plat_init.S
index 6d99a30..8d06c53 100644
--- a/core/arch/arm/plat-zynq7k/plat_init.S
+++ b/core/arch/arm/plat-zynq7k/plat_init.S
@@ -90,20 +90,16 @@ UNWIND(	.fnstart)
 	 * PCR = 0x00000001
 	 * - no change latency, enable clk gating
 	 */
-	movw r0, #0x4000
-	movt r0, #0x0000
+	mov_imm r0, 0x00004000
 	write_sctlr r0
 
-	movw r0, #0x0041
-	movt r0, #0x0000
+	mov_imm r0, 0x00000041
 	write_actlr r0
 
-	movw r0, #0x0FFF
-	movt r0, #0x0002
+	mov_imm r0, 0x00020C00
 	write_nsacr r0
 
-	movw r0, #0x0000
-	movt r0, #0x0001
+	mov_imm r0, 0x00000001
 	write_pcr r0
 
 	mov pc, lr
diff --git a/core/arch/arm/plat-zynqmp/conf.mk b/core/arch/arm/plat-zynqmp/conf.mk
index 67570bc..5aea3a8 100644
--- a/core/arch/arm/plat-zynqmp/conf.mk
+++ b/core/arch/arm/plat-zynqmp/conf.mk
@@ -23,7 +23,6 @@ else
 $(call force,CFG_ARM32_core,y)
 endif
 
-CFG_TEE_FS_KEY_MANAGER_TEST ?= y
 CFG_WITH_STACK_CANARIES ?= y
 CFG_WITH_STATS ?= y
 CFG_CRYPTO_WITH_CE ?= y
diff --git a/core/arch/arm/plat-zynqmp/main.c b/core/arch/arm/plat-zynqmp/main.c
index 31b8475..a00d1ae 100644
--- a/core/arch/arm/plat-zynqmp/main.c
+++ b/core/arch/arm/plat-zynqmp/main.c
@@ -46,11 +46,12 @@
 
 static void main_fiq(void);
 static struct gic_data gic_data;
+static struct cdns_uart_data console_data __early_bss;
 
 static const struct thread_handlers handlers = {
 	.std_smc = tee_entry_std,
 	.fast_smc = tee_entry_fast,
-	.fiq = main_fiq,
+	.nintr = main_fiq,
 #if defined(CFG_WITH_ARM_TRUSTED_FW)
 	.cpu_on = cpu_on_handler,
 	.cpu_off = pm_do_nothing,
@@ -90,33 +91,9 @@ static void main_fiq(void)
 	gic_it_handle(&gic_data);
 }
 
-static vaddr_t console_base(void)
-{
-	static void *va;
-
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_SEC);
-		return (vaddr_t)va;
-	}
-
-	return CONSOLE_UART_BASE;
-}
-
 void console_init(void)
 {
-	cdns_uart_init(console_base(), CONSOLE_UART_CLK_IN_HZ,
-		       CONSOLE_BAUDRATE);
-}
-
-void console_putc(int ch)
-{
-	if (ch == '\n')
-		cdns_uart_putc('\r', console_base());
-	cdns_uart_putc(ch, console_base());
-}
-
-void console_flush(void)
-{
-	cdns_uart_flush(console_base());
+	cdns_uart_init(&console_data, CONSOLE_UART_BASE,
+		       CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
 }
diff --git a/core/arch/arm/pta/core_fs_htree_tests.c b/core/arch/arm/pta/core_fs_htree_tests.c
new file mode 100644
index 0000000..23be98c
--- /dev/null
+++ b/core/arch/arm/pta/core_fs_htree_tests.c
@@ -0,0 +1,456 @@
+/*
+ * Copyright (c) 2017, 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 <string.h>
+#include <tee/fs_htree.h>
+#include <tee/tee_fs_rpc.h>
+#include <trace.h>
+#include <types_ext.h>
+#include <util.h>
+
+#include "core_self_tests.h"
+
+/*
+ * The smallest blocks size that can hold two struct
+ * tee_fs_htree_node_image or two struct tee_fs_htree_image.
+ */
+#define TEST_BLOCK_SIZE		144
+
+struct test_aux {
+	uint8_t *data;
+	size_t data_len;
+	size_t data_alloced;
+	uint8_t *block;
+};
+
+static TEE_Result test_get_offs_size(enum tee_fs_htree_type type, size_t idx,
+				     uint8_t vers, size_t *offs, size_t *size)
+{
+	const size_t node_size = sizeof(struct tee_fs_htree_node_image);
+	const size_t block_nodes = TEST_BLOCK_SIZE / (node_size * 2);
+	size_t pbn;
+	size_t bidx;
+
+	COMPILE_TIME_ASSERT(TEST_BLOCK_SIZE >
+			    sizeof(struct tee_fs_htree_node_image) * 2);
+	COMPILE_TIME_ASSERT(TEST_BLOCK_SIZE >
+			    sizeof(struct tee_fs_htree_image) * 2);
+
+	assert(vers == 0 || vers == 1);
+
+	/*
+	 * File layout
+	 *
+	 * phys block 0:
+	 * tee_fs_htree_image vers 0 @ offs = 0
+	 * tee_fs_htree_image vers 1 @ offs = sizeof(tee_fs_htree_image)
+	 *
+	 * phys block 1:
+	 * tee_fs_htree_node_image 0  vers 0 @ offs = 0
+	 * tee_fs_htree_node_image 0  vers 1 @ offs = node_size
+	 *
+	 * phys block 2:
+	 * data block 0 vers 0
+	 *
+	 * phys block 3:
+	 * tee_fs_htree_node_image 1  vers 0 @ offs = 0
+	 * tee_fs_htree_node_image 1  vers 1 @ offs = node_size
+	 *
+	 * phys block 4:
+	 * data block 0 vers 1
+	 *
+	 * ...
+	 */
+
+	switch (type) {
+	case TEE_FS_HTREE_TYPE_HEAD:
+		*offs = sizeof(struct tee_fs_htree_image) * vers;
+		*size = sizeof(struct tee_fs_htree_image);
+		return TEE_SUCCESS;
+	case TEE_FS_HTREE_TYPE_NODE:
+		pbn = 1 + ((idx / block_nodes) * block_nodes * 2);
+		*offs = pbn * TEST_BLOCK_SIZE +
+			2 * node_size * (idx % block_nodes) +
+			node_size * vers;
+		*size = node_size;
+		return TEE_SUCCESS;
+	case TEE_FS_HTREE_TYPE_BLOCK:
+		bidx = 2 * idx + vers;
+		pbn = 2 + bidx + bidx / (block_nodes * 2 - 1);
+		*offs = pbn * TEST_BLOCK_SIZE;
+		*size = TEST_BLOCK_SIZE;
+		return TEE_SUCCESS;
+	default:
+		return TEE_ERROR_GENERIC;
+	}
+}
+
+static TEE_Result test_read_init(void *aux, struct tee_fs_rpc_operation *op,
+				 enum tee_fs_htree_type type, size_t idx,
+				 uint8_t vers, void **data)
+{
+	TEE_Result res;
+	struct test_aux *a = aux;
+	size_t offs;
+	size_t sz;
+
+	res = test_get_offs_size(type, idx, vers, &offs, &sz);
+	if (res == TEE_SUCCESS) {
+		memset(op, 0, sizeof(*op));
+		op->params[0].u.value.a = (vaddr_t)aux;
+		op->params[0].u.value.b = offs;
+		op->params[0].u.value.c = sz;
+		*data = a->block;
+	}
+
+	return res;
+}
+
+static void *uint_to_ptr(uintptr_t p)
+{
+	return (void *)p;
+}
+
+static TEE_Result test_read_final(struct tee_fs_rpc_operation *op,
+				  size_t *bytes)
+{
+	struct test_aux *a = uint_to_ptr(op->params[0].u.value.a);
+	size_t offs = op->params[0].u.value.b;
+	size_t sz = op->params[0].u.value.c;
+
+	if (offs + sz <= a->data_len)
+		*bytes = sz;
+	else if (offs <= a->data_len)
+		*bytes = a->data_len - offs;
+	else
+		*bytes = 0;
+
+	memcpy(a->block, a->data + offs, *bytes);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_write_init(void *aux, struct tee_fs_rpc_operation *op,
+				  enum tee_fs_htree_type type, size_t idx,
+				  uint8_t vers, void **data)
+{
+	return test_read_init(aux, op, type, idx, vers, data);
+}
+
+static TEE_Result test_write_final(struct tee_fs_rpc_operation *op)
+{
+	struct test_aux *a = uint_to_ptr(op->params[0].u.value.a);
+	size_t offs = op->params[0].u.value.b;
+	size_t sz = op->params[0].u.value.c;
+	size_t end = offs + sz;
+
+	if (end > a->data_alloced) {
+		EMSG("out of bounds");
+		return TEE_ERROR_GENERIC;
+	}
+
+	memcpy(a->data + offs, a->block, sz);
+	if (end > a->data_len)
+		a->data_len = end;
+	return TEE_SUCCESS;
+
+}
+
+static const struct tee_fs_htree_storage test_htree_ops = {
+	.block_size = TEST_BLOCK_SIZE,
+	.rpc_read_init = test_read_init,
+	.rpc_read_final = test_read_final,
+	.rpc_write_init = test_write_init,
+	.rpc_write_final = test_write_final,
+};
+
+#define CHECK_RES(res, cleanup)						\
+		do {							\
+			TEE_Result _res = (res);			\
+									\
+			if (_res != TEE_SUCCESS) {			\
+				EMSG("error: res = %#" PRIx32, _res);	\
+				{ cleanup; }				\
+			}						\
+		} while (0)
+
+static uint32_t val_from_bn_n_salt(size_t bn, size_t n, uint8_t salt)
+{
+	assert(bn < UINT16_MAX);
+	assert(n < UINT8_MAX);
+	return SHIFT_U32(n, 16) | SHIFT_U32(bn, 8) | salt;
+}
+
+static TEE_Result write_block(struct tee_fs_htree **ht, size_t bn, uint8_t salt)
+{
+	uint32_t b[TEST_BLOCK_SIZE / sizeof(uint32_t)];
+	size_t n;
+
+	for (n = 0; n < ARRAY_SIZE(b); n++)
+		b[n] = val_from_bn_n_salt(bn, n, salt);
+
+	return tee_fs_htree_write_block(ht, bn, b);
+}
+
+static TEE_Result read_block(struct tee_fs_htree **ht, size_t bn, uint8_t salt)
+{
+	TEE_Result res;
+	uint32_t b[TEST_BLOCK_SIZE / sizeof(uint32_t)];
+	size_t n;
+
+	res = tee_fs_htree_read_block(ht, bn, b);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	for (n = 0; n < ARRAY_SIZE(b); n++) {
+		if (b[n] != val_from_bn_n_salt(bn, n, salt)) {
+			DMSG("Unpected b[%zu] %#" PRIx32
+			     "(expected %#" PRIx32 ")",
+			     n, b[n], val_from_bn_n_salt(bn, n, salt));
+			return TEE_ERROR_SECURITY;
+		}
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result do_range(TEE_Result (*fn)(struct tee_fs_htree **ht,
+					    size_t bn, uint8_t salt),
+			   struct tee_fs_htree **ht, size_t begin,
+			   size_t num_blocks, size_t salt)
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t n;
+
+	for (n = 0; n < num_blocks; n++) {
+		res = fn(ht, n + begin, salt);
+		CHECK_RES(res, goto out);
+	}
+
+out:
+	return res;
+}
+
+static TEE_Result do_range_backwards(TEE_Result (*fn)(struct tee_fs_htree **ht,
+						      size_t bn, uint8_t salt),
+				     struct tee_fs_htree **ht, size_t begin,
+				     size_t num_blocks, size_t salt)
+{
+	TEE_Result res = TEE_SUCCESS;
+	size_t n;
+
+	for (n = 0; n < num_blocks; n++) {
+		res = fn(ht, num_blocks - 1 - n + begin, salt);
+		CHECK_RES(res, goto out);
+	}
+
+out:
+	return res;
+}
+
+static TEE_Result htree_test_rewrite(struct test_aux *aux, size_t num_blocks,
+				     size_t w_unsync_begin, size_t w_unsync_num)
+{
+	TEE_Result res;
+	struct tee_fs_htree *ht = NULL;
+	size_t salt = 23;
+
+	assert((w_unsync_begin + w_unsync_num) <= num_blocks);
+
+	aux->data_len = 0;
+	memset(aux->data, 0xce, aux->data_alloced);
+
+	res = tee_fs_htree_open(true, &test_htree_ops, aux, &ht);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Intialize all blocks and verify that they read back as
+	 * expected.
+	 */
+	res = do_range(write_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Write all blocks again, but starting from the end using a new
+	 * salt, then verify that that read back as expected.
+	 */
+	salt++;
+	res = do_range_backwards(write_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Use a new salt to write all blocks once more and verify that
+	 * they read back as expected.
+	 */
+	salt++;
+	res = do_range(write_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Sync the changes of the nodes to memory, verify that all
+	 * blocks are read back as expected.
+	 */
+	res = tee_fs_htree_sync_to_storage(&ht);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Close and reopen the hash-tree
+	 */
+	tee_fs_htree_close(&ht);
+	res = tee_fs_htree_open(false, &test_htree_ops, aux, &ht);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Verify that all blocks are read as expected.
+	 */
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Rewrite a few blocks and verify that all blocks are read as
+	 * expected.
+	 */
+	res = do_range_backwards(write_block, &ht, w_unsync_begin, w_unsync_num,
+				 salt + 1);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, w_unsync_begin, salt);
+	CHECK_RES(res, goto out);
+	res = do_range(read_block, &ht, w_unsync_begin, w_unsync_num, salt + 1);
+	CHECK_RES(res, goto out);
+	res = do_range(read_block, &ht, w_unsync_begin + w_unsync_num,
+			num_blocks - (w_unsync_begin + w_unsync_num), salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Rewrite the blocks from above again with another salt and
+	 * verify that they are read back as expected.
+	 */
+	res = do_range(write_block, &ht, w_unsync_begin, w_unsync_num,
+		       salt + 2);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, w_unsync_begin, salt);
+	CHECK_RES(res, goto out);
+	res = do_range(read_block, &ht, w_unsync_begin, w_unsync_num, salt + 2);
+	CHECK_RES(res, goto out);
+	res = do_range(read_block, &ht, w_unsync_begin + w_unsync_num,
+			num_blocks - (w_unsync_begin + w_unsync_num), salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Skip tee_fs_htree_sync_to_storage() and call
+	 * tee_fs_htree_close() directly to undo the changes since last
+	 * call to tee_fs_htree_sync_to_storage().  Reopen the hash-tree
+	 * and verify that recent changes indeed was discarded.
+	 */
+	tee_fs_htree_close(&ht);
+	res = tee_fs_htree_open(false, &test_htree_ops, aux, &ht);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+	/*
+	 * Close, reopen and verify that all blocks are read as expected
+	 * again.
+	 */
+	tee_fs_htree_close(&ht);
+	res = tee_fs_htree_open(false, &test_htree_ops, aux, &ht);
+	CHECK_RES(res, goto out);
+
+	res = do_range(read_block, &ht, 0, num_blocks, salt);
+	CHECK_RES(res, goto out);
+
+out:
+	tee_fs_htree_close(&ht);
+	return res;
+}
+
+TEE_Result core_fs_htree_tests(uint32_t nParamTypes,
+			       TEE_Param pParams[TEE_NUM_PARAMS] __unused)
+{
+	TEE_Result res;
+	struct test_aux aux;
+	size_t num_blocks = 10;
+	size_t offs;
+	size_t sz;
+	size_t n;
+	size_t m;
+	size_t o;
+
+	if (nParamTypes)
+		return TEE_ERROR_BAD_PARAMETERS;
+
+	res = test_get_offs_size(TEE_FS_HTREE_TYPE_BLOCK, num_blocks, 1,
+				 &offs, &sz);
+	CHECK_RES(res, return res);
+
+	aux.data_alloced = offs + sz;
+	aux.data = malloc(aux.data_alloced);
+	aux.block = malloc(TEST_BLOCK_SIZE);
+	if (!aux.data || !aux.block) {
+		res = TEE_ERROR_OUT_OF_MEMORY;
+		goto out;
+	}
+
+	/*
+	 * n is the number of block we're going to initialize/use.
+	 * m is the offset from where we'll rewrite blocks and expect
+	 * the changes to be visible until tee_fs_htree_close() is called
+	 * without a call to tee_fs_htree_sync_to_storage() before.
+	 * o is the number of blocks we're rewriting starting at m.
+	 */
+	for (n = 0; n < num_blocks; n += 3) {
+		for (m = 0; m < n; m += 3) {
+			for (o = 0; o < (n - m); o++) {
+				res = htree_test_rewrite(&aux, n, m, o);
+				CHECK_RES(res, goto out);
+				o += 2;
+			}
+		}
+	}
+
+
+out:
+	free(aux.data);
+	free(aux.block);
+	return res;
+}
+
diff --git a/core/arch/arm/pta/core_self_tests.h b/core/arch/arm/pta/core_self_tests.h
index ed98669..b775430 100644
--- a/core/arch/arm/pta/core_self_tests.h
+++ b/core/arch/arm/pta/core_self_tests.h
@@ -34,4 +34,7 @@
 TEE_Result core_self_tests(uint32_t nParamTypes,
 		TEE_Param pParams[TEE_NUM_PARAMS]);
 
+TEE_Result core_fs_htree_tests(uint32_t nParamTypes,
+		TEE_Param pParams[TEE_NUM_PARAMS]);
+
 #endif /*CORE_SELF_TESTS_H*/
diff --git a/core/arch/arm/pta/interrupt_tests.c b/core/arch/arm/pta/interrupt_tests.c
index bc307a8..2b9ea00 100644
--- a/core/arch/arm/pta/interrupt_tests.c
+++ b/core/arch/arm/pta/interrupt_tests.c
@@ -174,7 +174,7 @@ static TEE_Result test_ppi(void)
 	itr_add(&ppi_handler);
 	itr_enable(TEST_PPI_ID);
 
-	exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ);
+	exceptions = thread_mask_exceptions(THREAD_EXCP_FOREIGN_INTR);
 	expect_ppi_value[get_core_pos()]++;
 	itr_raise_pi(TEST_PPI_ID);
 	thread_unmask_exceptions(exceptions);
diff --git a/core/arch/arm/pta/pta_self_tests.c b/core/arch/arm/pta/pta_invoke_tests.c
index 6472356..33691b0 100644
--- a/core/arch/arm/pta/pta_self_tests.c
+++ b/core/arch/arm/pta/pta_invoke_tests.c
@@ -24,23 +24,21 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+
 #include <compiler.h>
-#include <types_ext.h>
 #include <kernel/pseudo_ta.h>
-#include <trace.h>
-#include <tee_api_types.h>
+#include <mm/core_memprot.h>
+#include <pta_invoke_tests.h>
+#include <string.h>
+#include <tee/cache.h>
 #include <tee_api_defines.h>
-#include "core_self_tests.h"
-
-#define TA_NAME		"sta_self_tests.ta"
+#include <tee_api_types.h>
+#include <trace.h>
+#include <types_ext.h>
 
-#define STA_SELF_TEST_UUID \
-		{ 0xd96a5b40, 0xc3e5, 0x21e3, \
-			{ 0x87, 0x94, 0x10, 0x02, 0xa5, 0xd5, 0xc6, 0x1b } }
+#include "core_self_tests.h"
 
-#define CMD_TRACE	0
-#define CMD_PARAMS	1
-#define CMD_SELF_TESTS	2
+#define TA_NAME		"invoke_tests.pta"
 
 static TEE_Result test_trace(uint32_t param_types __unused,
 			TEE_Param params[TEE_NUM_PARAMS] __unused)
@@ -196,6 +194,116 @@ static TEE_Result test_entry_params(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
 }
 
 /*
+ * Test access to Secure Data Path memory from pseudo TAs
+ */
+
+static TEE_Result test_inject_sdp(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
+{
+	char *src = p[0].memref.buffer;
+	char *dst = p[1].memref.buffer;
+	size_t sz = p[0].memref.size;
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INPUT,
+					  TEE_PARAM_TYPE_MEMREF_OUTPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != type) {
+		DMSG("bad parameter types");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (p[1].memref.size < sz) {
+		p[1].memref.size = sz;
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+
+	if (!core_vbuf_is(CORE_MEM_NSEC_SHM, src, sz) ||
+	    !core_vbuf_is(CORE_MEM_SDP_MEM, dst, sz)) {
+		DMSG("bad memref secure attribute");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (cache_operation(TEE_CACHEFLUSH, dst, sz) != TEE_SUCCESS)
+		return TEE_ERROR_GENERIC;
+
+	memcpy(dst, src, sz);
+
+	if (cache_operation(TEE_CACHEFLUSH, dst, sz) != TEE_SUCCESS)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_transform_sdp(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
+{
+	char *buf = p[0].memref.buffer;
+	size_t sz = p[0].memref.size;
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INOUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != type) {
+		DMSG("bad parameter types");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (!core_vbuf_is(CORE_MEM_SDP_MEM, buf, sz)) {
+		DMSG("bad memref secure attribute");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (cache_operation(TEE_CACHEFLUSH, buf, sz) != TEE_SUCCESS)
+		return TEE_ERROR_GENERIC;
+
+	for (; sz; sz--, buf++)
+		*buf = ~(*buf) + 1;
+
+	if (cache_operation(TEE_CACHEFLUSH, buf, sz) != TEE_SUCCESS)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result test_dump_sdp(uint32_t type, TEE_Param p[TEE_NUM_PARAMS])
+{
+	char *src = p[0].memref.buffer;
+	char *dst = p[1].memref.buffer;
+	size_t sz = p[0].memref.size;
+	uint32_t exp_pt = TEE_PARAM_TYPES(TEE_PARAM_TYPE_MEMREF_INPUT,
+					  TEE_PARAM_TYPE_MEMREF_OUTPUT,
+					  TEE_PARAM_TYPE_NONE,
+					  TEE_PARAM_TYPE_NONE);
+
+	if (exp_pt != type) {
+		DMSG("bad parameter types");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (p[1].memref.size < sz) {
+		p[1].memref.size = sz;
+		return TEE_ERROR_SHORT_BUFFER;
+	}
+
+	if (!core_vbuf_is(CORE_MEM_SDP_MEM, src, sz) ||
+	    !core_vbuf_is(CORE_MEM_NSEC_SHM, dst, sz)) {
+		DMSG("bad memref secure attribute");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	if (cache_operation(TEE_CACHEFLUSH, dst, sz) != TEE_SUCCESS)
+		return TEE_ERROR_GENERIC;
+
+	memcpy(dst, src, sz);
+
+	if (cache_operation(TEE_CACHEFLUSH, dst, sz) != TEE_SUCCESS)
+		return TEE_ERROR_GENERIC;
+
+	return TEE_SUCCESS;
+}
+
+/*
  * Trusted Application Entry Points
  */
 
@@ -230,20 +338,30 @@ static TEE_Result invoke_command(void *pSessionContext __unused,
 	DMSG("command entry point for pseudo ta \"%s\"", TA_NAME);
 
 	switch (nCommandID) {
-	case CMD_TRACE:
+	case PTA_INVOKE_TESTS_CMD_TRACE:
 		return test_trace(nParamTypes, pParams);
-	case CMD_PARAMS:
+	case PTA_INVOKE_TESTS_CMD_PARAMS:
 		return test_entry_params(nParamTypes, pParams);
-	case CMD_SELF_TESTS:
+	case PTA_INVOKE_TESTS_CMD_COPY_NSEC_TO_SEC:
+		return test_inject_sdp(nParamTypes, pParams);
+	case PTA_INVOKE_TESTS_CMD_READ_MODIFY_SEC:
+		return test_transform_sdp(nParamTypes, pParams);
+	case PTA_INVOKE_TESTS_CMD_COPY_SEC_TO_NSEC:
+		return test_dump_sdp(nParamTypes, pParams);
+	case PTA_INVOKE_TESTS_CMD_SELF_TESTS:
 		return core_self_tests(nParamTypes, pParams);
+#if defined(CFG_WITH_USER_TA)
+	case PTA_INVOKE_TESTS_CMD_FS_HTREE:
+		return core_fs_htree_tests(nParamTypes, pParams);
+#endif
 	default:
 		break;
 	}
 	return TEE_ERROR_BAD_PARAMETERS;
 }
 
-pseudo_ta_register(.uuid = STA_SELF_TEST_UUID, .name = TA_NAME,
-		   .flags = PTA_DEFAULT_FLAGS,
+pseudo_ta_register(.uuid = PTA_INVOKE_TESTS_UUID, .name = TA_NAME,
+		   .flags = PTA_DEFAULT_FLAGS | TA_FLAG_SECURE_DATA_PATH,
 		   .create_entry_point = create_ta,
 		   .destroy_entry_point = destroy_ta,
 		   .open_session_entry_point = open_session,
diff --git a/core/arch/arm/pta/sub.mk b/core/arch/arm/pta/sub.mk
index 3d961d4..4333e86 100644
--- a/core/arch/arm/pta/sub.mk
+++ b/core/arch/arm/pta/sub.mk
@@ -1,6 +1,9 @@
-srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += pta_self_tests.c
+srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += pta_invoke_tests.c
 srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += core_self_tests.c
 srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += interrupt_tests.c
+ifeq ($(CFG_WITH_USER_TA),y)
+srcs-$(CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += core_fs_htree_tests.c
+endif
 srcs-$(CFG_WITH_STATS) += stats.c
 srcs-$(CFG_TA_GPROF_SUPPORT) += gprof.c
 
@@ -8,7 +11,3 @@ ifeq ($(CFG_SE_API),y)
 srcs-$(CFG_SE_API_SELF_TEST) += se_api_self_tests.c
 cppflags-se_api_self_tests.c-y += -Icore/tee/se
 endif
-
-ifeq ($(CFG_WITH_USER_TA),y)
-srcs-$(CFG_TEE_FS_KEY_MANAGER_TEST) += tee_fs_key_manager_tests.c
-endif
diff --git a/core/arch/arm/pta/tee_fs_key_manager_tests.c b/core/arch/arm/pta/tee_fs_key_manager_tests.c
deleted file mode 100644
index f9dc714..0000000
--- a/core/arch/arm/pta/tee_fs_key_manager_tests.c
+++ /dev/null
@@ -1,375 +0,0 @@
-/*
- * 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 <kernel/pseudo_ta.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <tee/tee_fs_key_manager.h>
-#include <trace.h>
-
-#define TA_NAME		"tee_fs_key_manager_tests.ta"
-
-#define CMD_SELF_TESTS	0
-
-#define ENC_FS_KEY_MANAGER_TEST_UUID \
-		{ 0x17E5E280, 0xD12E, 0x11E4,  \
-		{ 0xA4, 0x1A, 0x00, 0x02, 0xA5, 0xD5, 0xC5, 0x1B } }
-
-#define DUMP_BUF_MAX	256
-
-static uint8_t test_data[] = {
-	0x00, 0x6E, 0x04, 0x57, 0x08, 0xFB, 0x71, 0x96,
-	0x00, 0x2E, 0x55, 0x3D, 0x02, 0xC3, 0xA6, 0x92,
-	0x00, 0xC3, 0xEF, 0x8A, 0xB2, 0x34, 0x53, 0xE6,
-	0x00, 0x74, 0x9C, 0xD6, 0x36, 0xE7, 0xA8, 0x00
-};
-
-static char *print_buf(char *buf, size_t *remain_size, const char *fmt, ...)
-	__attribute__((__format__(__printf__, 3, 4)));
-
-static 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;
-}
-
-static 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]);
-}
-
-static 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);
-}
-
-/*
- * Trusted Application Entry Points
- */
-
-static TEE_Result test_file_decrypt_with_invalid_content(void)
-{
-	TEE_Result res = TEE_SUCCESS;
-	size_t header_size;
-	size_t encrypt_data_out_size;
-	uint8_t *encrypt_data_out = NULL;
-	size_t decrypt_data_out_size;
-	uint8_t *decrypt_data_out = NULL;
-	uint8_t tmp_byte;
-	uint8_t encrypted_fek[TEE_FS_KM_FEK_SIZE];
-
-	DMSG("Start");
-
-	/* data encryption */
-	header_size = tee_fs_get_header_size(META_FILE);
-
-	encrypt_data_out_size = header_size + sizeof(test_data);
-	encrypt_data_out = malloc(encrypt_data_out_size);
-	if (!encrypt_data_out) {
-		EMSG("malloc for encrypt data buffer failed");
-		res = TEE_ERROR_OUT_OF_MEMORY;
-		goto exit;
-	}
-
-	res = tee_fs_encrypt_file(META_FILE,
-			test_data, sizeof(test_data),
-			encrypt_data_out, &encrypt_data_out_size,
-			encrypted_fek);
-	if (res != TEE_SUCCESS) {
-		EMSG("file encryption failed");
-		goto exit;
-	}
-
-	/* data decryption */
-	decrypt_data_out_size = sizeof(test_data);
-	decrypt_data_out = malloc(decrypt_data_out_size);
-	if (!decrypt_data_out) {
-		EMSG("malloc for decrypt data buffer failed");
-		res = TEE_ERROR_OUT_OF_MEMORY;
-		goto exit;
-	}
-
-	/* case1: data decryption with modified encrypted_key */
-	tmp_byte = *(encrypt_data_out + 4);
-	*(encrypt_data_out + 4) = ~tmp_byte;
-
-	DMSG("case1: decryption with modified encrypted FEK");
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res == TEE_ERROR_MAC_INVALID) {
-		DMSG("case1: passed, return code=%x", res);
-	} else {
-		EMSG("case1: failed, return code=%x", res);
-		res = TEE_ERROR_GENERIC;
-		goto exit;
-	}
-
-	*(encrypt_data_out + 4) = tmp_byte;
-
-	/* case2: data decryption with modified iv */
-	tmp_byte = *(encrypt_data_out + 20);
-	*(encrypt_data_out + 20) = ~tmp_byte;
-
-	DMSG("case2: decryption with modified IV");
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res == TEE_ERROR_MAC_INVALID) {
-		DMSG("case2: passed, return code=%x", res);
-	} else {
-		EMSG("case2: failed, return code=%x", res);
-		res = TEE_ERROR_GENERIC;
-		goto exit;
-	}
-
-	*(encrypt_data_out + 20) = tmp_byte;
-
-	/* case3: data decryption with modified cipher text */
-	tmp_byte = *(encrypt_data_out + encrypt_data_out_size - 5);
-	*(encrypt_data_out + encrypt_data_out_size - 5) = ~tmp_byte;
-
-	DMSG("case3: decryption with modified cipher text");
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res == TEE_ERROR_MAC_INVALID) {
-		DMSG("case3: passed, return code=%x", res);
-	} else {
-		EMSG("case3: failed, return code=%x", res);
-		res = TEE_ERROR_GENERIC;
-		goto exit;
-	}
-
-	*(encrypt_data_out + encrypt_data_out_size - 5) = tmp_byte;
-
-	/* case4: data decryption with shorter cipher text length */
-	DMSG("case4: decryption with shorter cipher text length");
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size - 1,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res == TEE_ERROR_MAC_INVALID) {
-		DMSG("case4: passed, return code=%x", res);
-	} else {
-		EMSG("case4: failed, return code=%x", res);
-		res = TEE_ERROR_GENERIC;
-		goto exit;
-	}
-
-	/* case5: data decryption with shorter plain text buffer */
-	decrypt_data_out_size = sizeof(test_data) - 1;
-
-	DMSG("case5: decryption with shorter plain text buffer");
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res == TEE_ERROR_SHORT_BUFFER) {
-		DMSG("case5: passed, return code=%x", res);
-	} else {
-		EMSG("case5: failed, return code=%x", res);
-		res = TEE_ERROR_GENERIC;
-		goto exit;
-	}
-
-	decrypt_data_out_size = encrypt_data_out_size;
-
-	/* data decryption with correct encrypted data */
-	DMSG("good path test - decryption with correct data");
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res != TEE_SUCCESS) {
-		EMSG("failed to decrypted data, return code=%x", res);
-		goto exit;
-	}
-
-	/* data comparison */
-	if (memcmp(test_data, decrypt_data_out, sizeof(test_data)) != 0) {
-		EMSG("decrypted data doest not correct");
-		res = TEE_ERROR_GENERIC;
-	} else {
-		DMSG("good path test - passed");
-	}
-
-exit:
-	if (encrypt_data_out != NULL)
-		free(encrypt_data_out);
-
-	if (decrypt_data_out != NULL)
-		free(decrypt_data_out);
-
-	DMSG("Finish");
-
-	return res;
-}
-
-static TEE_Result test_file_decrypt_success(void)
-{
-	TEE_Result res = TEE_SUCCESS;
-	size_t header_size;
-	size_t encrypt_data_out_size;
-	uint8_t *encrypt_data_out = NULL;
-	size_t decrypt_data_out_size;
-	uint8_t *decrypt_data_out = NULL;
-	uint8_t encrypted_fek[TEE_FS_KM_FEK_SIZE];
-
-	DMSG("Start");
-
-	res = tee_fs_generate_fek(encrypted_fek, TEE_FS_KM_FEK_SIZE);
-	if (res != TEE_SUCCESS)
-		goto exit;
-
-	/* data encryption */
-	header_size = tee_fs_get_header_size(META_FILE);
-
-	encrypt_data_out_size = header_size + sizeof(test_data);
-	encrypt_data_out = malloc(encrypt_data_out_size);
-	if (!encrypt_data_out) {
-		EMSG("malloc for encrypt data buffer failed");
-		res = TEE_ERROR_OUT_OF_MEMORY;
-		goto exit;
-	}
-
-	res = tee_fs_encrypt_file(META_FILE,
-			test_data, sizeof(test_data),
-			encrypt_data_out, &encrypt_data_out_size,
-			encrypted_fek);
-	if (res != TEE_SUCCESS) {
-		EMSG("file encryption failed");
-		goto exit;
-	}
-
-
-	/* data decryption */
-	decrypt_data_out_size = sizeof(test_data);
-	decrypt_data_out = malloc(decrypt_data_out_size);
-	if (!decrypt_data_out) {
-		EMSG("malloc for decrypt data buffer failed");
-		res = TEE_ERROR_OUT_OF_MEMORY;
-		goto exit;
-	}
-
-	res = tee_fs_decrypt_file(META_FILE,
-			encrypt_data_out, encrypt_data_out_size,
-			decrypt_data_out, &decrypt_data_out_size,
-			encrypted_fek);
-	if (res != TEE_SUCCESS)
-		goto exit;
-
-	/* data comparison */
-	if (memcmp(test_data, decrypt_data_out, sizeof(test_data)) != 0) {
-		EMSG("Data compare failed");
-		res = TEE_ERROR_GENERIC;
-	}
-
-exit:
-	/* dump data for debug */
-	if (res != TEE_SUCCESS)
-		DMSG("return code = %x", res);
-	else {
-		DMSG("Test Data (%zu bytes)", sizeof(test_data));
-		print_hex(test_data, sizeof(test_data));
-		DMSG("Encrypted Data (%zu bytes)", encrypt_data_out_size);
-		print_hex(encrypt_data_out, encrypt_data_out_size);
-		DMSG("Decrypted Data (%zu bytes)", decrypt_data_out_size);
-		print_hex(decrypt_data_out, decrypt_data_out_size);
-	}
-
-	if (encrypt_data_out != NULL)
-		free(encrypt_data_out);
-
-	if (decrypt_data_out != NULL)
-		free(decrypt_data_out);
-
-	DMSG("Finish");
-
-	return res;
-}
-
-static TEE_Result self_tests(
-			uint32_t nParamTypes __unused,
-			TEE_Param pParams[TEE_NUM_PARAMS] __unused)
-{
-	TEE_Result res;
-
-	res = test_file_decrypt_success();
-	if (res != TEE_SUCCESS)
-		return res;
-
-	res = test_file_decrypt_with_invalid_content();
-	if (res != TEE_SUCCESS)
-		return res;
-
-	return TEE_SUCCESS;
-}
-
-static TEE_Result invoke_command(void *pSessionContext __unused,
-		uint32_t nCommandID, uint32_t nParamTypes,
-		TEE_Param pParams[TEE_NUM_PARAMS])
-{
-	DMSG("command entry point for static ta \"%s\"", TA_NAME);
-
-	switch (nCommandID) {
-	case CMD_SELF_TESTS:
-		return self_tests(nParamTypes, pParams);
-	default:
-		break;
-	}
-	return TEE_ERROR_BAD_PARAMETERS;
-}
-
-pseudo_ta_register(.uuid = ENC_FS_KEY_MANAGER_TEST_UUID, .name = TA_NAME,
-		   .flags = PTA_DEFAULT_FLAGS,
-		   .invoke_command_entry_point = invoke_command);
diff --git a/core/arch/arm/sm/psci.c b/core/arch/arm/sm/psci.c
index b2bd645..32a897c 100644
--- a/core/arch/arm/sm/psci.c
+++ b/core/arch/arm/sm/psci.c
@@ -149,7 +149,7 @@ void tee_psci_handler(struct thread_smc_args *args)
 			;
 		break;
 	case PSCI_SYSTEM_RESET:
-		psci_system_off();
+		psci_system_reset();
 		while (1)
 			;
 		break;
diff --git a/core/arch/arm/sm/sm.c b/core/arch/arm/sm/sm.c
index 4a0c0f6..f3fa4af 100644
--- a/core/arch/arm/sm/sm.c
+++ b/core/arch/arm/sm/sm.c
@@ -39,6 +39,9 @@ bool sm_from_nsec(struct sm_ctx *ctx)
 {
 	uint32_t *nsec_r0 = (uint32_t *)(&ctx->nsec.r0);
 
+	if (!sm_platform_handler(ctx))
+		return false;
+
 #ifdef CFG_PSCI_ARM32
 	if (OPTEE_SMC_OWNER_NUM(*nsec_r0) == OPTEE_SMC_OWNER_STANDARD) {
 		smc_std_handler((struct thread_smc_args *)nsec_r0);
diff --git a/core/arch/arm/tee/arch_svc.c b/core/arch/arm/tee/arch_svc.c
index 8a89ce9..f6767d7 100644
--- a/core/arch/arm/tee/arch_svc.c
+++ b/core/arch/arm/tee/arch_svc.c
@@ -35,12 +35,12 @@
 #include <tee/tee_svc_cryp.h>
 #include <tee/tee_svc_storage.h>
 #include <tee/se/svc.h>
+#include <tee/svc_cache.h>
 #include <tee_syscall_numbers.h>
 #include <trace.h>
 #include <util.h>
 
 #include "arch_svc_private.h"
-#include "svc_cache.h"
 
 #if (TRACE_LEVEL == TRACE_FLOW) && defined(CFG_TEE_CORE_TA_TRACE)
 #define TRACE_SYSCALLS
@@ -201,8 +201,8 @@ void tee_svc_handler(struct thread_svc_regs *regs)
 	/* TA has just entered kernel mode */
 	tee_ta_update_session_utime_suspend();
 
-	/* Restore IRQ which are disabled on exception entry */
-	thread_restore_irq();
+	/* Restore foreign interrupts which are disabled on exception entry */
+	thread_restore_foreign_intr();
 
 	get_scn_max_args(regs, &scn, &max_args);
 
diff --git a/core/arch/arm/tee/cache.c b/core/arch/arm/tee/cache.c
new file mode 100644
index 0000000..ff4bab3
--- /dev/null
+++ b/core/arch/arm/tee/cache.c
@@ -0,0 +1,79 @@
+/*
+ * Copyright (c) 2014, STMicroelectronics International N.V.
+ * 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 <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+#include <tee/cache.h>
+
+/*
+ * tee_uta_cache_operation - dynamic cache clean/inval request from a TA.
+ * It follows ARM recommendation:
+ *     http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0246d/Beicdhde.html
+ * Note that this implementation assumes dsb operations are part of
+ * cache_op_inner(), and outer cache sync are part of cache_op_outer().
+ */
+TEE_Result cache_operation(enum utee_cache_operation op, void *va, size_t len)
+{
+	TEE_Result res;
+	paddr_t pa;
+
+	pa = virt_to_phys(va);
+	if (!pa)
+		return TEE_ERROR_ACCESS_DENIED;
+
+	switch (op) {
+	case TEE_CACHEFLUSH:
+#ifdef CFG_PL310 /* prevent initial L1 clean in case there is no outer L2 */
+		/* Clean L1, Flush L2, Flush L1 */
+		res = cache_op_inner(DCACHE_AREA_CLEAN, va, len);
+		if (res != TEE_SUCCESS)
+			return res;
+		res = cache_op_outer(DCACHE_AREA_CLEAN_INV, pa, len);
+		if (res != TEE_SUCCESS)
+			return res;
+#endif
+		return cache_op_inner(DCACHE_AREA_CLEAN_INV, va, len);
+
+	case TEE_CACHECLEAN:
+		/* Clean L1, Clean L2 */
+		res = cache_op_inner(DCACHE_AREA_CLEAN, va, len);
+		if (res != TEE_SUCCESS)
+			return res;
+		return cache_op_outer(DCACHE_AREA_CLEAN, pa, len);
+
+	case TEE_CACHEINVALIDATE:
+		/* Inval L2, Inval L1 */
+		res = cache_op_outer(DCACHE_AREA_INVALIDATE, pa, len);
+		if (res != TEE_SUCCESS)
+			return res;
+		return cache_op_inner(DCACHE_AREA_INVALIDATE, va, len);
+
+	default:
+		return TEE_ERROR_NOT_SUPPORTED;
+	}
+}
diff --git a/core/arch/arm/tee/entry_fast.c b/core/arch/arm/tee/entry_fast.c
index 0e80dc8..da7cd31 100644
--- a/core/arch/arm/tee/entry_fast.c
+++ b/core/arch/arm/tee/entry_fast.c
@@ -82,17 +82,21 @@ static void tee_entry_fastcall_l2cc_mutex(struct thread_smc_args *args)
 
 static void tee_entry_exchange_capabilities(struct thread_smc_args *args)
 {
-	if (args->a1) {
-		/*
-		 * Either unknown capability or
-		 * OPTEE_SMC_NSEC_CAP_UNIPROCESSOR, in either case we can't
-		 * deal with it.
-		 *
-		 * The memory mapping of shared memory is defined as normal
-		 * shared memory for SMP systems and normal memory for UP
-		 * systems. Currently we map all memory as shared in secure
-		 * world.
-		 */
+	/*
+	 * Currently we ignore OPTEE_SMC_NSEC_CAP_UNIPROCESSOR.
+	 *
+	 * The memory mapping of shared memory is defined as normal
+	 * shared memory for SMP systems and normal memory for UP
+	 * systems. Currently we map all memory as shared in secure
+	 * world.
+	 *
+	 * When translation tables are created with shared bit cleared for
+	 * uniprocessor systems we'll need to check
+	 * OPTEE_SMC_NSEC_CAP_UNIPROCESSOR.
+	 */
+
+	if (args->a1 & ~OPTEE_SMC_NSEC_CAP_UNIPROCESSOR) {
+		/* Unknown capability. */
 		args->a0 = OPTEE_SMC_RETURN_ENOTAVAIL;
 		return;
 	}
diff --git a/core/arch/arm/tee/entry_std.c b/core/arch/arm/tee/entry_std.c
index 29c3b74..233d731 100644
--- a/core/arch/arm/tee/entry_std.c
+++ b/core/arch/arm/tee/entry_std.c
@@ -51,28 +51,45 @@ static struct tee_ta_session_head tee_open_sessions =
 TAILQ_HEAD_INITIALIZER(tee_open_sessions);
 
 static struct mobj *shm_mobj;
+#ifdef CFG_SECURE_DATA_PATH
+static struct mobj **sdp_mem_mobjs;
+#endif
 
-static TEE_Result set_mem_param(const struct optee_msg_param *param,
-				struct param_mem *mem)
+static bool param_mem_from_mobj(struct param_mem *mem, struct mobj *mobj,
+				const paddr_t pa, const size_t sz)
 {
 	paddr_t b;
-	size_t sz;
-	size_t tsz;
 
-	if (mobj_get_pa(shm_mobj, 0, 0, &b) != TEE_SUCCESS)
-		panic("Failed to be PA of shared memory MOBJ");
+	if (mobj_get_pa(mobj, 0, 0, &b) != TEE_SUCCESS)
+		panic("mobj_get_pa failed");
 
-	sz = shm_mobj->size;
-	tsz = param->u.tmem.size;
-	if (param->u.tmem.buf_ptr && !tsz)
-		tsz++;
-	if (!core_is_buffer_inside(param->u.tmem.buf_ptr, tsz, b, sz))
-		return TEE_ERROR_BAD_PARAMETERS;
+	if (!core_is_buffer_inside(pa, MAX(sz, 1UL), b, mobj->size))
+		return false;
 
-	mem->mobj = shm_mobj;
-	mem->offs = param->u.tmem.buf_ptr - b;
-	mem->size = param->u.tmem.size;
-	return TEE_SUCCESS;
+	mem->mobj = mobj;
+	mem->offs = pa - b;
+	mem->size = sz;
+	return true;
+}
+
+/* fill 'struct param_mem' structure if buffer matches a valid memory object */
+static TEE_Result assign_mobj_to_param_mem(const paddr_t pa, const size_t sz,
+					   struct param_mem *mem)
+{
+	struct mobj __maybe_unused **mobj;
+
+	/* belongs to nonsecure shared memory ? */
+	if (param_mem_from_mobj(mem, shm_mobj, pa, sz))
+		return TEE_SUCCESS;
+
+#ifdef CFG_SECURE_DATA_PATH
+	/* belongs to SDP memories ? */
+	for (mobj = sdp_mem_mobjs; *mobj; mobj++)
+		if (param_mem_from_mobj(mem, *mobj, pa, sz))
+			return TEE_SUCCESS;
+#endif
+
+	return TEE_ERROR_BAD_PARAMETERS;
 }
 
 static TEE_Result copy_in_params(const struct optee_msg_param *params,
@@ -115,7 +132,9 @@ static TEE_Result copy_in_params(const struct optee_msg_param *params,
 		case OPTEE_MSG_ATTR_TYPE_TMEM_INOUT:
 			pt[n] = TEE_PARAM_TYPE_MEMREF_INPUT + attr -
 				OPTEE_MSG_ATTR_TYPE_TMEM_INPUT;
-			res = set_mem_param(params + n, &ta_param->u[n].mem);
+			res = assign_mobj_to_param_mem(params[n].u.tmem.buf_ptr,
+						       params[n].u.tmem.size,
+						       &ta_param->u[n].mem);
 			if (res != TEE_SUCCESS)
 				return res;
 			break;
@@ -346,7 +365,8 @@ void tee_entry_std(struct thread_smc_args *smc_args)
 		return;
 	}
 
-	thread_set_irq(true);	/* Enable IRQ for STD calls */
+	/* Enable foreign interrupts for STD calls */
+	thread_set_foreign_intr(true);
 	switch (arg->cmd) {
 	case OPTEE_MSG_CMD_OPEN_SESSION:
 		entry_open_session(smc_args, arg, num_params);
@@ -380,6 +400,12 @@ static TEE_Result default_mobj_init(void)
 	if (!mobj_sec_ddr)
 		panic("Failed to register secure ta ram");
 
+#ifdef CFG_SECURE_DATA_PATH
+	sdp_mem_mobjs = core_sdp_mem_create_mobjs();
+	if (!sdp_mem_mobjs)
+		panic("Failed to register SDP memory");
+#endif
+
 	return TEE_SUCCESS;
 }
 
diff --git a/core/arch/arm/tee/pta_socket.c b/core/arch/arm/tee/pta_socket.c
index d696773..de27c55 100644
--- a/core/arch/arm/tee/pta_socket.c
+++ b/core/arch/arm/tee/pta_socket.c
@@ -38,7 +38,7 @@ static uint32_t get_instance_id(struct tee_ta_session *sess)
 	return sess->ctx->ops->get_instance_id(sess->ctx);
 }
 
-static TEE_Result socket_open(struct tee_ta_session *sess, uint32_t param_types,
+static TEE_Result socket_open(uint32_t instance_id, uint32_t param_types,
 			      TEE_Param params[TEE_NUM_PARAMS])
 {
 	TEE_Result res;
@@ -65,7 +65,7 @@ static TEE_Result socket_open(struct tee_ta_session *sess, uint32_t param_types,
 
 	msg_params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[0].u.value.a = OPTEE_MRC_SOCKET_OPEN;
-	msg_params[0].u.value.b = get_instance_id(sess);
+	msg_params[0].u.value.b = instance_id;
 
 	msg_params[1].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[1].u.value.a = params[0].value.b; /* server port number */
@@ -90,8 +90,7 @@ static TEE_Result socket_open(struct tee_ta_session *sess, uint32_t param_types,
 	return res;
 }
 
-static TEE_Result socket_close(struct tee_ta_session *sess,
-			       uint32_t param_types,
+static TEE_Result socket_close(uint32_t instance_id, uint32_t param_types,
 			       TEE_Param params[TEE_NUM_PARAMS])
 {
 	struct optee_msg_param msg_params[1];
@@ -110,13 +109,13 @@ static TEE_Result socket_close(struct tee_ta_session *sess,
 
 	msg_params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[0].u.value.a = OPTEE_MRC_SOCKET_CLOSE;
-	msg_params[0].u.value.b = get_instance_id(sess);
+	msg_params[0].u.value.b = instance_id;
 	msg_params[0].u.value.c = params[0].value.a;
 
 	return thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 1, msg_params);
 }
 
-static TEE_Result socket_send(struct tee_ta_session *sess, uint32_t param_types,
+static TEE_Result socket_send(uint32_t instance_id, uint32_t param_types,
 			      TEE_Param params[TEE_NUM_PARAMS])
 {
 	TEE_Result res;
@@ -143,7 +142,7 @@ static TEE_Result socket_send(struct tee_ta_session *sess, uint32_t param_types,
 
 	msg_params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[0].u.value.a = OPTEE_MRC_SOCKET_SEND;
-	msg_params[0].u.value.b = get_instance_id(sess);
+	msg_params[0].u.value.b = instance_id;
 	msg_params[0].u.value.c = params[0].value.a; /* handle */
 
 	/* buffer */
@@ -162,7 +161,7 @@ static TEE_Result socket_send(struct tee_ta_session *sess, uint32_t param_types,
 	return res;
 }
 
-static TEE_Result socket_recv(struct tee_ta_session *sess, uint32_t param_types,
+static TEE_Result socket_recv(uint32_t instance_id, uint32_t param_types,
 			      TEE_Param params[TEE_NUM_PARAMS])
 {
 	TEE_Result res;
@@ -189,7 +188,7 @@ static TEE_Result socket_recv(struct tee_ta_session *sess, uint32_t param_types,
 
 	msg_params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[0].u.value.a = OPTEE_MRC_SOCKET_RECV;
-	msg_params[0].u.value.b = get_instance_id(sess);
+	msg_params[0].u.value.b = instance_id;
 	msg_params[0].u.value.c = params[0].value.a; /* handle */
 
 	/* buffer */
@@ -209,8 +208,7 @@ static TEE_Result socket_recv(struct tee_ta_session *sess, uint32_t param_types,
 	return res;
 }
 
-static TEE_Result socket_ioctl(struct tee_ta_session *sess,
-			       uint32_t param_types,
+static TEE_Result socket_ioctl(uint32_t instance_id, uint32_t param_types,
 			       TEE_Param params[TEE_NUM_PARAMS])
 {
 	TEE_Result res;
@@ -237,7 +235,7 @@ static TEE_Result socket_ioctl(struct tee_ta_session *sess,
 
 	msg_params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[0].u.value.a = OPTEE_MRC_SOCKET_IOCTL;
-	msg_params[0].u.value.b = get_instance_id(sess);
+	msg_params[0].u.value.b = instance_id;
 	msg_params[0].u.value.c = params[0].value.a; /* handle */
 
 	/* buffer */
@@ -257,7 +255,7 @@ static TEE_Result socket_ioctl(struct tee_ta_session *sess,
 	return res;
 }
 
-typedef TEE_Result (*ta_func)(struct tee_ta_session *sess, uint32_t param_types,
+typedef TEE_Result (*ta_func)(uint32_t instance_id, uint32_t param_types,
 			      TEE_Param params[TEE_NUM_PARAMS]);
 
 static const ta_func ta_funcs[] = {
@@ -274,7 +272,7 @@ static const ta_func ta_funcs[] = {
 
 static TEE_Result pta_socket_open_session(uint32_t param_types __unused,
 			TEE_Param pParams[TEE_NUM_PARAMS] __unused,
-			void **sess_ctx __unused)
+			void **sess_ctx)
 {
 	struct tee_ta_session *s;
 
@@ -283,7 +281,7 @@ static TEE_Result pta_socket_open_session(uint32_t param_types __unused,
 	if (!s)
 		return TEE_ERROR_ACCESS_DENIED;
 
-	*sess_ctx = s;
+	*sess_ctx = (void *)(vaddr_t)get_instance_id(s);
 
 	return TEE_SUCCESS;
 }
@@ -297,7 +295,7 @@ static void pta_socket_close_session(void *sess_ctx)
 
 	msg_params[0].attr = OPTEE_MSG_ATTR_TYPE_VALUE_INPUT;
 	msg_params[0].u.value.a = OPTEE_MRC_SOCKET_CLOSE_ALL;
-	msg_params[0].u.value.b = get_instance_id(sess_ctx);
+	msg_params[0].u.value.b = (vaddr_t)sess_ctx;
 
 	res = thread_rpc_cmd(OPTEE_MSG_RPC_CMD_SOCKET, 1, msg_params);
 	if (res != TEE_SUCCESS)
@@ -308,7 +306,7 @@ static TEE_Result pta_socket_invoke_command(void *sess_ctx, uint32_t cmd_id,
 			uint32_t param_types, TEE_Param params[TEE_NUM_PARAMS])
 {
 	if (cmd_id < ARRAY_SIZE(ta_funcs) && ta_funcs[cmd_id])
-		return ta_funcs[cmd_id](sess_ctx, param_types, params);
+		return ta_funcs[cmd_id]((vaddr_t)sess_ctx, param_types, params);
 
 	return TEE_ERROR_NOT_IMPLEMENTED;
 }
diff --git a/core/arch/arm/tee/sub.mk b/core/arch/arm/tee/sub.mk
index 0ee9f64..d95c38c 100644
--- a/core/arch/arm/tee/sub.mk
+++ b/core/arch/arm/tee/sub.mk
@@ -10,3 +10,4 @@ endif
 srcs-y += entry_std.c
 srcs-y += entry_fast.c
 srcs-y += init.c
+srcs-y += cache.c
diff --git a/core/arch/arm/tee/svc_cache.c b/core/arch/arm/tee/svc_cache.c
index 88b89a9..49ee3cf 100644
--- a/core/arch/arm/tee/svc_cache.c
+++ b/core/arch/arm/tee/svc_cache.c
@@ -25,32 +25,27 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include <types_ext.h>
-#include <utee_types.h>
+
 #include <kernel/tee_ta_manager.h>
 #include <mm/tee_mmu.h>
-#include <mm/core_memprot.h>
-
-#include "svc_cache.h"
+#include <tee/cache.h>
+#include <tee/svc_cache.h>
 
-/*
- * tee_uta_cache_operation - dynamic cache clean/inval request from a TA
- * It follows ARM recommendation:
- *     http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0246d/Beicdhde.html
- * Note that this implementation assumes dsb operations are part of
- * cache_maintenance_l1(), and L2 cache sync are part of
- * cache_maintenance_l2()
- */
-static TEE_Result cache_operation(struct tee_ta_session *sess,
-			enum utee_cache_operation op, void *va, size_t len)
+TEE_Result syscall_cache_operation(void *va, size_t len, unsigned long op)
 {
-	TEE_Result ret;
-	paddr_t pa = 0;
-	struct user_ta_ctx *utc = to_user_ta_ctx(sess->ctx);
+	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;
 
 	if ((sess->ctx->flags & TA_FLAG_CACHE_MAINTENANCE) == 0)
 		return TEE_ERROR_NOT_SUPPORTED;
 
+	utc = to_user_ta_ctx(sess->ctx);
+
 	/*
 	 * TAs are allowed to operate cache maintenance on TA memref parameters
 	 * only, not on the TA private memory.
@@ -58,57 +53,11 @@ static TEE_Result cache_operation(struct tee_ta_session *sess,
 	if (tee_mmu_is_vbuf_intersect_ta_private(utc, va, len))
 		return TEE_ERROR_ACCESS_DENIED;
 
-	ret = tee_mmu_check_access_rights(utc, TEE_MEMORY_ACCESS_READ |
+	res = tee_mmu_check_access_rights(utc, TEE_MEMORY_ACCESS_READ |
 					  TEE_MEMORY_ACCESS_ANY_OWNER,
 					  (uaddr_t)va, len);
-	if (ret != TEE_SUCCESS)
-		return TEE_ERROR_ACCESS_DENIED;
-
-	pa = virt_to_phys(va);
-	if (!pa)
-		return TEE_ERROR_ACCESS_DENIED;
-
-	switch (op) {
-	case TEE_CACHEFLUSH:
-		/* Clean L1, Flush L2, Flush L1 */
-		ret = cache_maintenance_l1(DCACHE_AREA_CLEAN, va, len);
-		if (ret != TEE_SUCCESS)
-			return ret;
-		ret = cache_maintenance_l2(L2CACHE_AREA_CLEAN_INV, pa, len);
-		if (ret != TEE_SUCCESS)
-			return ret;
-		return cache_maintenance_l1(DCACHE_AREA_CLEAN_INV, va, len);
-
-	case TEE_CACHECLEAN:
-		/* Clean L1, Clean L2 */
-		ret = cache_maintenance_l1(DCACHE_AREA_CLEAN, va, len);
-		if (ret != TEE_SUCCESS)
-			return ret;
-		return cache_maintenance_l2(L2CACHE_AREA_CLEAN, pa, len);
-
-	case TEE_CACHEINVALIDATE:
-		/* Inval L2, Inval L1 */
-		ret = cache_maintenance_l2(L2CACHE_AREA_INVALIDATE, pa, len);
-		if (ret != TEE_SUCCESS)
-			return ret;
-		return cache_maintenance_l1(DCACHE_AREA_INVALIDATE, va, len);
-
-	default:
-		return TEE_ERROR_NOT_SUPPORTED;
-	}
-}
-
-TEE_Result syscall_cache_operation(void *va, size_t len, unsigned long op)
-{
-	TEE_Result res;
-	struct tee_ta_session *s = NULL;
-
-	res = tee_ta_get_current_session(&s);
 	if (res != TEE_SUCCESS)
-		return res;
-
-	if ((s->ctx->flags & TA_FLAG_CACHE_MAINTENANCE) == 0)
-		return TEE_ERROR_NOT_SUPPORTED;
+		return TEE_ERROR_ACCESS_DENIED;
 
-	return cache_operation(s, op, va, len);
+	return cache_operation(op, va, len);
 }
diff --git a/core/drivers/cdns_uart.c b/core/drivers/cdns_uart.c
index 3b4e4b9..8672093 100644
--- a/core/drivers/cdns_uart.c
+++ b/core/drivers/cdns_uart.c
@@ -24,9 +24,11 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include <compiler.h>
+#include <assert.h>
 #include <drivers/cdns_uart.h>
 #include <io.h>
+#include <keep.h>
+#include <mm/core_mmu.h>
 #include <util.h>
 
 #define CDNS_UART_CONTROL		0
@@ -52,31 +54,44 @@
 #define CDNS_UART_IRQ_RXTRIG		BIT(0)
 #define CDNS_UART_IRQ_RXTOUT		BIT(8)
 
-void cdns_uart_flush(vaddr_t base)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
+	struct cdns_uart_data *pd =
+		container_of(chip, struct cdns_uart_data, chip);
+
+	return io_pa_or_va(&pd->base);
+}
+
+static void cdns_uart_flush(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
 	while (!(read32(base + CDNS_UART_CHANNEL_STATUS) &
-				CDNS_UART_CHANNEL_STATUS_TEMPTY))
+		 CDNS_UART_CHANNEL_STATUS_TEMPTY))
 		;
 }
 
-/*
- * we rely on the bootloader having set up the HW correctly, we just enable
- * transmitter/receiver here, just in case.
- */
-void cdns_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
+static bool cdns_uart_have_rx_data(struct serial_chip *chip)
 {
-	if (!base || !uart_clk || !baud_rate)
-		return;
+	vaddr_t base = chip_to_base(chip);
 
-	/* Enable UART and RX/TX */
-	write32(CDNS_UART_CONTROL_RXEN | CDNS_UART_CONTROL_TXEN,
-		base + CDNS_UART_CONTROL);
+	return !(read32(base + CDNS_UART_CHANNEL_STATUS) &
+			CDNS_UART_CHANNEL_STATUS_REMPTY);
+}
+
+static int cdns_uart_getchar(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
 
-	cdns_uart_flush(base);
+	while (!cdns_uart_have_rx_data(chip))
+		;
+	return read32(base + CDNS_UART_FIFO) & 0xff;
 }
 
-void cdns_uart_putc(int ch, vaddr_t base)
+static void cdns_uart_putc(struct serial_chip *chip, int ch)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	/* Wait until there is space in the FIFO */
 	while (read32(base + CDNS_UART_CHANNEL_STATUS) &
 			CDNS_UART_CHANNEL_STATUS_TFUL)
@@ -86,15 +101,31 @@ void cdns_uart_putc(int ch, vaddr_t base)
 	write32(ch, base + CDNS_UART_FIFO);
 }
 
-bool cdns_uart_have_rx_data(vaddr_t base)
-{
-	return !(read32(base + CDNS_UART_CHANNEL_STATUS) &
-			CDNS_UART_CHANNEL_STATUS_REMPTY);
-}
 
-int cdns_uart_getchar(vaddr_t base)
+static const struct serial_ops cdns_uart_ops = {
+	.flush = cdns_uart_flush,
+	.getchar = cdns_uart_getchar,
+	.have_rx_data = cdns_uart_have_rx_data,
+	.putc = cdns_uart_putc,
+};
+KEEP_PAGER(cdns_uart_ops);
+
+/*
+ * we rely on the bootloader having set up the HW correctly, we just enable
+ * transmitter/receiver here, just in case.
+ */
+void cdns_uart_init(struct cdns_uart_data *pd, paddr_t base, uint32_t uart_clk,
+		uint32_t baud_rate)
 {
-	while (!cdns_uart_have_rx_data(base))
-		;
-	return read32(base + CDNS_UART_FIFO) & 0xff;
+	pd->base.pa = base;
+	pd->chip.ops = &cdns_uart_ops;
+
+	if (!uart_clk || !baud_rate)
+		return;
+
+	/* Enable UART and RX/TX */
+	write32(CDNS_UART_CONTROL_RXEN | CDNS_UART_CONTROL_TXEN,
+		base + CDNS_UART_CONTROL);
+
+	cdns_uart_flush(&pd->chip);
 }
diff --git a/core/drivers/hi16xx_uart.c b/core/drivers/hi16xx_uart.c
index 76e769d..3f8a3ca 100644
--- a/core/drivers/hi16xx_uart.c
+++ b/core/drivers/hi16xx_uart.c
@@ -24,8 +24,12 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#include <assert.h>
 #include <drivers/hi16xx_uart.h>
 #include <io.h>
+#include <keep.h>
+#include <mm/core_mmu.h>
+#include <util.h>
 
 /* Register offsets */
 
@@ -76,16 +80,66 @@
 #define UART_USR_RFNE_BIT	3	/* Receive FIFO not empty bit */
 #define UART_USR_RFF_BIT	4	/* Receive FIFO full bit */
 
-void hi16xx_uart_flush(vaddr_t base)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
+	struct hi16xx_uart_data *pd =
+		container_of(chip, struct hi16xx_uart_data, chip);
+
+	return io_pa_or_va(&pd->base);
+}
+
+static void hi16xx_uart_flush(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	while (!(read32(base + UART_USR) & UART_USR_TFE_BIT))
+		;
+}
+
+static void hi16xx_uart_putc(struct serial_chip *chip, int ch)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	/* Wait until TX FIFO is empty */
 	while (!(read32(base + UART_USR) & UART_USR_TFE_BIT))
 		;
+
+	/* Put character into TX FIFO */
+	write32(ch & 0xFF, base + UART_THR);
+}
+
+static bool hi16xx_uart_have_rx_data(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	return (read32(base + UART_USR) & UART_USR_RFNE_BIT);
+}
+
+static int hi16xx_uart_getchar(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	while (!hi16xx_uart_have_rx_data(chip))
+		;
+	return read32(base + UART_RBR) & 0xFF;
 }
 
-void hi16xx_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
+static const struct serial_ops hi16xx_uart_ops = {
+	.flush = hi16xx_uart_flush,
+	.getchar = hi16xx_uart_getchar,
+	.have_rx_data = hi16xx_uart_have_rx_data,
+	.putc = hi16xx_uart_putc,
+};
+KEEP_PAGER(hi16xx_uart_ops);
+
+void hi16xx_uart_init(struct hi16xx_uart_data *pd, paddr_t base,
+		      uint32_t uart_clk, uint32_t baud_rate)
 {
 	uint16_t freq_div = uart_clk / (16 * baud_rate);
 
+	pd->base.pa = base;
+	pd->chip.ops = &hi16xx_uart_ops;
+
 	/* Enable (and clear) FIFOs */
 	write32(UART_FCR_FIFO_EN, base + UART_FCR);
 
@@ -104,28 +158,6 @@ void hi16xx_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
 	/* Disable interrupt mode */
 	write32(0, base + UART_IEL);
 
-	hi16xx_uart_flush(base);
-}
-
-void hi16xx_uart_putc(int ch, vaddr_t base)
-{
-	/* Wait until TX FIFO is empty */
-	while (!(read32(base + UART_USR) & UART_USR_TFE_BIT))
-		;
-
-	/* Put character into TX FIFO */
-	write32(ch & 0xFF, base + UART_THR);
-}
-
-bool hi16xx_uart_have_rx_data(vaddr_t base)
-{
-	return (read32(base + UART_USR) & UART_USR_RFNE_BIT);
-}
-
-int hi16xx_uart_getchar(vaddr_t base)
-{
-	while (!hi16xx_uart_have_rx_data(base))
-		;
-	return read32(base + UART_RBR) & 0xFF;
+	hi16xx_uart_flush(&pd->chip);
 }
 
diff --git a/core/drivers/imx_uart.c b/core/drivers/imx_uart.c
index b66d905..dd15ddb 100644
--- a/core/drivers/imx_uart.c
+++ b/core/drivers/imx_uart.c
@@ -25,12 +25,11 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <platform_config.h>
-
+#include <assert.h>
 #include <drivers/imx_uart.h>
-#include <console.h>
 #include <io.h>
-#include <compiler.h>
+#include <keep.h>
+#include <util.h>
 
 /* Register definitions */
 #define URXD  0x0  /* Receiver Register */
@@ -80,33 +79,57 @@
 #define  UTS_RXFULL	 (1<<3)	 /* RxFIFO full */
 #define  UTS_SOFTRST	 (1<<0)	 /* Software reset */
 
-void imx_uart_init(vaddr_t __unused vbase)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
-	/*
-	 * Do nothing, debug uart(uart0) share with normal world,
-	 * everything for uart0 intialization is done in bootloader.
-	 */
+	struct imx_uart_data *pd =
+		container_of(chip, struct imx_uart_data, chip);
+
+	return io_pa_or_va(&pd->base);
 }
 
-void imx_uart_flush_tx_fifo(vaddr_t base)
+static void imx_uart_flush(struct serial_chip *chip)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	while (!(read32(base + UTS) & UTS_TXEMPTY))
 		;
 }
 
-int imx_uart_getchar(vaddr_t base)
+static int imx_uart_getchar(struct serial_chip *chip)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	while (read32(base + UTS) & UTS_RXEMPTY)
 		;
 
 	return (read32(base + URXD) & URXD_RX_DATA);
 }
 
-void imx_uart_putc(const char c, vaddr_t base)
+static void imx_uart_putc(struct serial_chip *chip, int ch)
 {
-	write32(c, base + UTXD);
+	vaddr_t base = chip_to_base(chip);
+
+	write32(ch, base + UTXD);
 
-	/* wait until sent */
+	/* Wait until sent */
 	while (!(read32(base + UTS) & UTS_TXEMPTY))
 		;
 }
+
+static const struct serial_ops imx_uart_ops = {
+	.flush = imx_uart_flush,
+	.getchar = imx_uart_getchar,
+	.putc = imx_uart_putc,
+};
+KEEP_PAGER(imx_uart_ops);
+
+void imx_uart_init(struct imx_uart_data *pd, paddr_t base)
+{
+	pd->base.pa = base;
+	pd->chip.ops = &imx_uart_ops;
+
+	/*
+	 * Do nothing, debug uart(uart0) share with normal world,
+	 * everything for uart0 initialization is done in bootloader.
+	 */
+}
diff --git a/core/drivers/ns16550.c b/core/drivers/ns16550.c
index 710b351..62c6d7f 100644
--- a/core/drivers/ns16550.c
+++ b/core/drivers/ns16550.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -27,6 +28,8 @@
 
 #include <drivers/ns16550.h>
 #include <io.h>
+#include <keep.h>
+#include <util.h>
 
 /* uart register defines */
 #define UART_RBR	0x0
@@ -42,16 +45,45 @@
 /* uart status register bits */
 #define UART_LSR_THRE	0x20 /* Transmit-hold-register empty */
 
-void ns16550_flush(vaddr_t base)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
+	struct ns16550_data *pd =
+		container_of(chip, struct ns16550_data, chip);
+
+	return io_pa_or_va(&pd->base);
+}
+
+static void ns16550_flush(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
 	while ((read8(base + UART_LSR) & UART_LSR_THRE) == 0)
 		;
 }
 
-void ns16550_putc(int ch, vaddr_t base)
+static void ns16550_putc(struct serial_chip *chip, int ch)
 {
-	ns16550_flush(base);
+	vaddr_t base = chip_to_base(chip);
+
+	ns16550_flush(chip);
 
 	/* write out charset to Transmit-hold-register */
 	write8(ch, base + UART_THR);
 }
+
+static const struct serial_ops ns16550_ops = {
+	.flush = ns16550_flush,
+	.putc = ns16550_putc,
+};
+KEEP_PAGER(ns16550_ops);
+
+void ns16550_init(struct ns16550_data *pd, paddr_t base)
+{
+	pd->base.pa = base;
+	pd->chip.ops = &ns16550_ops;
+
+	/*
+	 * Do nothing, uart driver shared with normal world,
+	 * everything for uart driver initialization is done in bootloader.
+	 */
+}
diff --git a/core/drivers/pl011.c b/core/drivers/pl011.c
index 8c03090..7263707 100644
--- a/core/drivers/pl011.c
+++ b/core/drivers/pl011.c
@@ -24,8 +24,11 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#include <assert.h>
 #include <drivers/pl011.h>
 #include <io.h>
+#include <keep.h>
+#include <util.h>
 
 #define UART_DR		0x00 /* data register */
 #define UART_RSR_ECR	0x04 /* receive status or error clear */
@@ -89,14 +92,64 @@
 #define UART_IMSC_RTIM		(1 << 6)
 #define UART_IMSC_RXIM		(1 << 4)
 
-void pl011_flush(vaddr_t base)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
+	struct pl011_data *pd =
+		container_of(chip, struct pl011_data, chip);
+
+	return io_pa_or_va(&pd->base);
+}
+
+static void pl011_flush(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
 	while (!(read32(base + UART_FR) & UART_FR_TXFE))
 		;
 }
 
-void pl011_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
+static bool pl011_have_rx_data(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	return !(read32(base + UART_FR) & UART_FR_RXFE);
+}
+
+static int pl011_getchar(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	while (!pl011_have_rx_data(chip))
+		;
+	return read32(base + UART_DR) & 0xff;
+}
+
+static void pl011_putc(struct serial_chip *chip, int ch)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	/* Wait until there is space in the FIFO */
+	while (read32(base + UART_FR) & UART_FR_TXFF)
+		;
+
+	/* Send the character */
+	write32(ch, base + UART_DR);
+}
+
+static const struct serial_ops pl011_ops = {
+	.flush = pl011_flush,
+	.getchar = pl011_getchar,
+	.have_rx_data = pl011_have_rx_data,
+	.putc = pl011_putc,
+};
+KEEP_PAGER(pl011_ops);
+
+void pl011_init(struct pl011_data *pd, paddr_t base, uint32_t uart_clk,
+		uint32_t baud_rate)
 {
+	pd->base.pa = base;
+	pd->chip.ops = &pl011_ops;
+
 	/* Clear all errors */
 	write32(0, base + UART_RSR_ECR);
 	/* Disable everything */
@@ -118,30 +171,6 @@ void pl011_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate)
 	/* Enable UART and RX/TX */
 	write32(UART_CR_UARTEN | UART_CR_TXE | UART_CR_RXE, base + UART_CR);
 
-	pl011_flush(base);
-}
-
-void pl011_putc(int ch, vaddr_t base)
-{
-	/*
-	 * Wait until there is space in the FIFO
-	 */
-	while (read32(base + UART_FR) & UART_FR_TXFF)
-		;
-
-	/* Send the character */
-	write32(ch, base + UART_DR);
-}
-
-bool pl011_have_rx_data(vaddr_t base)
-{
-	return !(read32(base + UART_FR) & UART_FR_RXFE);
-}
-
-int pl011_getchar(vaddr_t base)
-{
-	while (!pl011_have_rx_data(base))
-		;
-	return read32(base + UART_DR) & 0xff;
+	pl011_flush(&pd->chip);
 }
 
diff --git a/core/drivers/pl050.c b/core/drivers/pl050.c
index fa5feab..aedaf71 100644
--- a/core/drivers/pl050.c
+++ b/core/drivers/pl050.c
@@ -26,8 +26,9 @@
  */
 #include <compiler.h>
 #include <drivers/pl050.h>
-#include <util.h>
 #include <io.h>
+#include <keep.h>
+#include <util.h>
 
 #define KMI_ICR		0x00
 #define KMI_STAT	0x04
@@ -91,6 +92,7 @@ static const struct serial_ops pl050_ops = {
 	.have_rx_data = pl050_have_rx_data,
 	.getchar = pl050_getchar,
 };
+KEEP_PAGER(pl050_ops);
 
 void pl050_init(struct pl050_data *pd, vaddr_t base, uint32_t clk)
 {
diff --git a/core/drivers/scif.c b/core/drivers/scif.c
index 02fd49d..ff0cec8 100644
--- a/core/drivers/scif.c
+++ b/core/drivers/scif.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, GlobalLogic
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -24,16 +25,18 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include <compiler.h>
+#include <drivers/scif.h>
 #include <io.h>
+#include <keep.h>
 #include <util.h>
-#include <drivers/scif.h>
 
+#define SCIF_SCSCR		(0x08)
 #define SCIF_SCFSR		(0x10)
 #define SCIF_SCFTDR		(0x0C)
 #define SCIF_SCFCR		(0x18)
 #define SCIF_SCFDR		(0x1C)
 
+#define SCSCR_TE		BIT(5)
 #define SCFSR_TDFE		BIT(5)
 #define SCFSR_TEND		BIT(6)
 
@@ -41,20 +44,26 @@
 
 #define SCIF_TX_FIFO_SIZE	16
 
-void scif_uart_flush(vaddr_t base)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
-	while (!(read16(base + SCIF_SCFSR) & SCFSR_TEND))
-		;
+	struct scif_uart_data *pd =
+		container_of(chip, struct scif_uart_data, chip);
+
+	return io_pa_or_va(&pd->base);
 }
 
-void scif_uart_init(vaddr_t base)
+static void scif_uart_flush(struct serial_chip *chip)
 {
-	/* Bootloader should initialize device for us */
-	scif_uart_flush(base);
+	vaddr_t base = chip_to_base(chip);
+
+	while (!(read16(base + SCIF_SCFSR) & SCFSR_TEND))
+		;
 }
 
-void scif_uart_putc(int ch, vaddr_t base)
+static void scif_uart_putc(struct serial_chip *chip, int ch)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	/* Wait until there is space in the FIFO */
 	while ((read16(base + SCIF_SCFDR) >> SCFDR_T_SHIFT) >=
 		SCIF_TX_FIFO_SIZE)
@@ -63,3 +72,20 @@ void scif_uart_putc(int ch, vaddr_t base)
 	write16(read16(base + SCIF_SCFSR) & ~(SCFSR_TEND | SCFSR_TDFE),
 		base + SCIF_SCFSR);
 }
+
+static const struct serial_ops scif_uart_ops = {
+	.flush = scif_uart_flush,
+	.putc = scif_uart_putc,
+};
+KEEP_PAGER(scif_uart_ops);
+
+void scif_uart_init(struct scif_uart_data *pd, paddr_t base)
+{
+	pd->base.pa = base;
+	pd->chip.ops = &scif_uart_ops;
+
+	/* Set Transmit Enable in Control register */
+	write16(read16(base + SCIF_SCSCR) | SCSCR_TE, base + SCIF_SCSCR);
+
+	scif_uart_flush(&pd->chip);
+}
diff --git a/core/drivers/serial8250_uart.c b/core/drivers/serial8250_uart.c
index 1dd21de..5e86158 100644
--- a/core/drivers/serial8250_uart.c
+++ b/core/drivers/serial8250_uart.c
@@ -24,12 +24,13 @@
  * 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 <drivers/serial8250_uart.h>
+#include <compiler.h>
 #include <console.h>
+#include <drivers/serial8250_uart.h>
 #include <io.h>
-#include <compiler.h>
+#include <keep.h>
+#include <util.h>
 
 /* uart register defines */
 #define UART_RHR	0x0
@@ -49,45 +50,73 @@
 #define LSR_EMPTY	(LSR_TEMT | LSR_THRE)
 #define LSR_DR		0x01 /* DATA Ready */
 
-void serial8250_uart_init(vaddr_t __unused base,
-		uint32_t __unused uart_clk, uint32_t __unused baud_rate)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
-	/*
-	 * do nothing, debug uart(uart0) share with normal world,
-	 * everything for uart0 is ready now.
-	 */
+	struct serial8250_uart_data *pd =
+		container_of(chip, struct serial8250_uart_data, chip);
+
+	return io_pa_or_va(&pd->base);
 }
 
-void serial8250_uart_flush_tx_fifo(vaddr_t base)
+static void serial8250_uart_flush(struct serial_chip *chip)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	while (1) {
 		uint8_t state = read8(base + UART_LSR);
 
-		/* waiting transmit fifo empty */
+		/* Wait until transmit FIFO is empty */
 		if ((state & LSR_EMPTY) == LSR_EMPTY)
 			break;
 	}
 }
 
-bool serial8250_uart_have_rx_data(vaddr_t base)
+static bool serial8250_uart_have_rx_data(struct serial_chip *chip)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	return (read32(base + UART_LSR) & LSR_DR);
 }
 
-void serial8250_uart_putc(int ch, vaddr_t base)
+static int serial8250_uart_getchar(struct serial_chip *chip)
 {
-	serial8250_uart_flush_tx_fifo(base);
+	vaddr_t base = chip_to_base(chip);
 
-	/* write out charset to transmit fifo */
-	write8(ch, base + UART_THR);
-}
-
-int serial8250_uart_getchar(vaddr_t base)
-{
-	while (!serial8250_uart_have_rx_data(base)) {
-		/* transmit fifo is empty, waiting again. */
+	while (!serial8250_uart_have_rx_data(chip)) {
+		/* Transmit FIFO is empty, waiting again */
 		;
 	}
 	return read8(base + UART_RHR);
 }
 
+static void serial8250_uart_putc(struct serial_chip *chip, int ch)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	serial8250_uart_flush(chip);
+
+	/* Write out character to transmit FIFO */
+	write8(ch, base + UART_THR);
+}
+
+static const struct serial_ops serial8250_uart_ops = {
+	.flush = serial8250_uart_flush,
+	.getchar = serial8250_uart_getchar,
+	.have_rx_data = serial8250_uart_have_rx_data,
+	.putc = serial8250_uart_putc,
+};
+KEEP_PAGER(serial8250_uart_ops);
+
+void serial8250_uart_init(struct serial8250_uart_data *pd, paddr_t base,
+			  uint32_t __unused uart_clk,
+			  uint32_t __unused baud_rate)
+
+{
+	pd->base.pa = base;
+	pd->chip.ops = &serial8250_uart_ops;
+
+	/*
+	 * do nothing, debug uart(uart0) share with normal world,
+	 * everything for uart0 is ready now.
+	 */
+}
diff --git a/core/drivers/sprd_uart.c b/core/drivers/sprd_uart.c
index fdaa1b6..c77f595 100644
--- a/core/drivers/sprd_uart.c
+++ b/core/drivers/sprd_uart.c
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -24,8 +25,10 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#include <io.h>
 #include <drivers/sprd_uart.h>
+#include <io.h>
+#include <keep.h>
+#include <util.h>
 
 /* Register definitions */
 #define UART_TXD		0x0000
@@ -36,43 +39,57 @@
 #define STS1_RXF_CNT_MASK	0x00ff  /* Rx FIFO data counter mask */
 #define STS1_TXF_CNT_MASK	0xff00 /* Tx FIFO data counter mask */
 
-static uint32_t sprd_uart_read(vaddr_t base, uint32_t reg)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
-	return read32(base + reg);
-}
+	struct sprd_uart_data *pd =
+		container_of(chip, struct sprd_uart_data, chip);
 
-static void sprd_uart_write(vaddr_t base, uint32_t reg, uint32_t value)
-{
-	write32(value, base + reg);
+	return io_pa_or_va(&pd->base);
 }
 
-static void sprd_uart_wait_xmit_done(vaddr_t base)
+static void sprd_uart_flush(struct serial_chip *chip)
 {
-	while (sprd_uart_read(base, UART_STS1) & STS1_TXF_CNT_MASK)
+	vaddr_t base = chip_to_base(chip);
+
+	while (read32(base + UART_STS1) & STS1_TXF_CNT_MASK)
 		;
 }
 
-static void sprd_uart_wait_rx_data(vaddr_t base)
+static bool sprd_uart_have_rx_data(struct serial_chip *chip)
 {
-	while (!(sprd_uart_read(base, UART_STS1) & STS1_RXF_CNT_MASK))
-		;
+	vaddr_t base = chip_to_base(chip);
+
+	return !!(read32(base + UART_STS1) & STS1_RXF_CNT_MASK);
 }
 
-void sprd_uart_flush(vaddr_t base)
+static void sprd_uart_putc(struct serial_chip *chip, int ch)
 {
-	sprd_uart_wait_xmit_done(base);
+	vaddr_t base = chip_to_base(chip);
+
+	sprd_uart_flush(chip);
+	write32(base + UART_TXD, ch);
 }
 
-void sprd_uart_putc(vaddr_t base, unsigned char ch)
+static int sprd_uart_getchar(struct serial_chip *chip)
 {
-	sprd_uart_wait_xmit_done(base);
+	vaddr_t base = chip_to_base(chip);
+
+	while (!sprd_uart_have_rx_data(chip))
+		;
 
-	sprd_uart_write(base, UART_TXD, (uint32_t)ch);
+	return read32(base + UART_RXD) & 0xff;
 }
 
-unsigned char sprd_uart_getc(vaddr_t base)
-{
-	sprd_uart_wait_rx_data(base);
+static const struct serial_ops sprd_uart_ops = {
+	.flush = sprd_uart_flush,
+	.getchar = sprd_uart_getchar,
+	.have_rx_data = sprd_uart_have_rx_data,
+	.putc = sprd_uart_putc,
+};
+KEEP_PAGER(sprd_uart_ops);
 
-	return sprd_uart_read(base, UART_RXD) & 0xff;
+void sprd_uart_init(struct sprd_uart_data *pd, paddr_t base)
+{
+	pd->base.pa = base;
+	pd->chip.ops = &sprd_uart_ops;
 }
diff --git a/core/arch/arm/plat-ti/console.c b/core/drivers/stih_asc.c
index 48f0f65..7f69ce7 100644
--- a/core/arch/arm/plat-ti/console.c
+++ b/core/drivers/stih_asc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -24,44 +24,52 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#include <drivers/stih_asc.h>
+#include <io.h>
+#include <keep.h>
+#include <util.h>
 
-#include <console.h>
-#include <drivers/serial8250_uart.h>
-#include <mm/core_memprot.h>
-#include <platform_config.h>
+#define ASC_BAUDRATE		0x00
+#define ASC_TXBUFFER		0x04
+#define ASC_STATUS		0x14
 
-register_phys_mem(MEM_AREA_IO_NSEC,
-		  CONSOLE_UART_BASE,
-		  SERIAL8250_UART_REG_SIZE);
+#define ASC_STATUS_TX_EMPTY		BIT(1)
+#define ASC_STATUS_TX_HALF_EMPTY	BIT(2)
 
-static vaddr_t console_base(void)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
-	static void *va __early_bss;
+	struct stih_asc_pd *pd =
+		container_of(chip, struct stih_asc_pd, chip);
 
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_NSEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
+	return io_pa_or_va(&pd->base);
 }
 
-void console_init(void)
+static void stih_asc_flush(struct serial_chip *chip)
 {
-	serial8250_uart_init(console_base(), CONSOLE_UART_CLK_IN_HZ,
-			     CONSOLE_BAUDRATE);
+	vaddr_t base = chip_to_base(chip);
+
+	while (!(read32(base + ASC_STATUS) & ASC_STATUS_TX_EMPTY))
+		;
 }
 
-void console_putc(int ch)
+static void stih_asc_putc(struct serial_chip *chip, int ch)
 {
-	vaddr_t base = console_base();
+	vaddr_t base = chip_to_base(chip);
+
+	while (!(read32(base + ASC_STATUS) & ASC_STATUS_TX_HALF_EMPTY))
+		;
 
-	if (ch == '\n')
-		serial8250_uart_putc('\r', base);
-	serial8250_uart_putc(ch, base);
+	write32(ch, base + ASC_TXBUFFER);
 }
 
-void console_flush(void)
+static const struct serial_ops stih_asc_ops = {
+	.flush = stih_asc_flush,
+	.putc = stih_asc_putc,
+};
+KEEP_PAGER(stih_asc_ops);
+
+void stih_asc_init(struct stih_asc_pd *pd, vaddr_t base)
 {
-	serial8250_uart_flush_tx_fifo(console_base());
+	pd->base.pa = base;
+	pd->chip.ops = &stih_asc_ops;
 }
diff --git a/core/drivers/sub.mk b/core/drivers/sub.mk
index 609b378..015f5c3 100644
--- a/core/drivers/sub.mk
+++ b/core/drivers/sub.mk
@@ -17,3 +17,4 @@ srcs-$(CFG_HI16XX_UART) += hi16xx_uart.c
 srcs-$(CFG_HI16XX_RNG) += hi16xx_rng.c
 srcs-$(CFG_SCIF) += scif.c
 srcs-$(CFG_DRA7_RNG) += dra7_rng.c
+srcs-$(CFG_STIH_UART) += stih_asc.c
diff --git a/core/drivers/sunxi_uart.c b/core/drivers/sunxi_uart.c
index 433c423..b83da02 100644
--- a/core/drivers/sunxi_uart.c
+++ b/core/drivers/sunxi_uart.c
@@ -24,11 +24,10 @@
  * 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 <drivers/sunxi_uart.h>
 #include <io.h>
-#include <compiler.h>
+#include <keep.h>
+#include <util.h>
 
 /* uart register defines */
 #define UART_REG_RBR 	(0x00)
@@ -55,28 +54,46 @@
 #define UART_REG_USR_RFNE (0x1 << 0x3)
 #define UART_REG_USR_RFF  (0x1 << 0x4)
 
-void sunxi_uart_init(vaddr_t __unused base)
+static vaddr_t chip_to_base(struct serial_chip *chip)
 {
-	/* do nothing, debug uart(uart0) share with normal world,
-	 * everything for uart0 is ready now.
-	 */
+	struct sunxi_uart_data *pd =
+		container_of(chip, struct sunxi_uart_data, chip);
+
+	return io_pa_or_va(&pd->base);
 }
 
-void sunxi_uart_flush(vaddr_t base)
+static void sunxi_uart_flush(struct serial_chip *chip)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	while (read32(base + UART_REG_TFL)) {
 		/* waiting transmit fifo empty */
 		;
 	}
 }
 
-bool sunxi_uart_have_rx_data(vaddr_t base)
+static bool sunxi_uart_have_rx_data(struct serial_chip *chip)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	return read32(base + UART_REG_RFL);
 }
 
-void sunxi_uart_putc(int ch, vaddr_t base)
+static int sunxi_uart_getchar(struct serial_chip *chip)
+{
+	vaddr_t base = chip_to_base(chip);
+
+	while (!sunxi_uart_have_rx_data(chip)) {
+		/* transmit fifo is empty, waiting again. */
+		;
+	}
+	return read32(base + UART_REG_RBR) & 0xff;
+}
+
+static void sunxi_uart_putc(struct serial_chip *chip, int ch)
 {
+	vaddr_t base = chip_to_base(chip);
+
 	while (!(read32(base + UART_REG_USR) & UART_REG_USR_TFNF)) {
 		/* transmit fifo is full, waiting again. */
 		;
@@ -86,12 +103,21 @@ void sunxi_uart_putc(int ch, vaddr_t base)
 	write8(ch, base + UART_REG_THR);
 }
 
-int sunxi_uart_getchar(vaddr_t base)
+static const struct serial_ops sunxi_uart_ops = {
+	.flush = sunxi_uart_flush,
+	.getchar = sunxi_uart_getchar,
+	.have_rx_data = sunxi_uart_have_rx_data,
+	.putc = sunxi_uart_putc,
+};
+KEEP_PAGER(sunxi_uart_ops);
+
+void sunxi_uart_init(struct sunxi_uart_data *pd, paddr_t base)
 {
-	while (!sunxi_uart_have_rx_data(base)) {
-		/* transmit fifo is empty, waiting again. */
-		;
-	}
-	return read32(base + UART_REG_RBR) & 0xff;
-}
+	pd->base.pa = base;
+	pd->chip.ops = &sunxi_uart_ops;
 
+	/*
+	 * Do nothing, debug uart(uart0) share with normal world,
+	 * everything for uart0 is ready now.
+	 */
+}
diff --git a/core/include/console.h b/core/include/console.h
index 0fe8e49..d8df7f1 100644
--- a/core/include/console.h
+++ b/core/include/console.h
@@ -32,5 +32,8 @@ void console_init(void);
 void console_putc(int ch);
 void console_flush(void);
 
+struct serial_chip;
+void register_serial_console(struct serial_chip *chip);
+
 #endif /* CONSOLE_H */
 
diff --git a/core/include/drivers/cdns_uart.h b/core/include/drivers/cdns_uart.h
index 6688fd3..d97ee37 100644
--- a/core/include/drivers/cdns_uart.h
+++ b/core/include/drivers/cdns_uart.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, Xilinx Inc
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -28,15 +29,14 @@
 #define CDNS_UART_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
-void cdns_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate);
+struct cdns_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void cdns_uart_putc(int ch, vaddr_t base);
-
-void cdns_uart_flush(vaddr_t base);
-
-bool cdns_uart_have_rx_data(vaddr_t base);
-
-int cdns_uart_getchar(vaddr_t base);
+void cdns_uart_init(struct cdns_uart_data *pd, paddr_t base, uint32_t uart_clk,
+		uint32_t baud_rate);
 
 #endif /* CDNS_UART_H */
diff --git a/core/include/drivers/hi16xx_uart.h b/core/include/drivers/hi16xx_uart.h
index a7d4f0c..d3bd727 100644
--- a/core/include/drivers/hi16xx_uart.h
+++ b/core/include/drivers/hi16xx_uart.h
@@ -33,18 +33,16 @@
 #define HI16XX_UART_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
 #define HI16XX_UART_REG_SIZE 0xF8
 
-void hi16xx_uart_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate);
+struct hi16xx_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void hi16xx_uart_putc(int ch, vaddr_t base);
-
-void hi16xx_uart_flush(vaddr_t base);
-
-bool hi16xx_uart_have_rx_data(vaddr_t base);
-
-int hi16xx_uart_getchar(vaddr_t base);
+void hi16xx_uart_init(struct hi16xx_uart_data *pd, paddr_t base,
+		      uint32_t uart_clk, uint32_t baud_rate);
 
 #endif /* HI16XX_UART_H */
-
diff --git a/core/include/drivers/imx_uart.h b/core/include/drivers/imx_uart.h
index db63227..bbcc953 100644
--- a/core/include/drivers/imx_uart.h
+++ b/core/include/drivers/imx_uart.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (C) 2015 Freescale Semiconductor, Inc.
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -28,15 +29,13 @@
 #define IMX_UART_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
-void imx_uart_init(vaddr_t base);
+struct imx_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void imx_uart_putc(const char ch, vaddr_t base);
-
-void imx_uart_flush_tx_fifo(vaddr_t base);
-
-bool imx_uart_have_rx_data(vaddr_t base);
-
-int imx_uart_getchar(vaddr_t base);
+void imx_uart_init(struct imx_uart_data *pd, paddr_t base);
 
 #endif /* IMX_UART_H */
diff --git a/core/include/drivers/ns16550.h b/core/include/drivers/ns16550.h
index e865871..b8d1049 100644
--- a/core/include/drivers/ns16550.h
+++ b/core/include/drivers/ns16550.h
@@ -28,9 +28,13 @@
 #define NS16550_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
-void ns16550_putc(int ch, vaddr_t base);
+struct ns16550_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void ns16550_flush(vaddr_t base);
+void ns16550_init(struct ns16550_data *pd, paddr_t base);
 
 #endif /* NS16550_H */
diff --git a/core/include/drivers/pl011.h b/core/include/drivers/pl011.h
index b83f2b2..872b7d1 100644
--- a/core/include/drivers/pl011.h
+++ b/core/include/drivers/pl011.h
@@ -28,18 +28,16 @@
 #define PL011_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
 #define PL011_REG_SIZE	0x1000
 
-void pl011_init(vaddr_t base, uint32_t uart_clk, uint32_t baud_rate);
+struct pl011_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void pl011_putc(int ch, vaddr_t base);
-
-void pl011_flush(vaddr_t base);
-
-bool pl011_have_rx_data(vaddr_t base);
-
-int pl011_getchar(vaddr_t base);
+void pl011_init(struct pl011_data *pd, paddr_t base, uint32_t uart_clk,
+		uint32_t baud_rate);
 
 #endif /* PL011_H */
-
diff --git a/core/include/drivers/scif.h b/core/include/drivers/scif.h
index d9d1055..f621e6c 100644
--- a/core/include/drivers/scif.h
+++ b/core/include/drivers/scif.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, GlobalLogic
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -28,13 +29,15 @@
 #define SCIF_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
 #define SCIF_REG_SIZE	0x1000
 
-void scif_uart_flush(vaddr_t base);
+struct scif_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void scif_uart_init(vaddr_t base);
-
-void scif_uart_putc(int ch, vaddr_t base);
+void scif_uart_init(struct scif_uart_data *pd, paddr_t base);
 
 #endif /* SCIF */
diff --git a/core/include/drivers/serial.h b/core/include/drivers/serial.h
index b8f00df..c1e9ebe 100644
--- a/core/include/drivers/serial.h
+++ b/core/include/drivers/serial.h
@@ -27,6 +27,12 @@
 #ifndef __DRIVERS_SERIAL_H
 #define __DRIVERS_SERIAL_H
 
+#include <assert.h>
+#include <stdbool.h>
+#include <types_ext.h>
+#include <mm/core_memprot.h>
+#include <mm/core_mmu.h>
+
 struct serial_chip {
 	const struct serial_ops *ops;
 };
@@ -38,4 +44,25 @@ struct serial_ops {
 	int (*getchar)(struct serial_chip *chip);
 };
 
+struct io_pa_va {
+	paddr_t pa;
+	vaddr_t va;
+};
+
+/*
+ * Helper function to return a physical or virtual address for a device,
+ * depending on whether the MMU is enabled or not
+ */
+static inline vaddr_t io_pa_or_va(struct io_pa_va *p)
+{
+	assert(p->pa);
+	if (cpu_mmu_enabled()) {
+		if (!p->va)
+			p->va = (vaddr_t)phys_to_virt_io(p->pa);
+		assert(p->va);
+		return p->va;
+	}
+	return p->pa;
+}
+
 #endif /*__DRIVERS_SERIASERIAL_H*/
diff --git a/core/include/drivers/serial8250_uart.h b/core/include/drivers/serial8250_uart.h
index 5b8985d..d8b5051 100644
--- a/core/include/drivers/serial8250_uart.h
+++ b/core/include/drivers/serial8250_uart.h
@@ -28,19 +28,17 @@
 #define SERIAL8250_UART_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
 #define SERIAL8250_UART_REG_SIZE 0x20
 
-void serial8250_uart_init(vaddr_t base,
-		uint32_t uart_clk, uint32_t baud_rate);
+struct serial8250_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void serial8250_uart_putc(int ch, vaddr_t base);
-
-void serial8250_uart_flush_tx_fifo(vaddr_t base);
-
-bool serial8250_uart_have_rx_data(vaddr_t base);
-
-int serial8250_uart_getchar(vaddr_t base);
+void serial8250_uart_init(struct serial8250_uart_data *pd, paddr_t base,
+			  uint32_t uart_clk, uint32_t baud_rate);
 
 #endif /* SERIAL8250_UART_H */
 
diff --git a/core/include/drivers/sprd_uart.h b/core/include/drivers/sprd_uart.h
index c70bfb9..f89f914 100644
--- a/core/include/drivers/sprd_uart.h
+++ b/core/include/drivers/sprd_uart.h
@@ -1,5 +1,6 @@
 /*
  * Copyright (c) 2016, Spreadtrum Communications Inc.
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -28,12 +29,14 @@
 #define SPRD_UART_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
-void sprd_uart_flush(vaddr_t base);
+struct sprd_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void sprd_uart_putc(vaddr_t base, unsigned char ch);
-
-unsigned char sprd_uart_getc(vaddr_t base);
+void sprd_uart_init(struct sprd_uart_data *pd, paddr_t base);
 
 #endif /* SPRD_UART_H */
 
diff --git a/core/include/tee/tee_fs_defs.h b/core/include/drivers/stih_asc.h
index 9c11de9..a7cbee2 100644
--- a/core/include/tee/tee_fs_defs.h
+++ b/core/include/drivers/stih_asc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -24,41 +24,20 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
+#ifndef STIH_ASC_H
+#define STIH_ASC_H
 
-#ifndef TEE_FS_DEFS_H
-#define TEE_FS_DEFS_H
+#include <drivers/serial.h>
+#include <types_ext.h>
 
-/*
- * tee_fs_open
- */
-#define TEE_FS_O_RDONLY 0x1
-#define TEE_FS_O_WRONLY 0x2
-#define TEE_FS_O_RDWR   0x4
-#define TEE_FS_O_CREATE 0x8
-#define TEE_FS_O_EXCL   0x10
-#define TEE_FS_O_APPEND 0x20
-#define TEE_FS_O_TRUNC  0x40
+#define STIH_ASC_REG_SIZE	0x1000
 
-/*
- * tee_fs_lseek
- */
-#define TEE_FS_SEEK_SET 0x1
-#define TEE_FS_SEEK_END 0x2
-#define TEE_FS_SEEK_CUR 0x4
+struct stih_asc_pd {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-/*
- * file modes
- */
-#define TEE_FS_S_IWUSR 0x1
-#define TEE_FS_S_IRUSR 0x2
-#define TEE_FS_S_IXUSR 0x4
+void stih_asc_init(struct stih_asc_pd *pb, vaddr_t base);
 
-/*
- * access modes
- * X_OK is not supported
- */
-#define TEE_FS_R_OK    0x1
-#define TEE_FS_W_OK    0x2
-#define TEE_FS_F_OK    0x4
+#endif /* STIH_ASC_H */
 
-#endif
diff --git a/core/include/drivers/sunxi_uart.h b/core/include/drivers/sunxi_uart.h
index 2b33641..a5a2b8c 100644
--- a/core/include/drivers/sunxi_uart.h
+++ b/core/include/drivers/sunxi_uart.h
@@ -28,16 +28,14 @@
 #define SUNXI_UART_H
 
 #include <types_ext.h>
+#include <drivers/serial.h>
 
-void sunxi_uart_init(vaddr_t base);
+struct sunxi_uart_data {
+	struct io_pa_va base;
+	struct serial_chip chip;
+};
 
-void sunxi_uart_putc(int ch, vaddr_t base);
-
-void sunxi_uart_flush(vaddr_t base);
-
-bool sunxi_uart_have_rx_data(vaddr_t base);
-
-int sunxi_uart_getchar(vaddr_t base);
+void sunxi_uart_init(struct sunxi_uart_data *pd, paddr_t base);
 
 #endif /*SUNXI_UART_H*/
 
diff --git a/core/include/io.h b/core/include/io.h
index 510bf33..83b2efc 100644
--- a/core/include/io.h
+++ b/core/include/io.h
@@ -30,12 +30,6 @@
 #include <stdint.h>
 #include <types_ext.h>
 
-/*
- * IO access macro, please avoid using this macro, since it's going to be
- * deprecated.
- */
-#define  IO(addr)  (*((volatile unsigned long *)(addr)))
-
 static inline void write8(uint8_t val, vaddr_t addr)
 {
 	*(volatile uint8_t *)addr = val;
diff --git a/core/arch/arm/plat-stm/asc.h b/core/include/tee/cache.h
index bbf574c..d32e365 100644
--- a/core/arch/arm/plat-stm/asc.h
+++ b/core/include/tee/cache.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, STMicroelectronics International N.V.
+ * Copyright (c) 2015, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -24,12 +24,11 @@
  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  * POSSIBILITY OF SUCH DAMAGE.
  */
-#ifndef ASC_H
-#define ASC_H
+#ifndef TEE_CACHE_H
+#define TEE_CACHE_H
 
-#include <types_ext.h>
+#include <utee_types.h>
 
-extern int __asc_xmit_char(const char p, vaddr_t base);
-extern void __asc_flush(vaddr_t base);
+TEE_Result cache_operation(enum utee_cache_operation op, void *va, size_t len);
 
-#endif
+#endif /* TEE_CACHE_H */
diff --git a/core/include/tee/fs_htree.h b/core/include/tee/fs_htree.h
new file mode 100644
index 0000000..3d280db
--- /dev/null
+++ b/core/include/tee/fs_htree.h
@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2017, 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_FS_HTREE_H
+#define __TEE_FS_HTREE_H
+
+/*
+ * The purpose of this API is to provide file integrity and confidentiality
+ * in order to implement secure storage. On-disk data structures are
+ * duplicated to make updates atomic, an update is finalized to disk with
+ * tee_fs_htree_sync_to_storage().
+ *
+ * This implementation doesn't provide rollback protection, it only
+ * guarantees the integrity and confidentiality of the file.
+ */
+
+#include <tee_api_types.h>
+#include <utee_defines.h>
+
+#define TEE_FS_HTREE_HASH_SIZE		TEE_SHA256_HASH_SIZE
+#define TEE_FS_HTREE_IV_SIZE		16
+#define TEE_FS_HTREE_FEK_SIZE		16
+#define TEE_FS_HTREE_TAG_SIZE		16
+
+/* Internal struct provided to let the rpc callbacks know the size if needed */
+struct tee_fs_htree_node_image {
+	/* Note that calc_node_hash() depends on hash first in struct */
+	uint8_t hash[TEE_FS_HTREE_HASH_SIZE];
+	uint8_t iv[TEE_FS_HTREE_IV_SIZE];
+	uint8_t tag[TEE_FS_HTREE_TAG_SIZE];
+	uint16_t flags;
+};
+
+/*
+ * This struct is not interpreted by the hash tree, it's up to the user of
+ * the interface to update etc if needed.
+ */
+struct tee_fs_htree_meta {
+	uint64_t length;
+};
+
+/* Internal struct needed by struct tee_fs_htree_image */
+struct tee_fs_htree_imeta {
+	struct tee_fs_htree_meta meta;
+	uint32_t max_node_id;
+};
+
+/* Internal struct provided to let the rpc callbacks know the size if needed */
+struct tee_fs_htree_image {
+	uint8_t iv[TEE_FS_HTREE_IV_SIZE];
+	uint8_t tag[TEE_FS_HTREE_TAG_SIZE];
+	uint8_t enc_fek[TEE_FS_HTREE_FEK_SIZE];
+	uint8_t imeta[sizeof(struct tee_fs_htree_imeta)];
+	uint32_t counter;
+};
+
+/**
+ * enum tee_fs_htree_type - type of hash tree element
+ * @TEE_FS_HTREE_TYPE_HEAD: indicates a struct tee_fs_htree_image
+ * @TEE_FS_HTREE_TYPE_NODE: indicates a struct tee_fs_htree_node_image
+ * @TEE_FS_HTREE_TYPE_BLOCK: indicates a data block
+ */
+enum tee_fs_htree_type {
+	TEE_FS_HTREE_TYPE_HEAD,
+	TEE_FS_HTREE_TYPE_NODE,
+	TEE_FS_HTREE_TYPE_BLOCK,
+};
+
+struct tee_fs_rpc_operation;
+
+/**
+ * struct tee_fs_htree_storage - storage description supplied by user of
+ * this interface
+ * @block_size:		size of data blocks
+ * @rpc_read_init:	initialize a struct tee_fs_rpc_operation for an RPC read
+ *			operation
+ * @rpc_write_init:	initialize a struct tee_fs_rpc_operation for an RPC
+ *			write operation
+ *
+ * The @idx arguments starts counting from 0. The @vers arguments are either
+ * 0 or 1. The @data arguments is a pointer to a buffer in non-secure shared
+ * memory where the encrypted data is stored.
+ */
+struct tee_fs_htree_storage {
+	size_t block_size;
+	TEE_Result (*rpc_read_init)(void *aux, struct tee_fs_rpc_operation *op,
+				    enum tee_fs_htree_type type, size_t idx,
+				    uint8_t vers, void **data);
+	TEE_Result (*rpc_read_final)(struct tee_fs_rpc_operation *op,
+				     size_t *bytes);
+	TEE_Result (*rpc_write_init)(void *aux, struct tee_fs_rpc_operation *op,
+				     enum tee_fs_htree_type type, size_t idx,
+				     uint8_t vers, void **data);
+	TEE_Result (*rpc_write_final)(struct tee_fs_rpc_operation *op);
+};
+
+struct tee_fs_htree;
+
+/**
+ * tee_fs_htree_open() - opens/creates a hash tree
+ * @create:	true if a new hash tree is to be created, else the hash tree
+ *		is read in and verified
+ * @stor:	storage description
+ * @stor_aux:	auxilary pointer supplied to callbacks in struct
+ *		tee_fs_htree_storage
+ * @ht:		returned hash tree on success
+ */
+TEE_Result tee_fs_htree_open(bool create,
+			     const struct tee_fs_htree_storage *stor,
+			     void *stor_aux, struct tee_fs_htree **ht);
+/**
+ * tee_fs_htree_close() - close a hash tree
+ * @ht:		hash tree
+ */
+void tee_fs_htree_close(struct tee_fs_htree **ht);
+
+/**
+ * tee_fs_htree_get_meta() - get a pointer to associated struct
+ * tee_fs_htree_meta
+ * @ht:		hash tree
+ */
+struct tee_fs_htree_meta *tee_fs_htree_get_meta(struct tee_fs_htree *ht);
+
+/**
+ * tee_fs_htree_sync_to_storage() - synchronize hash tree to storage
+ * @ht:		hash tree
+ *
+ * Frees the hash tree and sets *ht to NULL on failure and returns an error code
+ */
+TEE_Result tee_fs_htree_sync_to_storage(struct tee_fs_htree **ht);
+
+/**
+ * tee_fs_htree_truncate() - truncate a hash tree
+ * @ht:		hash tree
+ * @block_num:	the number of nodes to truncate to
+ *
+ * Frees the hash tree and sets *ht to NULL on failure and returns an error code
+ */
+TEE_Result tee_fs_htree_truncate(struct tee_fs_htree **ht, size_t block_num);
+
+/**
+ * tee_fs_htree_write_block() - encrypt and write a data block to storage
+ * @ht:		hash tree
+ * @block_num:	block number
+ * @block:	pointer to a block of stor->block_size size
+ *
+ * Frees the hash tree and sets *ht to NULL on failure and returns an error code
+ */
+TEE_Result tee_fs_htree_write_block(struct tee_fs_htree **ht, size_t block_num,
+				    const void *block);
+/**
+ * tee_fs_htree_write_block() - read and decrypt a data block from storage
+ * @ht:		hash tree
+ * @block_num:	block number
+ * @block:	pointer to a block of stor->block_size size
+ *
+ * Frees the hash tree and sets *ht to NULL on failure and returns an error code
+ */
+TEE_Result tee_fs_htree_read_block(struct tee_fs_htree **ht, size_t block_num,
+				   void *block);
+
+#endif /*__TEE_FS_HTREE_H*/
diff --git a/core/arch/arm/tee/svc_cache.h b/core/include/tee/svc_cache.h
index d5d4972..d5d4972 100644
--- a/core/arch/arm/tee/svc_cache.h
+++ b/core/include/tee/svc_cache.h
diff --git a/core/include/tee/tee_cryp_utl.h b/core/include/tee/tee_cryp_utl.h
index 99304f5..b303a59 100644
--- a/core/include/tee/tee_cryp_utl.h
+++ b/core/include/tee/tee_cryp_utl.h
@@ -50,5 +50,10 @@ TEE_Result tee_aes_cbc_cts_update(void *cbc_ctx, void *ecb_ctx,
 
 TEE_Result tee_prng_add_entropy(const uint8_t *in, size_t len);
 void plat_prng_add_jitter_entropy(void);
+/*
+ * The _norpc version must not invoke Normal World, or infinite recursion
+ * may occur. As an exception however, using mutexes is allowed.
+ */
+void plat_prng_add_jitter_entropy_norpc(void);
 
 #endif
diff --git a/core/include/tee/tee_fs.h b/core/include/tee/tee_fs.h
index 299ef74..81253da 100644
--- a/core/include/tee/tee_fs.h
+++ b/core/include/tee/tee_fs.h
@@ -38,30 +38,31 @@ typedef int64_t tee_fs_off_t;
 typedef uint32_t tee_fs_mode_t;
 
 struct tee_fs_dirent {
-	char *d_name;
+	uint8_t oid[TEE_OBJECT_ID_MAX_LEN];
+	size_t oidlen;
 };
 
 struct tee_fs_dir;
 struct tee_file_handle;
+struct tee_pobj;
 
 /*
  * tee_fs implements a POSIX like secure file system with GP extension
  */
 struct tee_file_operations {
-	TEE_Result (*open)(const char *name, struct tee_file_handle **fh);
-	TEE_Result (*create)(const char *name, struct tee_file_handle **fh);
+	TEE_Result (*open)(struct tee_pobj *po, struct tee_file_handle **fh);
+	TEE_Result (*create)(struct tee_pobj *po, struct tee_file_handle **fh);
 	void (*close)(struct tee_file_handle **fh);
-	TEE_Result (*read)(struct tee_file_handle *fh, void *buf, size_t *len);
-	TEE_Result (*write)(struct tee_file_handle *fh, const void *buf,
-			    size_t len);
-	TEE_Result (*seek)(struct tee_file_handle *fh, int32_t offs,
-			   TEE_Whence whence, int32_t *new_offs);
-	TEE_Result (*rename)(const char *old_name, const char *new_name,
+	TEE_Result (*read)(struct tee_file_handle *fh, size_t pos,
+			   void *buf, size_t *len);
+	TEE_Result (*write)(struct tee_file_handle *fh, size_t pos,
+			    const void *buf, size_t len);
+	TEE_Result (*rename)(struct tee_pobj *old_po, struct tee_pobj *new_po,
 			     bool overwrite);
-	TEE_Result (*remove)(const char *name);
+	TEE_Result (*remove)(struct tee_pobj *po);
 	TEE_Result (*truncate)(struct tee_file_handle *fh, size_t size);
 
-	TEE_Result (*opendir)(const char *name, struct tee_fs_dir **d);
+	TEE_Result (*opendir)(const TEE_UUID *uuid, struct tee_fs_dir **d);
 	TEE_Result (*readdir)(struct tee_fs_dir *d, struct tee_fs_dirent **ent);
 	void (*closedir)(struct tee_fs_dir *d);
 };
diff --git a/core/include/tee/tee_fs_key_manager.h b/core/include/tee/tee_fs_key_manager.h
index 7f26d26..b7259aa 100644
--- a/core/include/tee/tee_fs_key_manager.h
+++ b/core/include/tee/tee_fs_key_manager.h
@@ -33,62 +33,17 @@
 
 #define TEE_FS_KM_CHIP_ID_LENGTH    32
 #define TEE_FS_KM_HMAC_ALG          TEE_ALG_HMAC_SHA256
-#define TEE_FS_KM_AUTH_ENC_ALG      TEE_ALG_AES_GCM
 #define TEE_FS_KM_ENC_FEK_ALG       TEE_ALG_AES_ECB_NOPAD
 #define TEE_FS_KM_SSK_SIZE          TEE_SHA256_HASH_SIZE
 #define TEE_FS_KM_TSK_SIZE          TEE_SHA256_HASH_SIZE
 #define TEE_FS_KM_FEK_SIZE          16  /* bytes */
-#define TEE_FS_KM_IV_LEN            12  /* bytes */
-#define TEE_FS_KM_MAX_TAG_LEN       16  /* bytes */
 
-
-#define BLOCK_FILE_SHIFT	12
-
-#define BLOCK_FILE_SIZE		(1 << BLOCK_FILE_SHIFT)
-
-#define NUM_BLOCKS_PER_FILE	1024
-
-enum tee_fs_file_type {
-	META_FILE,
-	BLOCK_FILE
-};
-
-struct tee_fs_file_info {
-	uint64_t length;
-	uint32_t backup_version_table[NUM_BLOCKS_PER_FILE / 32];
-};
-
-struct tee_fs_file_meta {
-	struct tee_fs_file_info info;
-	uint8_t encrypted_fek[TEE_FS_KM_FEK_SIZE];
-	uint32_t counter;
-};
-
-struct common_header {
-	uint8_t iv[TEE_FS_KM_IV_LEN];
-	uint8_t tag[TEE_FS_KM_MAX_TAG_LEN];
-};
-
-struct meta_header {
-	uint8_t encrypted_key[TEE_FS_KM_FEK_SIZE];
-	struct common_header common;
-};
-
-struct block_header {
-	struct common_header common;
-};
-
-size_t tee_fs_get_header_size(enum tee_fs_file_type type);
 TEE_Result tee_fs_generate_fek(uint8_t *encrypted_fek, int fek_size);
-TEE_Result tee_fs_encrypt_file(enum tee_fs_file_type file_type,
-		const uint8_t *plaintext, size_t plaintext_size,
-		uint8_t *ciphertext, size_t *ciphertext_size,
-		const uint8_t *encrypted_fek);
-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 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 tee_fs_fek_crypt(TEE_OperationMode mode, const uint8_t *in_key,
+			    size_t size, uint8_t *out_key);
+
 #endif
diff --git a/core/include/tee/tee_fs_rpc.h b/core/include/tee/tee_fs_rpc.h
index 4d73c4b..43c7631 100644
--- a/core/include/tee/tee_fs_rpc.h
+++ b/core/include/tee/tee_fs_rpc.h
@@ -44,8 +44,8 @@ struct tee_fs_rpc_operation {
 	size_t num_params;
 };
 
-TEE_Result tee_fs_rpc_open(uint32_t id, const char *fname, int *fd);
-TEE_Result tee_fs_rpc_create(uint32_t id, const char *fname, int *fd);
+TEE_Result tee_fs_rpc_open(uint32_t id, struct tee_pobj *po, int *fd);
+TEE_Result tee_fs_rpc_create(uint32_t id, struct tee_pobj *po, int *fd);
 TEE_Result tee_fs_rpc_close(uint32_t id, int fd);
 
 TEE_Result tee_fs_rpc_read_init(struct tee_fs_rpc_operation *op,
@@ -61,11 +61,11 @@ TEE_Result tee_fs_rpc_write_final(struct tee_fs_rpc_operation *op);
 
 
 TEE_Result tee_fs_rpc_truncate(uint32_t id, int fd, size_t len);
-TEE_Result tee_fs_rpc_remove(uint32_t id, const char *fname);
-TEE_Result tee_fs_rpc_rename(uint32_t id, const char *old_fname,
-			     const char *new_fname, bool overwrite);
+TEE_Result tee_fs_rpc_remove(uint32_t id, struct tee_pobj *po);
+TEE_Result tee_fs_rpc_rename(uint32_t id, struct tee_pobj *old,
+			     struct tee_pobj *new, bool overwrite);
 
-TEE_Result tee_fs_rpc_opendir(uint32_t id, const char *name,
+TEE_Result tee_fs_rpc_opendir(uint32_t id, const TEE_UUID *uuid,
 				  struct tee_fs_dir **d);
 TEE_Result tee_fs_rpc_closedir(uint32_t id, struct tee_fs_dir *d);
 TEE_Result tee_fs_rpc_readdir(uint32_t id, struct tee_fs_dir *d,
diff --git a/core/include/tee/tee_obj.h b/core/include/tee/tee_obj.h
index bb82c9c..2224233 100644
--- a/core/include/tee/tee_obj.h
+++ b/core/include/tee/tee_obj.h
@@ -40,9 +40,9 @@ struct tee_obj {
 	bool busy;		/* true if used by an operation */
 	uint32_t have_attrs;	/* bitfield identifying set properties */
 	void *attr;
+	size_t ds_pos;
 	struct tee_pobj *pobj;	/* ptr to persistant object */
 	struct tee_file_handle *fh;
-	uint32_t ds_size;	/* data stream size */
 	uint32_t flags;		/* permission flags for persistent objects */
 };
 
diff --git a/core/include/tee/tee_pobj.h b/core/include/tee/tee_pobj.h
index db7c9a6..29b7f77 100644
--- a/core/include/tee/tee_pobj.h
+++ b/core/include/tee/tee_pobj.h
@@ -40,12 +40,14 @@ struct tee_pobj {
 	void *obj_id;
 	uint32_t obj_id_len;
 	uint32_t flags;
+	bool temporary;
 	/* Filesystem handling this object */
 	const struct tee_file_operations *fops;
 };
 
 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,
+			uint32_t flags, bool temporary,
+			const struct tee_file_operations *fops,
 			struct tee_pobj **obj);
 
 TEE_Result tee_pobj_release(struct tee_pobj *obj);
diff --git a/core/include/tee/tee_svc_storage.h b/core/include/tee/tee_svc_storage.h
index 7e4c10e..d3f0858 100644
--- a/core/include/tee/tee_svc_storage.h
+++ b/core/include/tee/tee_svc_storage.h
@@ -82,11 +82,11 @@ void tee_svc_storage_close_all_enum(struct user_ta_ctx *utc);
 
 void tee_svc_storage_init(void);
 
-char *tee_svc_storage_create_filename(struct tee_ta_session *sess,
-				      void *object_id,
-				      uint32_t object_id_len,
-				      bool transient);
+struct tee_pobj;
+TEE_Result tee_svc_storage_create_filename(void *buf, size_t blen,
+					   struct tee_pobj *po, bool transient);
 
-char *tee_svc_storage_create_dirname(struct tee_ta_session *sess);
+TEE_Result tee_svc_storage_create_dirname(void *buf, size_t blen,
+					  const TEE_UUID *uuid);
 
 #endif /* TEE_SVC_STORAGE_H */
diff --git a/core/arch/arm/plat-sunxi/console.c b/core/kernel/console.c
index b985316..2fce361 100644
--- a/core/arch/arm/plat-sunxi/console.c
+++ b/core/kernel/console.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014, Allwinner Technology Co., Ltd.
+ * Copyright (c) 2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -25,35 +25,32 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <platform_config.h>
-#include <drivers/sunxi_uart.h>
-#include <mm/core_memprot.h>
 #include <console.h>
+#include <compiler.h>
+#include <drivers/serial.h>
+#include <stdlib.h>
 
-static vaddr_t console_base(void)
+static struct serial_chip *serial_console __early_bss;
+
+void __weak console_putc(int ch)
 {
-	static void *va;
+	if (!serial_console)
+		return;
 
-	if (cpu_mmu_enabled()) {
-		if (!va)
-			va = phys_to_virt(CONSOLE_UART_BASE, MEM_AREA_IO_SEC);
-		return (vaddr_t)va;
-	}
-	return CONSOLE_UART_BASE;
+	if (ch == '\n')
+		serial_console->ops->putc(serial_console, '\r');
+	serial_console->ops->putc(serial_console, ch);
 }
 
-
-void console_init(void)
+void __weak console_flush(void)
 {
-	sunxi_uart_init(console_base());
-}
+	if (!serial_console)
+		return;
 
-void console_putc(int ch)
-{
-	sunxi_uart_putc(ch, console_base());
+	serial_console->ops->flush(serial_console);
 }
 
-void console_flush(void)
+void register_serial_console(struct serial_chip *chip)
 {
-	sunxi_uart_flush(console_base());
+	serial_console = chip;
 }
diff --git a/core/kernel/sub.mk b/core/kernel/sub.mk
index aa00ae5..963e078 100644
--- a/core/kernel/sub.mk
+++ b/core/kernel/sub.mk
@@ -1,4 +1,5 @@
 srcs-y += assert.c
+srcs-y += console.c
 srcs-y += tee_ta_manager.c
 srcs-y += tee_misc.c
 srcs-y += panic.c
diff --git a/core/kernel/tee_ta_manager.c b/core/kernel/tee_ta_manager.c
index c0c4545..a3651e5 100644
--- a/core/kernel/tee_ta_manager.c
+++ b/core/kernel/tee_ta_manager.c
@@ -43,6 +43,7 @@
 #include <kernel/user_ta.h>
 #include <mm/core_mmu.h>
 #include <mm/core_memprot.h>
+#include <mm/mobj.h>
 #include <mm/tee_mmu.h>
 #include <tee/tee_svc_cryp.h>
 #include <tee/tee_obj.h>
@@ -277,6 +278,58 @@ static TEE_Result check_client(struct tee_ta_session *s, const TEE_Identity *id)
 	return TEE_SUCCESS;
 }
 
+/*
+ * Check if invocation parameters matches TA properties
+ *
+ * @s - current session handle
+ * @param - already identified memory references hold a valid 'mobj'.
+ *
+ * Policy:
+ * - All TAs can access 'non-secure' shared memory.
+ * - All TAs can access TEE private memory (seccpy)
+ * - Only SDP flagged TAs can accept SDP memory references.
+ */
+#ifndef CFG_SECURE_DATA_PATH
+static bool check_params(struct tee_ta_session *sess __unused,
+			 struct tee_ta_param *param __unused)
+{
+	/*
+	 * When CFG_SECURE_DATA_PATH is not enabled, SDP memory references
+	 * are rejected at OP-TEE core entry. Hence here all TAs have same
+	 * permissions regarding memory reference parameters.
+	 */
+	return true;
+}
+#else
+static bool check_params(struct tee_ta_session *sess,
+			 struct tee_ta_param *param)
+{
+	int n;
+
+	/*
+	 * When CFG_SECURE_DATA_PATH is enabled, OP-TEE entry allows SHM and
+	 * SDP memory references. Only TAs flagged SDP can access SDP memory.
+	 */
+	if (sess->ctx->flags & TA_FLAG_SECURE_DATA_PATH)
+		return true;
+
+	for (n = 0; n < TEE_NUM_PARAMS; n++) {
+		uint32_t param_type = TEE_PARAM_TYPE_GET(param->types, n);
+		struct param_mem *mem = &param->u[n].mem;
+
+		if (param_type != TEE_PARAM_TYPE_MEMREF_INPUT &&
+		    param_type != TEE_PARAM_TYPE_MEMREF_OUTPUT &&
+		    param_type != TEE_PARAM_TYPE_MEMREF_INOUT)
+			continue;
+		if (!mem->size)
+			continue;
+		if (mobj_is_sdp_mem(mem->mobj))
+			return false;
+	}
+	return true;
+}
+#endif
+
 static void set_invoke_timeout(struct tee_ta_session *sess,
 				      uint32_t cancel_req_to)
 {
@@ -481,6 +534,9 @@ TEE_Result tee_ta_open_session(TEE_ErrorOrigin *err,
 		return res;
 	}
 
+	if (!check_params(s, param))
+		return TEE_ERROR_BAD_PARAMETERS;
+
 	ctx = s->ctx;
 
 	if (ctx->panicked) {
@@ -536,6 +592,9 @@ TEE_Result tee_ta_invoke_command(TEE_ErrorOrigin *err,
 	if (check_client(sess, clnt_id) != TEE_SUCCESS)
 		return TEE_ERROR_BAD_PARAMETERS; /* intentional generic error */
 
+	if (!check_params(sess, param))
+		return TEE_ERROR_BAD_PARAMETERS;
+
 	if (sess->ctx->panicked) {
 		DMSG("   Panicked !");
 		*err = TEE_ORIGIN_TEE;
diff --git a/core/lib/libtomcrypt/src/tee_ltc_provider.c b/core/lib/libtomcrypt/src/tee_ltc_provider.c
index 06dc983..a89640b 100644
--- a/core/lib/libtomcrypt/src/tee_ltc_provider.c
+++ b/core/lib/libtomcrypt/src/tee_ltc_provider.c
@@ -171,7 +171,7 @@ static TEE_Result tee_ltc_prng_init(struct tee_ltc_prng *prng)
 
 	prng->index = prng_index;
 
-	plat_prng_add_jitter_entropy();
+	plat_prng_add_jitter_entropy_norpc();
 
 	return  TEE_SUCCESS;
 }
diff --git a/core/tee/fs_htree.c b/core/tee/fs_htree.c
new file mode 100644
index 0000000..53cb81e
--- /dev/null
+++ b/core/tee/fs_htree.c
@@ -0,0 +1,937 @@
+/*
+ * Copyright (c) 2017, 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 <initcall.h>
+#include <kernel/tee_common_otp.h>
+#include <optee_msg_supplicant.h>
+#include <stdlib.h>
+#include <string_ext.h>
+#include <string.h>
+#include <tee/fs_htree.h>
+#include <tee/tee_cryp_provider.h>
+#include <tee/tee_fs_key_manager.h>
+#include <tee/tee_fs_rpc.h>
+#include <utee_defines.h>
+#include <util.h>
+
+#define TEE_FS_HTREE_CHIP_ID_SIZE	32
+#define TEE_FS_HTREE_HASH_ALG		TEE_ALG_SHA256
+#define TEE_FS_HTREE_TSK_SIZE		TEE_FS_HTREE_HASH_SIZE
+#define TEE_FS_HTREE_ENC_ALG		TEE_ALG_AES_ECB_NOPAD
+#define TEE_FS_HTREE_ENC_SIZE		TEE_AES_BLOCK_SIZE
+#define TEE_FS_HTREE_SSK_SIZE		TEE_FS_HTREE_HASH_SIZE
+
+#define TEE_FS_HTREE_AUTH_ENC_ALG	TEE_ALG_AES_GCM
+#define TEE_FS_HTREE_HMAC_ALG		TEE_ALG_HMAC_SHA256
+
+#define BLOCK_NUM_TO_NODE_ID(num)	((num) + 1)
+
+#define NODE_ID_TO_BLOCK_NUM(id)	((id) - 1)
+
+/*
+ * The hash tree is implemented as a binary tree with the purpose to ensure
+ * integrity of the data in the nodes. The data in the nodes their turn
+ * provides both integrity and confidentiality of the data blocks.
+ *
+ * The hash tree is saved in a file as:
+ * +----------------------------+
+ * | htree_image.0		|
+ * | htree_image.1		|
+ * +----------------------------+
+ * | htree_node_image.1.0	|
+ * | htree_node_image.1.1	|
+ * +----------------------------+
+ * | htree_node_image.2.0	|
+ * | htree_node_image.2.1	|
+ * +----------------------------+
+ * | htree_node_image.3.0	|
+ * | htree_node_image.3.1	|
+ * +----------------------------+
+ * | htree_node_image.4.0	|
+ * | htree_node_image.4.1	|
+ * +----------------------------+
+ * ...
+ *
+ * htree_image is the header of the file, there's two instances of it. One
+ * which is committed and the other is used when updating the file. Which
+ * is committed is indicated by the "counter" field, the one with the
+ * largest value is selected.
+ *
+ * htree_node_image is a node in the hash tree, each node has two instances
+ * which is committed is decided by the parent node .flag bit
+ * HTREE_NODE_COMMITTED_CHILD. Which version is the committed version of
+ * node 1 is determined by the by the lowest bit of the counter field in
+ * the header.
+ *
+ * Note that nodes start counting at 1 while blocks at 0, this means that
+ * block 0 is represented by node 1.
+ *
+ * Where different elements are stored in the file is managed by the file
+ * system. In the case of SQL FS the version of the node/block is ignored
+ * as the atomic update is finalized with a call to
+ * tee_fs_rpc_end_transaction().
+ */
+
+#define HTREE_NODE_COMMITTED_BLOCK	BIT32(0)
+/* n is 0 or 1 */
+#define HTREE_NODE_COMMITTED_CHILD(n)	BIT32(1 + (n))
+
+struct htree_node {
+	size_t id;
+	bool dirty;
+	bool block_updated;
+	struct tee_fs_htree_node_image node;
+	struct htree_node *parent;
+	struct htree_node *child[2];
+};
+
+struct tee_fs_htree {
+	struct htree_node root;
+	struct tee_fs_htree_image head;
+	uint8_t fek[TEE_FS_HTREE_FEK_SIZE];
+	struct tee_fs_htree_imeta imeta;
+	bool dirty;
+	const struct tee_fs_htree_storage *stor;
+	void *stor_aux;
+};
+
+struct traverse_arg;
+typedef TEE_Result (*traverse_cb_t)(struct traverse_arg *targ,
+				    struct htree_node *node);
+struct traverse_arg {
+	struct tee_fs_htree *ht;
+	traverse_cb_t cb;
+	void *arg;
+};
+
+static TEE_Result rpc_read(struct tee_fs_htree *ht, enum tee_fs_htree_type type,
+			   size_t idx, size_t vers, void *data, size_t dlen)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	size_t bytes;
+	void *p;
+
+	res = ht->stor->rpc_read_init(ht->stor_aux, &op, type, idx, vers, &p);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = ht->stor->rpc_read_final(&op, &bytes);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (bytes != dlen)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	memcpy(data, p, dlen);
+	return TEE_SUCCESS;
+}
+
+static TEE_Result rpc_read_head(struct tee_fs_htree *ht, size_t vers,
+				struct tee_fs_htree_image *head)
+{
+	return rpc_read(ht, TEE_FS_HTREE_TYPE_HEAD, 0, vers,
+			head, sizeof(*head));
+}
+
+static TEE_Result rpc_read_node(struct tee_fs_htree *ht, size_t node_id,
+				size_t vers,
+				struct tee_fs_htree_node_image *node)
+{
+	return rpc_read(ht, TEE_FS_HTREE_TYPE_NODE, node_id - 1, vers,
+			node, sizeof(*node));
+}
+
+static TEE_Result rpc_write(struct tee_fs_htree *ht,
+			    enum tee_fs_htree_type type, size_t idx,
+			    size_t vers, const void *data, size_t dlen)
+{
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	void *p;
+
+	res = ht->stor->rpc_write_init(ht->stor_aux, &op, type, idx, vers, &p);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	memcpy(p, data, dlen);
+	return ht->stor->rpc_write_final(&op);
+}
+
+static TEE_Result rpc_write_head(struct tee_fs_htree *ht, size_t vers,
+				 const struct tee_fs_htree_image *head)
+{
+	return rpc_write(ht, TEE_FS_HTREE_TYPE_HEAD, 0, vers,
+			 head, sizeof(*head));
+}
+
+static TEE_Result rpc_write_node(struct tee_fs_htree *ht, size_t node_id,
+				 size_t vers,
+				 const struct tee_fs_htree_node_image *node)
+{
+	return rpc_write(ht, TEE_FS_HTREE_TYPE_NODE, node_id - 1, vers,
+			 node, sizeof(*node));
+}
+
+static TEE_Result traverse_post_order(struct traverse_arg *targ,
+				      struct htree_node *node)
+{
+	TEE_Result res;
+
+	/*
+	 * This function is recursing but not very deep, only with Log(N)
+	 * maximum depth.
+	 */
+
+	if (!node)
+		return TEE_SUCCESS;
+
+	res = traverse_post_order(targ, node->child[0]);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = traverse_post_order(targ, node->child[1]);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return targ->cb(targ, node);
+}
+
+static TEE_Result htree_traverse_post_order(struct tee_fs_htree *ht,
+					    traverse_cb_t cb, void *arg)
+{
+	struct traverse_arg targ = { ht, cb, arg };
+
+	return traverse_post_order(&targ, &ht->root);
+}
+
+static size_t node_id_to_level(size_t node_id)
+{
+	assert(node_id && node_id < UINT_MAX);
+	/* Calculate level of the node, root node (1) has level 1 */
+	return sizeof(unsigned int) * 8 - __builtin_clz(node_id);
+}
+
+static struct htree_node *find_closest_node(struct tee_fs_htree *ht,
+					    size_t node_id)
+{
+	struct htree_node *node = &ht->root;
+	size_t level = node_id_to_level(node_id);
+	size_t n;
+
+	/* n = 1 because root node is level 1 */
+	for (n = 1; n < level; n++) {
+		struct htree_node *child;
+		size_t bit_idx;
+
+		/*
+		 * The difference between levels of the current node and
+		 * the node we're looking for tells which bit decides
+		 * direction in the tree.
+		 *
+		 * As the first bit has index 0 we'll subtract 1
+		 */
+		bit_idx = level - n - 1;
+		child = node->child[((node_id >> bit_idx) & 1)];
+		if (!child)
+			return node;
+		node = child;
+	}
+
+	return node;
+}
+
+static struct htree_node *find_node(struct tee_fs_htree *ht, size_t node_id)
+{
+	struct htree_node *node = find_closest_node(ht, node_id);
+
+	if (node && node->id == node_id)
+		return node;
+	return NULL;
+}
+
+static TEE_Result get_node(struct tee_fs_htree *ht, bool create,
+			   size_t node_id, struct htree_node **node_ret)
+{
+	struct htree_node *node;
+	struct htree_node *nc;
+	size_t n;
+
+	node = find_closest_node(ht, node_id);
+	if (!node)
+		return TEE_ERROR_GENERIC;
+	if (node->id == node_id)
+		goto ret_node;
+
+	/*
+	 * Trying to read beyond end of file should be caught earlier than
+	 * here.
+	 */
+	if (!create)
+		return TEE_ERROR_GENERIC;
+
+	/*
+	 * Add missing nodes, some nodes may already be there. When we've
+	 * processed the range all nodes up to node_id will be in the tree.
+	 */
+	for (n = node->id + 1; n <= node_id; n++) {
+		node = find_closest_node(ht, n);
+		if (node->id == n)
+			continue;
+		/* Node id n should be a child of node */
+		assert((n >> 1) == node->id);
+		assert(!node->child[n & 1]);
+
+		nc = calloc(1, sizeof(*nc));
+		if (!nc)
+			return TEE_ERROR_OUT_OF_MEMORY;
+		nc->id = n;
+		nc->parent = node;
+		node->child[n & 1] = nc;
+		node = nc;
+	}
+
+	if (node->id > ht->imeta.max_node_id)
+		ht->imeta.max_node_id = node->id;
+
+ret_node:
+	*node_ret = node;
+	return TEE_SUCCESS;
+}
+
+static int get_idx_from_counter(uint32_t counter0, uint32_t counter1)
+{
+	if (!(counter0 & 1)) {
+		if (!(counter1 & 1))
+			return 0;
+		if (counter0 > counter1)
+			return 0;
+		else
+			return 1;
+	}
+
+	if (counter1 & 1)
+		return 1;
+	else
+		return -1;
+}
+
+static TEE_Result init_head_from_data(struct tee_fs_htree *ht)
+{
+	TEE_Result res;
+	struct tee_fs_htree_image head[2];
+	int idx;
+
+	for (idx = 0; idx < 2; idx++) {
+		res = rpc_read_head(ht, idx, head + idx);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	idx = get_idx_from_counter(head[0].counter, head[1].counter);
+	if (idx < 0)
+		return TEE_ERROR_SECURITY;
+
+	res = rpc_read_node(ht, 1, idx, &ht->root.node);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ht->head = head[idx];
+	ht->root.id = 1;
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result init_tree_from_data(struct tee_fs_htree *ht)
+{
+	TEE_Result res;
+	struct tee_fs_htree_node_image node_image;
+	struct htree_node *node;
+	struct htree_node *nc;
+	size_t committed_version;
+	size_t node_id = 2;
+
+	while (node_id <= ht->imeta.max_node_id) {
+		node = find_node(ht, node_id >> 1);
+		if (!node)
+			return TEE_ERROR_GENERIC;
+		committed_version = !!(node->node.flags &
+				    HTREE_NODE_COMMITTED_CHILD(node_id & 1));
+
+		res = rpc_read_node(ht, node_id, committed_version,
+				    &node_image);
+		if (res != TEE_SUCCESS)
+			return res;
+
+		res = get_node(ht, true, node_id, &nc);
+		if (res != TEE_SUCCESS)
+			return res;
+		nc->node = node_image;
+		node_id++;
+	}
+
+	return TEE_SUCCESS;
+}
+
+static TEE_Result calc_node_hash(struct htree_node *node, void *ctx,
+				 uint8_t *digest)
+{
+	TEE_Result res;
+	uint32_t alg = TEE_FS_HTREE_HASH_ALG;
+	uint8_t *ndata = (uint8_t *)&node->node + sizeof(node->node.hash);
+	size_t nsize = sizeof(node->node) - sizeof(node->node.hash);
+
+	res = crypto_ops.hash.init(ctx, alg);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = crypto_ops.hash.update(ctx, alg, ndata, nsize);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	if (node->child[0]) {
+		res = crypto_ops.hash.update(ctx, alg,
+					     node->child[0]->node.hash,
+					     sizeof(node->child[0]->node.hash));
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	if (node->child[1]) {
+		res = crypto_ops.hash.update(ctx, alg,
+					     node->child[1]->node.hash,
+					     sizeof(node->child[1]->node.hash));
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	return crypto_ops.hash.final(ctx, alg, digest, TEE_FS_HTREE_HASH_SIZE);
+}
+
+static TEE_Result authenc_init(void **ctx_ret, TEE_OperationMode mode,
+			       struct tee_fs_htree *ht,
+			       struct tee_fs_htree_node_image *ni,
+			       size_t payload_len)
+{
+	TEE_Result res = TEE_SUCCESS;
+	const uint32_t alg = TEE_FS_HTREE_AUTH_ENC_ALG;
+	uint8_t *ctx;
+	size_t ctx_size;
+	size_t aad_len = TEE_FS_HTREE_FEK_SIZE + TEE_FS_HTREE_IV_SIZE;
+	uint8_t *iv;
+
+	if (ni) {
+		iv = ni->iv;
+	} else {
+		iv = ht->head.iv;
+		aad_len += TEE_FS_HTREE_HASH_SIZE + sizeof(ht->head.counter);
+	}
+
+	if (mode == TEE_MODE_ENCRYPT) {
+		res = crypto_ops.prng.read(iv, TEE_FS_HTREE_IV_SIZE);
+		if (res != TEE_SUCCESS)
+			return res;
+	}
+
+	res = crypto_ops.authenc.get_ctx_size(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, alg, mode,
+				      ht->fek, TEE_FS_HTREE_FEK_SIZE,
+				      iv, TEE_FS_HTREE_IV_SIZE,
+				      TEE_FS_HTREE_TAG_SIZE, aad_len,
+				      payload_len);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	if (!ni) {
+		res = crypto_ops.authenc.update_aad(ctx, alg, mode,
+						    ht->root.node.hash,
+						    TEE_FS_HTREE_FEK_SIZE);
+		if (res != TEE_SUCCESS)
+			goto exit;
+
+		res = crypto_ops.authenc.update_aad(ctx, alg, mode,
+						    (void *)&ht->head.counter,
+						    sizeof(ht->head.counter));
+		if (res != TEE_SUCCESS)
+			goto exit;
+	}
+
+	res = crypto_ops.authenc.update_aad(ctx, alg, mode, ht->head.enc_fek,
+					    TEE_FS_HTREE_FEK_SIZE);
+	if (res != TEE_SUCCESS)
+		goto exit;
+
+	res = crypto_ops.authenc.update_aad(ctx, alg, mode, iv,
+					    TEE_FS_HTREE_IV_SIZE);
+
+exit:
+	if (res == TEE_SUCCESS)
+		*ctx_ret = ctx;
+	else
+		free(ctx);
+
+	return res;
+}
+
+static TEE_Result authenc_decrypt_final(void *ctx, const uint8_t *tag,
+					const void *crypt, size_t len,
+					void *plain)
+{
+	TEE_Result res;
+	size_t out_size = len;
+
+	res = crypto_ops.authenc.dec_final(ctx, TEE_FS_HTREE_AUTH_ENC_ALG,
+					   crypt, len, plain, &out_size,
+					   tag, TEE_FS_HTREE_TAG_SIZE);
+	crypto_ops.authenc.final(ctx, TEE_FS_HTREE_AUTH_ENC_ALG);
+	free(ctx);
+
+	if (res == TEE_SUCCESS && out_size != len)
+		return TEE_ERROR_GENERIC;
+	if (res == TEE_ERROR_MAC_INVALID)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	return res;
+}
+
+static TEE_Result authenc_encrypt_final(void *ctx, uint8_t *tag,
+					const void *plain, size_t len,
+					void *crypt)
+{
+	TEE_Result res;
+	size_t out_size = len;
+	size_t out_tag_size = TEE_FS_HTREE_TAG_SIZE;
+
+	res = crypto_ops.authenc.enc_final(ctx, TEE_FS_HTREE_AUTH_ENC_ALG,
+					   plain, len, crypt, &out_size,
+					   tag, &out_tag_size);
+	crypto_ops.authenc.final(ctx, TEE_FS_HTREE_AUTH_ENC_ALG);
+	free(ctx);
+
+	if (res == TEE_SUCCESS &&
+	    (out_size != len || out_tag_size != TEE_FS_HTREE_TAG_SIZE))
+		return TEE_ERROR_GENERIC;
+
+	return res;
+}
+
+static TEE_Result verify_root(struct tee_fs_htree *ht)
+{
+	TEE_Result res;
+	void *ctx;
+
+	res = tee_fs_fek_crypt(TEE_MODE_DECRYPT, ht->head.enc_fek,
+			       sizeof(ht->fek), ht->fek);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	res = authenc_init(&ctx, TEE_MODE_DECRYPT, ht, NULL, sizeof(ht->imeta));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return authenc_decrypt_final(ctx, ht->head.tag, ht->head.imeta,
+				     sizeof(ht->imeta), &ht->imeta);
+}
+
+static TEE_Result verify_node(struct traverse_arg *targ,
+			      struct htree_node *node)
+{
+	void *ctx = targ->arg;
+	TEE_Result res;
+	uint8_t digest[TEE_FS_HTREE_HASH_SIZE];
+
+	res = calc_node_hash(node, ctx, digest);
+	if (res == TEE_SUCCESS &&
+	    buf_compare_ct(digest, node->node.hash, sizeof(digest)))
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	return res;
+}
+
+static TEE_Result verify_tree(struct tee_fs_htree *ht)
+{
+	TEE_Result res;
+	size_t size;
+	void *ctx;
+
+	if (!crypto_ops.hash.get_ctx_size || !crypto_ops.hash.init ||
+	    !crypto_ops.hash.update || !crypto_ops.hash.final)
+		return TEE_ERROR_NOT_SUPPORTED;
+
+	res = crypto_ops.hash.get_ctx_size(TEE_FS_HTREE_HASH_ALG, &size);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	ctx = malloc(size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = htree_traverse_post_order(ht, verify_node, ctx);
+	free(ctx);
+
+	return res;
+}
+
+static TEE_Result init_root_node(struct tee_fs_htree *ht)
+{
+	TEE_Result res;
+	size_t size;
+	void *ctx;
+
+	res = crypto_ops.hash.get_ctx_size(TEE_FS_HTREE_HASH_ALG, &size);
+	if (res != TEE_SUCCESS)
+		return res;
+	ctx = malloc(size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ht->root.id = 1;
+
+	res = calc_node_hash(&ht->root, ctx, ht->root.node.hash);
+	free(ctx);
+
+	return res;
+}
+
+TEE_Result tee_fs_htree_open(bool create,
+			     const struct tee_fs_htree_storage *stor,
+			     void *stor_aux, struct tee_fs_htree **ht_ret)
+{
+	TEE_Result res;
+	struct tee_fs_htree *ht = calloc(1, sizeof(*ht));
+
+	if (!ht)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	ht->stor = stor;
+	ht->stor_aux = stor_aux;
+
+	if (create) {
+		const struct tee_fs_htree_image dummy_head = { .counter = 0 };
+
+		res = crypto_ops.prng.read(ht->fek, sizeof(ht->fek));
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = tee_fs_fek_crypt(TEE_MODE_ENCRYPT, ht->fek,
+				       sizeof(ht->fek), ht->head.enc_fek);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = init_root_node(ht);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		ht->dirty = true;
+		res = tee_fs_htree_sync_to_storage(&ht);
+		if (res != TEE_SUCCESS)
+			goto out;
+		res = rpc_write_head(ht, 0, &dummy_head);
+	} else {
+		res = init_head_from_data(ht);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = verify_root(ht);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = init_tree_from_data(ht);
+		if (res != TEE_SUCCESS)
+			goto out;
+
+		res = verify_tree(ht);
+	}
+out:
+	if (res == TEE_SUCCESS)
+		*ht_ret = ht;
+	else
+		tee_fs_htree_close(&ht);
+	return res;
+}
+
+struct tee_fs_htree_meta *tee_fs_htree_get_meta(struct tee_fs_htree *ht)
+{
+	return &ht->imeta.meta;
+}
+
+static TEE_Result free_node(struct traverse_arg *targ __unused,
+			    struct htree_node *node)
+{
+	if (node->parent)
+		free(node);
+	return TEE_SUCCESS;
+}
+
+void tee_fs_htree_close(struct tee_fs_htree **ht)
+{
+	if (!*ht)
+		return;
+	htree_traverse_post_order(*ht, free_node, NULL);
+	free(*ht);
+	*ht = NULL;
+}
+
+static TEE_Result htree_sync_node_to_storage(struct traverse_arg *targ,
+					     struct htree_node *node)
+{
+	TEE_Result res;
+	uint8_t vers;
+
+	/*
+	 * The node can be dirty while the block isn't updated due to
+	 * updated children, but if block is updated the node has to be
+	 * dirty.
+	 */
+	assert(node->dirty >= node->block_updated);
+
+	if (!node->dirty)
+		return TEE_SUCCESS;
+
+	if (node->parent) {
+		uint32_t f = HTREE_NODE_COMMITTED_CHILD(node->id & 1);
+
+		node->parent->dirty = true;
+		node->parent->node.flags ^= f;
+		vers = !!(node->parent->node.flags & f);
+	} else {
+		/*
+		 * Counter isn't updated yet, it's increased just before
+		 * writing the header.
+		 */
+		vers = !(targ->ht->head.counter & 1);
+	}
+
+	res = calc_node_hash(node, targ->arg, node->node.hash);
+	if (res != TEE_SUCCESS)
+		return res;
+
+	node->dirty = false;
+	node->block_updated = false;
+
+	return rpc_write_node(targ->ht, node->id, vers, &node->node);
+}
+
+static TEE_Result update_root(struct tee_fs_htree *ht)
+{
+	TEE_Result res;
+	void *ctx;
+
+	ht->head.counter++;
+
+	res = authenc_init(&ctx, TEE_MODE_ENCRYPT, ht, NULL, sizeof(ht->imeta));
+	if (res != TEE_SUCCESS)
+		return res;
+
+	return authenc_encrypt_final(ctx, ht->head.tag, &ht->imeta,
+				     sizeof(ht->imeta), &ht->head.imeta);
+}
+
+TEE_Result tee_fs_htree_sync_to_storage(struct tee_fs_htree **ht_arg)
+{
+	TEE_Result res;
+	struct tee_fs_htree *ht = *ht_arg;
+	size_t size;
+	void *ctx;
+
+	if (!ht)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	if (!ht->dirty)
+		return TEE_SUCCESS;
+
+	res = crypto_ops.hash.get_ctx_size(TEE_FS_HTREE_HASH_ALG, &size);
+	if (res != TEE_SUCCESS)
+		return res;
+	ctx = malloc(size);
+	if (!ctx)
+		return TEE_ERROR_OUT_OF_MEMORY;
+
+	res = htree_traverse_post_order(ht, htree_sync_node_to_storage, ctx);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	/* All the nodes are written to storage now. Time to update root. */
+	res = update_root(ht);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = rpc_write_head(ht, ht->head.counter & 1, &ht->head);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	ht->dirty = false;
+out:
+	free(ctx);
+	if (res != TEE_SUCCESS)
+		tee_fs_htree_close(ht_arg);
+	return res;
+}
+
+static TEE_Result get_block_node(struct tee_fs_htree *ht, bool create,
+				 size_t block_num, struct htree_node **node)
+{
+	TEE_Result res;
+	struct htree_node *nd;
+
+	res = get_node(ht, create, BLOCK_NUM_TO_NODE_ID(block_num), &nd);
+	if (res == TEE_SUCCESS)
+		*node = nd;
+
+	return res;
+}
+
+TEE_Result tee_fs_htree_write_block(struct tee_fs_htree **ht_arg,
+				    size_t block_num, const void *block)
+{
+	struct tee_fs_htree *ht = *ht_arg;
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	struct htree_node *node = NULL;
+	uint8_t block_vers;
+	void *ctx;
+	void *enc_block;
+
+	if (!ht)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	res = get_block_node(ht, true, block_num, &node);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	if (!node->block_updated)
+		node->node.flags ^= HTREE_NODE_COMMITTED_BLOCK;
+
+	block_vers = !!(node->node.flags & HTREE_NODE_COMMITTED_BLOCK);
+	res = ht->stor->rpc_write_init(ht->stor_aux, &op,
+				       TEE_FS_HTREE_TYPE_BLOCK, block_num,
+				       block_vers, &enc_block);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = authenc_init(&ctx, TEE_MODE_ENCRYPT, ht, &node->node,
+			   ht->stor->block_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+	res = authenc_encrypt_final(ctx, node->node.tag, block,
+				    ht->stor->block_size, enc_block);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = ht->stor->rpc_write_final(&op);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	node->block_updated = true;
+	node->dirty = true;
+	ht->dirty = true;
+out:
+	if (res != TEE_SUCCESS)
+		tee_fs_htree_close(ht_arg);
+	return res;
+}
+
+TEE_Result tee_fs_htree_read_block(struct tee_fs_htree **ht_arg,
+				   size_t block_num, void *block)
+{
+	struct tee_fs_htree *ht = *ht_arg;
+	TEE_Result res;
+	struct tee_fs_rpc_operation op;
+	struct htree_node *node;
+	uint8_t block_vers;
+	size_t len;
+	void *ctx;
+	void *enc_block;
+
+	if (!ht)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	res = get_block_node(ht, false, block_num, &node);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	block_vers = !!(node->node.flags & HTREE_NODE_COMMITTED_BLOCK);
+	res = ht->stor->rpc_read_init(ht->stor_aux, &op,
+				      TEE_FS_HTREE_TYPE_BLOCK, block_num,
+				      block_vers, &enc_block);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = ht->stor->rpc_read_final(&op, &len);
+	if (res != TEE_SUCCESS)
+		goto out;
+	if (len != ht->stor->block_size) {
+		res = TEE_ERROR_CORRUPT_OBJECT;
+		goto out;
+	}
+
+	res = authenc_init(&ctx, TEE_MODE_DECRYPT, ht, &node->node,
+			   ht->stor->block_size);
+	if (res != TEE_SUCCESS)
+		goto out;
+
+	res = authenc_decrypt_final(ctx, node->node.tag, enc_block,
+				    ht->stor->block_size, block);
+out:
+	if (res != TEE_SUCCESS)
+		tee_fs_htree_close(ht_arg);
+	return res;
+}
+
+TEE_Result tee_fs_htree_truncate(struct tee_fs_htree **ht_arg, size_t block_num)
+{
+	struct tee_fs_htree *ht = *ht_arg;
+	size_t node_id = BLOCK_NUM_TO_NODE_ID(block_num);
+	struct htree_node *node;
+
+	if (!ht)
+		return TEE_ERROR_CORRUPT_OBJECT;
+
+	while (node_id < ht->imeta.max_node_id) {
+		node = find_closest_node(ht, ht->imeta.max_node_id);
+		assert(node && node->id == ht->imeta.max_node_id);
+		assert(!node->child[0] && !node->child[1]);
+		assert(node->parent);
+		assert(node->parent->child[node->id & 1] == node);
+		node->parent->child[node->id & 1] = NULL;
+		free(node);
+		ht->imeta.max_node_id--;
+		ht->dirty = true;
+	}
+
+	return TEE_SUCCESS;
+}
diff --git a/core/tee/sub.mk b/core/tee/sub.mk
index 32f0f98..97c4b7a 100644
--- a/core/tee/sub.mk
+++ b/core/tee/sub.mk
@@ -31,6 +31,8 @@ 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 \
+       CFG_TEE_CORE_EMBED_INTERNAL_TESTS) += fs_htree.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
diff --git a/core/tee/tee_cryp_utl.c b/core/tee/tee_cryp_utl.c
index fa01161..b63e790 100644
--- a/core/tee/tee_cryp_utl.c
+++ b/core/tee/tee_cryp_utl.c
@@ -392,6 +392,13 @@ __weak void plat_prng_add_jitter_entropy(void)
 		tee_prng_add_entropy((uint8_t *)&current, sizeof(current));
 }
 
+__weak void plat_prng_add_jitter_entropy_norpc(void)
+{
+#ifndef CFG_SECURE_TIME_SOURCE_REE
+	plat_prng_add_jitter_entropy();
+#endif
+}
+
 static TEE_Result tee_cryp_init(void)
 {
 	if (crypto_ops.init)
diff --git a/core/tee/tee_fs_key_manager.c b/core/tee/tee_fs_key_manager.c
index c827cef..fa579c6 100644
--- a/core/tee/tee_fs_key_manager.c
+++ b/core/tee/tee_fs_key_manager.c
@@ -55,16 +55,6 @@ struct tee_fs_ssk {
 	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";
 
@@ -109,17 +99,17 @@ exit:
 	return res;
 }
 
-static TEE_Result fek_crypt(TEE_OperationMode mode,
-		uint8_t *key, int size)
+TEE_Result tee_fs_fek_crypt(TEE_OperationMode mode, const uint8_t *in_key,
+			    size_t size, uint8_t *out_key)
 {
 	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];
+	uint8_t dst_key[size];
 	struct tee_ta_session *sess;
 
-	if (!key)
+	if (!in_key || !out_key)
 		return TEE_ERROR_BAD_PARAMETERS;
 
 	if (size != TEE_FS_KM_FEK_SIZE)
@@ -151,13 +141,13 @@ static TEE_Result fek_crypt(TEE_OperationMode mode,
 		goto exit;
 
 	res = crypto_ops.cipher.update(ctx, TEE_FS_KM_ENC_FEK_ALG,
-			mode, true, key, size, dst_key);
+			mode, true, in_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));
+	memcpy(out_key, dst_key, sizeof(dst_key));
 
 exit:
 	free(ctx);
@@ -170,11 +160,6 @@ 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;
@@ -204,94 +189,6 @@ static TEE_Result tee_fs_init_key_manager(void)
 	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;
@@ -303,117 +200,7 @@ TEE_Result tee_fs_generate_fek(uint8_t *buf, int buf_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);
+	return tee_fs_fek_crypt(TEE_MODE_ENCRYPT, buf, TEE_FS_KM_FEK_SIZE, buf);
 }
 
 static TEE_Result sha256(uint8_t *out, size_t out_size, const uint8_t *in,
@@ -518,8 +305,8 @@ TEE_Result tee_fs_crypt_block(uint8_t *out, const uint8_t *in, size_t size,
 	     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);
+	res = tee_fs_fek_crypt(TEE_MODE_DECRYPT, encrypted_fek,
+			       TEE_FS_KM_FEK_SIZE, fek);
 	if (res != TEE_SUCCESS)
 		return res;
 
@@ -550,4 +337,3 @@ exit:
 }
 
 service_init_late(tee_fs_init_key_manager);
-
diff --git a/core/tee/tee_fs_rpc.c b/core/tee/tee_fs_rpc.c
index 5e1078a..3ce1ba8 100644
--- a/core/tee/tee_fs_rpc.c
+++ b/core/tee/tee_fs_rpc.c
@@ -26,14 +26,17 @@
  */
 
 #include <assert.h>
+#include <kernel/tee_misc.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 <string.h>
 #include <tee/tee_fs.h>
 #include <tee/tee_fs_rpc.h>
+#include <tee/tee_pobj.h>
+#include <tee/tee_svc_storage.h>
 #include <trace.h>
 #include <util.h>
 
@@ -48,16 +51,15 @@ static TEE_Result operation_commit(struct tee_fs_rpc_operation *op)
 }
 
 static TEE_Result operation_open(uint32_t id, unsigned int cmd,
-				 const char *fname, int *fd)
+				 struct tee_pobj *po, 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);
+	va = tee_fs_rpc_cache_alloc(TEE_FS_NAME_MAX, &pa, &cookie);
 	if (!va)
 		return TEE_ERROR_OUT_OF_MEMORY;
 
@@ -66,9 +68,12 @@ static TEE_Result operation_open(uint32_t id, unsigned int 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.size = TEE_FS_NAME_MAX;
 	op.params[1].u.tmem.shm_ref = cookie;
-	strlcpy(va, fname, fname_size);
+	res = tee_svc_storage_create_filename(va, TEE_FS_NAME_MAX,
+					      po, po->temporary);
+	if (res != TEE_SUCCESS)
+		return res;
 
 	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
 
@@ -79,14 +84,14 @@ static TEE_Result operation_open(uint32_t id, unsigned int cmd,
 	return res;
 }
 
-TEE_Result tee_fs_rpc_open(uint32_t id, const char *fname, int *fd)
+TEE_Result tee_fs_rpc_open(uint32_t id, struct tee_pobj *po, int *fd)
 {
-	return operation_open(id, OPTEE_MRF_OPEN, fname, fd);
+	return operation_open(id, OPTEE_MRF_OPEN, po, fd);
 }
 
-TEE_Result tee_fs_rpc_create(uint32_t id, const char *fname, int *fd)
+TEE_Result tee_fs_rpc_create(uint32_t id, struct tee_pobj *po, int *fd)
 {
-	return operation_open(id, OPTEE_MRF_CREATE, fname, fd);
+	return operation_open(id, OPTEE_MRF_CREATE, po, fd);
 }
 
 TEE_Result tee_fs_rpc_close(uint32_t id, int fd)
@@ -196,15 +201,15 @@ TEE_Result tee_fs_rpc_truncate(uint32_t id, int fd, size_t len)
 	return operation_commit(&op);
 }
 
-TEE_Result tee_fs_rpc_remove(uint32_t id, const char *fname)
+TEE_Result tee_fs_rpc_remove(uint32_t id, struct tee_pobj *po)
 {
+	TEE_Result res;
 	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);
+	va = tee_fs_rpc_cache_alloc(TEE_FS_NAME_MAX, &pa, &cookie);
 	if (!va)
 		return TEE_ERROR_OUT_OF_MEMORY;
 
@@ -213,25 +218,27 @@ TEE_Result tee_fs_rpc_remove(uint32_t id, const char *fname)
 
 	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.size = TEE_FS_NAME_MAX;
 	op.params[1].u.tmem.shm_ref = cookie;
-	strlcpy(va, fname, name_len);
+	res = tee_svc_storage_create_filename(va, TEE_FS_NAME_MAX,
+					      po, po->temporary);
+	if (res != TEE_SUCCESS)
+		return res;
 
 	return operation_commit(&op);
 }
 
-TEE_Result tee_fs_rpc_rename(uint32_t id, const char *old_fname,
-			     const char *new_fname, bool overwrite)
+TEE_Result tee_fs_rpc_rename(uint32_t id, struct tee_pobj *old,
+			     struct tee_pobj *new, bool overwrite)
 {
+	TEE_Result res;
 	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;
+	bool temp;
 
-	va = tee_fs_rpc_cache_alloc(old_fname_size + new_fname_size,
-				    &pa, &cookie);
+	va = tee_fs_rpc_cache_alloc(TEE_FS_NAME_MAX * 2, &pa, &cookie);
 	if (!va)
 		return TEE_ERROR_OUT_OF_MEMORY;
 
@@ -241,20 +248,37 @@ TEE_Result tee_fs_rpc_rename(uint32_t id, const char *old_fname,
 
 	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.size = TEE_FS_NAME_MAX;
 	op.params[1].u.tmem.shm_ref = cookie;
-	strlcpy(va, old_fname, old_fname_size);
+	if (new)
+		temp = old->temporary;
+	else
+		temp = true;
+	res = tee_svc_storage_create_filename(va, TEE_FS_NAME_MAX,
+					      old, temp);
+	if (res != TEE_SUCCESS)
+		return res;
 
 	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.buf_ptr = pa + TEE_FS_NAME_MAX;
+	op.params[2].u.tmem.size = TEE_FS_NAME_MAX;
 	op.params[2].u.tmem.shm_ref = cookie;
-	strlcpy(va + old_fname_size, new_fname, new_fname_size);
+	if (new) {
+		res = tee_svc_storage_create_filename(va + TEE_FS_NAME_MAX,
+						      TEE_FS_NAME_MAX,
+						      new, new->temporary);
+	} else {
+		res = tee_svc_storage_create_filename(va + TEE_FS_NAME_MAX,
+						      TEE_FS_NAME_MAX,
+						      old, false);
+	}
+	if (res != TEE_SUCCESS)
+		return res;
 
 	return operation_commit(&op);
 }
 
-TEE_Result tee_fs_rpc_opendir(uint32_t id, const char *name,
+TEE_Result tee_fs_rpc_opendir(uint32_t id, const TEE_UUID *uuid,
 			      struct tee_fs_dir **d)
 {
 	TEE_Result res;
@@ -262,13 +286,12 @@ TEE_Result tee_fs_rpc_opendir(uint32_t id, const char *name,
 	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);
+	va = tee_fs_rpc_cache_alloc(TEE_FS_NAME_MAX, &pa, &cookie);
 	if (!va) {
 		res = TEE_ERROR_OUT_OF_MEMORY;
 		goto err_exit;
@@ -279,9 +302,11 @@ TEE_Result tee_fs_rpc_opendir(uint32_t id, const char *name,
 
 	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.size = TEE_FS_NAME_MAX;
 	op.params[1].u.tmem.shm_ref = cookie;
-	strlcpy(va, name, name_len);
+	res = tee_svc_storage_create_dirname(va, TEE_FS_NAME_MAX, uuid);
+	if (res != TEE_SUCCESS)
+		return res;
 
 	op.params[2].attr = OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT;
 
@@ -308,8 +333,6 @@ TEE_Result tee_fs_rpc_closedir(uint32_t id, struct tee_fs_dir *d)
 	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);
 }
@@ -344,9 +367,9 @@ TEE_Result tee_fs_rpc_readdir(uint32_t id, struct tee_fs_dir *d,
 	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)
+	d->d.oidlen = tee_hs2b(va, d->d.oid, strnlen(va, max_name_len),
+			       sizeof(d->d.oid));
+	if (!d->d.oidlen)
 		return TEE_ERROR_OUT_OF_MEMORY;
 
 	*ent = &d->d;
diff --git a/core/tee/tee_obj.c b/core/tee/tee_obj.c
index 78cf937..6dcfbf3 100644
--- a/core/tee/tee_obj.c
+++ b/core/tee/tee_obj.c
@@ -31,7 +31,6 @@
 #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>
@@ -79,32 +78,20 @@ void tee_obj_close_all(struct user_ta_ctx *utc)
 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);
+	res = fops->open(o->pobj, &fh);
 	if (res == TEE_ERROR_CORRUPT_OBJECT) {
 		EMSG("Object corrupt\n");
+		fops->remove(o->pobj);
 		tee_obj_close(to_user_ta_ctx(sess->ctx), o);
-		fops->remove(file);
 	}
 
-	free(file);
 	fops->close(&fh);
-exit:
 	return res;
 }
 
diff --git a/core/tee/tee_pobj.c b/core/tee/tee_pobj.c
index a7aee31..0f9fb46 100644
--- a/core/tee/tee_pobj.c
+++ b/core/tee/tee_pobj.c
@@ -80,7 +80,8 @@ static TEE_Result tee_pobj_check_access(uint32_t oflags, uint32_t nflags)
 }
 
 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,
+			uint32_t flags, bool temporary,
+			const struct tee_file_operations *fops,
 			struct tee_pobj **obj)
 {
 	struct tee_pobj *o;
@@ -100,10 +101,12 @@ TEE_Result tee_pobj_get(TEE_UUID *uuid, void *obj_id, uint32_t obj_id_len,
 	}
 
 	if (*obj) {
+		if (temporary != (*obj)->temporary) {
+			res = TEE_ERROR_ACCESS_CONFLICT;
+			goto out;
+		}
 		res = tee_pobj_check_access((*obj)->flags, flags);
-		if (res != TEE_SUCCESS)
-			*obj = NULL;
-		else
+		if (res == TEE_SUCCESS)
 			(*obj)->refcnt++;
 		goto out;
 	}
@@ -119,6 +122,7 @@ TEE_Result tee_pobj_get(TEE_UUID *uuid, void *obj_id, uint32_t obj_id_len,
 	memcpy(&o->uuid, uuid, sizeof(TEE_UUID));
 	o->flags = flags;
 	o->fops = fops;
+	o->temporary = temporary;
 
 	o->obj_id = malloc(obj_id_len);
 	if (o->obj_id == NULL) {
@@ -134,6 +138,8 @@ TEE_Result tee_pobj_get(TEE_UUID *uuid, void *obj_id, uint32_t obj_id_len,
 
 	res = TEE_SUCCESS;
 out:
+	if (res != TEE_SUCCESS)
+		*obj = NULL;
 	mutex_unlock(&pobjs_mutex);
 	return res;
 }
diff --git a/core/tee/tee_ree_fs.c b/core/tee/tee_ree_fs.c
index 7a82acb..544ed3e 100644
--- a/core/tee/tee_ree_fs.c
+++ b/core/tee/tee_ree_fs.c
@@ -26,125 +26,45 @@
  */
 
 #include <assert.h>
-#include <kernel/thread.h>
 #include <kernel/mutex.h>
 #include <kernel/panic.h>
+#include <kernel/thread.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 <string.h>
 #include <sys/queue.h>
+#include <tee/fs_htree.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;
+	struct tee_fs_htree *ht;
 	int fd;
 };
 
-static inline int pos_to_block_num(int position)
+static 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)
+static TEE_Result ree_fs_opendir_rpc(const TEE_UUID *uuid,
+				     struct tee_fs_dir **d)
 
 {
-	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_FS, name, d);
+	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_FS, uuid, d);
 }
 
 static void ree_fs_closedir_rpc(struct tee_fs_dir *d)
@@ -159,330 +79,187 @@ static TEE_Result ree_fs_readdir_rpc(struct tee_fs_dir *d,
 	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)
+static TEE_Result out_of_place_write(struct tee_fs_fd *fdp, size_t pos,
+				     const void *buf, size_t len)
 {
 	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;
-
+	size_t start_block_num = pos_to_block_num(pos);
+	size_t end_block_num = pos_to_block_num(pos + len - 1);
+	size_t remain_bytes = len;
+	uint8_t *data_ptr = (uint8_t *)buf;
+	uint8_t *block;
+	struct tee_fs_htree_meta *meta = tee_fs_htree_get_meta(fdp->ht);
 
-	res = tee_fs_rpc_write_init(&op, OPTEE_MSG_RPC_CMD_FS, fdp->fd,
-				    offs, ciphertext_size, &ciphertext);
-	if (res != TEE_SUCCESS)
-		return res;
+	block = malloc(BLOCK_SIZE);
+	if (!block)
+		return TEE_ERROR_OUT_OF_MEMORY;
 
-	res = tee_fs_encrypt_file(file_type, data_in, data_in_size,
-				  ciphertext, &ciphertext_size, encrypted_fek);
-	if (res != TEE_SUCCESS)
-		return res;
+	while (start_block_num <= end_block_num) {
+		size_t offset = pos % BLOCK_SIZE;
+		size_t size_to_write = MIN(remain_bytes, (size_t)BLOCK_SIZE);
 
-	return tee_fs_rpc_write_final(&op);
-}
+		if (size_to_write + offset > BLOCK_SIZE)
+			size_to_write = BLOCK_SIZE - offset;
 
-/*
- * 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;
+		if (start_block_num * BLOCK_SIZE <
+		    ROUNDUP(meta->length, BLOCK_SIZE)) {
+			res = tee_fs_htree_read_block(&fdp->ht,
+						      start_block_num, block);
+			if (res != TEE_SUCCESS)
+				goto exit;
+		} else {
+			memset(block, 0, BLOCK_SIZE);
+		}
 
-	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;
+		if (data_ptr)
+			memcpy(block + offset, data_ptr, size_to_write);
+		else
+			memset(block + offset, 0, size_to_write);
 
-	res = tee_fs_rpc_read_final(&op, &bytes);
-	if (res != TEE_SUCCESS)
-		return res;
+		res = tee_fs_htree_write_block(&fdp->ht, start_block_num,
+					       block);
+		if (res != TEE_SUCCESS)
+			goto exit;
 
-	if (!bytes) {
-		*data_out_size = 0;
-		return TEE_SUCCESS;
+		if (data_ptr)
+			data_ptr += size_to_write;
+		remain_bytes -= size_to_write;
+		start_block_num++;
+		pos += size_to_write;
 	}
 
-	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);
-}
+	if (pos > meta->length)
+		meta->length = pos;
 
-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);
+exit:
+	free(block);
+	return res;
 }
 
-static TEE_Result commit_meta_file(struct tee_fs_fd *fdp,
-				   struct tee_fs_file_meta *new_meta)
+static TEE_Result get_offs_size(enum tee_fs_htree_type type, size_t idx,
+				uint8_t vers, size_t *offs, size_t *size)
 {
-	TEE_Result res;
-
-	new_meta->counter = fdp->meta_counter + 1;
+	const size_t node_size = sizeof(struct tee_fs_htree_node_image);
+	const size_t block_nodes = BLOCK_SIZE / (node_size * 2);
+	size_t pbn;
+	size_t bidx;
 
-	res = write_meta_file(fdp, new_meta);
-	if (res != TEE_SUCCESS)
-		return res;
+	assert(vers == 0 || vers == 1);
 
 	/*
-	 * From now on the new meta is successfully committed,
-	 * change tee_fs_fd accordingly
+	 * File layout
+	 *
+	 * phys block 0:
+	 * tee_fs_htree_image vers 0 @ offs = 0
+	 * tee_fs_htree_image vers 1 @ offs = sizeof(tee_fs_htree_image)
+	 *
+	 * phys block 1:
+	 * tee_fs_htree_node_image 0  vers 0 @ offs = 0
+	 * tee_fs_htree_node_image 0  vers 1 @ offs = node_size
+	 * tee_fs_htree_node_image 1  vers 0 @ offs = node_size * 2
+	 * tee_fs_htree_node_image 1  vers 1 @ offs = node_size * 3
+	 * ...
+	 * tee_fs_htree_node_image 61 vers 0 @ offs = node_size * 122
+	 * tee_fs_htree_node_image 61 vers 1 @ offs = node_size * 123
+	 *
+	 * phys block 2:
+	 * data block 0 vers 0
+	 *
+	 * phys block 3:
+	 * data block 0 vers 1
+	 *
+	 * ...
+	 * phys block 63:
+	 * data block 61 vers 0
+	 *
+	 * phys block 64:
+	 * data block 61 vers 1
+	 *
+	 * phys block 65:
+	 * tee_fs_htree_node_image 62  vers 0 @ offs = 0
+	 * tee_fs_htree_node_image 62  vers 1 @ offs = node_size
+	 * tee_fs_htree_node_image 63  vers 0 @ offs = node_size * 2
+	 * tee_fs_htree_node_image 63  vers 1 @ offs = node_size * 3
+	 * ...
+	 * tee_fs_htree_node_image 121 vers 0 @ offs = node_size * 122
+	 * tee_fs_htree_node_image 121 vers 1 @ offs = node_size * 123
+	 *
+	 * ...
 	 */
-	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);
+	switch (type) {
+	case TEE_FS_HTREE_TYPE_HEAD:
+		*offs = sizeof(struct tee_fs_htree_image) * vers;
+		*size = sizeof(struct tee_fs_htree_image);
+		return TEE_SUCCESS;
+	case TEE_FS_HTREE_TYPE_NODE:
+		pbn = 1 + ((idx / block_nodes) * block_nodes * 2);
+		*offs = pbn * BLOCK_SIZE +
+			2 * node_size * (idx % block_nodes) +
+			node_size * vers;
+		*size = node_size;
+		return TEE_SUCCESS;
+	case TEE_FS_HTREE_TYPE_BLOCK:
+		bidx = 2 * idx + vers;
+		pbn = 2 + bidx + bidx / (block_nodes * 2 - 1);
+		*offs = pbn * BLOCK_SIZE;
+		*size = BLOCK_SIZE;
+		return TEE_SUCCESS;
+	default:
+		return TEE_ERROR_GENERIC;
+	}
 }
 
-static TEE_Result read_meta_counter(struct tee_fs_fd *fdp)
+static TEE_Result ree_fs_rpc_read_init(void *aux,
+				       struct tee_fs_rpc_operation *op,
+				       enum tee_fs_htree_type type, size_t idx,
+				       uint8_t vers, void **data)
 {
+	struct tee_fs_fd *fdp = aux;
 	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;
+	size_t offs;
+	size_t size;
 
-	res = tee_fs_rpc_read_final(&op, &bytes);
+	res = get_offs_size(type, idx, vers, &offs, &size);
 	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;
+	return tee_fs_rpc_read_init(op, OPTEE_MSG_RPC_CMD_FS, fdp->fd,
+				    offs, size, data);
 }
 
-static TEE_Result read_meta(struct tee_fs_fd *fdp, const char *fname)
+static TEE_Result ree_fs_rpc_write_init(void *aux,
+					struct tee_fs_rpc_operation *op,
+					enum tee_fs_htree_type type, size_t idx,
+					uint8_t vers, void **data)
 {
+	struct tee_fs_fd *fdp = aux;
 	TEE_Result res;
+	size_t offs;
+	size_t size;
 
-	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);
+	res = get_offs_size(type, idx, vers, &offs, &size);
 	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);
+	return tee_fs_rpc_write_init(op, OPTEE_MSG_RPC_CMD_FS, fdp->fd,
+				     offs, size, data);
 }
 
-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 const struct tee_fs_htree_storage ree_fs_storage_ops = {
+	.block_size = BLOCK_SIZE,
+	.rpc_read_init = ree_fs_rpc_read_init,
+	.rpc_read_final = tee_fs_rpc_read_final,
+	.rpc_write_init = ree_fs_rpc_write_init,
+	.rpc_write_final = tee_fs_rpc_write_final,
+};
 
-static TEE_Result open_internal(const char *file, bool create,
+static TEE_Result open_internal(struct tee_pobj *po, 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;
@@ -491,17 +268,22 @@ static TEE_Result open_internal(const char *file, bool create,
 	mutex_lock(&ree_fs_mutex);
 
 	if (create)
-		res = create_meta(fdp, file);
+		res = tee_fs_rpc_create(OPTEE_MSG_RPC_CMD_FS, po, &fdp->fd);
 	else
-		res = read_meta(fdp, file);
+		res = tee_fs_rpc_open(OPTEE_MSG_RPC_CMD_FS, po, &fdp->fd);
+
+	if (res != TEE_SUCCESS)
+		goto out;
 
+	res = tee_fs_htree_open(create, &ree_fs_storage_ops, fdp, &fdp->ht);
+out:
 	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);
+			tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, po);
 		free(fdp);
 	}
 
@@ -509,14 +291,15 @@ static TEE_Result open_internal(const char *file, bool create,
 	return res;
 }
 
-static TEE_Result ree_fs_open(const char *file, struct tee_file_handle **fh)
+static TEE_Result ree_fs_open(struct tee_pobj *po, struct tee_file_handle **fh)
 {
-	return open_internal(file, false, fh);
+	return open_internal(po, false, fh);
 }
 
-static TEE_Result ree_fs_create(const char *file, struct tee_file_handle **fh)
+static TEE_Result ree_fs_create(struct tee_pobj *po,
+				struct tee_file_handle **fh)
 {
-	return open_internal(file, true, fh);
+	return open_internal(po, true, fh);
 }
 
 static void ree_fs_close(struct tee_file_handle **fh)
@@ -524,106 +307,53 @@ static void ree_fs_close(struct tee_file_handle **fh)
 	struct tee_fs_fd *fdp = (struct tee_fs_fd *)*fh;
 
 	if (fdp) {
+		tee_fs_htree_close(&fdp->ht);
 		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;
+	struct tee_fs_htree_meta *meta = tee_fs_htree_get_meta(fdp->ht);
 
-	if (new_file_len > MAX_FILE_SIZE)
-		return TEE_ERROR_BAD_PARAMETERS;
+	if ((size_t)new_file_len > meta->length) {
+		size_t ext_len = new_file_len - meta->length;
 
-	new_meta = fdp->meta;
-	new_meta.info.length = new_file_len;
+		res = out_of_place_write(fdp, meta->length, NULL, ext_len);
+		if (res != TEE_SUCCESS)
+			return res;
+	} else {
+		size_t offs;
+		size_t sz;
+
+		res = get_offs_size(TEE_FS_HTREE_TYPE_BLOCK,
+				    ROUNDUP(new_file_len, BLOCK_SIZE) /
+					BLOCK_SIZE, 1, &offs, &sz);
+		if (res != TEE_SUCCESS)
+			return res;
 
-	if ((size_t)new_file_len > old_file_len) {
-		size_t ext_len = new_file_len - old_file_len;
-		int orig_pos = fdp->pos;
+		res = tee_fs_htree_truncate(&fdp->ht,
+					    new_file_len / BLOCK_SIZE);
+		if (res != TEE_SUCCESS)
+			return res;
 
-		fdp->pos = old_file_len;
-		res = out_of_place_write(fdp, NULL, ext_len, &new_meta);
-		fdp->pos = orig_pos;
+		res = tee_fs_rpc_truncate(OPTEE_MSG_RPC_CMD_FS, fdp->fd,
+					  offs + sz);
 		if (res != TEE_SUCCESS)
 			return res;
+
+		meta->length = new_file_len;
 	}
 
-	return commit_meta_file(fdp, &new_meta);
+	return tee_fs_htree_sync_to_storage(&fdp->ht);
 }
 
-static TEE_Result ree_fs_read(struct tee_file_handle *fh, void *buf,
-			      size_t *len)
+static TEE_Result ree_fs_read(struct tee_file_handle *fh, size_t pos,
+			      void *buf, size_t *len)
 {
 	TEE_Result res;
 	int start_block_num;
@@ -632,16 +362,15 @@ static TEE_Result ree_fs_read(struct tee_file_handle *fh, void *buf,
 	uint8_t *data_ptr = buf;
 	uint8_t *block = NULL;
 	struct tee_fs_fd *fdp = (struct tee_fs_fd *)fh;
+	struct tee_fs_htree_meta *meta = tee_fs_htree_get_meta(fdp->ht);
 
 	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)
+	if ((pos + remain_bytes) < remain_bytes || pos > meta->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;
+	else if (pos + remain_bytes > meta->length)
+		remain_bytes = meta->length - pos;
 
 	*len = remain_bytes;
 
@@ -650,8 +379,8 @@ static TEE_Result ree_fs_read(struct tee_file_handle *fh, void *buf,
 		goto exit;
 	}
 
-	start_block_num = pos_to_block_num(fdp->pos);
-	end_block_num = pos_to_block_num(fdp->pos + remain_bytes - 1);
+	start_block_num = pos_to_block_num(pos);
+	end_block_num = pos_to_block_num(pos + remain_bytes - 1);
 
 	block = malloc(BLOCK_SIZE);
 	if (!block) {
@@ -660,24 +389,21 @@ static TEE_Result ree_fs_read(struct tee_file_handle *fh, void *buf,
 	}
 
 	while (start_block_num <= end_block_num) {
-		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t offset = 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;
+		res = tee_fs_htree_read_block(&fdp->ht, 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;
+		pos += size_to_read;
 
 		start_block_num++;
 	}
@@ -688,27 +414,10 @@ exit:
 	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)
+static TEE_Result ree_fs_write(struct tee_file_handle *fh, size_t pos,
+			       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;
 
@@ -717,31 +426,31 @@ static TEE_Result ree_fs_write(struct tee_file_handle *fh, const void *buf,
 
 	mutex_lock(&ree_fs_mutex);
 
-	file_size = fdp->meta.info.length;
+	file_size = tee_fs_htree_get_meta(fdp->ht)->length;
 
-	if ((fdp->pos + len) > MAX_FILE_SIZE || (fdp->pos + len) < len) {
+	if ((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 (file_size < pos) {
+		res = ree_fs_ftruncate_internal(fdp, pos);
 		if (res != TEE_SUCCESS)
 			goto exit;
 	}
 
-	new_meta = fdp->meta;
-	res = out_of_place_write(fdp, buf, len, &new_meta);
+	res = out_of_place_write(fdp, pos, buf, len);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
-	res = commit_meta_file(fdp, &new_meta);
 exit:
+	if (res == TEE_SUCCESS)
+		res = tee_fs_htree_sync_to_storage(&fdp->ht);
 	mutex_unlock(&ree_fs_mutex);
 	return res;
 }
 
-static TEE_Result ree_fs_rename(const char *old, const char *new,
+static TEE_Result ree_fs_rename(struct tee_pobj *old, struct tee_pobj *new,
 				bool overwrite)
 {
 	TEE_Result res;
@@ -753,12 +462,12 @@ static TEE_Result ree_fs_rename(const char *old, const char *new,
 	return res;
 }
 
-static TEE_Result ree_fs_remove(const char *file)
+static TEE_Result ree_fs_remove(struct tee_pobj *po)
 {
 	TEE_Result res;
 
 	mutex_lock(&ree_fs_mutex);
-	res = tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, file);
+	res = tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_FS, po);
 	mutex_unlock(&ree_fs_mutex);
 
 	return res;
@@ -782,7 +491,6 @@ const struct tee_file_operations ree_fs_ops = {
 	.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,
diff --git a/core/tee/tee_rpmb_fs.c b/core/tee/tee_rpmb_fs.c
index 229bce7..dcac98b 100644
--- a/core/tee/tee_rpmb_fs.c
+++ b/core/tee/tee_rpmb_fs.c
@@ -26,22 +26,24 @@
  */
 
 #include <assert.h>
-#include <kernel/tee_common.h>
 #include <kernel/mutex.h>
 #include <kernel/panic.h>
+#include <kernel/tee_common.h>
 #include <kernel/tee_common_otp.h>
+#include <kernel/tee_misc.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 <string.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 <tee/tee_pobj.h>
+#include <tee/tee_svc_storage.h>
 #include <trace.h>
 #include <util.h>
 
@@ -89,8 +91,6 @@ struct rpmb_file_handle {
 	char filename[TEE_RPMB_FS_FILENAME_LENGTH];
 	/* Address for current entry in RPMB */
 	uint32_t rpmb_fat_address;
-	/* Current position */
-	uint32_t pos;
 };
 
 /**
@@ -106,13 +106,10 @@ struct rpmb_fs_partition {
 };
 
 /**
- * A node in a list of directory entries. entry->name is a
- * pointer to name here.
+ * A node in a list of directory entries.
  */
 struct tee_rpmb_fs_dirent {
 	struct tee_fs_dirent entry;
-	char name[TEE_RPMB_FS_FILENAME_LENGTH];
-	/* */
 	SIMPLEQ_ENTRY(tee_rpmb_fs_dirent) link;
 };
 
@@ -1581,7 +1578,6 @@ out:
 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);
@@ -1592,7 +1588,8 @@ static void dump_fh(struct rpmb_file_handle *fh __unused)
 }
 #endif
 
-static struct rpmb_file_handle *alloc_file_handle(const char *filename)
+static struct rpmb_file_handle *alloc_file_handle(struct tee_pobj *po,
+						  bool temporary)
 {
 	struct rpmb_file_handle *fh = NULL;
 
@@ -1600,8 +1597,10 @@ static struct rpmb_file_handle *alloc_file_handle(const char *filename)
 	if (!fh)
 		return NULL;
 
-	if (filename)
-		strlcpy(fh->filename, filename, sizeof(fh->filename));
+	if (po)
+		tee_svc_storage_create_filename(fh->filename,
+						sizeof(fh->filename), po,
+						temporary);
 
 	return fh;
 }
@@ -1696,7 +1695,7 @@ static TEE_Result rpmb_fs_setup(void)
 	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);
+	fh = alloc_file_handle(NULL, false);
 	if (!fh) {
 		res = TEE_ERROR_OUT_OF_MEMORY;
 		goto out;
@@ -1924,29 +1923,17 @@ again:
 	return res;
 }
 
-static TEE_Result rpmb_fs_open_internal(const char *file, bool create,
+static TEE_Result rpmb_fs_open_internal(struct tee_pobj *po, 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);
+	fh = alloc_file_handle(po, po->temporary);
 	if (!fh) {
 		res = TEE_ERROR_OUT_OF_MEMORY;
 		goto out;
@@ -1988,7 +1975,8 @@ static TEE_Result rpmb_fs_open_internal(const char *file, bool 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));
+			memcpy(fh->fat_entry.filename, fh->filename,
+				strlen(fh->filename));
 			/* Start address and size are 0 */
 			fh->fat_entry.flags = FILE_IS_ACTIVE;
 
@@ -2025,8 +2013,8 @@ static void rpmb_fs_close(struct tee_file_handle **tfh)
 	*tfh = NULL;
 }
 
-static TEE_Result rpmb_fs_read(struct tee_file_handle *tfh, void *buf,
-			       size_t *len)
+static TEE_Result rpmb_fs_read(struct tee_file_handle *tfh, size_t pos,
+			       void *buf, size_t *len)
 {
 	TEE_Result res;
 	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
@@ -2043,29 +2031,28 @@ static TEE_Result rpmb_fs_read(struct tee_file_handle *tfh, void *buf,
 	if (res != TEE_SUCCESS)
 		goto out;
 
-	if (fh->pos >= fh->fat_entry.data_size) {
+	if (pos >= fh->fat_entry.data_size) {
 		*len = 0;
 		goto out;
 	}
 
-	size = MIN(size, fh->fat_entry.data_size - fh->pos);
+	size = MIN(size, fh->fat_entry.data_size - pos);
 	if (size) {
 		res = tee_rpmb_read(CFG_RPMB_FS_DEV_ID,
-				    fh->fat_entry.start_address + fh->pos, buf,
+				    fh->fat_entry.start_address + 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)
+static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, size_t pos,
+				const void *buf, size_t size)
 {
 	TEE_Result res;
 	struct rpmb_file_handle *fh = (struct rpmb_file_handle *)tfh;
@@ -2108,8 +2095,8 @@ static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, const void *buf,
 	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;
+	end = pos + size;
+	start_addr = fh->fat_entry.start_address + pos;
 
 	if (end <= fh->fat_entry.data_size &&
 	    tee_rpmb_write_is_atomic(CFG_RPMB_FS_DEV_ID, start_addr, size)) {
@@ -2143,7 +2130,7 @@ static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, const void *buf,
 				goto out;
 		}
 
-		memcpy(newbuf + fh->pos, buf, size);
+		memcpy(newbuf + pos, buf, size);
 
 		newaddr = tee_mm_get_smem(mm);
 		res = tee_rpmb_write(CFG_RPMB_FS_DEV_ID, newaddr, newbuf,
@@ -2158,7 +2145,6 @@ static TEE_Result rpmb_fs_write(struct tee_file_handle *tfh, const void *buf,
 			goto out;
 	}
 
-	fh->pos += size;
 out:
 	mutex_unlock(&rpmb_mutex);
 	if (pool_result)
@@ -2169,68 +2155,14 @@ out:
 	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)
+static TEE_Result rpmb_fs_remove(struct tee_pobj *po)
 {
 	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);
+	fh = alloc_file_handle(po, po->temporary);
 	if (!fh) {
 		res = TEE_ERROR_OUT_OF_MEMORY;
 		goto out;
@@ -2250,39 +2182,33 @@ out:
 	return res;
 }
 
-static  TEE_Result rpmb_fs_rename(const char *old_name, const char *new_name,
+static  TEE_Result rpmb_fs_rename(struct tee_pobj *old, struct tee_pobj *new,
 				  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) {
+	if (!old) {
 		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 (new)
+		fh_old = alloc_file_handle(old, old->temporary);
+	else
+		fh_old = alloc_file_handle(old, true);
 	if (!fh_old) {
 		res = TEE_ERROR_OUT_OF_MEMORY;
 		goto out;
 	}
 
-	fh_new = alloc_file_handle(new_name);
+	if (new)
+		fh_new = alloc_file_handle(new, new->temporary);
+	else
+		fh_new = alloc_file_handle(old, false);
 	if (!fh_new) {
 		res = TEE_ERROR_OUT_OF_MEMORY;
 		goto out;
@@ -2307,7 +2233,8 @@ static  TEE_Result rpmb_fs_rename(const char *old_name, const char *new_name,
 	}
 
 	memset(fh_old->fat_entry.filename, 0, TEE_RPMB_FS_FILENAME_LENGTH);
-	memcpy(fh_old->fat_entry.filename, new_name, new_len);
+	memcpy(fh_old->fat_entry.filename, fh_new->filename,
+	       strlen(fh_new->filename));
 
 	res = write_fat_entry(fh_old, false);
 
@@ -2477,15 +2404,20 @@ static TEE_Result rpmb_fs_dir_populate(const char *path,
 						goto out;
 					}
 
-					memset(next, 0, sizeof(*next));
-					next->entry.d_name = next->name;
-					memcpy(next->name,
-						&filename[pathlen],
-						filelen - pathlen);
+					next->entry.oidlen = tee_hs2b(
+						(uint8_t *)&filename[pathlen],
+						next->entry.oid,
+						filelen - pathlen,
+						sizeof(next->entry.oid));
+					if (next->entry.oidlen) {
+						SIMPLEQ_INSERT_TAIL(&dir->next,
+								    next, link);
+						current = next;
+					} else {
+						free(next);
+						next = NULL;
+					}
 
-					SIMPLEQ_INSERT_TAIL(&dir->next, next,
-							    link);
-					current = next;
 				}
 			}
 
@@ -2514,32 +2446,25 @@ out:
 	return res;
 }
 
-static TEE_Result rpmb_fs_opendir(const char *path, struct tee_fs_dir **dir)
+static TEE_Result rpmb_fs_opendir(const TEE_UUID *uuid, 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) {
+	if (!uuid || !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) {
+	memset(path_local, 0, sizeof(path_local));
+	if (tee_svc_storage_create_dirname(path_local, sizeof(path_local) - 1,
+					   uuid) != TEE_SUCCESS) {
 		res = TEE_ERROR_BAD_PARAMETERS;
 		goto out;
 	}
-
-	memset(path_local, 0, sizeof(path_local));
-	memcpy(path_local, path, len);
+	len = strlen(path_local);
 
 	/* Add a slash to correctly match the full directory name. */
 	if (path_local[len - 1] != '/')
@@ -2591,14 +2516,15 @@ static void rpmb_fs_closedir(struct tee_fs_dir *dir)
 	}
 }
 
-static TEE_Result rpmb_fs_open(const char *file, struct tee_file_handle **fh)
+static TEE_Result rpmb_fs_open(struct tee_pobj *po, struct tee_file_handle **fh)
 {
-	return rpmb_fs_open_internal(file, false, fh);
+	return rpmb_fs_open_internal(po, false, fh);
 }
 
-static TEE_Result rpmb_fs_create(const char *file, struct tee_file_handle **fh)
+static TEE_Result rpmb_fs_create(struct tee_pobj *po,
+				 struct tee_file_handle **fh)
 {
-	return rpmb_fs_open_internal(file, true, fh);
+	return rpmb_fs_open_internal(po, true, fh);
 }
 
 const struct tee_file_operations rpmb_fs_ops = {
@@ -2607,7 +2533,6 @@ const struct tee_file_operations rpmb_fs_ops = {
 	.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,
diff --git a/core/tee/tee_sql_fs.c b/core/tee/tee_sql_fs.c
index e38e1bc..bfc04f6 100644
--- a/core/tee/tee_sql_fs.c
+++ b/core/tee/tee_sql_fs.c
@@ -29,15 +29,6 @@
  * 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>
@@ -45,13 +36,12 @@
 #include <optee_msg_supplicant.h>
 #include <stdio.h>
 #include <stdlib.h>
-#include <string.h>
 #include <string_ext.h>
+#include <string.h>
 #include <sys/queue.h>
+#include <tee/fs_htree.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>
@@ -61,22 +51,10 @@
 #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;
+	struct tee_fs_htree *ht;
 	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;
@@ -96,9 +74,10 @@ static TEE_Result sql_fs_end_transaction_rpc(bool rollback)
 					     rollback);
 }
 
-static TEE_Result sql_fs_opendir_rpc(const char *name, struct tee_fs_dir **d)
+static TEE_Result sql_fs_opendir_rpc(const TEE_UUID *uuid,
+				     struct tee_fs_dir **d)
 {
-	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_SQL_FS, name, d);
+	return tee_fs_rpc_opendir(OPTEE_MSG_RPC_CMD_SQL_FS, uuid, d);
 }
 
 static TEE_Result sql_fs_readdir_rpc(struct tee_fs_dir *d,
@@ -107,15 +86,15 @@ static TEE_Result sql_fs_readdir_rpc(struct tee_fs_dir *d,
 	return tee_fs_rpc_readdir(OPTEE_MSG_RPC_CMD_SQL_FS, d, ent);
 }
 
-static TEE_Result sql_fs_remove_rpc(const char *file)
+static TEE_Result sql_fs_remove_rpc(struct tee_pobj *po)
 {
-	return tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, file);
+	return tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, po);
 }
 
-static TEE_Result sql_fs_rename_rpc(const char *old, const char *nw,
+static TEE_Result sql_fs_rename_rpc(struct tee_pobj *old, struct tee_pobj *new,
 				    bool overwrite)
 {
-	return tee_fs_rpc_rename(OPTEE_MSG_RPC_CMD_SQL_FS, old, nw, overwrite);
+	return tee_fs_rpc_rename(OPTEE_MSG_RPC_CMD_SQL_FS, old, new, overwrite);
 }
 
 static void sql_fs_closedir_rpc(struct tee_fs_dir *d)
@@ -128,21 +107,6 @@ static void sql_fs_closedir_rpc(struct tee_fs_dir *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)
@@ -150,131 +114,108 @@ 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)
+static TEE_Result get_offs_size(enum tee_fs_htree_type type, size_t idx,
+				size_t *offs, size_t *size)
 {
-	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);
+	const size_t node_size = sizeof(struct tee_fs_htree_node_image);
+	const size_t block_nodes = BLOCK_SIZE / node_size;
+	size_t pbn;
+	size_t bidx;
+
+
+	/*
+	 * File layout
+	 *
+	 * phys block 0:
+	 * tee_fs_htree_image @ offs = 0
+	 *
+	 * phys block 1:
+	 * tee_fs_htree_node_image 0  @ offs = 0
+	 * tee_fs_htree_node_image 1  @ offs = node_size * 2
+	 * ...
+	 * tee_fs_htree_node_image 61 @ offs = node_size * 122
+	 *
+	 * phys block 2:
+	 * data block 0
+	 *
+	 * ...
+	 *
+	 * phys block 64:
+	 * data block 61
+	 *
+	 * phys block 65:
+	 * tee_fs_htree_node_image 62  @ offs = 0
+	 * tee_fs_htree_node_image 63  @ offs = node_size * 2
+	 * ...
+	 * tee_fs_htree_node_image 121 @ offs = node_size * 123
+	 *
+	 * ...
+	 */
+
+	switch (type) {
+	case TEE_FS_HTREE_TYPE_HEAD:
+		*offs = 0;
+		*size = sizeof(struct tee_fs_htree_image);
+		return TEE_SUCCESS;
+	case TEE_FS_HTREE_TYPE_NODE:
+		pbn = 1 + ((idx / block_nodes) * block_nodes);
+		*offs = pbn * BLOCK_SIZE + node_size * (idx % block_nodes);
+		*size = node_size;
+		return TEE_SUCCESS;
+	case TEE_FS_HTREE_TYPE_BLOCK:
+		bidx = idx;
+		pbn = 2 + bidx + bidx / (block_nodes - 1);
+		*offs = pbn * BLOCK_SIZE;
+		*size = BLOCK_SIZE;
+		return TEE_SUCCESS;
+	default:
+		return TEE_ERROR_GENERIC;
+	}
 }
 
-static TEE_Result create_meta(struct sql_fs_fd *fdp, const char *fname)
+static TEE_Result sql_fs_rpc_read_init(void *aux,
+				       struct tee_fs_rpc_operation *op,
+				       enum tee_fs_htree_type type, size_t idx,
+				       uint8_t vers __unused, void **data)
 {
+	struct sql_fs_fd *fdp = aux;
 	TEE_Result res;
+	size_t offs;
+	size_t size;
 
-	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);
+	res = get_offs_size(type, idx, &offs, &size);
 	if (res != TEE_SUCCESS)
 		return res;
 
-	return write_meta(fdp);
+	return tee_fs_rpc_read_init(op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd,
+				    offs, size, data);
 }
 
-static TEE_Result read_meta(struct sql_fs_fd *fdp, const char *fname)
+static TEE_Result sql_fs_rpc_write_init(void *aux,
+					struct tee_fs_rpc_operation *op,
+					enum tee_fs_htree_type type, size_t idx,
+					uint8_t vers __unused, void **data)
 {
+	struct sql_fs_fd *fdp = aux;
 	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;
+	size_t offs;
+	size_t size;
 
-	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);
+	res = get_offs_size(type, idx, &offs, &size);
 	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);
+	return tee_fs_rpc_write_init(op, OPTEE_MSG_RPC_CMD_SQL_FS, fdp->fd,
+				     offs, size, data);
 }
 
-/* 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);
-}
+static const struct tee_fs_htree_storage sql_fs_storage_ops = {
+	.block_size = BLOCK_SIZE,
+	.rpc_read_init = sql_fs_rpc_read_init,
+	.rpc_read_final = tee_fs_rpc_read_final,
+	.rpc_write_init = sql_fs_rpc_write_init,
+	.rpc_write_final = tee_fs_rpc_write_final,
+};
 
 /*
  * Partial write (< BLOCK_SIZE) into a block: read/update/write
@@ -296,16 +237,21 @@ static TEE_Result write_block_partial(struct sql_fs_fd *fdp, size_t bnum,
 	if (!buf)
 		return TEE_ERROR_OUT_OF_MEMORY;
 
-	res = read_block(fdp, bnum, buf);
-	if (res != TEE_SUCCESS)
-		goto exit;
+	if (bnum * BLOCK_SIZE <
+	    ROUNDUP(tee_fs_htree_get_meta(fdp->ht)->length, BLOCK_SIZE)) {
+		res = tee_fs_htree_read_block(&fdp->ht, bnum, buf);
+		if (res != TEE_SUCCESS)
+			goto exit;
+	} else {
+		memset(buf, 0, BLOCK_SIZE);
+	}
 
 	if (data)
 		memcpy(buf + offset, data, len);
 	else
 		memset(buf + offset, 0, len);
 
-	res = write_block(fdp, bnum, buf);
+	res = tee_fs_htree_write_block(&fdp->ht, bnum, buf);
 exit:
 	free(buf);
 	return res;
@@ -315,32 +261,42 @@ 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;
+	struct tee_fs_htree_meta *meta = tee_fs_htree_get_meta(fdp->ht);
 
-	if (new_length == old_length)
+	if ((size_t)new_length == meta->length)
 		return TEE_SUCCESS;
 
 	sql_fs_begin_transaction_rpc();
 
-	if (new_length < old_length) {
+	if ((size_t)new_length < meta->length) {
 		/* Trim unused blocks */
-		int old_last_block = block_num(old_length);
+		int old_last_block = block_num(meta->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);
+			size_t offs;
+			size_t sz;
+
+			res = get_offs_size(TEE_FS_HTREE_TYPE_BLOCK,
+					    ROUNDUP(new_length, BLOCK_SIZE) /
+						BLOCK_SIZE, &offs, &sz);
+			if (res != TEE_SUCCESS)
+				goto exit;
+
+			res = tee_fs_htree_truncate(&fdp->ht,
+						    new_length / BLOCK_SIZE);
+			if (res != TEE_SUCCESS)
+				goto exit;
+
 			res = tee_fs_rpc_truncate(OPTEE_MSG_RPC_CMD_SQL_FS,
-						  fdp->fd, off);
+						  fdp->fd, offs + sz);
 			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);
+		tee_fs_off_t off = meta->length % BLOCK_SIZE;
+		size_t bnum = block_num(meta->length);
 		size_t end_bnum = block_num(new_length);
 
 		while (bnum <= end_bnum) {
@@ -354,70 +310,28 @@ static TEE_Result sql_fs_ftruncate_internal(struct sql_fs_fd *fdp,
 		}
 	}
 
-	fdp->meta.length = new_length;
-	res = write_meta(fdp);
+	meta->length = new_length;
+	res = TEE_SUCCESS;
 exit:
+	if (res == TEE_SUCCESS)
+		res = tee_fs_htree_sync_to_storage(&fdp->ht);
 	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);
+		tee_fs_htree_close(&fdp->ht);
+		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,
+static TEE_Result open_internal(struct tee_pobj *po, bool create,
 				struct tee_file_handle **fh)
 {
 	TEE_Result res;
@@ -432,36 +346,41 @@ static TEE_Result open_internal(const char *file, bool create,
 	mutex_lock(&sql_fs_mutex);
 
 	if (create)
-		res = create_meta(fdp, file);
+		res = tee_fs_rpc_create(OPTEE_MSG_RPC_CMD_SQL_FS, po, &fdp->fd);
 	else
-		res = read_meta(fdp, file);
+		res = tee_fs_rpc_open(OPTEE_MSG_RPC_CMD_SQL_FS, po, &fdp->fd);
+	if (res != TEE_SUCCESS)
+		goto out;
 
+	res = tee_fs_htree_open(create, &sql_fs_storage_ops, fdp, &fdp->ht);
+out:
 	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);
+			tee_fs_rpc_remove(OPTEE_MSG_RPC_CMD_SQL_FS, po);
 		free(fdp);
 	}
 	mutex_unlock(&sql_fs_mutex);
 	return res;
 }
 
-static TEE_Result sql_fs_open(const char *file, struct tee_file_handle **fh)
+static TEE_Result sql_fs_open(struct tee_pobj *po, struct tee_file_handle **fh)
 {
-	return open_internal(file, false, fh);
+	return open_internal(po, false, fh);
 }
 
-static TEE_Result sql_fs_create(const char *file, struct tee_file_handle **fh)
+static TEE_Result sql_fs_create(struct tee_pobj *po,
+				struct tee_file_handle **fh)
 {
-	return open_internal(file, true, fh);
+	return open_internal(po, true, fh);
 }
 
 
-static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
-			      size_t *len)
+static TEE_Result sql_fs_read(struct tee_file_handle *fh, size_t pos,
+			      void *buf, size_t *len)
 {
 	TEE_Result res;
 	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
@@ -470,14 +389,15 @@ static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
 	uint8_t *block = NULL;
 	int start_block_num;
 	int end_block_num;
+	size_t file_size;
 
 	mutex_lock(&sql_fs_mutex);
 
-	if ((fdp->pos + remain_bytes) < remain_bytes ||
-	    fdp->pos > (tee_fs_off_t)fdp->meta.length)
+	file_size = tee_fs_htree_get_meta(fdp->ht)->length;
+	if ((pos + remain_bytes) < remain_bytes || pos > file_size)
 		remain_bytes = 0;
-	else if (fdp->pos + remain_bytes > fdp->meta.length)
-		remain_bytes = fdp->meta.length - fdp->pos;
+	else if (pos + remain_bytes > file_size)
+		remain_bytes = file_size - pos;
 
 	*len = remain_bytes;
 
@@ -486,8 +406,8 @@ static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
 		goto exit;
 	}
 
-	start_block_num = block_num(fdp->pos);
-	end_block_num = block_num(fdp->pos + remain_bytes - 1);
+	start_block_num = block_num(pos);
+	end_block_num = block_num(pos + remain_bytes - 1);
 
 	block = malloc(BLOCK_SIZE);
 	if (!block) {
@@ -496,17 +416,13 @@ static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
 	}
 
 	while (start_block_num <= end_block_num) {
-		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t offset = 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);
+		res = tee_fs_htree_read_block(&fdp->ht, start_block_num, block);
 		if (res != TEE_SUCCESS)
 			goto exit;
 
@@ -514,7 +430,7 @@ static TEE_Result sql_fs_read(struct tee_file_handle *fh, void *buf,
 
 		data_ptr += size_to_read;
 		remain_bytes -= size_to_read;
-		fdp->pos += size_to_read;
+		pos += size_to_read;
 
 		start_block_num++;
 	}
@@ -525,11 +441,12 @@ exit:
 	return res;
 }
 
-static TEE_Result sql_fs_write(struct tee_file_handle *fh, const void *buf,
-			       size_t len)
+static TEE_Result sql_fs_write(struct tee_file_handle *fh, size_t pos,
+			       const void *buf, size_t len)
 {
 	TEE_Result res;
 	struct sql_fs_fd *fdp = (struct sql_fs_fd *)fh;
+	struct tee_fs_htree_meta *meta = tee_fs_htree_get_meta(fdp->ht);
 	size_t remain_bytes = len;
 	const uint8_t *data_ptr = buf;
 	int start_block_num;
@@ -542,18 +459,18 @@ static TEE_Result sql_fs_write(struct tee_file_handle *fh, const void *buf,
 
 	sql_fs_begin_transaction_rpc();
 
-	if (fdp->meta.length < (size_t)fdp->pos) {
+	if (meta->length < pos) {
 		/* Fill hole */
-		res = sql_fs_ftruncate_internal(fdp, fdp->pos);
+		res = sql_fs_ftruncate_internal(fdp, pos);
 		if (res != TEE_SUCCESS)
 			goto exit;
 	}
 
-	start_block_num = block_num(fdp->pos);
-	end_block_num = block_num(fdp->pos + len - 1);
+	start_block_num = block_num(pos);
+	end_block_num = block_num(pos + len - 1);
 
 	while (start_block_num <= end_block_num) {
-		tee_fs_off_t offset = fdp->pos % BLOCK_SIZE;
+		size_t offset = pos % BLOCK_SIZE;
 		size_t size_to_write = MIN(remain_bytes, (size_t)BLOCK_SIZE);
 
 		if (size_to_write + offset > BLOCK_SIZE)
@@ -566,16 +483,17 @@ static TEE_Result sql_fs_write(struct tee_file_handle *fh, const void *buf,
 
 		data_ptr += size_to_write;
 		remain_bytes -= size_to_write;
-		fdp->pos += size_to_write;
+		pos += size_to_write;
 
 		start_block_num++;
 	}
 
-	if (fdp->meta.length < (size_t)fdp->pos) {
-		fdp->meta.length = fdp->pos;
-		res = write_meta(fdp);
-	}
+	if (pos > meta->length)
+		meta->length = pos;
+
 exit:
+	if (res == TEE_SUCCESS)
+		res = tee_fs_htree_sync_to_storage(&fdp->ht);
 	sql_fs_end_transaction_rpc(res != TEE_SUCCESS);
 	mutex_unlock(&sql_fs_mutex);
 	return res;
@@ -599,7 +517,6 @@ const struct tee_file_operations sql_fs_ops = {
 	.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,
diff --git a/core/tee/tee_svc_storage.c b/core/tee/tee_svc_storage.c
index 916ddca..d852be1 100644
--- a/core/tee/tee_svc_storage.c
+++ b/core/tee/tee_svc_storage.c
@@ -32,7 +32,6 @@
 #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>
@@ -99,12 +98,6 @@ struct tee_storage_enum {
 	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)
@@ -140,114 +133,77 @@ static TEE_Result tee_svc_close_enum(struct user_ta_ctx *utc,
 }
 
 /* "/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)
+TEE_Result tee_svc_storage_create_filename(void *buf, size_t blen,
+					   struct tee_pobj *po, bool transient)
 {
-	uint8_t *file;
+	uint8_t *file = buf;
 	uint32_t pos = 0;
 	uint32_t hslen = 1 /* Leading slash */
-			+ TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID) + object_id_len)
+			+ TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID) + po->obj_id_len)
 			+ 1; /* Intermediate slash */
 
 	/* +1 for the '.' (temporary persistent object) */
 	if (transient)
 		hslen++;
 
-	file = malloc(hslen);
-	if (!file)
-		return NULL;
+	if (blen < hslen)
+		return TEE_ERROR_SHORT_BUFFER;
 
 	file[pos++] = '/';
-	pos += tee_b2hs((uint8_t *)&sess->ctx->uuid, &file[pos],
+	pos += tee_b2hs((uint8_t *)&po->uuid, &file[pos],
 			sizeof(TEE_UUID), hslen);
 	file[pos++] = '/';
 
 	if (transient)
 		file[pos++] = '.';
 
-	tee_b2hs(object_id, file + pos, object_id_len, hslen - pos);
+	tee_b2hs(po->obj_id, file + pos, po->obj_id_len, hslen - pos);
 
-	return (char *)file;
+	return TEE_SUCCESS;
 }
 
 /* "/TA_uuid" */
-char *tee_svc_storage_create_dirname(struct tee_ta_session *sess)
+TEE_Result tee_svc_storage_create_dirname(void *buf, size_t blen,
+					  const TEE_UUID *uuid)
 {
-	uint8_t *dir;
+	uint8_t *dir = buf;
 	uint32_t hslen = TEE_B2HS_HSBUF_SIZE(sizeof(TEE_UUID)) + 1;
 
-	dir = malloc(hslen);
-	if (!dir)
-		return NULL;
+	if (blen < hslen)
+		return TEE_ERROR_SHORT_BUFFER;
 
 	dir[0] = '/';
-	tee_b2hs((uint8_t *)&sess->ctx->uuid, dir + 1, sizeof(TEE_UUID),
-		 hslen);
+	tee_b2hs((uint8_t *)uuid, dir + 1, sizeof(TEE_UUID), hslen);
 
-	return (char *)dir;
+	return TEE_SUCCESS;
 }
 
 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;
-	}
-
+	o->pobj->fops->remove(o->pobj);
 	tee_obj_close(to_user_ta_ctx(sess->ctx), o);
-	fops->remove(file);
-	free(file);
 
-	res = TEE_SUCCESS;
-
-exit:
-	return res;
+	return TEE_SUCCESS;
 }
 
-static TEE_Result tee_svc_storage_read_head(struct tee_ta_session *sess,
-				     struct tee_obj *o)
+static TEE_Result tee_svc_storage_read_head(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;
+	const struct tee_file_operations *fops = o->pobj->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);
+	res = fops->open(o->pobj, &o->fh);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
 	/* read head */
 	bytes = sizeof(struct tee_svc_storage_head);
-	res = fops->read(o->fh, &head, &bytes);
+	res = fops->read(o->fh, 0, &head, &bytes);
 	if (res != TEE_SUCCESS) {
 		if (res == TEE_ERROR_CORRUPT_OBJECT)
 			EMSG("Head corrupt\n");
@@ -263,6 +219,7 @@ static TEE_Result tee_svc_storage_read_head(struct tee_ta_session *sess,
 	if (res != TEE_SUCCESS)
 		goto exit;
 
+	o->ds_pos = sizeof(struct tee_svc_storage_head) + head.meta_size;
 	if (head.meta_size) {
 		attr = malloc(head.meta_size);
 		if (!attr) {
@@ -272,7 +229,8 @@ static TEE_Result tee_svc_storage_read_head(struct tee_ta_session *sess,
 
 		/* read meta */
 		bytes = head.meta_size;
-		res = fops->read(o->fh, attr, &bytes);
+		res = fops->read(o->fh, sizeof(struct tee_svc_storage_head),
+				 attr, &bytes);
 		if (res != TEE_SUCCESS || bytes != head.meta_size) {
 			res = TEE_ERROR_CORRUPT_OBJECT;
 			goto exit;
@@ -291,7 +249,6 @@ static TEE_Result tee_svc_storage_read_head(struct tee_ta_session *sess,
 
 exit:
 	free(attr);
-	free(file);
 
 	return res;
 }
@@ -299,63 +256,22 @@ exit:
 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;
+	size_t pos = offsetof(struct tee_svc_storage_head, ds_size);
 
-	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;
+	return o->pobj->fops->write(o->fh, pos, &ds_size, sizeof(uint32_t));
 }
 
-static TEE_Result tee_svc_storage_init_file(struct tee_ta_session *sess,
-					    struct tee_obj *o,
+static TEE_Result tee_svc_storage_init_file(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;
+	const struct tee_file_operations *fops = o->pobj->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);
+	res = fops->create(o->pobj, &o->fh);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
@@ -389,6 +305,8 @@ static TEE_Result tee_svc_storage_init_file(struct tee_ta_session *sess,
 			goto exit;
 	}
 
+	o->ds_pos = sizeof(struct tee_svc_storage_head) + attr_size;
+
 	/* write head */
 	head.magic = TEE_SVC_STORAGE_MAGIC;
 	head.head_size = sizeof(struct tee_svc_storage_head);
@@ -401,12 +319,13 @@ static TEE_Result tee_svc_storage_init_file(struct tee_ta_session *sess,
 	head.have_attrs = o->have_attrs;
 
 	/* write head */
-	res = fops->write(o->fh, &head, sizeof(struct tee_svc_storage_head));
+	res = fops->write(o->fh, 0, &head, sizeof(struct tee_svc_storage_head));
 	if (res != TEE_SUCCESS)
 		goto exit;
 
 	/* write meta */
-	res = fops->write(o->fh, attr, attr_size);
+	res = fops->write(o->fh, sizeof(struct tee_svc_storage_head),
+			  attr, attr_size);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
@@ -415,11 +334,10 @@ static TEE_Result tee_svc_storage_init_file(struct tee_ta_session *sess,
 
 	/* write data to fs if needed */
 	if (data && len)
-		res = fops->write(o->fh, data, len);
+		res = fops->write(o->fh, o->ds_pos, data, len);
 
 exit:
 	free(attr);
-	free(tmpfile);
 	fops->close(&o->fh);
 
 	return res;
@@ -462,7 +380,7 @@ TEE_Result syscall_storage_obj_open(unsigned long storage_id, void *object_id,
 		goto err;
 
 	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
-			   object_id_len, flags, fops, &po);
+			   object_id_len, flags, false, fops, &po);
 	if (res != TEE_SUCCESS)
 		goto err;
 
@@ -479,7 +397,7 @@ TEE_Result syscall_storage_obj_open(unsigned long storage_id, void *object_id,
 	o->pobj = po;
 	tee_obj_add(utc, o);
 
-	res = tee_svc_storage_read_head(sess, o);
+	res = tee_svc_storage_read_head(o);
 	if (res != TEE_SUCCESS) {
 		if (res == TEE_ERROR_CORRUPT_OBJECT) {
 			EMSG("Object corrupt");
@@ -496,11 +414,6 @@ TEE_Result syscall_storage_obj_open(unsigned long storage_id, void *object_id,
 	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:
@@ -528,9 +441,7 @@ TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
 	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;
@@ -555,7 +466,7 @@ TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
 		goto err;
 
 	res = tee_pobj_get((void *)&sess->ctx->uuid, object_id,
-			   object_id_len, flags, fops, &po);
+			   object_id_len, flags, true, fops, &po);
 	if (res != TEE_SUCCESS)
 		goto err;
 
@@ -588,33 +499,17 @@ TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
 			goto err;
 	}
 
-	res = tee_svc_storage_init_file(sess, o, attr_o, data, len);
+	res = tee_svc_storage_init_file(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));
+	po->temporary = false;
+	res = fops->rename(po, NULL, !!(flags & TEE_DATA_FLAG_OVERWRITE));
 	if (res != TEE_SUCCESS)
 		goto rmfile;
 
-	res = fops->open(file, &o->fh);
+	res = fops->open(po, &o->fh);
 	if (res != TEE_SUCCESS)
 		goto err;
 
@@ -628,35 +523,26 @@ TEE_Result syscall_storage_obj_create(unsigned long storage_id, void *object_id,
 	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;
+	return TEE_SUCCESS;
 
 oclose:
 	tee_obj_close(utc, o);
-	goto exit;
+	return res;
 
 rmfile:
-	fops->remove(tmpfile);
+	fops->remove(po);
 
 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 (res == TEE_ERROR_CORRUPT_OBJECT && po)
+		fops->remove(po);
 	if (o)
 		fops->close(&o->fh);
 	if (po)
 		tee_pobj_release(po);
 	free(o);
 
-exit:
-	free(file);
-	free(tmpfile);
-
 	return res;
 }
 
@@ -665,9 +551,7 @@ 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)
@@ -684,16 +568,9 @@ TEE_Result syscall_storage_obj_del(unsigned long obj)
 	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;
+	res = o->pobj->fops->remove(o->pobj);
 	tee_obj_close(utc, o);
 
-	res = fops->remove(file);
-	free(file);
 	return res;
 }
 
@@ -743,31 +620,16 @@ TEE_Result syscall_storage_obj_rename(unsigned long obj, void *object_id,
 	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);
+			   false, fops, &po);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
 	/* move */
-	res = fops->rename(old_file, new_file, false /* no overwrite */);
+	res = fops->rename(o->pobj, po, false /* no overwrite */);
 	if (res == TEE_ERROR_GENERIC)
 		goto exit;
 
@@ -843,48 +705,39 @@ TEE_Result syscall_storage_reset_enum(unsigned long obj_enum)
 	if (res != TEE_SUCCESS)
 		return res;
 
-	e->fops->closedir(e->dir);
-	e->fops = NULL;
-	e->dir = NULL;
+	if (e->fops) {
+		e->fops->closedir(e->dir);
+		e->fops = NULL;
+		e->dir = NULL;
+	}
+	assert(!e->dir);
 
 	return TEE_SUCCESS;
 }
 
-static TEE_Result tee_svc_storage_set_enum(char *d_name,
+static TEE_Result tee_svc_storage_set_enum(struct tee_fs_dirent *d,
 			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;
+	o->pobj->obj_id = malloc(d->oidlen);
+	if (!o->pobj->obj_id)
+		return TEE_ERROR_OUT_OF_MEMORY;
 
-exit:
-	return res;
+	memcpy(o->pobj->obj_id, d->oid, d->oidlen);
+	o->pobj->obj_id_len = d->oidlen;
+	o->pobj->fops = fops;
 
+	return TEE_SUCCESS;
 }
 
 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);
@@ -902,17 +755,8 @@ TEE_Result syscall_storage_start_enum(unsigned long obj_enum,
 		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;
+	return fops->opendir(&sess->ctx->uuid, &e->dir);
 }
 
 TEE_Result syscall_storage_next_enum(unsigned long obj_enum,
@@ -975,11 +819,12 @@ TEE_Result syscall_storage_next_enum(unsigned long obj_enum,
 		goto exit;
 	}
 
-	res = tee_svc_storage_set_enum(d->d_name, e->fops, o);
+	o->pobj->uuid = sess->ctx->uuid;
+	res = tee_svc_storage_set_enum(d, e->fops, o);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
-	res = tee_svc_storage_read_head(sess, o);
+	res = tee_svc_storage_read_head(o);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
@@ -1040,7 +885,8 @@ TEE_Result syscall_storage_obj_read(unsigned long obj, void *data, size_t len,
 		goto exit;
 
 	bytes = len;
-	res = o->pobj->fops->read(o->fh, data, &bytes);
+	res = o->pobj->fops->read(o->fh, o->ds_pos + o->info.dataPosition,
+				  data, &bytes);
 	if (res != TEE_SUCCESS) {
 		EMSG("Error code=%x\n", (uint32_t)res);
 		if (res == TEE_ERROR_CORRUPT_OBJECT) {
@@ -1092,7 +938,8 @@ TEE_Result syscall_storage_obj_write(unsigned long obj, void *data, size_t len)
 	if (res != TEE_SUCCESS)
 		goto exit;
 
-	res = o->pobj->fops->write(o->fh, data, len);
+	res = o->pobj->fops->write(o->fh, o->ds_pos + o->info.dataPosition,
+				   data, len);
 	if (res != TEE_SUCCESS)
 		goto exit;
 
@@ -1163,39 +1010,50 @@ TEE_Result syscall_storage_obj_seek(unsigned long obj, int32_t offset,
 	TEE_Result res;
 	struct tee_ta_session *sess;
 	struct tee_obj *o;
-	int32_t off;
 	size_t attr_size;
+	tee_fs_off_t new_pos;
 
 	res = tee_ta_get_current_session(&sess);
 	if (res != TEE_SUCCESS)
-		goto exit;
+		return res;
 
 	res = tee_obj_get(to_user_ta_ctx(sess->ctx),
 			  tee_svc_uref_to_vaddr(obj), &o);
 	if (res != TEE_SUCCESS)
-		goto exit;
+		return res;
 
-	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT)) {
-		res = TEE_ERROR_BAD_STATE;
-		goto exit;
-	}
+	if (!(o->info.handleFlags & TEE_HANDLE_FLAG_PERSISTENT))
+		return TEE_ERROR_BAD_STATE;
 
 	res = tee_obj_attr_to_binary(o, NULL, &attr_size);
 	if (res != TEE_SUCCESS)
-		goto exit;
+		return res;
 
-	off = offset;
-	if (whence == TEE_DATA_SEEK_SET)
-		off += sizeof(struct tee_svc_storage_head) + attr_size;
+	switch (whence) {
+	case TEE_DATA_SEEK_SET:
+		new_pos = offset;
+		break;
+	case TEE_DATA_SEEK_CUR:
+		new_pos = o->info.dataPosition + offset;
+		break;
+	case TEE_DATA_SEEK_END:
+		new_pos = o->info.dataSize + offset;
+		break;
+	default:
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
 
-	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);
+	if (new_pos < 0)
+		new_pos = 0;
 
-exit:
-	return res;
+	if (new_pos > TEE_DATA_MAX_POSITION) {
+		EMSG("Position is beyond TEE_DATA_MAX_POSITION");
+		return TEE_ERROR_BAD_PARAMETERS;
+	}
+
+	o->info.dataPosition = new_pos;
+
+	return TEE_SUCCESS;
 }
 
 void tee_svc_storage_close_all_enum(struct user_ta_ctx *utc)
diff --git a/documentation/images/secure_storage/block_data_encryption.odg b/documentation/images/secure_storage/block_data_encryption.odg
index 655c81d..53a3b6f 100644
--- a/documentation/images/secure_storage/block_data_encryption.odg
+++ b/documentation/images/secure_storage/block_data_encryption.odg
diff --git a/documentation/images/secure_storage/block_data_encryption.png b/documentation/images/secure_storage/block_data_encryption.png
index b05f2c0..b2ce849 100644
--- a/documentation/images/secure_storage/block_data_encryption.png
+++ b/documentation/images/secure_storage/block_data_encryption.png
diff --git a/documentation/images/secure_storage/meta_data_encryption.odg b/documentation/images/secure_storage/meta_data_encryption.odg
index f6785d4..5ccb9b3 100644
--- a/documentation/images/secure_storage/meta_data_encryption.odg
+++ b/documentation/images/secure_storage/meta_data_encryption.odg
diff --git a/documentation/images/secure_storage/meta_data_encryption.png b/documentation/images/secure_storage/meta_data_encryption.png
index 9c7f8c4..5a50d73 100644
--- a/documentation/images/secure_storage/meta_data_encryption.png
+++ b/documentation/images/secure_storage/meta_data_encryption.png
diff --git a/documentation/interrupt_handling.md b/documentation/interrupt_handling.md
index 7ca8121..8063357 100644
--- a/documentation/interrupt_handling.md
+++ b/documentation/interrupt_handling.md
@@ -19,8 +19,18 @@ The processor is configured to use:
 * Monitor vector for SMC exceptions
 * State vector for IRQ exceptions
 
-Interrupts handled by secure world are sent as FIQs and interrupts handled
-by normal world are sent as IRQs.
+Two types of interrupt are defined in optee_os:
+* Native interrupt - The interrupt handled by optee_os
+  (for example: secure interrupt)
+* Foreign interrupt - The interrupt not handled by optee_os
+  (for example: non-secure interrupt which is handled by normal world)
+
+For ARM GICv2 mode, native interrupt is sent as FIQ and foreign interrupt is
+sent as IRQ.
+
+For ARM GICv3 mode, foreign interrupt is sent as FIQ which could be handled
+by either secure world (EL3 in AArch64) or normal world. This mode is not
+supported yet.
 
 Since IRQs are received using the state vector the actual vector used
 depends on the current state of the CPU. If the NS (non-secure) bit in SCR
diff --git a/documentation/optee_design.md b/documentation/optee_design.md
index 073b46b..cb0a5af 100644
--- a/documentation/optee_design.md
+++ b/documentation/optee_design.md
@@ -151,12 +151,13 @@ table when the TA context is activated.
 ![Select xlation table](images/xlat_table.png "Select xlation table")
 
 ## Translation tables and switching to normal world
-When switching to normal world either via an IRQ or RPC there is a chance
-that secure world will resume execution on a different CPU. This means that
-the new CPU need to be configured with the context of the currently active
-TA. This is solved by always setting the TA context in the CPU when
-resuming execution. Here is room for improvements since it is more likely
-than not that it is the same CPU that resumes execution in secure world.
+When switching to normal world either via a foreign interrupt or RPC there
+is a chance that secure world will resume execution on a different CPU.
+This means that the new CPU need to be configured with the context of the
+currently active TA. This is solved by always setting the TA context in
+the CPU when resuming execution. Here is room for improvements since it is
+more likely than not that it is the same CPU that resumes execution in
+secure world.
 
 # 6. Stacks
 Different stacks are used during different stages. The stacks are:
@@ -216,11 +217,16 @@ is restored it will continue at the next instruction as if this function did a
 normal return. CPU switches to use the temp stack before returning to normal
 world.
 
-## IRQ exit
-IRQ exit occurs when OP-TEE receives an IRQ, which is always handled in normal
-world. IRQ exit is similar to RPC exit but it is `thread_irq_handler()` and
-`elx_irq()` (respectively for ARMv7-A/Aarch32 and for Aarch64) that saves the
-thread state instead. The thread is resumed in the same way though.
+## Foreign interrupt exit
+Foreign interrupt exit occurs when OP-TEE receives a foreign interrupt. For ARM
+GICv2 mode, foreign interrupt is sent as IRQ which is always handled in normal
+world. Foreign interrupt exit is similar to RPC exit but it is
+`thread_irq_handler()` and `elx_irq()` (respectively for ARMv7-A/Aarch32 and
+for Aarch64) that saves the thread state instead. The thread is resumed in the
+same way though.
+For ARM GICv3 mode, foreign interrupt is sent as FIQ which could be handled by
+either secure world (EL3 in AArch64) or normal world. This mode is not supported
+yet.
 
 *Notes for ARMv7/AArch32:*
 SP_IRQ is initialized to temp stack instead of a separate stack.  Prior to
@@ -233,7 +239,8 @@ original `SP_EL0` is saved in the thread context to be restored when resuming.
 ## Resume entry
 OP-TEE is entered using the temp stack in the same way as for normal entry. The
 thread to resume is looked up and the state is restored to resume execution. The
-procedure to resume from an RPC exit or an IRQ exit is exactly the same.
+procedure to resume from an RPC exit or an foreign interrupt exit is exactly
+the same.
 
 ## Syscall
 Syscalls are executed using the thread stack.
diff --git a/documentation/porting_guidelines.md b/documentation/porting_guidelines.md
new file mode 100644
index 0000000..3ec197e
--- /dev/null
+++ b/documentation/porting_guidelines.md
@@ -0,0 +1,401 @@
+Porting guidelines for OP-TEE
+=============================
+
+1. [Introduction](#1-introduction)
+2. [Add a new platform](#2-add-a-new-platform)
+3. [Hardware Unique Key](#3-hardware-unique-key)
+4. [Secure Clock](#4-secure-clock)
+5. [Root and Chain of Trust](#5-root-and-chain-of-trust)
+6. [Hardware Crypto IP](#6-hardware-crypto-ip)
+7. [Power Management / PSCI](#7-power-management--psci)
+8. [Memory firewalls / TZASC](#8-memory-firewalls--tzasc)
+9. [Trusted Application private/public keypair](#9-trusted-application-privatepublic-keypair)
+
+## 1. Introduction
+---------------
+This document serves a dual purpose:
+* Serve as a base for getting OP-TEE up and running on a new device with initial
+  xtest validation passing. This is the first part of this document (section 2).
+* Highlight the missing pieces if you intend to make intend to make a real
+  secure product, that is what the second part of this document is about.
+
+We are trying our best to implement full end to end security in OP-TEE in a
+generic way, but due to the nature of devices being different, NDA etc, it is
+not always possible for us to do so and in those cases, we most often try to
+write a generic API, but we will just stub the code. This porting guideline
+highlights the missing pieces that must be addressed in a real secure consumer
+device. Hopefully we will sooner or later get access to devices where we at
+least can make reference implementations publicly available to everyone for the
+missing pieces we are talking about here.
+
+## 2. Add a new platform
+The first thing you need to do after you have decided to port OP-TEE to another
+device is to add a new platform device. That can either be adding a new platform
+variant (`PLATFORM_FLAVOR`) if it is a device from a family already supported,
+or it can be a brand new platform family (`PLATFORM`). Typically this initial
+setup involve configuring UART, memory addresses etc. For simplicity let us call
+our fictive platform for "gendev" just so we have something to refer to when
+writing examples further down.
+
+### 2.1 core/arch/arm
+In `core/arch/arm` you will find all the currently supported devices. That is
+where you are supposed to add a new platform or modify an existing one.
+Typically you will find this set of files in a specific platform folder:
+```bash
+$ ls
+conf.mk  kern.ld.S  link.mk  main.c  platform_config.h  sub.mk
+```
+
+So for the gendev platform it means that the files should be placed in this
+folder:
+```bash
+core/arch/arm/plat-gendev
+```
+
+##### conf.mk
+This is the device specific makefile where you define configurations unique to
+your platform. Add good start for a new platform would be:
+```Makefile
+PLATFORM_FLAVOR ?= gendev-flav
+PLATFORM_FLAVOR_$(PLATFORM_FLAVOR) := y
+
+# 32-bit flags
+arm32-platform-cpuarch := cortex-a15
+arm32-platform-cflags += -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags += -mcpu=$(arm32-platform-cpuarch)
+arm32-platform-aflags += -mfpu=neon
+
+# Running 32-bit core?
+CFG_ARM32_core ?= y
+ta-targets = ta_arm32
+
+# How many threads are enabled in TEE core
+CFG_NUM_THREADS ?= 4
+
+# What kind of UART should be used?
+CFG_8250_UART ?= y
+#CFG_PL011 ?= y
+
+# Enable power management stubs
+CFG_PM_STUBS := y
+
+# Use the generic boot
+CFG_GENERIC_BOOT := y
+
+# Enable internal tests by default
+CFG_TEE_CORE_EMBED_INTERNAL_TESTS ?= y
+
+# User software random number generator
+CFG_WITH_SOFTWARE_PRNG ?= y
+
+# Does the device support crypto extensions?
+CFG_CRYPTO_WITH_CE ?= n
+```
+There are probably quite a few other flags that could be useful or even
+necessary. Please refer to the other `conf.mk` file in the already existing
+platforms.
+
+##### kern.ld.S
+This is your linker script. As it turns out, most of the existing platforms use
+the same linker script and therefore most likely you will only need to add this
+single line to the file:
+```
+#include "../kernel/kern.ld.S"
+```
+
+##### link.mk
+This is the makefile for the linker, just as for the linker script, most
+platforms use the same and generic makefile for the linker, so adding only this
+should probably be sufficient:
+```Makefile
+include core/arch/arm/kernel/link.mk
+```
+
+##### main.c
+This platform specific file will contain power management handlers and code
+related to the UART. We will talk more about the information related to the
+handlers further down in this document. For our gendev device it could look like
+this (here we are excluding the necessary license header to save some space):
+
+```c
+#include <console.h>
+#include <drivers/serial8250_uart.h>
+#include <kernel/generic_boot.h>
+#include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
+#include <mm/core_memprot.h>
+#include <mm/tee_pager.h>
+#include <platform_config.h>
+#include <stdint.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+
+static void main_fiq(void)
+{
+	panic();
+}
+
+static const struct thread_handlers handlers = {
+	.std_smc = tee_entry_std,
+	.fast_smc = tee_entry_fast,
+	.nintr = main_fiq,
+	.cpu_on = cpu_on_handler,
+	.cpu_off = pm_do_nothing,
+	.cpu_suspend = pm_do_nothing,
+	.cpu_resume = pm_do_nothing,
+	.system_off = pm_do_nothing,
+	.system_reset = pm_do_nothing,
+};
+
+const struct thread_handlers *generic_boot_get_handlers(void)
+{
+	return &handlers;
+}
+
+/*
+ * Register the physical memory area for peripherals etc. Here we are
+ * registering the UART console.
+ */
+register_phys_mem(MEM_AREA_IO_NSEC, CONSOLE_UART_BASE, SERIAL8250_UART_REG_SIZE);
+
+static struct serial8250_uart_data console_data __early_bss;
+
+void console_init(void)
+{
+	serial8250_uart_init(&console_data, CONSOLE_UART_BASE,
+			     CONSOLE_UART_CLK_IN_HZ, CONSOLE_BAUDRATE);
+	register_serial_console(&console_data.chip);
+}
+```
+
+##### platform_config.h
+This is a mandatory header file for every platform, since there are several
+files relaying upon the existence of this particular file. This file is where
+you will find the major differences between different platforms, since this is
+where you do the memory configuration, define base addresses etc. we are going to
+list a few here, but it probably makes more sense to have a look at the already
+existing `platform_config.h` files for the other platforms. Our fictive gendev
+could look like this:
+
+```c
+#ifndef PLATFORM_CONFIG_H
+#define PLATFORM_CONFIG_H
+
+/* Make stacks aligned to data cache line length */
+#define STACK_ALIGNMENT		64
+
+#ifdef ARM64
+#ifdef CFG_WITH_PAGER
+#error "Pager not supported for ARM64"
+#endif
+#endif /* ARM64 */
+
+/* 8250 UART */
+#define CONSOLE_UART_BASE	0xcafebabe /* UART0 */
+#define CONSOLE_BAUDRATE	115200
+#define CONSOLE_UART_CLK_IN_HZ	19200000
+
+#define DRAM0_BASE		0x00000000
+#define DRAM0_SIZE		0x40000000
+
+/* Below ARM-TF */
+#define CFG_SHMEM_START		0x08000000
+#define CFG_SHMEM_SIZE		(4 * 1024 * 1024)
+
+/* If your device has SRAM */
+#define TZSRAM_BASE		0x3F000000
+#define TZSRAM_SIZE		(200 * 1024)
+
+/* Otherwise or in addition, use DDR */
+#define TZDRAM_BASE		0x60000000
+#define TZDRAM_SIZE		(32 * 1024 * 1024)
+
+#define CFG_TEE_CORE_NB_CORE	4
+
+#define CFG_TEE_RAM_VA_SIZE	(4 * 1024 * 1024)
+
+#define CFG_TEE_LOAD_ADDR	(TZDRAM_BASE + 0x20000)
+
+#define CFG_TEE_RAM_PH_SIZE	CFG_TEE_RAM_VA_SIZE
+#define CFG_TEE_RAM_START	TZDRAM_BASE
+
+#define CFG_TA_RAM_START	ROUNDUP((TZDRAM_BASE + CFG_TEE_RAM_VA_SIZE), \
+					CORE_MMU_DEVICE_SIZE)
+#define CFG_TA_RAM_SIZE        (16 * 1024 * 1024)
+#endif /* PLATFORM_CONFIG_H */
+```
+This is minimal amount of information in the `platform_config.h` file. I.e, the
+memory layout for on-chip and external RAM. Note that parts of the DDR typically
+will need to be shared with normal world, so there is need for some kind of
+memory firewall for this (more about that further down). As you can see we have
+also added the UART configuration here, i.e., the `DEVICE0_xyz` part.
+
+### 2.2 Devices officially in OP-TEE?
+We do encourage everyone to submit their board support to the OP-TEE project
+itself, so it becomes part of the official releases and will be maintained by
+the OP-TEE community itself. If you intend to do so, then there are a few more
+things that you are supposed to do.
+
+#### 2.2.1 Update README.md
+There is a section ([3. Platforms Supported]) that lists all devices officially
+supported in OP-TEE, that is where you also shall list your device. It should
+contain the name of the platform, then composite `PLATFORM` flag and whether the
+device is publicly available or not.
+
+#### 2.2.2 Update travis.xml
+Since we are using Travis to test pull request etc, we would like that you also
+all your device to the [travis] file, so that it will at least be built when
+someone is doing a pull request. Add a line saying:
+
+```
+- PLATFORM=gendev  PLATFORM_FLAVOR=gendev-flav  make -j8 all -s
+```
+#### 2.2.3 Maintainer
+If you are submitting the board support upstream and cannot give Linaro
+maintainers a device, then we are going to ask you to become the maintainer for
+the device you have added. This means that you should also update the
+[MAINTAINERS.md] file accordingly. By being a maintainer for a device you are
+responsible to keep it up to date and you will be asked every quarter as part of
+the OP-TEE release schedule to test your device running the latest OP-TEE
+software.
+
+#### 2.2.4 Update build.git
+This isn't strictly necessary, but we are trying to create repo setup(s) for the
+device(s) that we are in charge of. That makes it very easy for newcomers to get
+started with a certain platform. So please consider creating a new [manifest]
+for the device you have added to OP-TEE.
+
+## 3. Hardware Unique Key
+Most devices have some kind of Hardware Unique Key (HUK) that is mainly used to
+derive other keys. The HUK could for example be used when deriving keys used in
+secure storage etc. The important thing with the HUK is that it needs to be well
+protected and in the best case the HUK should never ever be readable directly
+from software, not even from the secure side. There are different solutions to
+this, crypto accelerator might have support for it or, it could involve another
+secure co-processor.
+
+In OP-TEE the HUK **is** just **stubbed** and you will see that in the function
+called `tee_otp_get_hw_unique_key()` in `core/include/kernel/tee_common_otp.h`.
+In a real secure product you **must** replace this with something else. If your
+device lacks the hardware support for a HUK, then you must at least change this
+to something else than just zeroes. But, remember it is not good secure practice
+to store a key in software, especially not the key that is the root for
+everything else, so this is not something we recommend that you should do.
+
+## 4. Secure Clock
+The Time API in GlobalPlatform Internal Core API specification defines three
+sources of time; system time, TA persistent time and REE time. The REE time
+is by nature considered as an unsecure source of time, but the other two should
+in a fully trustable hardware make use of trustable source of time, i.e., a
+secure clock. Note that from GlobalPlatform point of view it is not required to
+make use of a secure clock, i.e., it is OK to use time from REE, but the level
+of trust should be reflected by the `gpd.tee.systemTime.protectionLevel`
+property and the `gpd.tee.TAPersistentTime.protectionLevel` property (100=REE
+controlled clock, 1000=TEE controlled clock). So the functions that one needs to
+pay attention to are `tee_time_get_sys_time()` and `tee_time_get_ta_time()`. If
+your hardware has a secure clock, then you probably want to change the
+implementation there to instead use the secure clock (and then you would also
+need to update the property accordingly, i.e.,
+`tee_time_get_sys_time_protection_level()` and the variable `ta_time_prot_lvl`
+in `tee_svc.c`).
+
+## 5. Root and Chain of Trust
+To be able to assure that your devices are running the (untampered) binaries you
+intended to run you will need to establish some kind of trust anchor on the
+devices.
+
+The most common way of doing that is to put the root public key in some
+read only memory on the device. Quite often SoC's/OEM's stores public key(s)
+directly or the hash(es) of the public key(s) in [OTP]. When the boot ROM (which
+indeed needs to be ROM) is about to load the first stage bootloader it typically
+reads the public key from the software binary itself, hash the key and compare
+it to the key in OTP. If they are matching, then the boot ROM can be sure that
+the first stage bootloader was indeed signed with the corresponding private key.
+
+In OP-TEE you will not find any code at all related to this and this is a good
+example when it is hard for us to do this in a generic way since device
+manufacturers all tend to do this in their own unique way and they are not very
+keen on sharing their low level boot details and security implementation with
+the rest of the world. This is especially true on ARMv7-A. For ARMv8-A it looks
+bit better, since ARM in ARM Trusted Firmware have implemented and defined how a
+abstract the chain of trust (see [auth-framework.md]). We have successfully
+verified OP-TEE by using the authentication framework from ARM Trusted Firmware
+(see [optee_with_auth_framework.md] for the details).
+
+## 6. Hardware Crypto IP
+By default OP-TEE uses a software crypto library (currently LibTomCrypt) and you
+have the ability to enable Crypto Extensions that were introduced with ARMv8-A
+(if the device is capable of that). Some of the devices we have in our hands do
+have hardware crypto IP's, but due to NDA's etc it has not been possible to
+enable it. If you have a device capable of doing crypto operations on a
+dedicated crypto block and you prefer to use that in favor for the software
+implementation, then you will need to implement a new `crypto_ops` structure and
+write the low level driver that communicates with the device. Our [crypto.md]
+file describes how to add and implement a new `struct crypto_ops`. Since the
+communication with crypto blocks tends to be quite different depending on what
+kind of crypto block you have, we have not written how that should be done. It
+might be that we do that in the future when get hold of a device where we can
+use the crypto block.
+
+## 7. Power Management / PSCI
+In section 2 when we talked about the file `main.c`, we added a couple of
+handlers related to power management, we are talking about the following lines:
+```
+	.cpu_on = cpu_on_handler,
+	.cpu_off = pm_do_nothing,
+	.cpu_suspend = pm_do_nothing,
+	.cpu_resume = pm_do_nothing,
+	.system_off = pm_do_nothing,
+	.system_reset = pm_do_nothing,
+```
+The only function that actually does something there is the `cpu_on` function,
+the rest of them are stubbed. The main reason for that is because we think that
+how to suspend and resume is a device dependent thing. The code in OP-TEE is
+prepared so that callbacks etc from ARM Trusted Firmware will be routed to
+OP-TEE, but since the function(s) are just stubbed we will not do anything and
+just return. In a real production device, you would probably want to save and
+restore CPU states, secure hardware IPs' registers and TZASC and other memory
+firewall related setting when these callbacks are being called.
+
+## 8. Memory firewalls / TZASC
+ARM have defined a system IP / SoC peripheral called TrustZone Address Space
+Controller (TZASC, see [TZC-380] and [TZC-400]). TZASC can be used to configure
+DDR memory into separate regions in the physcial address space, where each
+region can have an individual security level setting. After enabling TZASC, it
+will perform security checks on transactions to memory or peripherals. It is not
+always the case that TZASC is on a device, in some cases the SoC has developed
+something equivalent. In OP-TEE this is very well reflected, i.e., different
+platforms have different ways of protecting their memory. On ARMv8-A platforms
+we are in most of the cases using ARM Trusted Firmware as the boot firmware and
+there the secure bootloader is the one that configures secure vs non-secure
+memory using TZASC (see [plat_arm_security_setup] in ARM-TF). The takeaway here
+is that you must make sure that you have configured whatever memory firewall your
+device has such that it has a secure and a non-secure memory area.
+
+## 9. Trusted Application private/public keypair
+By default all Trusted Applications (TA's) are signed with the pre-generated
+2048-bit RSA development key (private key). This key is located in the `keys`
+folder (in the root of optee_os.git) and is named `default_ta.pem`. This key
+**must** be replaced with your own key and you should **never ever** check-in
+this private key in the source code tree when in use in a real product. The
+recommended way to store private keys is to use some kind of [HSM] (Hardware
+Security Module), but an alternative would be temporary put the private key on a
+computer considered as secure when you are about to sign TA's intended to be
+used in real products. Typically it is only a few number of people having access
+to this type of key in company. The key handling in OP-TEE is currently a bit
+limited since we only support a single key which is used for all TA's. We have
+plans on extending this to make it a bit more flexible. Exactly when that will
+happen has not been decided yet.
+
+[3. Platforms Supported]: ../README.md#3-platforms-supported
+[auth-framework.md]: https://github.com/ARM-software/arm-trusted-firmware/blob/master/docs/auth-framework.md
+[crypto.md]: crypto.md
+[HSM]: https://en.wikipedia.org/wiki/Hardware_security_module
+[manifest]: https://github.com/OP-TEE/build#6-manifests
+[MAINTAINERS.md]: ../MAINTAINERS.md
+[optee_with_auth_framework.md]: optee_with_auth_framework.md
+[OTP]: https://en.wikipedia.org/wiki/Programmable_read-only_memory
+[plat_arm_security_setup]: https://github.com/ARM-software/arm-trusted-firmware/search?utf8=%E2%9C%93&q=plat_arm_security_setup&type=
+[TZC-380]: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.ddi0431c/index.html
+[TZC-400]: http://infocenter.arm.com/help/index.jsp?topic=/com.arm.doc.100325_0001_02_en/index.html
+[travis]: ../.travis.yml
diff --git a/documentation/secure_storage.md b/documentation/secure_storage.md
index 9202325..9aa370c 100644
--- a/documentation/secure_storage.md
+++ b/documentation/secure_storage.md
@@ -19,7 +19,7 @@ by CFG_REE_FS=y.
 of an eMMC device, and is enabled by setting `CFG_RPMB_FS=y`. It is described
 in [secure_storage_rpmb.md](secure_storage_rpmb.md).
 - The third one stores objects in a SQLite database in normal world. It is
-enabled by `CFG_SQL_FS=y`. See [secure_storage_sql.md](secure_storage_sql.db).
+enabled by `CFG_SQL_FS=y`. See [secure_storage_sql.md](secure_storage_sql.md).
 
 It is possible to use the normal world filesystems and the RPMB implementations
 simultaneously. For this, three OP-TEE specific storage identifiers have been
@@ -42,6 +42,7 @@ The rest of this document describes the REE FS only.
 storage service calls
 - **[core/tee/tee_ree_fs.c](../core/tee/tee_ree_fs):** TEE file system & REE
 file operation interface
+- **[core/tee/fs_htree.c](../core/tee/fs_htree.c):** Hash tree
 - **[core/tee/tee_fs_key_manager.c](../core/tee/tee_fs_key_manager.c):** Key
 manager
 - **[lib/libutee/](../lib/libutee/):** GlobalPlatform Internal API library
@@ -72,7 +73,7 @@ Below is an excerpt from the specification listing the most vital requirements:
    instances of that TA but separated from the other TAs.
 5. The Trusted Storage must provide a minimum level of protection against
    rollback attacks. It is accepted that the actually physical storage may be in
-   an unsecure areas and so is vulnerable to actions from outside of the TEE.
+   an insecure area and so is vulnerable to actions from outside of the TEE.
    Typically, an implementation may rely on the REE for that purpose (protection
    level 100) or on hardware assets controlled by the TEE (protection level
    1000).
@@ -87,45 +88,6 @@ file system. For each TA, OP-TEE use the TA's UUID to create a standalone folder
 for it under the secure storage space folder. For a persistent object belonging
 to a specific TA, OP-TEE creates a TEE file is object-id under the TA folder.
 
-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 instruct 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 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 ]
-```
-
-The reason why we store the TEE file content in many small blocks is to
-accelerate the file update speed when handling a large file. The block size
-(FILE_BLOCK_SIZE) and the maximum number of blocks of a TEE file
-(NUM_BLOCKS_PER_FILE) are defined in
-[core/tee/tee_ree_fs.c](../core/tee/tee_ree_fs.c).
-
-For now, the default block size is 4KB and the maximum number of blocks of a
-TEE file is 1024.
-
 ## Key Manager
 
 Key manager is an component in TEE file system, and is responsible for handling
@@ -177,22 +139,57 @@ PRNG (pesudo random number generator) for the TEE file and store the encrypted
 FEK in meta file. FEK is used for encrypting/decrypting the TEE file information
 stored in meta file or the data stored in block file.
 
+## Hash Tree
+
+The hash tree is responsible for handling data encryption and decryption of
+a secure storage file.
+
+The hash tree is implemented as a binary tree where
+each node (`struct tee_fs_htree_node_image` below) in the tree protects its
+two child nodes and a data block.
+
+The meta data is stored in a header (`struct tee_fs_htree_image` below)
+which also protects the top node.
+
+All fields (header, nodes, and blocks) are duplicated with two versions, 0
+and 1, to ensure atomic updates. See
+[core/tee/fs_htree.c](../core/tee/fs_htree.c) for details.
+
 ### Meta Data Encryption Flow
 
 ![Meta Data Encryption](images/secure_storage/meta_data_encryption.png
 "Meta data encryption")
 
 A new meta IV will be generated by PRNG when a meta data needs to be updated.
-The default size of meta IV is defined in
-[core/include/tee/tee_fs_key_manager.h](../core/include/tee/tee_fs_key_manager.h)
+The size of meta IV is defined in
+[core/include/tee/fs_htree.h](../core/include/tee/fs_htree.h)
 
-The data structure of meta data is defined in
-[core/tee/tee_fs_private.h](../core/tee/tee_fs_private.h) as follows:
+The data structures of meta data and node data are defined in
+[core/include/tee/fs_htree.h](../core/include/tee/fs_htree.h) as follows:
 
 ```
-struct tee_fs_file_info {
-    size_t length;
-    uint32_t backup_version_table[NUM_BLOCKS_PER_FILE / 32];
+struct tee_fs_htree_node_image {
+        uint8_t hash[TEE_FS_HTREE_HASH_SIZE];
+        uint8_t iv[TEE_FS_HTREE_IV_SIZE];
+        uint8_t tag[TEE_FS_HTREE_TAG_SIZE];
+        uint16_t flags;
+};
+
+struct tee_fs_htree_meta {
+        uint64_t length;
+};
+
+struct tee_fs_htree_imeta {
+        struct tee_fs_htree_meta meta;
+        uint32_t max_node_id;
+};
+
+struct tee_fs_htree_image {
+        uint8_t iv[TEE_FS_HTREE_IV_SIZE];
+        uint8_t tag[TEE_FS_HTREE_TAG_SIZE];
+        uint8_t enc_fek[TEE_FS_HTREE_FEK_SIZE];
+        uint8_t imeta[sizeof(struct tee_fs_htree_imeta)];
+        uint32_t counter;
 };
 ```
 
@@ -202,8 +199,8 @@ struct tee_fs_file_info {
 "Block data encryption")
 
 A new block IV will be generated by PRNG when a block data needs to be updated.
-The default size of block IV is defined in
-[core/include/tee/tee_fs_key_manager.h](../core/include/tee/tee_fs_key_manager.h)
+The size of block IV is defined in
+[core/include/tee/fs_htree.h](../core/include/tee/fs_htree.h)
 
 ## Atomic Operation
 
diff --git a/documentation/secure_storage_sql.md b/documentation/secure_storage_sql.md
index 675c794..620e477 100644
--- a/documentation/secure_storage_sql.md
+++ b/documentation/secure_storage_sql.md
@@ -45,21 +45,18 @@ the SQL filesystem is the **tee_file_operations** structure `sql_fs_ops`.
 ## The SQL filesystem
 
 The secure side of the SQL FS implementation is mostly in
-[core/tee/tee_sql_fs.c](../core/tee/tee_sql_fs.c). This file maps the operations
-in `sql_fs_ops` such as `open`, `truncate`, `read`, `write`, `seek`
-and so on, to similar operations on a file that is a container for the encrypted
-data and associated meta-data. This container is created and manipulated by
-`tee-supplicant` on request from the secure OS. Its layout is like this:
-
-```
-   [       File meta-data       ] [      Block #0        ] [Block #1]...
-   [meta_header|sql_fs_file_meta] [block_header|user data] [        ]...
-```
+[core/tee/tee_sql_fs.c](../core/tee/tee_sql_fs.c). This file maps the
+operations in `sql_fs_ops` such as `open`, `truncate`, `read`, `write`
+and so on, to similar operations on a file that is a container for
+the encrypted data and associated meta-data. This container is created and
+manipulated by `tee-supplicant` on request from the secure OS. Its logical
+layout is similar to REE FS except that there's only a single version of
+each field as atomic updates are ensured by **libsqlfs** instead.
 
 How this file is stored in the SQLite database is private to **libsqlfs**. From
 the point of view of OP-TEE, it is a byte-addressable linear file on which
 atomic updates can be performed through a standard interface (`open`,
-`truncate`, `seek`, `read`, `write`...) with the addition of `begin_transaction`
+`truncate`, `read`, `write`...) with the addition of `begin_transaction`
 and `end_transaction`.
 
 ## Encryption
diff --git a/lib/libutee/include/pta_invoke_tests.h b/lib/libutee/include/pta_invoke_tests.h
new file mode 100644
index 0000000..1667af8
--- /dev/null
+++ b/lib/libutee/include/pta_invoke_tests.h
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) 2017, 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 __PTA_INVOKE_TESTS_H
+#define __PTA_INVOKE_TESTS_H
+
+#define PTA_INVOKE_TESTS_UUID \
+		{ 0xd96a5b40, 0xc3e5, 0x21e3, \
+			{ 0x87, 0x94, 0x10, 0x02, 0xa5, 0xd5, 0xc6, 0x1b } }
+
+/* Trace some hello string. Parameters are not used/checked. */
+#define PTA_INVOKE_TESTS_CMD_TRACE		0
+
+/*
+ * Types of parameter drives the test sequences:
+ * - test on value parameters
+ * - test on SHM memory reference parameters
+ * - test on SDP memory reference parameters
+ */
+#define PTA_INVOKE_TESTS_CMD_PARAMS		1
+
+/* Run some core internal tests. Parameters are not used/checked. */
+#define PTA_INVOKE_TESTS_CMD_SELF_TESTS		2
+
+/*
+ * Secure data path: check that pTA can copy data from non-secure shared memory
+ * to SDP secure memory
+ *
+ * [in]     memref[0]        source (non-secure shared memory)
+ * [out]    memref[1]        destination (SDP secure memory)
+ */
+#define PTA_INVOKE_TESTS_CMD_COPY_NSEC_TO_SEC	3
+
+/*
+ * Secure data path: check that pTA can read data from SDP secure memory and
+ * write it back. Data are processed so that client check the expected
+ * read/write sequence succeed.
+ *
+ * [in/out]     memref[0]        SDP secure buffer to read from and write to
+ */
+#define PTA_INVOKE_TESTS_CMD_READ_MODIFY_SEC	4
+
+/*
+ * Secure data path: check that pTA can copy data from SDP secure memory to
+ * non-secure shared memory
+ *
+ * [in]     memref[0]        source (SDP secure memory)
+ * [out]    memref[1]        destination (non-secure shared memory)
+ */
+#define PTA_INVOKE_TESTS_CMD_COPY_SEC_TO_NSEC	5
+
+/*
+ * Tests FS hash-tree corner cases in error handling
+ */
+#define PTA_INVOKE_TESTS_CMD_FS_HTREE		6
+
+#endif /*__PTA_INVOKE_TESTS_H*/
+
diff --git a/lib/libutee/include/tee_internal_api.h b/lib/libutee/include/tee_internal_api.h
index e3eba8b..2c050e3 100644
--- a/lib/libutee/include/tee_internal_api.h
+++ b/lib/libutee/include/tee_internal_api.h
@@ -34,9 +34,4 @@
 #include <tee_api_types.h>
 #include <tee_ta_api.h>
 
-#define SLogTrace(...)
-#define SLogError(...)
-#define SLogWarning(...)
-#define S_VAR_NOT_USED(x)
-
 #endif
diff --git a/lib/libutee/include/user_ta_header.h b/lib/libutee/include/user_ta_header.h
index aef5500..6691ebc 100644
--- a/lib/libutee/include/user_ta_header.h
+++ b/lib/libutee/include/user_ta_header.h
@@ -36,11 +36,7 @@
 #define TA_FLAG_SINGLE_INSTANCE		(1 << 2)
 #define TA_FLAG_MULTI_SESSION		(1 << 3)
 #define TA_FLAG_INSTANCE_KEEP_ALIVE	(1 << 4) /* remains after last close */
-/*
- * TA_FLAG_UNSAFE_NW_PARAMS: May manipulate some secure memory based on
- * physical pointers from non-secure world
- */
-#define TA_FLAG_UNSAFE_NW_PARAMS	(1 << 5)
+#define TA_FLAG_SECURE_DATA_PATH	(1 << 5) /* accesses SDP memory */
 #define TA_FLAG_REMAP_SUPPORT		(1 << 6) /* use map/unmap syscalls */
 #define TA_FLAG_CACHE_MAINTENANCE	(1 << 7) /* use cache flush syscall */
 
diff --git a/lib/libutee/tee_user_mem.c b/lib/libutee/tee_user_mem.c
index 509a8ad..870ee92 100644
--- a/lib/libutee/tee_user_mem.c
+++ b/lib/libutee/tee_user_mem.c
@@ -340,7 +340,7 @@ void *tee_user_mem_alloc(size_t len, uint32_t hint)
 		    (((uintptr_t) cp) & 0xFF) ^ 0xC5;
 #endif
 
-		PB(TRACE_DEBUG, "Allocate: ", (void *)e);
+		PB(TRACE_FLOW, "Allocate: ", (void *)e);
 
 		buf = buf_addr(e);
 
@@ -439,7 +439,7 @@ void tee_user_mem_free(void *buffer)
 	cp = elem_addr(buffer);
 	e = (struct user_mem_elem *)(void *)cp;
 
-	PB(TRACE_DEBUG, "Free: ", (void *)e);
+	PB(TRACE_FLOW, "Free: ", (void *)e);
 
 #if (CFG_TEE_CORE_USER_MEM_DEBUG == 1)
 	if (!check_elem(e)) {
diff --git a/lib/libutils/isoc/bget_malloc.c b/lib/libutils/isoc/bget_malloc.c
index 9cb2751..a4f7759 100644
--- a/lib/libutils/isoc/bget_malloc.c
+++ b/lib/libutils/isoc/bget_malloc.c
@@ -117,7 +117,8 @@ static uint32_t malloc_lock(void)
 {
 	uint32_t exceptions;
 
-	exceptions = thread_mask_exceptions(THREAD_EXCP_IRQ | THREAD_EXCP_FIQ);
+	exceptions = thread_mask_exceptions(
+			THREAD_EXCP_NATIVE_INTR | THREAD_EXCP_FOREIGN_INTR);
 	cpu_spin_lock(&__malloc_spinlock);
 	return exceptions;
 }
diff --git a/mk/aosp_optee.mk b/mk/aosp_optee.mk
index 8017759..369ed53 100644
--- a/mk/aosp_optee.mk
+++ b/mk/aosp_optee.mk
@@ -1,13 +1,14 @@
 ##########################################################
 ## Common mk file used for Android to compile and       ##
-## integrate OP-TEE related components                   ##
-## Following flags need to be defined in device.mk    ##
+## integrate OP-TEE related components                  ##
+## Following flags need to be defined in optee*.mk      ##
 ##    OPTEE_OS_DIR                                      ##
 ##    OPTEE_TA_TARGETS                                  ##
 ##    OPTEE_CFG_ARM64_CORE                              ##
 ##    OPTEE_PLATFORM                                    ##
 ##    OPTEE_PLATFORM_FLAVOR                             ##
-## And BUILD_OPTEE_MK needs to be defined in device.mk  ##
+##    OPTEE_EXTRA_FLAGS (optional)                      ##
+## And BUILD_OPTEE_MK needs to be defined in optee*.mk  ##
 ## to point to this file                                ##
 ##                                                      ##
 ## local_module needs to be defined before including    ##
@@ -50,7 +51,8 @@ BUILD_OPTEE_OS:
 		CFG_ARM64_core=$(OPTEE_CFG_ARM64_CORE) \
 		PLATFORM=$(OPTEE_PLATFORM) \
 		PLATFORM_FLAVOR=$(OPTEE_PLATFORM_FLAVOR) \
-		$(CROSS_COMPILE_LINE)
+		$(CROSS_COMPILE_LINE) \
+		$(OPTEE_EXTRA_FLAGS)
 	@echo "Finished building optee_os..."
 
 endif
diff --git a/mk/config.mk b/mk/config.mk
index a18470d..6fe90f1 100644
--- a/mk/config.mk
+++ b/mk/config.mk
@@ -102,7 +102,7 @@ TEE_IMPL_VERSION ?= $(shell git describe --always --dirty=-dev 2>/dev/null || ec
 # with limited depth not including any tag, so there is really no guarantee
 # that TEE_IMPL_VERSION contains the major and minor revision numbers.
 CFG_OPTEE_REVISION_MAJOR ?= 2
-CFG_OPTEE_REVISION_MINOR ?= 3
+CFG_OPTEE_REVISION_MINOR ?= 4
 
 # Trusted OS implementation manufacturer name
 CFG_TEE_MANUFACTURER ?= LINARO
@@ -119,9 +119,6 @@ CFG_TEE_FW_MANUFACTURER ?= FW_MAN_UNDEF
 # TEE_STORAGE_PRIVATE is passed to the trusted storage API)
 CFG_REE_FS ?= y
 
-# REE filesystem block cache support
-CFG_REE_FS_BLOCK_CACHE ?= n
-
 # RPMB file system support
 CFG_RPMB_FS ?= n
 
@@ -230,3 +227,7 @@ endif
 # CFG_GP_SOCKETS
 # Enable Global Platform Sockets support
 CFG_GP_SOCKETS ?= y
+
+# Enable Secure Data Path support in OP-TEE core (TA may be invoked with
+# invocation parameters referring to specific secure memories).
+CFG_SECURE_DATA_PATH ?= n
diff --git a/scripts/checkpatch.sh b/scripts/checkpatch.sh
index 94d8c4f..3d8f1a6 100755
--- a/scripts/checkpatch.sh
+++ b/scripts/checkpatch.sh
@@ -10,7 +10,7 @@ hash $CHECKPATCH 2>/dev/null ||
 usage() {
   SCR=$(basename "$0")
   echo "Usage: $SCR [--working]                 Check working area"
-  echo "       $SCR <commit>                    Check specific commit"
+  echo "       $SCR <commit>...                 Check specific commit(s)"
   echo "       $SCR --diff <commit1> <commit2>  Check diff commit1...commit2"
   echo "       $SCR --cached                    Check staging area"
   echo "       $SCR --help                      This help"
@@ -35,8 +35,8 @@ case "$op" in
 		usage
 		;;
 	*)
-		echo "Checking commit:  "
-		checkpatch "$1"
+		echo "Checking commit(s):"
+		for c in $*; do checkpatch $c; done
 		;;
 
 esac
diff --git a/scripts/checkpatch_inc.sh b/scripts/checkpatch_inc.sh
index 487b60e..7d3e20e 100644
--- a/scripts/checkpatch_inc.sh
+++ b/scripts/checkpatch_inc.sh
@@ -2,7 +2,8 @@
 
 CHECKPATCH="${CHECKPATCH:-checkpatch.pl}"
 # checkpatch.pl will ignore the following paths
-CHECKPATCH_IGNORE=$(echo core/lib/lib{fdt,tomcrypt} lib/lib{png,utils,zlib})
+CHECKPATCH_IGNORE=$(echo core/lib/lib{fdt,tomcrypt} lib/lib{png,utils,zlib} \
+		core/arch/arm/plat-ti/api_monitor_index.h)
 _CP_EXCL=$(for p in $CHECKPATCH_IGNORE; do echo ":(exclude)$p" ; done)
 
 function _checkpatch() {
diff --git a/ta/ta.mk b/ta/ta.mk
index d4688a7..3b99b05 100644
--- a/ta/ta.mk
+++ b/ta/ta.mk
@@ -53,6 +53,7 @@ incfiles-extra-host += lib/libutils/ext/include/types_ext.h
 incfiles-extra-host += $(conf-file)
 incfiles-extra-host += $(conf-mk-file)
 incfiles-extra-host += core/include/tee/tee_fs_key_manager.h
+incfiles-extra-host += core/include/tee/fs_htree.h
 incfiles-extra-host += core/include/signed_hdr.h
 
 #
diff --git a/tef-optee_os b/tef-optee_os
deleted file mode 160000
-Subproject 1fed20f5471aa0dad5e4b4f79d1f2843ac88734